Revision 04114d13 scout/scoutsim/src/scout.cpp

View differences:

scout/scoutsim/src/scout.cpp
63 63
                 wxBitmap *path_bitmap,
64 64
                 float orient)
65 65
        : path_bitmap(path_bitmap)
66
	  , sonar_visual_on(sonar_visual_on)
67
	  , node (nh)
66
      , sonar_visual_on(sonar_visual_on)
67
      , node (nh)
68 68
          , scout_image(scout_image)
69 69
          , pos(pos)
70 70
          , orient(orient)
......
150 150
            motor_br_speed = absolute_to_mps(msg->br_speed);
151 151
        }
152 152
    }
153
	//scout sonar callback
154 153

  
155 154
    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
156
	                     sonar::sonar_toggle::Response &res)
155
                         sonar::sonar_toggle::Response &res)
157 156
    {
158 157
        if (req.set_on && !sonar_on)
159 158
        {
160
	        ROS_INFO("Turning on the sonar");
159
            ROS_INFO("Turning on the sonar");
161 160
            sonar_on = true;
162 161

  
163 162
        }
164 163
        else if (!req.set_on && sonar_on)
165 164
        {
166
	        ROS_INFO("Turning off the sonar");
165
            ROS_INFO("Turning off the sonar");
167 166
            sonar_on = false;
168 167
        }
169 168
        else
170 169
        {
171
        ROS_INFO("Sonar state remains unchanged");
170
            ROS_INFO("Sonar state remains unchanged");
172 171
        }
173

  
174
    res.ack = true;
175
    return true;
172
        res.ack = true;
173
        return true;
176 174
    }
177 175

  
178

  
179
bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
180
                           sonar::sonar_set_scan::Response &res)
181
{
182
    // Code to set the sonar to scan from
183
    // req.stop_l to req.stop_r
184
    if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
176
    bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
177
                                      sonar::sonar_set_scan::Response &res)
185 178
    {
186
    ROS_INFO("Setting sonar scan range to [%i, %i]",
187
             req.stop_l,
188
             req.stop_r);
189
    sonar_stop_l = req.stop_l;
190
    sonar_stop_r = req.stop_r;
191
    sonar_position = req.stop_l;
192
    sonar_direction = 1;
193
    res.ack = true;
194
    }
195
    else
196
    {
197
    ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
179
        // Code to set the sonar to scan from
180
        // req.stop_l to req.stop_r
181
        if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
182
        {
183
            ROS_INFO("Setting sonar scan range to [%i, %i]",
184
                     req.stop_l,
185
                     req.stop_r);
186
            sonar_stop_l = req.stop_l;
187
            sonar_stop_r = req.stop_r;
188
            sonar_position = req.stop_l;
189
            sonar_direction = 1;
190
            res.ack = true;
191
        }
192
        else
193
        {
194
            ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
195
        }
196
        return true;
198 197
    }
199
    return true;
200
}
201 198

  
202 199
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
203 200
                               scoutsim::SetPen::Response&)
......
284 281
            unsigned char r = walls_image.GetRed(d_x, d_y);
285 282
            unsigned char g = walls_image.GetGreen(d_x, d_y);
286 283
            unsigned char b = walls_image.GetBlue(d_x, d_y);
287
	
284
    
288 285
            reading = rgb_to_grey(r, g, b);
289
	    
286
        
290 287

  
291 288
            d++;
292 289
        }
293 290
        /// @todo Consider using different cutoffs for different features
294 291
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
295 292
        
296
	if(sonar_visual_on)
297
	{   
298
		if(isFront)
299
		{
300
			// draw a circle at the wall_x, wall_y where reading > 128
301
			sonar_dc.SelectObject(*path_bitmap);
302
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
303
			sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
304
			old_front_dx = d_x;
305
			old_front_dy = d_y;
306
		}
307
		else
308
		{
309
			// draw a circle at the wall_x, wall_y where reading > 128
310
			sonar_dc.SelectObject(*path_bitmap);
311
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
312
			sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
313
			old_back_dx = d_x;
314
			old_back_dy = d_y;
315
		}
316

  
317
		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
318
		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
319
		if (isFront) //for some reason isFront = (!isFront) is not working
320
		{
321
			isFront = FALSE;
322
		}
323
		else
324
		{
325
			isFront = TRUE;	
326
		}
327
			
328
	}
293
        if (sonar_visual_on)
294
        {   
295
            if (isFront)
296
            {
297
                // draw a circle at the wall_x, wall_y where reading > 128
298
                sonar_dc.SelectObject(*path_bitmap);
299
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
300
                sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
301
                old_front_dx = d_x;
302
                old_front_dy = d_y;
303
            }
304
            else
305
            {
306
                // draw a circle at the wall_x, wall_y where reading > 128
307
                sonar_dc.SelectObject(*path_bitmap);
308
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
309
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
310
                old_back_dx = d_x;
311
                old_back_dy = d_y;
312
            }
313

  
314
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
315
            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
316
            if (isFront) //for some reason isFront = (!isFront) is not working
317
            {
318
                isFront = FALSE;
319
            }
320
            else
321
            {
322
                isFront = TRUE; 
323
            }
324
                
325
        }
326

  
329 327
        return d;
330 328
    }
331 329

  
......
392 390

  
393 391
    /// Sends back the position of this scout so scoutsim can save
394 392
    /// the world state
393
    /// TODO remove dt param
395 394
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
396
					                    wxMemoryDC& sonar_dc,
395
                                        wxMemoryDC& sonar_dc,
397 396
                                        bool sonar_visual, 
398 397
                                        const wxImage& path_image,
399 398
                                        const wxImage& lines_image,
......
402 401
                                        wxColour sonar_color,
403 402
                                        world_state state)
404 403
    {
405

  
406 404
        sonar_visual_on = sonar_visual;
405

  
407 406
        // Assume that the two motors on the same side will be set to
408 407
        // roughly the same speed. Does not account for slip conditions
409 408
        // when they are set to different speeds.
......
428 427

  
429 428
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
430 429

  
431
        // Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
432 430
        pos.x += sin(orient + PI/2.0) * lin_dist;
433 431
        pos.y += cos(orient + PI/2.0) * lin_dist;
434 432

  

Also available in: Unified diff