Project

General

Profile

Revision eb9cff77

IDeb9cff771c06a2ed3f37c591ed4c1ee0a20df76d

Added by Hui Jun Tay over 11 years ago

Fixed sonar bug (corrected y direction)
Added sonar_toggle and set_scan command and callbacks
Cleaned up sonar_visual toggle code

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
	  , sonar_visual_on(sonar_visual_on)
67 67
	  , node (nh)
68 68
          , scout_image(scout_image)
69 69
          , pos(pos)
......
90 90
        set_pen_srv = node.advertiseService("set_pen",
91 91
                                            &Scout::setPenCallback,
92 92
                                            this);
93
        toggle_sonar_srv = node.advertiseService("sonar_toggle",
94
                                            &Scout::handle_sonar_toggle,
95
                                            this);
96
        set_sonar_srv = node.advertiseService("sonar_set_scan",
97
                                            &Scout::handle_sonar_set_scan,
98
                                            this);
93 99
        query_encoders_srv =
94 100
            node.advertiseService("query_encoders",
95 101
                                  &Scout::query_encoders_callback,
......
142 148
        }
143 149

  
144 150
    }
151
	//scout sonar callback
152

  
153
    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
154
	                     sonar::sonar_toggle::Response &res)
155
    {
156
        if (req.set_on && !sonar_on)
157
        {
158
	        ROS_INFO("Turning on the sonar");
159
            sonar_on = true;
160

  
161
        }
162
        else if (!req.set_on && sonar_on)
163
        {
164
	        ROS_INFO("Turning off the sonar");
165
            sonar_on = false;
166
        }
167
        else
168
        {
169
        ROS_INFO("Sonar state remains unchanged");
170
        }
171

  
172
    res.ack = true;
173
    return true;
174
    }
175

  
176

  
177
bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
178
                           sonar::sonar_set_scan::Response &res)
179
{
180
    // Code to set the sonar to scan from
181
    // req.stop_l to req.stop_r
182
    if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
183
    {
184
    ROS_INFO("Setting sonar scan range to [%i, %i]",
185
             req.stop_l,
186
             req.stop_r);
187
    sonar_stop_l = req.stop_l;
188
    sonar_stop_r = req.stop_r;
189
    sonar_position = req.stop_l;
190
    sonar_direction = 1;
191
    res.ack = true;
192
    }
193
    else
194
    {
195
    ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
196
    }
197
    return true;
198
}
145 199

  
146 200
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
147 201
                               scoutsim::SetPen::Response&)
......
194 248
    }
195 249

  
196 250
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
197
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
251
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
198 252
    {
199 253
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
200 254
        unsigned int d = 0;
201 255

  
202 256
        unsigned int reading = 0;
203
	int d_x = 0;
204
	int d_y = 0;
257
	    int d_x = 0;
258
	    int d_y = 0;
205 259
        do
206 260
        {
207 261
            d_x = x + (int) floor(d * cos(angle));
208
            d_y = y + (int) floor(d * sin(angle));
262
            d_y = y - (int) floor(d * sin(angle));
209 263

  
210 264
            // Out of image boundary
211 265
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
......
233 287
        /// @todo Consider using different cutoffs for different features
234 288
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
235 289
        
236
	if(sonar_on)
237
	{       
290
	if(sonar_visual_on)
291
	{   
238 292
		if(isFront)
239 293
		{
240 294
			// draw a circle at the wall_x, wall_y where reading > 128
......
271 325

  
272 326
    // x and y is current position of the sonar
273 327
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
274
                             double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
328
                             double robot_theta,wxMemoryDC& sonar_dc)
275 329
    {
276 330
        // Only rotate the sonar at the correct rate.
277 331
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
......
281 335
        last_sonar_time = ros::Time::now();
282 336

  
283 337
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
284
                                           sonar_position,sonar_dc, sonar_on);
338
                                           sonar_position,sonar_dc);
285 339
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
286
                                          sonar_position + 24,sonar_dc, sonar_on);
340
                                          sonar_position + 24,sonar_dc);
287 341

  
288 342
        // Publish
289 343
        sonar::sonar_distance msg;
......
334 388
    /// the world state
335 389
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
336 390
					wxMemoryDC& sonar_dc,
337
					bool sonar_on,
391
                    bool sonar_visual,
338 392
                                        const wxImage& path_image,
339 393
                                        const wxImage& lines_image,
340 394
                                        const wxImage& walls_image,
......
342 396
					wxColour sonar_color,
343 397
                                        world_state state)
