Revision f09d002e
behaviors now overwrite teleop (see comments in scout.cpp for details)
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 66 | 66 |
, sonar_visual_on(false) |
| 67 | 67 |
, sonar_on(true) |
| 68 | 68 |
, ignore_behavior(false) |
| 69 |
, current_teleop_scout("")
|
|
| 70 | 69 |
, node (nh) |
| 71 | 70 |
, scout_image(scout_image) |
| 72 | 71 |
, pos(pos) |
| ... | ... | |
| 120 | 119 |
sonar_stop_r = 23; |
| 121 | 120 |
sonar_direction = 1; |
| 122 | 121 |
sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0); |
| 122 |
|
|
| 123 |
// Init latch |
|
| 124 |
teleop_latch = 0; |
|
| 123 | 125 |
} |
| 124 | 126 |
|
| 125 | 127 |
float Scout::absolute_to_mps(int absolute_speed) |
| ... | ... | |
| 137 | 139 |
last_command_time = ros::WallTime::now(); |
| 138 | 140 |
|
| 139 | 141 |
//ignore non-teleop commands if commands if teleop is ON |
| 140 |
if (node.getNamespace() != current_teleop_scout || msg->teleop_ON) |
|
| 142 |
//if (node.getNamespace() != current_teleop_scout || msg->teleop_ON) |
|
| 143 |
//{
|
|
| 144 |
|
|
| 145 |
//latch value indicates number of uninterrupted teleop messages |
|
| 146 |
//before teleop latch shifts again |
|
| 147 |
if (!(msg->teleop_ON) && teleop_latch < 3) |
|
| 148 |
{
|
|
| 149 |
teleop_latch++; |
|
| 150 |
} |
|
| 151 |
|
|
| 152 |
if (!(msg->teleop_ON) || teleop_latch ==0) |
|
| 141 | 153 |
{
|
| 142 | 154 |
if(msg->fl_set) |
| 143 | 155 |
{
|
| ... | ... | |
| 155 | 167 |
{
|
| 156 | 168 |
motor_br_speed = absolute_to_mps(msg->br_speed); |
| 157 | 169 |
} |
| 170 |
} |
|
| 171 |
|
|
| 172 |
//if a teleop message comes through, decrease the latch |
|
| 173 |
//latch code works on the assumption there will be more behavior messages |
|
| 174 |
//than teleop messages |
|
| 175 |
if (msg->teleop_ON && teleop_latch>0) |
|
| 176 |
{
|
|
| 177 |
teleop_latch--; |
|
| 158 | 178 |
} |
| 179 |
//} |
|
| 159 | 180 |
|
| 160 | 181 |
} |
| 161 | 182 |
|
| ... | ... | |
| 405 | 426 |
const wxImage& walls_image, |
| 406 | 427 |
wxColour background_color, |
| 407 | 428 |
wxColour sonar_color, |
| 408 |
world_state state, |
|
| 409 |
string teleop_scoutname) |
|
| 429 |
world_state state) |
|
| 410 | 430 |
{
|
| 411 | 431 |
// Assume that the two motors on the same side will be set to |
| 412 | 432 |
// roughly the same speed. Does not account for slip conditions |
| ... | ... | |
| 419 | 439 |
float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH; |
| 420 | 440 |
|
| 421 | 441 |
//store currently teleop'd scoutname |
| 422 |
std::stringstream ss; |
|
| 423 |
ss << "/" << teleop_scoutname; |
|
| 424 |
current_teleop_scout = ss.str(); |
|
| 442 |
//std::stringstream ss;
|
|
| 443 |
//ss << "/" << teleop_scoutname;
|
|
| 444 |
//current_teleop_scout = ss.str();
|
|
| 425 | 445 |
|
| 426 | 446 |
Vector2 old_pos = pos; |
| 427 | 447 |
|
Also available in: Unified diff