Revision 04114d13
ID | 04114d139191ab6a6893f0af9770f0a8ed341778 |
Fixed teleop with new units, and changes the refresh rate to be closer to true m/s.
scout/scoutsim/src/scout.cpp | ||
---|---|---|
63 | 63 |
wxBitmap *path_bitmap, |
64 | 64 |
float orient) |
65 | 65 |
: path_bitmap(path_bitmap) |
66 |
, sonar_visual_on(sonar_visual_on)
|
|
67 |
, node (nh)
|
|
66 |
, sonar_visual_on(sonar_visual_on)
|
|
67 |
, node (nh)
|
|
68 | 68 |
, scout_image(scout_image) |
69 | 69 |
, pos(pos) |
70 | 70 |
, orient(orient) |
... | ... | |
150 | 150 |
motor_br_speed = absolute_to_mps(msg->br_speed); |
151 | 151 |
} |
152 | 152 |
} |
153 |
//scout sonar callback |
|
154 | 153 |
|
155 | 154 |
bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request &req, |
156 |
sonar::sonar_toggle::Response &res)
|
|
155 |
sonar::sonar_toggle::Response &res)
|
|
157 | 156 |
{ |
158 | 157 |
if (req.set_on && !sonar_on) |
159 | 158 |
{ |
160 |
ROS_INFO("Turning on the sonar");
|
|
159 |
ROS_INFO("Turning on the sonar");
|
|
161 | 160 |
sonar_on = true; |
162 | 161 |
|
163 | 162 |
} |
164 | 163 |
else if (!req.set_on && sonar_on) |
165 | 164 |
{ |
166 |
ROS_INFO("Turning off the sonar");
|
|
165 |
ROS_INFO("Turning off the sonar");
|
|
167 | 166 |
sonar_on = false; |
168 | 167 |
} |
169 | 168 |
else |
170 | 169 |
{ |
171 |
ROS_INFO("Sonar state remains unchanged"); |
|
170 |
ROS_INFO("Sonar state remains unchanged");
|
|
172 | 171 |
} |
173 |
|
|
174 |
res.ack = true; |
|
175 |
return true; |
|
172 |
res.ack = true; |
|
173 |
return true; |
|
176 | 174 |
} |
177 | 175 |
|
178 |
|
|
179 |
bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request &req, |
|
180 |
sonar::sonar_set_scan::Response &res) |
|
181 |
{ |
|
182 |
// Code to set the sonar to scan from |
|
183 |
// req.stop_l to req.stop_r |
|
184 |
if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r) |
|
176 |
bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request &req, |
|
177 |
sonar::sonar_set_scan::Response &res) |
|
185 | 178 |
{ |
186 |
ROS_INFO("Setting sonar scan range to [%i, %i]", |
|
187 |
req.stop_l, |
|
188 |
req.stop_r); |
|
189 |
sonar_stop_l = req.stop_l; |
|
190 |
sonar_stop_r = req.stop_r; |
|
191 |
sonar_position = req.stop_l; |
|
192 |
sonar_direction = 1; |
|
193 |
res.ack = true; |
|
194 |
} |
|
195 |
else |
|
196 |
{ |
|
197 |
ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r"); |
|
179 |
// Code to set the sonar to scan from |
|
180 |
// req.stop_l to req.stop_r |
|
181 |
if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r) |
|
182 |
{ |
|
183 |
ROS_INFO("Setting sonar scan range to [%i, %i]", |
|
184 |
req.stop_l, |
|
185 |
req.stop_r); |
|
186 |
sonar_stop_l = req.stop_l; |
|
187 |
sonar_stop_r = req.stop_r; |
|
188 |
sonar_position = req.stop_l; |
|
189 |
sonar_direction = 1; |
|
190 |
res.ack = true; |
|
191 |
} |
|
192 |
else |
|
193 |
{ |
|
194 |
ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r"); |
|
195 |
} |
|
196 |
return true; |
|
198 | 197 |
} |
199 |
return true; |
|
200 |
} |
|
201 | 198 |
|
202 | 199 |
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, |
203 | 200 |
scoutsim::SetPen::Response&) |
... | ... | |
284 | 281 |
unsigned char r = walls_image.GetRed(d_x, d_y); |
285 | 282 |
unsigned char g = walls_image.GetGreen(d_x, d_y); |
286 | 283 |
unsigned char b = walls_image.GetBlue(d_x, d_y); |
287 |
|
|
284 |
|
|
288 | 285 |
reading = rgb_to_grey(r, g, b); |
289 |
|
|
286 |
|
|
290 | 287 |
|
291 | 288 |
d++; |
292 | 289 |
} |
293 | 290 |
/// @todo Consider using different cutoffs for different features |
294 | 291 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
295 | 292 |
|
296 |
if(sonar_visual_on) |
|
297 |
{ |
|
298 |
if(isFront) |
|
299 |
{ |
|
300 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
301 |
sonar_dc.SelectObject(*path_bitmap); |
|
302 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
303 |
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2); |
|
304 |
old_front_dx = d_x; |
|
305 |
old_front_dy = d_y; |
|
306 |
} |
|
307 |
else |
|
308 |
{ |
|
309 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
310 |
sonar_dc.SelectObject(*path_bitmap); |
|
311 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
312 |
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2); |
|
313 |
old_back_dx = d_x; |
|
314 |
old_back_dy = d_y; |
|
315 |
} |
|
316 |
|
|
317 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value |
|
318 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2); |
|
319 |
if (isFront) //for some reason isFront = (!isFront) is not working |
|
320 |
{ |
|
321 |
isFront = FALSE; |
|
322 |
} |
|
323 |
else |
|
324 |
{ |
|
325 |
isFront = TRUE; |
|
326 |
} |
|
327 |
|
|
328 |
} |
|
293 |
if (sonar_visual_on) |
|
294 |
{ |
|
295 |
if (isFront) |
|
296 |
{ |
|
297 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
298 |
sonar_dc.SelectObject(*path_bitmap); |
|
299 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
300 |
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2); |
|
301 |
old_front_dx = d_x; |
|
302 |
old_front_dy = d_y; |
|
303 |
} |
|
304 |
else |
|
305 |
{ |
|
306 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
307 |
sonar_dc.SelectObject(*path_bitmap); |
|
308 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
309 |
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2); |
|
310 |
old_back_dx = d_x; |
|
311 |
old_back_dy = d_y; |
|
312 |
} |
|
313 |
|
|
314 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value |
|
315 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2); |
|
316 |
if (isFront) //for some reason isFront = (!isFront) is not working |
|
317 |
{ |
|
318 |
isFront = FALSE; |
|
319 |
} |
|
320 |
else |
|
321 |
{ |
|
322 |
isFront = TRUE; |
|
323 |
} |
|
324 |
|
|
325 |
} |
|
326 |
|
|
329 | 327 |
return d; |
330 | 328 |
} |
331 | 329 |
|
... | ... | |
392 | 390 |
|
393 | 391 |
/// Sends back the position of this scout so scoutsim can save |
394 | 392 |
/// the world state |
393 |
/// TODO remove dt param |
|
395 | 394 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
396 |
wxMemoryDC& sonar_dc,
|
|
395 |
wxMemoryDC& sonar_dc,
|
|
397 | 396 |
bool sonar_visual, |
398 | 397 |
const wxImage& path_image, |
399 | 398 |
const wxImage& lines_image, |
... | ... | |
402 | 401 |
wxColour sonar_color, |
403 | 402 |
world_state state) |
404 | 403 |
{ |
405 |
|
|
406 | 404 |
sonar_visual_on = sonar_visual; |
405 |
|
|
407 | 406 |
// Assume that the two motors on the same side will be set to |
408 | 407 |
// roughly the same speed. Does not account for slip conditions |
409 | 408 |
// when they are set to different speeds. |
... | ... | |
428 | 427 |
|
429 | 428 |
orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI)); |
430 | 429 |
|
431 |
// Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE) |
|
432 | 430 |
pos.x += sin(orient + PI/2.0) * lin_dist; |
433 | 431 |
pos.y += cos(orient + PI/2.0) * lin_dist; |
434 | 432 |
|
Also available in: Unified diff