Statistics
| Branch: | Revision:

root / scout / scoutsim / src / scout.cpp @ f09d002e

History | View | Annotate | Download (19.1 KB)

1
/**
2
 * The code in this package was developed using the structure of Willow
3
 * Garage's turtlesim package.  It was modified by the CMU Robotics Club
4
 * to be used as a simulator for the Colony Scout robot.
5
 *
6
 * All redistribution of this code is limited to the terms of Willow Garage's
7
 * licensing terms, as well as under permission from the CMU Robotics Club.
8
 * 
9
 * Copyright (c) 2011 Colony Project
10
 * 
11
 * Permission is hereby granted, free of charge, to any person
12
 * obtaining a copy of this software and associated documentation
13
 * files (the "Software"), to deal in the Software without
14
 * restriction, including without limitation the rights to use,
15
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
16
 * copies of the Software, and to permit persons to whom the
17
 * Software is furnished to do so, subject to the following
18
 * conditions:
19
 * 
20
 * The above copyright notice and this permission notice shall be
21
 * included in all copies or substantial portions of the Software.
22
 * 
23
 * Copyright (c) 2009, Willow Garage, Inc.
24
 * All rights reserved.
25
 * 
26
 * Redistribution and use in source and binary forms, with or without
27
 * modification, are permitted provided that the following conditions are met:
28
 * 
29
 *    Redistributions of source code must retain the above copyright
30
 *       notice, this list of conditions and the following disclaimer.
31
 *    Redistributions in binary form must reproduce the above copyright
32
 *       notice, this list of conditions and the following disclaimer in the
33
 *       documentation and/or other materials provided with the distribution.
34
 *    Neither the name of the Willow Garage, Inc. nor the names of its
35
 *       contributors may be used to endorse or promote products derived from
36
 *       this software without specific prior written permission.
37
 * 
38
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
39
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
40
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
41
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
42
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
43
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
44
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
45
 * OTHER DEALINGS IN THE SOFTWARE.
46
 */
47

    
48
#include "scout.h"
49

    
50
#include <wx/wx.h>
51

    
52
#define DEFAULT_PEN_R 0xb3
53
#define DEFAULT_PEN_G 0xb8
54
#define DEFAULT_PEN_B 0xff
55

    
56
using namespace std;
57

    
58
namespace scoutsim
59
{
60
    Scout::Scout(const ros::NodeHandle& nh,
61
                 const wxImage& scout_image,
62
                 const Vector2& pos,
63
                 wxBitmap *path_bitmap,
64
                 float orient)
65
        : path_bitmap(path_bitmap)
66
          , sonar_visual_on(false)
67
          , sonar_on(true)
68
          , ignore_behavior(false)
69
          , node (nh)
70
          , scout_image(scout_image)
71
          , pos(pos)
72
          , orient(orient)
73
          , motor_fl_speed(0)
74
          , motor_fr_speed(0)
75
          , motor_bl_speed(0)
76
          , motor_br_speed(0)
77
          , fl_ticks(0)
78
          , fr_ticks(0)
79
          , bl_ticks(0)
80
          , br_ticks(0)
81
          , pen_on(true)
82
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
83
    {
84
        pen.SetWidth(3);
85
        scout = wxBitmap(scout_image);
86

    
87
        motors_sub = node.subscribe("set_motors", 1, &Scout::setMotors, this);
88

    
89
        pose_pub = node.advertise<Pose>("pose", 1);
90
        color_pub = node.advertise<Color>("color_sensor", 1);
91
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
92
        set_pen_srv = node.advertiseService("set_pen",
93
                                            &Scout::setPenCallback,
94
                                            this);
95
        toggle_sonar_srv = node.advertiseService("sonar_toggle",
96
                                            &Scout::handle_sonar_toggle,
97
                                            this);
98
        set_sonar_srv = node.advertiseService("sonar_set_scan",
99
                                            &Scout::handle_sonar_set_scan,
100
                                            this);
101
        query_encoders_srv =
102
            node.advertiseService("query_encoders",
103
                                  &Scout::query_encoders_callback,
104
                                  this);
105

    
106
        query_linesensor_srv =
107
            node.advertiseService("query_linesensor",
108
                                  &Scout::query_linesensor_callback,
109
                                  this);
110

    
111
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
112
        {
113
            linesensor_readings.push_back(0);
114
        }
115

    
116
        // Initialize sonar
117
        sonar_position = 0;
118
        sonar_stop_l = 0;
119
        sonar_stop_r = 23;
120
        sonar_direction = 1;
121
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
122
        
123
        // Init latch
124
        teleop_latch = 0;
125
    }
126

    
127
    float Scout::absolute_to_mps(int absolute_speed)
128
    {
129
        return ((float) absolute_speed) * MAX_SPEED_MPS / MAX_ABSOLUTE_SPEED;
130
    }
131

    
132
    /**
133
     * A callback function that sets velocity based on a set_motors
134
     * request.
135
     * @todo Use "callback" in all callback function names? Or remove?
136
     */
137
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
138
    {
139
        last_command_time = ros::WallTime::now();
140

    
141
        //ignore non-teleop commands if commands if teleop is ON
142
        //if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
143
        //{
144

    
145
        //latch value indicates number of uninterrupted teleop messages
146
        //before teleop latch shifts again
147
        if (!(msg->teleop_ON) && teleop_latch < 3)
148
        {
149
            teleop_latch++;
150
        }
151

    
152
        if (!(msg->teleop_ON) || teleop_latch ==0)
153
        {
154
            if(msg->fl_set)
155
            {
156
                motor_fl_speed = absolute_to_mps(msg->fl_speed);
157
            }
158
            if(msg->fr_set)
159
            {
160
                motor_fr_speed = absolute_to_mps(msg->fr_speed);
161
            }
162
            if(msg->bl_set)
163
            {
164
                motor_bl_speed = absolute_to_mps(msg->bl_speed);
165
            }
166
            if(msg->br_set)
167
            {
168
                motor_br_speed = absolute_to_mps(msg->br_speed);
169
            }
170
        }        
171

    
172
        //if a teleop message comes through, decrease the latch
173
        //latch code works on the assumption there will be more behavior messages
174
        //than teleop messages
175
        if (msg->teleop_ON && teleop_latch>0)
176
        {
177
            teleop_latch--;
178
        }
179
        //}
180

    
181
    }
182

    
183
    bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request  &req,
184
                         sonar::sonar_toggle::Response &res)
