Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (16.7 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(sonar_visual_on)
67
          , node (nh)
68
          , scout_image(scout_image)
69
          , pos(pos)
70
          , orient(orient)
71
          , motor_fl_speed(0)
72
          , motor_fr_speed(0)
73
          , motor_bl_speed(0)
74
          , motor_br_speed(0)
75
          , fl_ticks(0)
76
          , fr_ticks(0)
77
          , bl_ticks(0)
78
          , br_ticks(0)
79
          , pen_on(true)
80
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
81
    {
82
        pen.SetWidth(3);
83
        scout = wxBitmap(scout_image);
84

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

    
87
        pose_pub = node.advertise<Pose>("pose", 1);
88
        color_pub = node.advertise<Color>("color_sensor", 1);
89
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
90
        set_pen_srv = node.advertiseService("set_pen",
91
                                            &Scout::setPenCallback,
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);
99
        query_encoders_srv =
100
            node.advertiseService("query_encoders",
101
                                  &Scout::query_encoders_callback,
102
                                  this);
103

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

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

    
114
        meter = scout.GetHeight();
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

    
124
    /**
125
     * A callback function that sets velocity based on a set_motors
126
     * request.
127
     * @todo Use "callback" in all callback function names? Or remove?
128
     */
129
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
130
    {
131
        last_command_time = ros::WallTime::now();
132

    
133
        if(msg->fl_set)
134
        {
135
            motor_fl_speed = msg->fl_speed;
136
        }
137
        if(msg->fr_set)
138
        {
139
            motor_fr_speed = msg->fr_speed;
140
        }
141
        if(msg->bl_set)
142
        {
143
            motor_bl_speed = msg->bl_speed;
144
        }
145
        if(msg->br_set)
146
        {
147
            motor_br_speed = msg->br_speed;
148
        }
149

    
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
}
199

    
200
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
201
                               scoutsim::SetPen::Response&)
202
    {
203
        pen_on = !req.off;
204
        if (req.off)
205
        {
206
            return true;
207
        }
208

    
209
        wxPen pen(wxColour(req.r, req.g, req.b));
210
        if (req.width != 0)
211
        {
212
            pen.SetWidth(req.width);
213
        }
214

    
215
        pen = pen;
216
        return true;
217
    }
218

    
219
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
220
                                        encoders::query_encoders::Response& res)
221
    {
222
        res.fl_distance = fl_ticks;
223
        res.fr_distance = fr_ticks;
224
        res.bl_distance = bl_ticks;
225
        res.br_distance = br_ticks;
226

    
227
        return true;
228
    }
229

    
230
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
231
                                          linesensor::query_linesensor::Response& res)
232
    {
233
        res.readings = linesensor_readings;
234

    
235
        return true;
236
    }
237

    
238
    // Scale to linesensor value
239
    unsigned int Scout::rgb_to_grey(unsigned char r,
240
                                    unsigned char g,
241
                                    unsigned char b)
242
    {
243
        // Should be 0 to 255
244
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
245

    
246
        /// @todo Convert to the proper range
247
        return 255 - grey;
248
    }
249

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

    
256
        unsigned int reading = 0;
257
            int d_x = 0;
258
            int d_y = 0;
259
        do
260
        {
261
            d_x = x + (int) floor(d * cos(angle));
262
            d_y = y - (int) floor(d * sin(angle));
263

    
264
            // Out of image boundary
265
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
266
                d_y < 0 || d_y >= walls_image.GetHeight())
267
            {
268
                return d;
269
            }
270

    
271
            // Max range
272
            if (d > scoutsim::SONAR_MAX_RANGE)
273
            {
274
                return d;
275
            }
276

    
277
            // Get the sonar reading at the current position of the sonar
278
            unsigned char r = walls_image.GetRed(d_x, d_y);
279
            unsigned char g = walls_image.GetGreen(d_x, d_y);
280
            unsigned char b = walls_image.GetBlue(d_x, d_y);
281
        
282
            reading = rgb_to_grey(r, g, b);
283
            
284

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

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

    
326
    // x and y is current position of the sonar
327
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
328
                             double robot_theta,wxMemoryDC& sonar_dc)
329
    {
330
        // Only rotate the sonar at the correct rate.
331
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
332
        {
333
            return;
334
        }
335
        last_sonar_time = ros::Time::now();
336

    
337
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
338
                                           sonar_position,sonar_dc);
339
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
340
                                          sonar_position + 24,sonar_dc);
341

    
342
        // Publish
343
        sonar::sonar_distance msg;
344
        msg.pos = sonar_position;
345
        msg.distance0 = d_front;
346
        msg.distance1 = d_back;
