Revision 04114d13
Fixed teleop with new units, and changes the refresh rate to be closer to true m/s.
| scout/motors/msg/set_motors.msg | ||
|---|---|---|
| 6 | 6 |
bool bl_set |
| 7 | 7 |
bool br_set |
| 8 | 8 |
|
| 9 |
# The absolute motor speeds to set (-255 to 255) |
|
| 9 |
# The absolute motor speeds to set (-128 to 127) |
|
| 10 |
# TODO check that this is the correct range |
|
| 10 | 11 |
int8 fl_speed |
| 11 | 12 |
int8 fr_speed |
| 12 | 13 |
int8 bl_speed |
| 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 |
|
| scout/scoutsim/src/scout_constants.h | ||
|---|---|---|
| 48 | 48 |
#ifndef _SCOUTSIM_SCOUT_CONSTANTS_ |
| 49 | 49 |
#define _SCOUTSIM_SCOUT_CONSTANTS_ |
| 50 | 50 |
|
| 51 |
/// TODO put these in a utility file somewhere? |
|
| 52 |
#define min(x,y) ((x < y) ? x : y) |
|
| 53 |
#define max(x,y) ((x > y) ? x : y) |
|
| 54 |
|
|
| 51 | 55 |
namespace scoutsim |
| 52 | 56 |
{
|
| 53 | 57 |
// Maximum speed which will be sent to scoutsim in absolute units |
| 54 |
const int MAX_ABSOLUTE_SPEED = 255; |
|
| 58 |
const int MAX_ABSOLUTE_SPEED = 127; |
|
| 59 |
const int MIN_ABSOLUTE_SPEED = -128; |
|
| 60 |
|
|
| 55 | 61 |
// Speed in m/s corresponding to maximum absolute speed |
| 56 | 62 |
const float MAX_SPEED_MPS = 1.0; |
| 57 | 63 |
|
| ... | ... | |
| 75 | 81 |
// Time it takes for the sonar to spin from position 0 to position 23 |
| 76 | 82 |
const float SONAR_HALF_SPIN_TIME = 0.5; |
| 77 | 83 |
|
| 78 |
const float SCOUTSIM_REFRESH_RATE = 0.01; // s
|
|
| 84 |
const float SCOUTSIM_REFRESH_RATE = 0.05; // s
|
|
| 79 | 85 |
} |
| 80 | 86 |
|
| 81 | 87 |
#endif |
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 314 | 314 |
// Runs every SCOUTSIM_REFRESH_RATE. |
| 315 | 315 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
| 316 | 316 |
{
|
| 317 |
cout << frame_count << endl; |
|
| 317 | 318 |
ros::spinOnce(); |
| 318 | 319 |
|
| 319 | 320 |
teleop(); |
| ... | ... | |
| 324 | 325 |
{
|
| 325 | 326 |
Close(); |
| 326 | 327 |
} |
| 328 |
|
|
| 329 |
frame_count++; |
|
| 327 | 330 |
} |
| 328 | 331 |
|
| 329 | 332 |
void SimFrame::onPaint(wxPaintEvent& evt) |
| ... | ... | |
| 391 | 394 |
} |
| 392 | 395 |
else if (wxGetKeyState(WXK_LEFT)) |
| 393 | 396 |
{
|
| 394 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
| 395 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
| 397 |
teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
| 398 |
teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
|
|
| 396 | 399 |
} |
| 397 | 400 |
else if (wxGetKeyState(WXK_RIGHT)) |
| 398 | 401 |
{
|
| 399 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
| 400 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
| 402 |
teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
|
|
| 403 |
teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
| 401 | 404 |
} |
| 402 | 405 |
} |
| 403 | 406 |
|
| ... | ... | |
| 411 | 414 |
{
|
| 412 | 415 |
teleop_fluid_speed -= TELEOP_FLUID_INC * 2; |
| 413 | 416 |
} |
| 414 |
else if (teleop_fluid_speed > 0)
|
|
| 417 |
else if (teleop_fluid_speed > TELEOP_FLUID_INC)
|
|
| 415 | 418 |
{
|
| 416 | 419 |
teleop_fluid_speed -= TELEOP_FLUID_INC; |
| 417 | 420 |
} |
| 418 |
else if (teleop_fluid_speed < 0)
|
|
| 421 |
else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
|
|
| 419 | 422 |
{
|
| 420 | 423 |
teleop_fluid_speed += TELEOP_FLUID_INC; |
| 421 | 424 |
} |
| 425 |
else |
|
| 426 |
{
|
|
| 427 |
teleop_fluid_speed = 0; |
|
| 428 |
} |
|
| 422 | 429 |
|
| 423 | 430 |
if (wxGetKeyState(WXK_LEFT)) |
| 424 | 431 |
{
|
| ... | ... | |
| 441 | 448 |
{
|
| 442 | 449 |
teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED; |
| 443 | 450 |
} |
| 444 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
|
|
| 451 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED) |
|
| 445 | 452 |
{
|
| 446 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
|
|
| 453 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED; |
|
| 447 | 454 |
} |
| 448 |
else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
|
|
| 455 |
else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED) |
|
| 449 | 456 |
{
|
| 450 |
teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
|
|
| 457 |
teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED; |
|
| 451 | 458 |
} |
| 452 | 459 |
|
| 453 |
teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
| 454 |
teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
| 460 |
int l_speed = teleop_fluid_speed + teleop_fluid_omega; |
|
| 461 |
int r_speed = teleop_fluid_speed - teleop_fluid_omega; |
|
| 462 |
|
|
| 463 |
teleop_l_speed = max(MIN_ABSOLUTE_SPEED, |
|
| 464 |
min(MAX_ABSOLUTE_SPEED, l_speed)); |
|
| 465 |
teleop_r_speed = max(MIN_ABSOLUTE_SPEED, |
|
| 466 |
min(MAX_ABSOLUTE_SPEED, r_speed)); |
|
| 455 | 467 |
} |
| 456 | 468 |
|
| 457 | 469 |
void SimFrame::teleop() |
| ... | ... | |
| 490 | 502 |
return; |
| 491 | 503 |
} |
| 492 | 504 |
|
| 493 |
if (frame_count % 3 == 0) |
|
| 494 |
{
|
|
| 495 |
path_image = path_bitmap.ConvertToImage(); |
|
| 496 |
Refresh(); |
|
| 497 |
} |
|
| 505 |
path_image = path_bitmap.ConvertToImage(); |
|
| 506 |
Refresh(); |
|
| 498 | 507 |
|
| 499 | 508 |
M_Scout::iterator it = scouts.begin(); |
| 500 | 509 |
M_Scout::iterator end = scouts.end(); |
| ... | ... | |
| 505 | 514 |
|
| 506 | 515 |
for (; it != end; ++it) |
| 507 | 516 |
{
|
| 508 |
it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on, |
|
| 517 |
it->second->update(SCOUTSIM_REFRESH_RATE, |
|
| 518 |
path_dc,sonar_dc,sonar_visual_on, |
|
| 509 | 519 |
path_image, lines_image, walls_image, |
| 510 | 520 |
path_dc.GetBackground().GetColour(), |
| 511 |
sonar_dc.GetBackground().GetColour(),
|
|
| 521 |
sonar_dc.GetBackground().GetColour(),
|
|
| 512 | 522 |
state); |
| 513 | 523 |
} |
| 514 |
|
|
| 515 |
frame_count++; |
|
| 516 | 524 |
} |
| 517 | 525 |
|
| 518 | 526 |
bool SimFrame::clearCallback(std_srvs::Empty::Request&, |
| scout/scoutsim/src/sim_frame.h | ||
|---|---|---|
| 77 | 77 |
#define ID_TELEOP_FLUID 9 |
| 78 | 78 |
#define ID_SONAR 10 |
| 79 | 79 |
|
| 80 |
// Absolute speeds (0 - 255)
|
|
| 81 |
#define TELEOP_PRECISE_SPEED 160
|
|
| 82 |
#define TELEOP_FLUID_MAX_SPEED 200
|
|
| 83 |
#define TELEOP_FLUID_INC 10
|
|
| 80 |
// Absolute speeds (-128 - 127)
|
|
| 81 |
#define TELEOP_PRECISE_SPEED 62
|
|
| 82 |
#define TELEOP_FLUID_MAX_SPEED 100
|
|
| 83 |
#define TELEOP_FLUID_INC 8
|
|
| 84 | 84 |
|
| 85 | 85 |
// Teleop types |
| 86 | 86 |
#define TELEOP_OFF 0 |
| ... | ... | |
| 112 | 112 |
DECLARE_EVENT_TABLE() |
| 113 | 113 |
|
| 114 | 114 |
private: |
| 115 |
|
|
| 115 | 116 |
void onUpdate(wxTimerEvent& evt); |
| 116 | 117 |
void onPaint(wxPaintEvent& evt); |
| 117 | 118 |
|
| ... | ... | |
| 136 | 137 |
ros::Publisher wireless_receive; |
| 137 | 138 |
|
| 138 | 139 |
// Teleop |
| 139 |
int teleop_l_speed;
|
|
| 140 |
int teleop_r_speed;
|
|
| 140 |
short teleop_l_speed;
|
|
| 141 |
short teleop_r_speed;
|
|
| 141 | 142 |
ros::Publisher teleop_pub; |
| 142 | 143 |
int teleop_type; |
| 143 | 144 |
|
Also available in: Unified diff