185
    {
186
        if (req.set_on && !sonar_on)
187
        {
188
            ROS_INFO("Turning on the sonar");
189
            sonar_on = true;
190

    
191
        }
192
        else if (!req.set_on && sonar_on)
193
        {
194
            ROS_INFO("Turning off the sonar");
195
            sonar_on = false;
196
        }
197
        else
198
        {
199
            ROS_INFO("Sonar state remains unchanged");
200
        }
201
        res.ack = true;
202
        return true;
203
    }
204

    
205
    bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request  &req,
206
                                      sonar::sonar_set_scan::Response &res)
207
    {
208
        // Code to set the sonar to scan from
209
        // req.stop_l to req.stop_r
210
        if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
211
        {
212
            ROS_INFO("Setting sonar scan range to [%i, %i]",
213
                     req.stop_l,
214
                     req.stop_r);
215
            sonar_stop_l = req.stop_l;
216
            sonar_stop_r = req.stop_r;
217
            sonar_position = req.stop_l;
218
            sonar_direction = 1;
219
            res.ack = true;
220
        }
221
        else
222
        {
223
            ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
224
        }
225
        return true;
226
    }
227

    
228
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
229
                               scoutsim::SetPen::Response&)
230
    {
231
        pen_on = !req.off;
232
        if (req.off)
233
        {
234
            return true;
235
        }
236

    
237
        wxPen pen(wxColour(req.r, req.g, req.b));
238
        if (req.width != 0)
239
        {
240
            pen.SetWidth(req.width);
241
        }
242

    
243
        pen = pen;
244
        return true;
245
    }
246

    
247
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
248
                                        encoders::query_encoders::Response& res)
249
    {
250
        res.fl_distance = fl_ticks;
251
        res.fr_distance = fr_ticks;
252
        res.bl_distance = bl_ticks;
253
        res.br_distance = br_ticks;
254

    
255
        return true;
256
    }
257

    
258
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
259
                                          linesensor::query_linesensor::Response& res)
260
    {
261
        res.readings = linesensor_readings;
262

    
263
        return true;
264
    }
265

    
266
    // Scale to linesensor value
267
    unsigned int Scout::rgb_to_grey(unsigned char r,
268
                                    unsigned char g,
269
                                    unsigned char b)
