Revision 9f547ef7
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.cpp | ||
|---|---|---|
| 56 | 56 |
namespace scoutsim |
| 57 | 57 |
{
|
| 58 | 58 |
Scout::Scout(const ros::NodeHandle& nh, |
| 59 |
const wxImage& scout_image, |
|
| 60 |
const Vector2& pos, |
|
| 61 |
float orient) |
|
| 59 |
const wxImage& scout_image,
|
|
| 60 |
const Vector2& pos,
|
|
| 61 |
float orient)
|
|
| 62 | 62 |
: node (nh) |
| 63 | 63 |
, scout_image(scout_image) |
| 64 | 64 |
, pos(pos) |
| 65 | 65 |
, orient(orient) |
| 66 |
, lin_vel(0.0) |
|
| 67 |
, ang_vel(0.0) |
|
| 66 |
, motor_fl_speed(0) |
|
| 67 |
, motor_fr_speed(0) |
|
| 68 |
, motor_bl_speed(0) |
|
| 69 |
, motor_br_speed(0) |
|
| 70 |
, fl_ticks(0) |
|
| 71 |
, fr_ticks(0) |
|
| 72 |
, bl_ticks(0) |
|
| 73 |
, br_ticks(0) |
|
| 68 | 74 |
, pen_on(true) |
| 69 | 75 |
, pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) |
| 70 | 76 |
{
|
| ... | ... | |
| 78 | 84 |
set_pen_srv = node.advertiseService("set_pen",
|
| 79 | 85 |
&Scout::setPenCallback, |
| 80 | 86 |
this); |
| 87 |
query_encoders_srv = |
|
| 88 |
node.advertiseService("query_encoders",
|
|
| 89 |
&Scout::query_encoders_callback, |
|
| 90 |
this); |
|
| 81 | 91 |
|
| 82 | 92 |
meter = scout.GetHeight(); |
| 83 | 93 |
} |
| ... | ... | |
| 108 | 118 |
motor_br_speed = msg->br_speed; |
| 109 | 119 |
} |
| 110 | 120 |
|
| 111 |
// Assume that the two motors on the same side will be set to |
|
| 112 |
// roughly the same speed. Does not account for slip conditions |
|
| 113 |
// when they are set to different speeds. |
|
| 114 |
float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2; |
|
| 115 |
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; |
|
| 116 |
|
|
| 117 |
// Set the linear and angular speeds |
|
| 118 |
lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2; |
|
| 119 |
ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed); |
|
| 120 | 121 |
} |
| 121 | 122 |
|
| 122 | 123 |
float Scout::getSonar(float angle) |
| ... | ... | |
| 144 | 145 |
return true; |
| 145 | 146 |
} |
| 146 | 147 |
|
| 148 |
bool Scout::query_encoders_callback(encoders::query_encoders::Request&, |
|
| 149 |
encoders::query_encoders::Response& res) |
|
| 150 |
{
|
|
| 151 |
res.fl_distance = fl_ticks; |
|
| 152 |
res.fr_distance = fr_ticks; |
|
| 153 |
res.bl_distance = bl_ticks; |
|
| 154 |
res.br_distance = br_ticks; |
|
| 155 |
|
|
| 156 |
return true; |
|
| 157 |
} |
|
| 158 |
|
|
| 147 | 159 |
void Scout::update(double dt, wxMemoryDC& path_dc, |
| 148 | 160 |
const wxImage& path_image, wxColour background_color, |
| 149 | 161 |
float canvas_width, float canvas_height) |
| 150 | 162 |
{
|
| 163 |
// Assume that the two motors on the same side will be set to |
|
| 164 |
// roughly the same speed. Does not account for slip conditions |
|
| 165 |
// when they are set to different speeds. |
|
| 166 |
float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2; |
|
| 167 |
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; |
|
| 168 |
|
|
| 169 |
// Set the linear and angular speeds |
|
| 170 |
float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2; |
|
| 171 |
float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed); |
|
| 172 |
|
|
| 151 | 173 |
Vector2 old_pos = pos; |
| 152 | 174 |
|
| 175 |
// Update encoders |
|
| 176 |
/// @todo replace |
|
| 177 |
fl_ticks += (unsigned int) motor_fl_speed; |
|
| 178 |
fr_ticks += (unsigned int) motor_fr_speed; |
|
| 179 |
bl_ticks += (unsigned int) motor_bl_speed; |
|
| 180 |
br_ticks += (unsigned int) motor_br_speed; |
|
| 181 |
|
|
| 153 | 182 |
orient = fmod(orient + ang_vel * dt, 2*PI); |
| 154 | 183 |
pos.x += sin(orient + PI/2.0) * lin_vel * dt; |
| 155 | 184 |
pos.y += cos(orient + PI/2.0) * lin_vel * dt; |
Also available in: Unified diff