Project

General

Profile

Revision 04114d13

ID04114d139191ab6a6893f0af9770f0a8ed341778

Added by Alex Zirbel over 11 years ago

Fixed teleop with new units, and changes the refresh rate to be closer to true m/s.

View differences:

scout/motors/msg/set_motors.msg
6 6
bool bl_set
7 7
bool br_set
8 8

  
9
# The absolute motor speeds to set (-255 to 255)
9
# The absolute motor speeds to set (-128 to 127)
10
# TODO check that this is the correct range
10 11
int8 fl_speed
11 12
int8 fr_speed
12 13
int8 bl_speed
scout/scoutsim/src/scout.cpp
63 63
                 wxBitmap *path_bitmap,
64 64
                 float orient)
65 65
        : path_bitmap(path_bitmap)
66
	  , sonar_visual_on(sonar_visual_on)
67
	  , node (nh)
66
      , sonar_visual_on(sonar_visual_on)
67
      , node (nh)
68 68
          , scout_image(scout_image)
69 69
          , pos(pos)
70 70
          , orient(orient)
......
150 150
            motor_br_speed = absolute_to_mps(msg->br_speed);
151 151
        }
152 152
    }
153
	//scout sonar callback
154 153

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

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

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

  
178

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

  
202 199
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
203 200
                               scoutsim::SetPen::Response&)
......
284 281
            unsigned char r = walls_image.GetRed(d_x, d_y);
285 282
            unsigned char g = walls_image.GetGreen(d_x, d_y);
286 283
            unsigned char b = walls_image.GetBlue(d_x, d_y);
287
	
284
    
288 285
            reading = rgb_to_grey(r, g, b);
289
	    
286
        
290 287

  
291 288
            d++;
292 289
        }
293 290
        /// @todo Consider using different cutoffs for different features
294 291
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
295 292
        
296
	if(sonar_visual_on)
297
	{   
298
		if(isFront)
299
		{
300
			// draw a circle at the wall_x, wall_y where reading > 128
301
			sonar_dc.SelectObject(*path_bitmap);
302
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
303
			sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
304
			old_front_dx = d_x;
305
			old_front_dy = d_y;
306
		}
307
		else
308
		{
309
			// draw a circle at the wall_x, wall_y where reading > 128
310
			sonar_dc.SelectObject(*path_bitmap);
311
			sonar_dc.SetBrush(*wxRED_BRUSH); //old value
312
			sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
313
			old_back_dx = d_x;
314
			old_back_dy = d_y;
315
		}
316

  
317
		sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
318
		sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
319
		if (isFront) //for some reason isFront = (!isFront) is not working
320
		{
321
			isFront = FALSE;
322
		}
323
		else
324
		{
325
			isFront = TRUE;	
326
		}
327
			
328
	}
293
        if (sonar_visual_on)
294
        {   
295
            if (isFront)
296
            {
297
                // draw a circle at the wall_x, wall_y where reading > 128
298
                sonar_dc.SelectObject(*path_bitmap);
299
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
300
                sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
301
                old_front_dx = d_x;
302
                old_front_dy = d_y;
303
            }
304
            else
305
            {
306
                // draw a circle at the wall_x, wall_y where reading > 128
307
                sonar_dc.SelectObject(*path_bitmap);
308
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
309
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
310
                old_back_dx = d_x;
311
                old_back_dy = d_y;
312
            }
313

  
314
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
315
            sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
316
            if (isFront) //for some reason isFront = (!isFront) is not working
317
            {
318
                isFront = FALSE;
319
            }
320
            else
321
            {
322
                isFront = TRUE; 
323
            }
324
                
325
        }
326

  
329 327
        return d;
330 328
    }
331 329

  
......
392 390

  
393 391
    /// Sends back the position of this scout so scoutsim can save
394 392
    /// the world state
393
    /// TODO remove dt param
395 394
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
396
					                    wxMemoryDC& sonar_dc,
395
                                        wxMemoryDC& sonar_dc,
397 396
                                        bool sonar_visual, 
398 397
                                        const wxImage& path_image,
399 398
                                        const wxImage& lines_image,
......
402 401
                                        wxColour sonar_color,
403 402
                                        world_state state)
404 403
    {
405

  
406 404
        sonar_visual_on = sonar_visual;
405

  
407 406
        // Assume that the two motors on the same side will be set to
408 407
        // roughly the same speed. Does not account for slip conditions
409 408
        // when they are set to different speeds.
......
428 427

  
429 428
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
430 429

  
431
        // Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
432 430
        pos.x += sin(orient + PI/2.0) * lin_dist;
433 431
        pos.y += cos(orient + PI/2.0) * lin_dist;
434 432

  
scout/scoutsim/src/scout_constants.h
48 48
#ifndef _SCOUTSIM_SCOUT_CONSTANTS_
49 49
#define _SCOUTSIM_SCOUT_CONSTANTS_
50 50

  
51
/// TODO put these in a utility file somewhere?
52
#define min(x,y) ((x < y) ? x : y)
53
#define max(x,y) ((x > y) ? x : y)
54

  
51 55
namespace scoutsim
52 56
{
53 57
    // Maximum speed which will be sent to scoutsim in absolute units
54
    const int MAX_ABSOLUTE_SPEED = 255;
58
    const int MAX_ABSOLUTE_SPEED = 127;
59
    const int MIN_ABSOLUTE_SPEED = -128;
60

  
55 61
    // Speed in m/s corresponding to maximum absolute speed
56 62
    const float MAX_SPEED_MPS = 1.0;
57 63

  
......
75 81
    // Time it takes for the sonar to spin from position 0 to position 23
76 82
    const float SONAR_HALF_SPIN_TIME = 0.5;
77 83

  
78
    const float SCOUTSIM_REFRESH_RATE = 0.01;   // s
84
    const float SCOUTSIM_REFRESH_RATE = 0.05;   // s
79 85
}
80 86

  
81 87
#endif
scout/scoutsim/src/sim_frame.cpp
314 314
    // Runs every SCOUTSIM_REFRESH_RATE.
315 315
    void SimFrame::onUpdate(wxTimerEvent& evt)
316 316
    {
317
        cout << frame_count << endl;
317 318
        ros::spinOnce();
318 319

  
319 320
        teleop();
......
324 325
        {
325 326
            Close();
326 327
        }
328

  
329
        frame_count++;
327 330
    }
328 331

  
329 332
    void SimFrame::onPaint(wxPaintEvent& evt)
......
391 394
        }
