Revision 68b23184

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
        {
scout/scoutsim/src/scout.h
113 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
					 wxMemoryDC& sonar_dc,
117
					 bool sonar_on,
117 118
                                         const wxImage& path_image,
118 119
                                         const wxImage& lines_image,
119 120
                                         const wxImage& walls_image,
......
134 135
                                     unsigned char g,
135 136
                                     unsigned char b);
136 137
            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
137
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc);
138
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
138 139
            void update_sonar(const wxImage& walls_image, int x, int y,
139
                              double robot_theta, wxMemoryDC& sonar_dc);
140
                              double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
140 141
            void update_linesensor(const wxImage& lines_image, int x, int y,
141 142
                                   double theta);
142 143
	    int old_dx;
143 144
	    int old_dy;
144 145
	    wxBitmap *path_bitmap;
146
	    bool sonar_on;
145 147

  
146 148
            ros::NodeHandle node;
147 149

  
scout/scoutsim/src/scoutsim.cpp
137 137
    EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop)
138 138
    EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise)
139 139
    EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid)
140
    EVT_MENU(ID_SONAR, scoutsim::SimFrame::showSonar)
140 141
END_EVENT_TABLE()
141 142

  
142 143
DECLARE_APP(ScoutApp);
scout/scoutsim/src/sim_frame.cpp
103 103
        wxBitmap lines_bitmap;
104 104
        wxBitmap walls_bitmap;
105 105
        path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str()));
106
        sonar_on = TRUE;
106 107

  
107 108
        // Try to load the file; if it fails, make a new blank file
108 109
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
......
150 151
        menuFile->Append(ID_QUIT, _("E&xit"));
151 152

  
152 153
        wxMenu *menuSim = new wxMenu;
154
        menuSim->Append(ID_SONAR, _("S&onar"));
153 155
        menuSim->Append(ID_CLEAR, _("&Clear"));
154 156

  
155 157
        wxMenu *menuView = new wxMenu;
......
268 270
        clear();
269 271
    }
270 272

  
273
    void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event))
274
    {
275
        sonar_on = not sonar_on;
276
	clear();
277
    }
278

  
279

  
271 280
    void SimFrame::showMap(wxCommandEvent& WXUNUSED(event))
272 281
    {
273 282
        display_map_name = map_base_name;
......
480 489

  
481 490
        for (; it != end; ++it)
482 491
        {
483
            it->second->update(0.016, path_dc,sonar_dc,
492
            it->second->update(0.016, path_dc,sonar_dc,sonar_on,
484 493
                               path_image, lines_image, walls_image,
485 494
                               path_dc.GetBackground().GetColour(),
486 495
			       sonar_dc.GetBackground().GetColour(),
scout/scoutsim/src/sim_frame.h
73 73
#define ID_TELEOP_NONE 7
74 74
#define ID_TELEOP_PRECISE 8
75 75
#define ID_TELEOP_FLUID 9
76
#define ID_SONAR 10
76 77

  
77 78
#define TELEOP_PRECISE_SPEED 30
78 79
#define TELEOP_FLUID_MAX_SPEED 80
......
102 103
            void stopTeleop(wxCommandEvent& event);
103 104
            void startTeleopPrecise(wxCommandEvent& event);
104 105
            void startTeleopFluid(wxCommandEvent& event);
106
	    void showSonar(wxCommandEvent& event);
105 107

  
106 108
            DECLARE_EVENT_TABLE()
107 109

  
......
164 166
            float width_in_meters;
165 167
            float height_in_meters;
166 168

  
169
	    bool sonar_on;
170

  
167 171
            std::string map_base_name;
168 172
            std::string map_lines_name;
169 173
            std::string map_walls_name;

Also available in: Unified diff