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