Revision 68b23184 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_on(sonar_on)
66 67
	  , node (nh)
67 68
          , scout_image(scout_image)
68 69
          , pos(pos)
......
193 194
    }
194 195

  
195 196
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
196
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
197
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
197 198
    {
198 199
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
199 200
        unsigned int d = 0;
......
232 233
        /// @todo Consider using different cutoffs for different features
233 234
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
234 235
        
235
        // draw a circle at the wall_x, wall_y where reading > 128
236
        sonar_dc.SelectObject(*path_bitmap);
237
        sonar_dc.SetBrush(*wxRED_BRUSH); //old value
238
        sonar_dc.DrawCircle(wxPoint(old_dx,old_dy), 2);
239

  
240
        sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
241
        sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
242
        old_dx = d_x;
243
	old_dy = d_y;
244

  
236
	if(sonar_on == TRUE)
237
	{        
238
		// draw a circle at the wall_x, wall_y where reading > 128
239
		sonar_dc.SelectObject(*path_bitmap);
240
		sonar_dc.SetBrush(*wxRED_BRUSH); //old value
241
		sonar_dc.DrawCircle(wxPoint(old_dx,old_dy), 2);
242

  
243
		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
244
		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
245
		old_dx = d_x;
246
		old_dy = d_y;
247
	}
245 248
        return d;
246 249
    }
247 250

  
248 251
    // x and y is current position of the sonar
249 252
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
250
                             double robot_theta,wxMemoryDC& sonar_dc)
253
                             double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
251 254
    {
252 255
        // Only rotate the sonar at the correct rate.
253 256
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
......
257 260
        last_sonar_time = ros::Time::now();
258 261

  
259 262
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
260
                                           sonar_position,sonar_dc);
263
                                           sonar_position,sonar_dc, sonar_on);
261 264
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
262
                                          sonar_position + 24,sonar_dc);
265
                                          sonar_position + 24,sonar_dc, sonar_on);
263 266

  
264 267
        // Publish
265 268
        sonar::sonar_distance msg;
......
310 313
    /// the world state
311 314
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
312 315
					wxMemoryDC& sonar_dc,
316
					bool sonar_on,
313 317
                                        const wxImage& path_image,
314 318
                                        const wxImage& lines_image,
315 319
                                        const wxImage& walls_image,
......
388 392
        update_sonar(walls_image,
389 393
                     canvas_x + scoutsim::SCOUT_SONAR_X,
390 394
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
391
                     p.theta,sonar_dc);
395
                     p.theta,sonar_dc,sonar_on);
392 396

  
393 397
        // Figure out (and publish) the color underneath the scout
394 398
        {

Also available in: Unified diff