Revision 9f547ef7
ID | 9f547ef719e5b32f52210cdb56bc7cae9eb16c7d |
Added scoutsim support for encoders.
To see the query_encoders.srv (used for accessing this data) in action, use: rosrun libscout test_encoders. You will see a stream of encoder data.
scout/scoutsim/src/scout.h | ||
---|---|---|
52 | 52 |
#include <boost/shared_ptr.hpp> |
53 | 53 |
|
54 | 54 |
#include <motors/set_motors.h> |
55 |
#include <encoders/query_encoders.h> |
|
55 | 56 |
|
56 | 57 |
#include <scoutsim/Pose.h> |
57 | 58 |
#include <scoutsim/SetPen.h> |
... | ... | |
110 | 111 |
float getSonar(float angle); |
111 | 112 |
bool setPenCallback(scoutsim::SetPen::Request&, |
112 | 113 |
scoutsim::SetPen::Response&); |
114 |
bool query_encoders_callback(encoders::query_encoders::Request&, |
|
115 |
encoders::query_encoders::Response&); |
|
113 | 116 |
|
114 | 117 |
ros::NodeHandle node; |
115 | 118 |
|
... | ... | |
119 | 122 |
Vector2 pos; |
120 | 123 |
float orient; |
121 | 124 |
|
125 |
/// @todo should these be an array or something? |
|
126 |
|
|
122 | 127 |
// Keep track of the last commanded speeds sent to the sim |
123 | 128 |
int motor_fl_speed; |
124 | 129 |
int motor_fr_speed; |
125 | 130 |
int motor_bl_speed; |
126 | 131 |
int motor_br_speed; |
127 | 132 |
|
133 |
// Keep track of encoder ticks for each motor |
|
134 |
unsigned int fl_ticks; |
|
135 |
unsigned int fr_ticks; |
|
136 |
unsigned int bl_ticks; |
|
137 |
unsigned int br_ticks; |
|
138 |
|
|
128 | 139 |
// Each scout has a unique id number, which is also displayed on its image. |
129 | 140 |
int scout_id; |
130 | 141 |
|
131 |
float lin_vel; |
|
132 |
float ang_vel; |
|
133 | 142 |
bool pen_on; |
134 | 143 |
wxPen pen; |
135 | 144 |
|
... | ... | |
137 | 146 |
ros::Publisher pose_pub; |
138 | 147 |
ros::Publisher color_pub; |
139 | 148 |
ros::ServiceServer set_pen_srv; |
149 |
ros::ServiceServer query_encoders_srv; |
|
140 | 150 |
|
141 | 151 |
ros::WallTime last_command_time; |
142 | 152 |
|
Also available in: Unified diff