Statistics
| Branch: | Revision:

root / scout / scoutsim / src / scout.cpp @ 93cebb99

History | View | Annotate | Download (20.3 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
/**
49
 * @file scout.cpp
50
 *
51
 * @ingroup scoutsim
52
 * @{
53
 */
54

    
55
#include "scout.h"
56

    
57
#include <wx/wx.h>
58

    
59
#define DEFAULT_PEN_R 0xb3
60
#define DEFAULT_PEN_G 0xb8
61
#define DEFAULT_PEN_B 0xff
62

    
63
using namespace std;
64

    
65
namespace scoutsim
66
{
67
    /**
68
     * The scout object, which is responsible for refreshing itself and
69
     * updating its position and simulated sensors.
70
     *
71
     * @ingroup scoutsim
72
     */
73
    Scout::Scout(const ros::NodeHandle& nh,
74
                 const wxImage& scout_image,
75
                 const Vector2& pos,
76
                 wxBitmap *path_bitmap,
77
                 float orient)
78
        : path_bitmap(path_bitmap)
79
          , sonar_visual_on(false)
80
          , sonar_on(true)
81
          , ignore_behavior(false)
82
          , node (nh)
83
          , scout_image(scout_image)
84
          , pos(pos)
85
          , orient(orient)
86
          , motor_fl_speed(0)
87
          , motor_fr_speed(0)
88
          , motor_bl_speed(0)
89
          , motor_br_speed(0)
90
          , fl_ticks(0)
91
          , fr_ticks(0)
92
          , bl_ticks(0)
93
          , br_ticks(0)
94
          , pen_on(true)
95
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
96
    {
97
        pen.SetWidth(3);
98
        scout = wxBitmap(scout_image);
99

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

    
102
        pose_pub = node.advertise<Pose>("pose", 1);
103
        color_pub = node.advertise<Color>("color_sensor", 1);
104
        sonar_pub = node.advertise< ::messages::sonar_distance>("sonar_distance", 1);
105
        set_pen_srv = node.advertiseService("set_pen",
106
                                            &Scout::setPenCallback,
107
                                            this);
108
        toggle_sonar_srv = node.advertiseService("sonar_toggle",
109
                                            &Scout::handle_sonar_toggle,
110
                                            this);
111
        set_sonar_srv = node.advertiseService("sonar_set_scan",
112
                                            &Scout::handle_sonar_set_scan,
113
                                            this);
114
        query_encoders_srv =
115
            node.advertiseService("query_encoders",
116
                                  &Scout::query_encoders_callback,
117
                                  this);
118

    
119
        query_linesensor_srv =
120
            node.advertiseService("query_linesensor",
121
                                  &Scout::query_linesensor_callback,
122
                                  this);
123

    
124
        query_BOM_srv =
125
            node.advertiseService("query_BOM",
126
                                  &Scout::query_BOM_callback,
127
                                  this);
128

    
129

    
130
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
131
        {
132
            linesensor_readings.push_back(0);
133
        }
134

    
135
        for (unsigned int i = 0; i < NUM_BOMS; i++)
136
        {
137
            BOM_readings.push_back(0);
138
            BOM_senders.push_back(0);
139
        }
140

    
141
        // Initialize sonar
142
        sonar_position = 0;
143
        sonar_stop_l = 0;
144
        sonar_stop_r = 23;
145
        sonar_direction = 1;
146
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
147
        
148
        // Init latch
149
        teleop_latch = 0;
150
    }
151

    
152
    float Scout::absolute_to_mps(int absolute_speed)
153
    {
154
        return ((float) absolute_speed) * MAX_SPEED_MPS / MAX_ABSOLUTE_SPEED;
155
    }
156

    
157
    /**
158
     * A callback function that sets velocity based on a set_motors
159
     * request.
160
     * @todo Use "callback" in all callback function names? Or remove?
161
     */
162
    void Scout::setMotors(const ::messages::set_motors::ConstPtr& msg)
163
    {
164
        last_command_time = ros::WallTime::now();
165

    
166
        //ignore non-teleop commands if commands if teleop is ON
167
        //if (node.getNamespace() != current_teleop_scout || msg->teleop_ON)
168
        //{
169

    
170
        //latch value indicates number of uninterrupted teleop messages
171
        //before teleop latch shifts again
172
        if (!(msg->teleop_ON) && teleop_latch < 3)
173
        {
174
            teleop_latch++;
175
        }
176

    
177
        if (!(msg->teleop_ON) || teleop_latch ==0)
178
        {
179
            if(msg->fl_set)
180
            {
181
                motor_fl_speed = absolute_to_mps(msg->fl_speed);
182
            }
183
            if(msg->fr_set)
184
            {
185
                motor_fr_speed = absolute_to_mps(msg->fr_speed);
186
            }
187
            if(msg->bl_set)
188
            {
189
                motor_bl_speed = absolute_to_mps(msg->bl_speed);
190
            }
191
            if(msg->br_set)
192
            {
193
                motor_br_speed = absolute_to_mps(msg->br_speed);
194
            }
195
        }        
196

    
197
        //if a teleop message comes through, decrease the latch
198
        //latch code works on the assumption there will be more behavior messages
199
        //than teleop messages
200
        if (msg->teleop_ON && teleop_latch>0)
201
        {
202
            teleop_latch--;
203
        }
204
        //}
205

    
206
    }
207

    
208
    bool Scout::handle_sonar_toggle(::messages::sonar_toggle::Request  &req,
209
                         ::messages::sonar_toggle::Response &res)
210
    {
211
        if (req.set_on && !sonar_on)
212
        {
213
            ROS_INFO("Turning on the sonar");
214
            sonar_on = true;
215

    
216
        }
217
        else if (!req.set_on && sonar_on)
218
        {
219
            ROS_INFO("Turning off the sonar");
220
            sonar_on = false;
221
        }
222
        else
223
        {
224
            ROS_INFO("Sonar state remains unchanged");
225
        }
226
        res.ack = true;
227
        return true;
228
    }
229

    
230
    bool Scout::handle_sonar_set_scan(::messages::sonar_set_scan::Request  &req,
231
                                      ::messages::sonar_set_scan::Response &res)
232
    {
233
        // Code to set the sonar to scan from
234
        // req.stop_l to req.stop_r
235
        if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r)
236
        {
237
            ROS_INFO("Setting sonar scan range to [%i, %i]",
238
                     req.stop_l,
239
                     req.stop_r);
240
            sonar_stop_l = req.stop_l;
241
            sonar_stop_r = req.stop_r;
242
            sonar_position = req.stop_l;
243
            sonar_direction = 1;
244
            res.ack = true;
245
        }
246
        else
247
        {
248
            ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r");
249
        }
250
        return true;
251
    }
252

    
253
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
254
                               scoutsim::SetPen::Response&)