270
    {
271
        // Should be 0 to 255
272
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
273

    
274
        /// @todo Convert to the proper range
275
        return 255 - grey;
276
    }
277

    
278
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
279
                                    double robot_theta, int sonar_pos, 
280
                                    wxMemoryDC& sonar_dc)
281
    {
282
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
283
        unsigned int d = 0;
284

    
285
        unsigned int reading = 0;
286

    
287
        int d_x = 0;
288
        int d_y = 0;
289

    
290
        do
291
        {
292
            d_x = x + (int) floor(d * cos(angle));
293
            d_y = y - (int) floor(d * sin(angle));
294

    
295
            // Out of image boundary
296
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
297
                d_y < 0 || d_y >= walls_image.GetHeight())
298
            {
299
                break;
300
            }
301

    
302
            // Max range
303
            if (d > scoutsim::SONAR_MAX_RANGE_PIX)
304
            {
305
                break;
306
            }
307

    
308
            // Get the sonar reading at the current position of the sonar
309
            unsigned char r = walls_image.GetRed(d_x, d_y);
310
            unsigned char g = walls_image.GetGreen(d_x, d_y);
311
            unsigned char b = walls_image.GetBlue(d_x, d_y);
312
    
313
            reading = rgb_to_grey(r, g, b);
314
        
315
            d++;
316
        }
317
        /// @todo Consider using different cutoffs for different features
318
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
319
        
320
        if (sonar_visual_on)
321
        {   
322
            if (isFront)
323
            {
324
                // draw a circle at the wall_x, wall_y where reading > 128
325
                sonar_dc.SelectObject(*path_bitmap);
326
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
327
                sonar_dc.DrawCircle(wxPoint(old_front_dx, old_front_dy), 2);
328
                old_front_dx = d_x;
329
                old_front_dy = d_y;
330
            }
331
            else
332
            {
333
                // draw a circle at the wall_x, wall_y where reading > 128
334
                sonar_dc.SelectObject(*path_bitmap);
335
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
336
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
337
                old_back_dx = d_x;
338
                old_back_dy = d_y;
339
            }
340

    
341
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
342
            sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2);
343
            if (isFront) // @todo for some reason isFront = (!isFront) is not working
344
            {
345
                isFront = FALSE;
346
            }
347
            else
348
            {
349
                isFront = TRUE; 
350
            }
351
        }
352

    
353
        // Convert from pixels to mm and return
354
        return (unsigned int) ((1000 / PIX_PER_METER) * d);
355
    }
356

    
357
    // x and y is current position of the sonar
358
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
359
                             double robot_theta,wxMemoryDC& sonar_dc)
360
    {
361
        // Only rotate the sonar at the correct rate.
362
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
363
        {
364
            return;
365
        }
366
        last_sonar_time = ros::Time::now();
367

    
368
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
369
                                           sonar_position, sonar_dc);
370
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
371
                                          sonar_position + 24, sonar_dc);
372

    
373
        // Publish
374
        sonar::sonar_distance msg;
375
        msg.pos = sonar_position;
376
        msg.distance0 = d_front;
377
        msg.distance1 = d_back;
378
        msg.stamp = ros::Time::now();
379

    
380
        sonar_pub.publish(msg);
381

    
382
        // Update the sonar rotation
383
        if (sonar_position + sonar_direction <= sonar_stop_r &&
384
            sonar_position + sonar_direction >= sonar_stop_l)
385
        {
386
            sonar_position = sonar_position + sonar_direction;
387
        }
388
        else
389
        {
390
            sonar_direction = -sonar_direction;
391
        }
392
    }
393

    
394
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
395
                                  double robot_theta)
396
    {
397
        linesensor_readings.clear();
398

    
399
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
400
        for (int s = 0; s < NUM_LINESENSORS; s++)
401
        {
402
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
403

    
404
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
405
                                  offset * sin(robot_theta));
406
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
407
                                  offset * cos(robot_theta));
408

    
409
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
410
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
411
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
412

    
413
            unsigned int reading = rgb_to_grey(r, g, b);
414

    
415
            linesensor_readings.push_back(reading);
416
        }
417
    }
418

    
419
    /// Sends back the position of this scout so scoutsim can save
420
    /// the world state
421
    /// TODO remove dt param
422
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
423
                                        wxMemoryDC& sonar_dc,
424
                                        const wxImage& path_image,
425
                                        const wxImage& lines_image,
