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 |
|
scout/scoutsim/src/scout.h | ||
---|---|---|
118 | 118 |
const wxImage& walls_image, |
119 | 119 |
wxColour background_color, |
120 | 120 |
wxColour sonar_color, |
121 |
world_state state,std::string teleop_scoutname);
|
|
121 |
world_state state); |
|
122 | 122 |
void paint(wxDC& dc); |
123 | 123 |
void set_sonar_visual(bool on); |
124 | 124 |
|
... | ... | |
151 | 151 |
int old_back_dx; |
152 | 152 |
int old_back_dy; |
153 | 153 |
bool isFront; |
154 |
|
|
155 |
int teleop_latch; |
|
154 | 156 |
|
155 | 157 |
wxBitmap *path_bitmap; |
156 | 158 |
bool sonar_visual_on; |
157 | 159 |
bool sonar_on; |
158 | 160 |
bool ignore_behavior; |
159 | 161 |
|
160 |
std::string current_teleop_scout; |
|
162 |
//std::string current_teleop_scout;
|
|
161 | 163 |
|
162 | 164 |
ros::NodeHandle node; |
163 | 165 |
|
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
549 | 549 |
|
550 | 550 |
void SimFrame::updateScouts() |
551 | 551 |
{ |
552 |
std::string teleop_out = ""; |
|
553 | 552 |
|
554 | 553 |
if (last_scout_update.isZero()) |
555 | 554 |
{ |
... | ... | |
570 | 569 |
for (; it != end; ++it) |
571 | 570 |
{ |
572 | 571 |
|
573 |
//default to scout1 |
|
574 |
if (teleop_type != TELEOP_OFF) |
|
575 |
{ |
|
576 |
teleop_out = teleop_scoutname; |
|
577 |
} |
|
578 |
|
|
579 | 572 |
it->second->update(SIM_TIME_REFRESH_RATE, |
580 | 573 |
path_dc, sonar_dc, |
581 | 574 |
path_image, lines_image, walls_image, |
582 | 575 |
path_dc.GetBackground().GetColour(), |
583 | 576 |
sonar_dc.GetBackground().GetColour(), |
584 |
state,teleop_out);
|
|
577 |
state); |
|
585 | 578 |
} |
586 | 579 |
|
587 | 580 |
for (unsigned int i = 0; i < ghost_scouts.size(); ++i) |
Also available in: Unified diff