Revision 60a90290
ID | 60a90290ba7db8bc65d251fcfcc805462ab6686a |
\Fixed teleop to work with behaviors. Teleop currently has priority over behaviors
scout/scoutsim/src/scout.cpp | ||
---|---|---|
65 | 65 |
: path_bitmap(path_bitmap) |
66 | 66 |
, sonar_visual_on(false) |
67 | 67 |
, sonar_on(true) |
68 |
, ignore_behavior(false) |
|
69 |
, current_teleop_scout("") |
|
68 | 70 |
, node (nh) |
69 | 71 |
, scout_image(scout_image) |
70 | 72 |
, pos(pos) |
... | ... | |
134 | 136 |
{ |
135 | 137 |
last_command_time = ros::WallTime::now(); |
136 | 138 |
|
137 |
if(msg->fl_set) |
|
139 |
//ignore non-teleop commands if commands if teleop is ON |
|
140 |
if (node.getNamespace() != current_teleop_scout || msg->teleop_ON) |
|
138 | 141 |
{ |
139 |
motor_fl_speed = absolute_to_mps(msg->fl_speed); |
|
140 |
} |
|
141 |
if(msg->fr_set) |
|
142 |
{ |
|
143 |
motor_fr_speed = absolute_to_mps(msg->fr_speed); |
|
144 |
} |
|
145 |
if(msg->bl_set) |
|
146 |
{ |
|
147 |
motor_bl_speed = absolute_to_mps(msg->bl_speed); |
|
148 |
} |
|
149 |
if(msg->br_set) |
|
150 |
{ |
|
151 |
motor_br_speed = absolute_to_mps(msg->br_speed); |
|
142 |
if(msg->fl_set) |
|
143 |
{ |
|
144 |
motor_fl_speed = absolute_to_mps(msg->fl_speed); |
|
145 |
} |
|
146 |
if(msg->fr_set) |
|
147 |
{ |
|
148 |
motor_fr_speed = absolute_to_mps(msg->fr_speed); |
|
149 |
} |
|
150 |
if(msg->bl_set) |
|
151 |
{ |
|
152 |
motor_bl_speed = absolute_to_mps(msg->bl_speed); |
|
153 |
} |
|
154 |
if(msg->br_set) |
|
155 |
{ |
|
156 |
motor_br_speed = absolute_to_mps(msg->br_speed); |
|
157 |
} |
|
152 | 158 |
} |
159 |
|
|
153 | 160 |
} |
154 | 161 |
|
155 | 162 |
bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request &req, |
... | ... | |
397 | 404 |
const wxImage& walls_image, |
398 | 405 |
wxColour background_color, |
399 | 406 |
wxColour sonar_color, |
400 |
world_state state) |
|
407 |
world_state state, |
|
408 |
string teleop_scoutname) |
|
401 | 409 |
{ |
402 | 410 |
// Assume that the two motors on the same side will be set to |
403 | 411 |
// roughly the same speed. Does not account for slip conditions |
... | ... | |
409 | 417 |
float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2; |
410 | 418 |
float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed); |
411 | 419 |
|
420 |
//store currently teleop'd scoutname |
|
421 |
std::stringstream ss; |
|
422 |
ss << "/" << teleop_scoutname; |
|
423 |
current_teleop_scout = ss.str(); |
|
424 |
|
|
412 | 425 |
Vector2 old_pos = pos; |
413 | 426 |
|
414 | 427 |
// Update encoders |
... | ... | |
439 | 452 |
int canvas_x = pos.x * PIX_PER_METER; |
440 | 453 |
int canvas_y = pos.y * PIX_PER_METER; |
441 | 454 |
|
455 |
|
|
442 | 456 |
{ |
443 | 457 |
wxImage rotated_image = |
444 | 458 |
scout_image.Rotate(orient - PI/2.0, |
Also available in: Unified diff