255
    {
256
        pen_on = !req.off;
257
        if (req.off)
258
        {
259
            return true;
260
        }
261

    
262
        wxPen pen(wxColour(req.r, req.g, req.b));
263
        if (req.width != 0)
264
        {
265
            pen.SetWidth(req.width);
266
        }
267

    
268
        pen = pen;
269
        return true;
270
    }
271

    
272
    bool Scout::query_encoders_callback(::messages::query_encoders::Request&,
273
                                        ::messages::query_encoders::Response& res)
274
    {
275
        res.fl_distance = fl_ticks;
276
        res.fr_distance = fr_ticks;
277
        res.bl_distance = bl_ticks;
278
        res.br_distance = br_ticks;
279

    
280
        return true;
281
    }
282

    
283
    bool Scout::query_linesensor_callback(::messages::query_linesensor::Request&,
284
                                          ::messages::query_linesensor::Response& res)
285
    {
286
        res.readings = linesensor_readings;
287

    
288
        return true;
289
    }
290

    
291
    bool Scout::query_BOM_callback(::messages::query_boms::Request&,
292
                                   ::messages::query_boms::Response& res)
293
    {
294
        res.readings = BOM_readings;
295
        res.senders = BOM_senders;
296

    
297
        return true;
298
    }
299

    
300
    // Scale to linesensor value
301
    unsigned int Scout::rgb_to_grey(unsigned char r,
302
                                    unsigned char g,
303
                                    unsigned char b)
304
    {
305
        // Should be 0 to 255
306
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
307

    
308
        /// @todo Convert to the proper range
309
        return 255 - grey;
310
    }
311

    
312
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
313
                                    double robot_theta, int sonar_pos, 
314
                                    wxMemoryDC& sonar_dc)
