Revision 6639ce9c scout/scoutsim/src/scout.cpp

View differences:

scout/scoutsim/src/scout.cpp
60 60
    Scout::Scout(const ros::NodeHandle& nh,
61 61
                 const wxImage& scout_image,
62 62
                 const Vector2& pos,
63
                 wxBitmap *path_bitmap,
63 64
                 float orient)
64
        : node (nh)
65
        : path_bitmap(path_bitmap)
66
	  , node (nh)
65 67
          , scout_image(scout_image)
66 68
          , pos(pos)
67 69
          , orient(orient)
......
191 193
    }
192 194

  
193 195
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
194
                                    double robot_theta, int sonar_pos)
196
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
195 197
    {
196 198
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
197 199
        unsigned int d = 0;
198 200

  
199 201
        unsigned int reading = 0;
202
	int d_x = 0;
203
	int d_y = 0;
200 204
        do
201 205
        {
202
            int d_x = x + (int) floor(d * cos(angle));
203
            int d_y = y + (int) floor(d * sin(angle));
206
            d_x = x + (int) floor(d * cos(angle));
207
            d_y = y + (int) floor(d * sin(angle));
204 208

  
205 209
            // Out of image boundary
206 210
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
......
219 223
            unsigned char r = walls_image.GetRed(d_x, d_y);
220 224
            unsigned char g = walls_image.GetGreen(d_x, d_y);
221 225
            unsigned char b = walls_image.GetBlue(d_x, d_y);
222

  
226
	
223 227
            reading = rgb_to_grey(r, g, b);
228
	    
224 229

  
225 230
            d++;
226 231
        }
227 232
        /// @todo Consider using different cutoffs for different features
228 233
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
234
        
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;
229 244

  
230 245
        return d;
231 246
    }
232 247

  
233 248
    // x and y is current position of the sonar
234 249
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
235
                             double robot_theta)
250
                             double robot_theta,wxMemoryDC& sonar_dc)
236 251
    {
237 252
        // Only rotate the sonar at the correct rate.
238 253
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
......
242 257
        last_sonar_time = ros::Time::now();
243 258

  
244 259
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
245
                                           sonar_position);
260
                                           sonar_position,sonar_dc);
246 261
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
247
                                          sonar_position + 24);
262
                                          sonar_position + 24,sonar_dc);
248 263

  
249 264
        // Publish
250 265
        sonar::sonar_distance msg;
......
294 309
    /// Sends back the position of this scout so scoutsim can save
295 310
    /// the world state
296 311
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
312
					wxMemoryDC& sonar_dc,
297 313
                                        const wxImage& path_image,
298 314
                                        const wxImage& lines_image,
299 315
                                        const wxImage& walls_image,
300 316
                                        wxColour background_color,
317
					wxColour sonar_color,
301 318
                                        world_state state)
302 319
    {
303 320
        // Assume that the two motors on the same side will be set to
......
371 388
        update_sonar(walls_image,
372 389
                     canvas_x + scoutsim::SCOUT_SONAR_X,
373 390
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
374
                     p.theta);
391
                     p.theta,sonar_dc);
375 392

  
376 393
        // Figure out (and publish) the color underneath the scout
377 394
        {
......
390 407
        {
391 408
            if (pos != old_pos)
392 409
            {
410
                path_dc.SelectObject(*path_bitmap);
393 411
                path_dc.SetPen(pen);
394 412
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
395 413
                                 old_pos.x * meter, old_pos.y * meter);

Also available in: Unified diff