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