315
    {
316
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
317
        unsigned int d = 0;
318

    
319
        unsigned int reading = 0;
320

    
321
        int d_x = 0;
322
        int d_y = 0;
323

    
324
        do
325
        {
326
            d_x = x + (int) floor(d * cos(angle));
327
            d_y = y - (int) floor(d * sin(angle));
328

    
329
            // Out of image boundary
330
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
331
                d_y < 0 || d_y >= walls_image.GetHeight())
332
            {
333
                break;
334
            }
335

    
336
            // Max range
337
            if (d > scoutsim::SONAR_MAX_RANGE_PIX)
338
            {
339
                break;
340
            }
341

    
342
            // Get the sonar reading at the current position of the sonar
343
            unsigned char r = walls_image.GetRed(d_x, d_y);
344
            unsigned char g = walls_image.GetGreen(d_x, d_y);
345
            unsigned char b = walls_image.GetBlue(d_x, d_y);
346
    
347
            reading = rgb_to_grey(r, g, b);
348
        
349
            d++;
350
        }
351
        /// @todo Consider using different cutoffs for different features
352
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
353
        
354
        if (sonar_visual_on)
355
        {   
356
            if (isFront)
357
            {
358
                // draw a circle at the wall_x, wall_y where reading > 128
359
                sonar_dc.SelectObject(*path_bitmap);
360
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
361
                sonar_dc.DrawCircle(wxPoint(old_front_dx, old_front_dy), 2);
362
                old_front_dx = d_x;
363
                old_front_dy = d_y;
364
            }
365
            else
366
            {
367
                // draw a circle at the wall_x, wall_y where reading > 128
368
                sonar_dc.SelectObject(*path_bitmap);
369
                sonar_dc.SetBrush(*wxRED_BRUSH); //old value
370
                sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
371
                old_back_dx = d_x;
372
                old_back_dy = d_y;
373
            }
374

    
375
            sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
376
            sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2);
377
            if (isFront) // @todo for some reason isFront = (!isFront) is not working
378
            {
379
                isFront = FALSE;
380
            }
381
            else
382
            {
383
                isFront = TRUE; 
384
            }
385
        }
386

    
387
        // Convert from pixels to mm and return
388
        return (unsigned int) ((1000 / PIX_PER_METER) * d);
389
    }
390

    
391
    // x and y is current position of the sonar
392
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
393
                             double robot_theta,wxMemoryDC& sonar_dc)
394
    {
395
        // Only rotate the sonar at the correct rate.
396
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
397
        {
398
            return;
399
        }
400
        last_sonar_time = ros::Time::now();
401

    
402
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
403
                                           sonar_position, sonar_dc);
404
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
405
                                          sonar_position + 24, sonar_dc);
406

    
407
        // Publish
408
        ::messages::sonar_distance msg;
409
        msg.pos = sonar_position;
410
        msg.distance0 = d_front;
411
        msg.distance1 = d_back;
412
        msg.stamp = ros::Time::now();
413

    
414
        sonar_pub.publish(msg);
415

    
416
        // Update the sonar rotation
417
        if (sonar_position + sonar_direction <= sonar_stop_r &&
418
            sonar_position + sonar_direction >= sonar_stop_l)
419
        {
420
            sonar_position = sonar_position + sonar_direction;
421
        }
422
        else
423
        {
424
            sonar_direction = -sonar_direction;
425
        }
426
    }
427

    
428
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
429
                                  double robot_theta)
430
    {
431
        linesensor_readings.clear();
432

    
433
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
434
        for (int s = 0; s < NUM_LINESENSORS; s++)
435
        {
436
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
437

    
438
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
439
                                  offset * sin(robot_theta));
440
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
441
                                  offset * cos(robot_theta));
442

    
443
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
444
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
445
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
446

    
447
            unsigned int reading = rgb_to_grey(r, g, b);
448

    
449
            linesensor_readings.push_back(reading);
450
        }
451
    }
452

    
453
    void Scout::update_BOM(int bom_index,
454
                           unsigned int bom_value,
455
                           unsigned int sender) {
456
        ROS_INFO("BOM%d: %d", bom_index, bom_value);
457
        BOM_readings[bom_index] = bom_value;
458
        BOM_senders[bom_index] = sender;
459
    }
460

    
461
    /// Sends back the position of this scout so scoutsim can save
462
    /// the world state
463
    /// @todo remove dt param
464
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
465
                                        wxMemoryDC& sonar_dc,