426
                                        const wxImage& walls_image,
427
                                        wxColour background_color,
428
                                        wxColour sonar_color,
429
                                        world_state state)
430
    {
431
        // Assume that the two motors on the same side will be set to
432
        // roughly the same speed. Does not account for slip conditions
433
        // when they are set to different speeds.
434
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
435
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
436

    
437
        // Find linear and angular movement in m
438
        float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
439
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
440

    
441
        //store currently teleop'd scoutname
442
        //std::stringstream ss;
443
        //ss << "/" << teleop_scoutname;
444
        //current_teleop_scout = ss.str();
445

    
446
        Vector2 old_pos = pos;
447

    
448
        // Update encoders
449
        fl_ticks += (unsigned int) (motor_fl_speed * SIM_TIME_REFRESH_RATE *
450
                                    ENCODER_TICKS_PER_METER);
451
        fr_ticks += (unsigned int) (motor_fr_speed * SIM_TIME_REFRESH_RATE *
452
                                    ENCODER_TICKS_PER_METER);
453
        bl_ticks += (unsigned int) (motor_bl_speed * SIM_TIME_REFRESH_RATE *
454
                                    ENCODER_TICKS_PER_METER);
455
        br_ticks += (unsigned int) (motor_br_speed * SIM_TIME_REFRESH_RATE *
456
                                    ENCODER_TICKS_PER_METER);
457

    
458
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
459

    
460
        pos.x += cos(orient) * lin_dist;
461
        pos.y -= sin(orient) * lin_dist; //Subtract because of the way the simulator handles y directions.
462

    
463
        // Clamp to screen size
464
        if (pos.x < 0 || pos.x >= state.canvas_width
465
                || pos.y < 0 || pos.y >= state.canvas_height)
466
        {
467
            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
468
        }
469

    
470
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
471
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
472

    
473
        int canvas_x = pos.x * PIX_PER_METER;
474
        int canvas_y = pos.y * PIX_PER_METER;
475

    
476

    
477
        {
478
            wxImage rotated_image =
479
                scout_image.Rotate(orient - PI/2.0,
480
                                   wxPoint(scout_image.GetWidth() / 2,
481
                                           scout_image.GetHeight() / 2),
482
                                   false);
483

    
484
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
485
            {
486
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
487
                {
488
                    if (rotated_image.GetRed(x, y) == 255
489
                            && rotated_image.GetBlue(x, y) == 255
490
                            && rotated_image.GetGreen(x, y) == 255)
491
                    {
492
                        rotated_image.SetAlpha(x, y, 0);
493
                    }
494
                }
495
            }
496

    
497
            scout = wxBitmap(rotated_image);
498
        }
499

    
500
        Pose p;
501
        p.x = pos.x;
502
        p.y = pos.y;
503
        p.theta = orient;
504
        p.linear_velocity = l_speed;
505
        p.angular_velocity = r_speed;
506
        pose_pub.publish(p);
507

    
508
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
509
        if (sonar_on)
510
        {
511
            update_sonar(walls_image,
512
                         canvas_x + scoutsim::SCOUT_SONAR_X,
513
                         canvas_y + scoutsim::SCOUT_SONAR_Y,
514
                         p.theta,sonar_dc);
515

    
516
        }
517

    
518
        // Figure out (and publish) the color underneath the scout
519
        {
520
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
521
            Color color;
522
            color.r = path_image.GetRed(canvas_x, canvas_y);
523
            color.g = path_image.GetGreen(canvas_x, canvas_y);
524
            color.b = path_image.GetBlue(canvas_x, canvas_y);
525
            color_pub.publish(color);
526
        }
527

    
528
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
529
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
530

    
531
        if (pen_on)
532
        {
533
            if (pos != old_pos)
534
            {
535
                path_dc.SelectObject(*path_bitmap);
536
                path_dc.SetPen(pen);
537
                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
538
                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
539
            }
540
        }
541

    
542
        geometry_msgs::Pose2D my_pose;
543
        my_pose.x = pos.x;
544
        my_pose.y = pos.y;
545
        my_pose.theta = orient;
546

    
547
        return my_pose;
548
    }
549

    
550
    void Scout::paint(wxDC& dc)
551
    {
552
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
553
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
554
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
555
    }
556

    
557
    void Scout::set_sonar_visual(bool on)
558
    {
559
        sonar_visual_on = on;
560
    }
561
}