Revision 6639ce9c

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);
scout/scoutsim/src/scout.h
109 109
    class Scout
110 110
    {
111 111
        public:
112
            Scout(const ros::NodeHandle& nh, const wxImage& scout_image,
113
                  const Vector2& pos, float orient);
112
            Scout(const ros::NodeHandle& nh,const wxImage& scout_image,
113
                  const Vector2& pos, wxBitmap *path_bitmap, float orient);
114 114

  
115 115
            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
116
					wxMemoryDC& sonar_dc,
116 117
                                         const wxImage& path_image,
117 118
                                         const wxImage& lines_image,
118 119
                                         const wxImage& walls_image,
119 120
                                         wxColour background_color,
121
         				 wxColour sonar_color,
120 122
                                         world_state state);
121 123
            void paint(wxDC& dc);
122 124

  
......
132 134
                                     unsigned char g,
133 135
                                     unsigned char b);
134 136
            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
135
                                     double robot_theta, int sonar_pos);
137
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc);
136 138
            void update_sonar(const wxImage& walls_image, int x, int y,
137
                              double robot_theta);
139
                              double robot_theta, wxMemoryDC& sonar_dc);
138 140
            void update_linesensor(const wxImage& lines_image, int x, int y,
139 141
                                   double theta);
142
	    int old_dx;
143
	    int old_dy;
144
	    wxBitmap *path_bitmap;
140 145

  
141 146
            ros::NodeHandle node;
142 147

  
scout/scoutsim/src/scout_constants.h
59 59

  
60 60
    const unsigned int SONAR_MAX_RANGE = 255;
61 61
    // Time it takes for the sonar to spin from position 0 to position 23
62
    const float SONAR_HALF_SPIN_TIME = 24.0;
62
    const float SONAR_HALF_SPIN_TIME = 0.5;
63 63
}
scout/scoutsim/src/sim_frame.cpp
217 217

  
218 218
        ScoutPtr t(new Scout(ros::NodeHandle(real_name),
219 219
                   scout_images[rand() % SCOUTSIM_NUM_SCOUTS],
220
                   Vector2(x, y), angle));
220
                   Vector2(x, y), &path_bitmap,angle));
221 221
        scouts[real_name] = t;
222 222

  
223 223
        ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",
......
269 269
        path_dc.SetBackground(wxBrush(wxColour(100, 100, 100)));
270 270
        path_dc.Clear();
271 271

  
272
        sonar_dc.SetBackground(wxBrush(wxColour(255, 0, 0)));
273
        sonar_dc.Clear();
274

  
275
        sonar_dc.SelectObject(path_bitmap);
276

  
272 277
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
273 278
        path_dc.SelectObject(path_bitmap);
274 279
        SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight()));
......
323 328

  
324 329
        for (; it != end; ++it)
325 330
        {
326
            it->second->update(0.016, path_dc,
331
            it->second->update(0.016, path_dc,sonar_dc,
327 332
                               path_image, lines_image, walls_image,
328 333
                               path_dc.GetBackground().GetColour(),
334
			       sonar_dc.GetBackground().GetColour(),
329 335
                               state);
330 336
        }
331 337

  
scout/scoutsim/src/sim_frame.h
114 114
            wxImage lines_image;
115 115
            wxImage walls_image;
116 116
            wxMemoryDC path_dc;
117
	    wxMemoryDC sonar_dc;
117 118

  
118 119
            uint64_t frame_count;
119 120

  

Also available in: Unified diff