392 395
        else if (wxGetKeyState(WXK_LEFT))
393 396
        {
394
            teleop_l_speed = -TELEOP_PRECISE_SPEED;
395
            teleop_r_speed = TELEOP_PRECISE_SPEED;
397
            teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
398
            teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
396 399
        }
397 400
        else if (wxGetKeyState(WXK_RIGHT))
398 401
        {
399
            teleop_l_speed = TELEOP_PRECISE_SPEED;
400
            teleop_r_speed = -TELEOP_PRECISE_SPEED;
402
            teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
403
            teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
401 404
        }
402 405
    }
403 406

  
......
411 414
        {
412 415
            teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
413 416
        }
414
        else if (teleop_fluid_speed > 0)
417
        else if (teleop_fluid_speed > TELEOP_FLUID_INC)
415 418
        {
416 419
            teleop_fluid_speed -= TELEOP_FLUID_INC;
417 420
        }
418
        else if (teleop_fluid_speed < 0)
421
        else if (teleop_fluid_speed < -TELEOP_FLUID_INC)
419 422
        {
420 423
            teleop_fluid_speed += TELEOP_FLUID_INC;
421 424
        }
425
        else
426
        {
427
            teleop_fluid_speed = 0;
428
        }
422 429

  
423 430
        if (wxGetKeyState(WXK_LEFT))
424 431
        {
......
441 448
        {
442 449
            teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED;
443 450
        }
444
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2)
451
        if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED)
445 452
        {
446
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2;
453
            teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED;
447 454
        }
448
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2)
455
        else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED)
449 456
        {
450
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2;
457
            teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED;
451 458
        }
452 459

  
453
        teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega;
454
        teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega;
460
        int l_speed = teleop_fluid_speed + teleop_fluid_omega;
461
        int r_speed = teleop_fluid_speed - teleop_fluid_omega;
462

  
463
        teleop_l_speed = max(MIN_ABSOLUTE_SPEED,
464
                             min(MAX_ABSOLUTE_SPEED, l_speed));
465
        teleop_r_speed = max(MIN_ABSOLUTE_SPEED,
466
                             min(MAX_ABSOLUTE_SPEED, r_speed));
455 467
    }
456 468

  
457 469
    void SimFrame::teleop()
......
490 502
            return;
491 503
        }
492 504

  
493
        if (frame_count % 3 == 0)
494
        {
495
            path_image = path_bitmap.ConvertToImage();
496
            Refresh();
497
        }
505
        path_image = path_bitmap.ConvertToImage();
506
        Refresh();
498 507

  
499 508
        M_Scout::iterator it = scouts.begin();
500 509
        M_Scout::iterator end = scouts.end();
......
505 514

  
506 515
        for (; it != end; ++it)
507 516
        {
508
            it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on,
517
            it->second->update(SCOUTSIM_REFRESH_RATE,
518
                               path_dc,sonar_dc,sonar_visual_on,
509 519
                               path_image, lines_image, walls_image,
510 520
                               path_dc.GetBackground().GetColour(),
511
			       sonar_dc.GetBackground().GetColour(),
521
                               sonar_dc.GetBackground().GetColour(),
512 522
                               state);
513 523
        }
514

  
515
        frame_count++;
516 524
    }
517 525

  
518 526
    bool SimFrame::clearCallback(std_srvs::Empty::Request&,
scout/scoutsim/src/sim_frame.h
77 77
#define ID_TELEOP_FLUID 9
78 78
#define ID_SONAR 10
79 79

  
80
// Absolute speeds (0 - 255)
81
#define TELEOP_PRECISE_SPEED 160
82
#define TELEOP_FLUID_MAX_SPEED 200
83
#define TELEOP_FLUID_INC 10
80
// Absolute speeds (-128 - 127)
81
#define TELEOP_PRECISE_SPEED 62
82
#define TELEOP_FLUID_MAX_SPEED 100
83
#define TELEOP_FLUID_INC 8
84 84

  
85 85
// Teleop types
86 86
#define TELEOP_OFF 0
......
112 112
            DECLARE_EVENT_TABLE()
113 113

  
114 114
        private:
115

  
115 116
            void onUpdate(wxTimerEvent& evt);
116 117
            void onPaint(wxPaintEvent& evt);
117 118

  
......
136 137
            ros::Publisher wireless_receive;
137 138

  
138 139
            // Teleop
139
            int teleop_l_speed;
140
            int teleop_r_speed;
140
            short teleop_l_speed;
141
            short teleop_r_speed;
141 142
            ros::Publisher teleop_pub;
142 143
            int teleop_type;
143 144

  

Also available in: Unified diff