Project

General

Profile

Revision 6639ce9c

ID6639ce9c5b5ca4644e9f66e71b7d79bc24cf4fb0
Parent dfb92d66
Child 43b327cb

Added by viki over 11 years ago

Edited Sonar to display points on simulator.

Changed rate of sonar scan to 0.5s
Made a new wxDC graphics object for sonar

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