Statistics
| Branch: | Revision:

scoutos / scout / scoutsim / src / scout.cpp @ a2e6bd4c

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

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

    
84
        pose_pub = node.advertise<Pose>("pose", 1);
85
        color_pub = node.advertise<Color>("color_sensor", 1);
86
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
87
        set_pen_srv = node.advertiseService("set_pen",
88
                                            &Scout::setPenCallback,
89
                                            this);
90
        query_encoders_srv =
91
            node.advertiseService("query_encoders",
92
                                  &Scout::query_encoders_callback,
93
                                  this);
94

    
95
        query_linesensor_srv =
96
            node.advertiseService("query_linesensor",
97
                                  &Scout::query_linesensor_callback,
98
                                  this);
99

    
100
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
101
        {
102
            linesensor_readings.push_back(0);
103
        }
104

    
105
        meter = scout.GetHeight();
106

    
107
        // Initialize sonar
108
        sonar_position = 0;
109
        sonar_stop_l = 0;
110
        sonar_stop_r = 23;
111
        sonar_direction = 1;
112
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
113
    }
114

    
115
    /**
116
     * A callback function that sets velocity based on a set_motors
117
     * request.
118
     * @todo Use "callback" in all callback function names? Or remove?
119
     */
120
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
121
    {
122
        last_command_time = ros::WallTime::now();
123

    
124
        if(msg->fl_set)
125
        {
126
            motor_fl_speed = msg->fl_speed;
127
        }
128
        if(msg->fr_set)
129
        {
130
            motor_fr_speed = msg->fr_speed;
131
        }
132
        if(msg->bl_set)
133
        {
134
            motor_bl_speed = msg->bl_speed;
135
        }
136
        if(msg->br_set)
137
        {
138
            motor_br_speed = msg->br_speed;
139
        }
140

    
141
    }
142

    
143
    float Scout::getSonar(float angle)
144
    {
145

    
146
        return 0.0;
147
    }
148

    
149
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
150
                               scoutsim::SetPen::Response&)
151
    {
152
        pen_on = !req.off;
153
        if (req.off)
154
        {
155
            return true;
156
        }
157

    
158
        wxPen pen(wxColour(req.r, req.g, req.b));
159
        if (req.width != 0)
160
        {
161
            pen.SetWidth(req.width);
162
        }
163

    
164
        pen = pen;
165
        return true;
166
    }
167

    
168
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
169
                                        encoders::query_encoders::Response& res)
170
    {
171
        res.fl_distance = fl_ticks;
172
        res.fr_distance = fr_ticks;
173
        res.bl_distance = bl_ticks;
174
        res.br_distance = br_ticks;
175

    
176
        return true;
177
    }
178

    
179
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
180
                                          linesensor::query_linesensor::Response& res)
181
    {
182
        res.readings = linesensor_readings;
183

    
184
        return true;
185
    }
186

    
187
    // Scale to linesensor value
188
    unsigned int Scout::rgb_to_grey(unsigned char r,
189
                                    unsigned char g,
190
                                    unsigned char b)
191
    {
192
        // Should be 0 to 255
193
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
194

    
195
        /// @todo Convert to the proper range
196
        return 255 - grey;
197
    }
198

    
199
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
200
                                    double robot_theta, int sonar_pos)
201
    {
202
        double angle = robot_theta + (180.0 * ((float) sonar_pos) / 24.0);
203
        unsigned int d = 0;
204

    
205
        unsigned int reading = 0;
206
        do
207
        {
208
            int d_x = x + (int) floor(d * cos(angle));
209
            int d_y = y + (int) floor(d * sin(angle));
210

    
211
            // Out of image boundary
212
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
213
                d_y < 0 || d_y >= walls_image.GetHeight())
214
            {
215
                return d;
216
            }
217

    
218
            // Max range
219
            if (d > scoutsim::SONAR_MAX_RANGE)
220
            {
221
                return d;
222
            }
223

    
224
            // Get the sonar reading at the current position of the sonar
225
            unsigned char r = walls_image.GetRed(d_x, d_y);
226
            unsigned char g = walls_image.GetGreen(d_x, d_y);
227
            unsigned char b = walls_image.GetBlue(d_x, d_y);
228

    
229
            reading = rgb_to_grey(r, g, b);
230

    
231
            d++;
232
        }
233
        while (reading < 128);
234

    
235
        return d;
236
    }
237

    
238
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
239
                             double robot_theta)
240
    {
241
        // Only rotate the sonar at the correct rate.
242
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
243
        {
244
            return;
245
        }
246
        last_sonar_time = ros::Time::now();
247

    
248
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
249
                                           sonar_position);
250
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
251
                                          sonar_position + 24);