466
                                        const wxImage& path_image,
467
                                        const wxImage& lines_image,
468
                                        const wxImage& walls_image,
469
                                        wxColour background_color,
470
                                        wxColour sonar_color,
471
                                        world_state state)
472
    {
473
        // Assume that the two motors on the same side will be set to
474
        // roughly the same speed. Does not account for slip conditions
475
        // when they are set to different speeds.
476
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
477
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
478

    
479
        // Find linear and angular movement in m
480
        float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2;
481
        float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
482

    
483
        //store currently teleop'd scoutname
484
        //std::stringstream ss;
485
        //ss << "/" << teleop_scoutname;
486
        //current_teleop_scout = ss.str();
487

    
488
        Vector2 old_pos = pos;
489

    
490
        // Update encoders
491
        fl_ticks += (unsigned int) (motor_fl_speed * SIM_TIME_REFRESH_RATE *
492
                                    ENCODER_TICKS_PER_METER);
493
        fr_ticks += (unsigned int) (motor_fr_speed * SIM_TIME_REFRESH_RATE *
494
                                    ENCODER_TICKS_PER_METER);
495
        bl_ticks += (unsigned int) (motor_bl_speed * SIM_TIME_REFRESH_RATE *
496
                                    ENCODER_TICKS_PER_METER);
497
        br_ticks += (unsigned int) (motor_br_speed * SIM_TIME_REFRESH_RATE *
498
                                    ENCODER_TICKS_PER_METER);
499

    
500
        orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI));
501

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

    
505
        // Clamp to screen size
506
        if (pos.x < 0 || pos.x >= state.canvas_width
507
                || pos.y < 0 || pos.y >= state.canvas_height)
508
        {
509
            ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
510
        }
511

    
512
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
513
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
514

    
515
        int canvas_x = pos.x * PIX_PER_METER;
516
        int canvas_y = pos.y * PIX_PER_METER;
517

    
518

    
519
        {
520
            wxImage rotated_image =
521
                scout_image.Rotate(orient - PI/2.0,
522
                                   wxPoint(scout_image.GetWidth() / 2,
523
                                           scout_image.GetHeight() / 2),
524
                                   false);
525

    
526
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
527
            {
528
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
529
                {
530
                    if (rotated_image.GetRed(x, y) == 255
531
                            && rotated_image.GetBlue(x, y) == 255
532
                            && rotated_image.GetGreen(x, y) == 255)
533
                    {
534
                        rotated_image.SetAlpha(x, y, 0);
535
                    }
536
                }
537
            }
538

    
539
            scout = wxBitmap(rotated_image);
540
        }
541

    
542
        Pose p;
543
        p.x = pos.x;
544
        p.y = pos.y;
545
        p.theta = orient;
546
        p.linear_velocity = l_speed;
547
        p.angular_velocity = r_speed;
548
        pose_pub.publish(p);
549

    
550
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
551
        if (sonar_on)
552
        {
553
            update_sonar(walls_image,
554
                         canvas_x + scoutsim::SCOUT_SONAR_X,
555
                         canvas_y + scoutsim::SCOUT_SONAR_Y,
556
                         p.theta,sonar_dc);
557

    
558
        }
559

    
560
        // Figure out (and publish) the color underneath the scout
561
        {
562
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
563
            Color color;
564
            color.r = path_image.GetRed(canvas_x, canvas_y);
565
            color.g = path_image.GetGreen(canvas_x, canvas_y);
566
            color.b = path_image.GetBlue(canvas_x, canvas_y);
567
            color_pub.publish(color);
568
        }
569

    
570
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
571
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
572

    
573
        if (pen_on)
574
        {
575
            if (pos != old_pos)
576
            {
577
                path_dc.SelectObject(*path_bitmap);
578
                path_dc.SetPen(pen);
579
                path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
580
                                 old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
581
            }
582
        }
583

    
584
        geometry_msgs::Pose2D my_pose;
585
        my_pose.x = pos.x;
586
        my_pose.y = pos.y;
587
        my_pose.theta = orient;
588

    
589
        return my_pose;
590
    }
591

    
592
    void Scout::paint(wxDC& dc)
593
    {
594
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
595
        dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
596
                      pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
597
    }
598

    
599
    void Scout::set_sonar_visual(bool on)
600
    {
601
        sonar_visual_on = on;
602
    }
603
}
604

    
605
/** @} */