Revision eb9cff77 scout/scoutsim/src/scout.cpp

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());

Also available in: Unified diff