Revision f09d002e

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

  
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