344 398
    {
399

  
400
        sonar_visual_on = sonar_visual;
345 401
        // Assume that the two motors on the same side will be set to
346 402
        // roughly the same speed. Does not account for slip conditions
347 403
        // when they are set to different speeds.
......
410 466
        pose_pub.publish(p);
411 467

  
412 468
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
413
        update_sonar(walls_image,
414
                     canvas_x + scoutsim::SCOUT_SONAR_X,
415
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
416
                     p.theta,sonar_dc,sonar_on);
469
        if (sonar_on)
470
        {
471
            update_sonar(walls_image,
472
                         canvas_x + scoutsim::SCOUT_SONAR_X,
473
                         canvas_y + scoutsim::SCOUT_SONAR_Y,
474
                         p.theta,sonar_dc);
417 475

  
476
        }
418 477
        // Figure out (and publish) the color underneath the scout
419 478
        {
420 479
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
scout/scoutsim/src/scout.h
56 56
#include <encoders/query_encoders.h>
57 57
#include <linesensor/query_linesensor.h>
58 58
#include <sonar/sonar_distance.h>
59
#include <sonar/sonar_toggle.h>
60
#include <sonar/sonar_set_scan.h>
59 61

  
60 62
#include <scoutsim/Pose.h>
61 63
#include <scoutsim/SetPen.h>
......
114 116

  
115 117
            geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc,
116 118
					 wxMemoryDC& sonar_dc,
117
					 bool sonar_on,
119
                     bool sonar_visual,
118 120
                                         const wxImage& path_image,
119 121
                                         const wxImage& lines_image,
120 122
                                         const wxImage& walls_image,
......
131 133
                                         encoders::query_encoders::Response&);
132 134
            bool query_linesensor_callback(linesensor::query_linesensor::Request&,
133 135
                                           linesensor::query_linesensor::Response&);
136
            bool handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
137
                                     sonar::sonar_toggle::Response &res);
138
            bool handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
139
                                       sonar::sonar_set_scan::Response &res);
134 140
            unsigned int rgb_to_grey(unsigned char r,
135 141
                                     unsigned char g,
136 142
                                     unsigned char b);
137 143
            unsigned int trace_sonar(const wxImage& walls_image, int x, int y,
138
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
144
                                     double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc);
139 145
            void update_sonar(const wxImage& walls_image, int x, int y,
140
                              double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
146
                              double robot_theta, wxMemoryDC& sonar_dc);
141 147
            void update_linesensor(const wxImage& lines_image, int x, int y,
142 148
                                   double theta);
143 149
	    int old_front_dx;
......
147 153
	    bool isFront;
148 154

  
149 155
	    wxBitmap *path_bitmap;
150
	    bool sonar_on;
156
	    bool sonar_visual_on;
157
        bool sonar_on;
151 158

  
152 159
            ros::NodeHandle node;
153 160

  
......
195 202
            ros::ServiceServer set_pen_srv;
196 203
            ros::ServiceServer query_encoders_srv;
197 204
            ros::ServiceServer query_linesensor_srv;
205
            ros::ServiceServer toggle_sonar_srv;
206
            ros::ServiceServer set_sonar_srv;
198 207

  
199 208
            ros::WallTime last_command_time;
200 209

  
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
        sonar_visual_on = TRUE;
107 107

  
108 108
        // Try to load the file; if it fails, make a new blank file
109 109
        if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str())))
......
272 272

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

  
......
489 489

  
490 490
        for (; it != end; ++it)
491 491
        {
492
            it->second->update(0.016, path_dc,sonar_dc,sonar_on,
492
            it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on,
493 493
                               path_image, lines_image, walls_image,
494 494
                               path_dc.GetBackground().GetColour(),
495 495
			       sonar_dc.GetBackground().GetColour(),
scout/scoutsim/src/sim_frame.h
166 166
            float width_in_meters;
167 167
            float height_in_meters;
168 168

  
169
	    bool sonar_on;
169
	    bool sonar_visual_on;
170 170

  
171 171
            std::string map_base_name;
172 172
            std::string map_lines_name;

Also available in: Unified diff