252

    
253
        // Publish
254
        sonar::sonar_distance msg;
255
        msg.pos = sonar_position;
256
        msg.distance0 = d_front;
257
        msg.distance1 = d_back;
258

    
259
        sonar_pub.publish(msg);
260

    
261
        // Update the sonar rotation
262
        if (sonar_position + sonar_direction <= sonar_stop_r &&
263
            sonar_position + sonar_direction >= sonar_stop_l)
264
        {
265
            sonar_position = sonar_position + sonar_direction;
266
        }
267
        else
268
        {
269
            sonar_direction = -sonar_direction;
270
        }
271
    }
272

    
273
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
274
                                  double robot_theta)
275
    {
276
        linesensor_readings.clear();
277

    
278
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
279
        for (int s = 0; s < NUM_LINESENSORS; s++)
280
        {
281
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
282

    
283
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
284
                                  offset * sin(robot_theta));
285
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
286
                                  offset * cos(robot_theta));
287

    
288
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
289
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
290
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
291

    
292
            unsigned int reading = rgb_to_grey(r, g, b);
293

    
294
            linesensor_readings.push_back(reading);
295
        }
296
    }
297

    
298
    /// Sends back the position of this scout so scoutsim can save
299
    /// the world state
300
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
301
                                        const wxImage& path_image,
302
                                        const wxImage& lines_image,
303
                                        const wxImage& walls_image,
304
                                        wxColour background_color,
305
                                        world_state state)
306
    {
307
        // Assume that the two motors on the same side will be set to
308
        // roughly the same speed. Does not account for slip conditions
309
        // when they are set to different speeds.
310
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
311
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
312

    
313
        // Set the linear and angular speeds
314
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
315
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
316

    
317
        Vector2 old_pos = pos;
318

    
319
        // Update encoders
320
        /// @todo replace
321
        fl_ticks += (unsigned int) motor_fl_speed;
322
        fr_ticks += (unsigned int) motor_fr_speed;
323
        bl_ticks += (unsigned int) motor_bl_speed;
324
        br_ticks += (unsigned int) motor_br_speed;
325

    
326
        orient = fmod(orient + ang_vel * dt, 2*PI);
327
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
328
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
329

    
330
        // Clamp to screen size
331
        if (pos.x < 0 || pos.x >= state.canvas_width
332
                || pos.y < 0 || pos.y >= state.canvas_height)
333
        {
334
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
335
        }
336

    
337
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
338
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
339

    
340
        int canvas_x = pos.x * meter;
341
        int canvas_y = pos.y * meter;
342

    
343
        {
344
            wxImage rotated_image =
345
                scout_image.Rotate(orient - PI/2.0,
346
                                   wxPoint(scout_image.GetWidth() / 2,
347
                                           scout_image.GetHeight() / 2),
348
                                   false);
349

    
350
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
351
            {
352
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
353
                {
354
                    if (rotated_image.GetRed(x, y) == 255
355
                            && rotated_image.GetBlue(x, y) == 255
356
                            && rotated_image.GetGreen(x, y) == 255)
357
                    {
358
                        rotated_image.SetAlpha(x, y, 0);
359
                    }
360
                }
361
            }
362

    
363
            scout = wxBitmap(rotated_image);
364
        }
365

    
366
        Pose p;
367
        p.x = pos.x;
368
        p.y = state.canvas_height - pos.y;
369
        p.theta = orient;
370
        p.linear_velocity = lin_vel;
371
        p.angular_velocity = ang_vel;
372
        pose_pub.publish(p);
373

    
374
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
375
        update_sonar(walls_image,
376
                     canvas_x + scoutsim::SCOUT_SONAR_X,
377
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
378
                     p.theta);
379

    
380
        // Figure out (and publish) the color underneath the scout
381
        {
382
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
383
            Color color;
384
            color.r = path_image.GetRed(canvas_x, canvas_y);
385
            color.g = path_image.GetGreen(canvas_x, canvas_y);
386
            color.b = path_image.GetBlue(canvas_x, canvas_y);
387
            color_pub.publish(color);
388
        }
389

    
390
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
391
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
392

    
393
        if (pen_on)
394
        {
395
            if (pos != old_pos)
396
            {
397
                path_dc.SetPen(pen);
398
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
399
                                 old_pos.x * meter, old_pos.y * meter);
400
            }
401
        }
402

    
403
        geometry_msgs::Pose2D my_pose;
404
        my_pose.x = pos.x;
405
        my_pose.y = pos.y;
406
        my_pose.theta = orient;
407

    
408
        return my_pose;
409
    }
410

    
411
    void Scout::paint(wxDC& dc)
412
    {
413
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
414
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
415
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
416
    }
417

    
418
}