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