347

    
348
        sonar_pub.publish(msg);
349

    
350
        // Update the sonar rotation
351
        if (sonar_position + sonar_direction <= sonar_stop_r &&
352
            sonar_position + sonar_direction >= sonar_stop_l)
353
        {
354
            sonar_position = sonar_position + sonar_direction;
355
        }
356
        else
357
        {
358
            sonar_direction = -sonar_direction;
359
        }
360
    }
361

    
362
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
363
                                  double robot_theta)
364
    {
365
        linesensor_readings.clear();
366

    
367
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
368
        for (int s = 0; s < NUM_LINESENSORS; s++)
369
        {
370
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
371

    
372
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
373
                                  offset * sin(robot_theta));
374
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
375
                                  offset * cos(robot_theta));
376

    
377
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
378
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
379
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
380

    
381
            unsigned int reading = rgb_to_grey(r, g, b);
382

    
383
            linesensor_readings.push_back(reading);
384
        }
385
    }
386

    
387
    /// Sends back the position of this scout so scoutsim can save
388
    /// the world state
389
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
390
                                        wxMemoryDC& sonar_dc,
391
                    bool sonar_visual,
392
                                        const wxImage& path_image,
393
                                        const wxImage& lines_image,
394
                                        const wxImage& walls_image,
395
                                        wxColour background_color,
396
                                        wxColour sonar_color,
397
                                        world_state state)
398
    {
399

    
400
        sonar_visual_on = sonar_visual;
401
        // Assume that the two motors on the same side will be set to
402
        // roughly the same speed. Does not account for slip conditions
403
        // when they are set to different speeds.
404
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
405
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
406

    
407
        // Set the linear and angular speeds
408
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
409
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
410

    
411
        Vector2 old_pos = pos;
412

    
413
        // Update encoders
414
        /// @todo replace
415
        fl_ticks += (unsigned int) motor_fl_speed;
416
        fr_ticks += (unsigned int) motor_fr_speed;
417
        bl_ticks += (unsigned int) motor_bl_speed;
418
        br_ticks += (unsigned int) motor_br_speed;
419

    
420
        orient = fmod(orient + ang_vel * dt, 2*PI);
421
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
422
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
423

    
424
        // Clamp to screen size
425
        if (pos.x < 0 || pos.x >= state.canvas_width
426
                || pos.y < 0 || pos.y >= state.canvas_height)
427
        {
428
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
429
        }
430

    
431
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
432
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
433

    
434
        int canvas_x = pos.x * meter;
435
        int canvas_y = pos.y * meter;
436

    
437
        {
438
            wxImage rotated_image =
439
                scout_image.Rotate(orient - PI/2.0,
440
                                   wxPoint(scout_image.GetWidth() / 2,
441
                                           scout_image.GetHeight() / 2),
442
                                   false);
443

    
444
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
445
            {
446
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
447
                {
448
                    if (rotated_image.GetRed(x, y) == 255
449
                            && rotated_image.GetBlue(x, y) == 255
450
                            && rotated_image.GetGreen(x, y) == 255)
451
                    {
452
                        rotated_image.SetAlpha(x, y, 0);
453
                    }
454
                }
455
            }
456

    
457
            scout = wxBitmap(rotated_image);
458
        }
459

    
460
        Pose p;
461
        p.x = pos.x;
462
        p.y = state.canvas_height - pos.y;
463
        p.theta = orient;
464
        p.linear_velocity = lin_vel;
465
        p.angular_velocity = ang_vel;
466
        pose_pub.publish(p);
467

    
468
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
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);
475

    
476
        }
477
        // Figure out (and publish) the color underneath the scout
478
        {
479
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
480
            Color color;
481
            color.r = path_image.GetRed(canvas_x, canvas_y);
482
            color.g = path_image.GetGreen(canvas_x, canvas_y);
483
            color.b = path_image.GetBlue(canvas_x, canvas_y);
484
            color_pub.publish(color);
485
        }
486

    
487
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
488
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
489

    
490
        if (pen_on)
491
        {
492
            if (pos != old_pos)
493
            {
494
                path_dc.SelectObject(*path_bitmap);
495
                path_dc.SetPen(pen);
496
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
497
                                 old_pos.x * meter, old_pos.y * meter);
498
            }
499
        }
500

    
501
        geometry_msgs::Pose2D my_pose;
502
        my_pose.x = pos.x;
503
        my_pose.y = pos.y;
504
        my_pose.theta = orient;
505

    
506
        return my_pose;
507
    }
508

    
509
    void Scout::paint(wxDC& dc)
510
    {
511
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
512
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
513
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
514
    }
515

    
516
}