Revision 60a90290 scout/scoutsim/src/scout.cpp

View differences:

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