Revision f09d002e scout/scoutsim/src/scout.cpp

View differences:

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