Statistics
| Branch: | Revision:

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

History | View | Annotate | Download (15.1 KB)

1 c492be62 Alex Zirbel
/**
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 266ae7f2 Alex Zirbel
 *
6 c492be62 Alex Zirbel
 * 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 266ae7f2 Alex Zirbel
 * All rights reserved.
25 c492be62 Alex Zirbel
 * 
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 266ae7f2 Alex Zirbel
 */
47
48 a8480867 Alex Zirbel
#include "scout.h"
49 266ae7f2 Alex Zirbel
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 af0d9743 Alex
using namespace std;
57
58 266ae7f2 Alex Zirbel
namespace scoutsim
59
{
60
    Scout::Scout(const ros::NodeHandle& nh,
61 9f547ef7 Alex Zirbel
                 const wxImage& scout_image,
62
                 const Vector2& pos,
63 6639ce9c viki
                 wxBitmap *path_bitmap,
64 9f547ef7 Alex Zirbel
                 float orient)
65 6639ce9c viki
        : path_bitmap(path_bitmap)
66 68b23184 viki
          , sonar_on(sonar_on)
67 6639ce9c viki
          , node (nh)
68 144137a1 Alex Zirbel
          , scout_image(scout_image)
69
          , pos(pos)
70
          , orient(orient)
71 9f547ef7 Alex Zirbel
          , 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 144137a1 Alex Zirbel
          , pen_on(true)
80
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
81 266ae7f2 Alex Zirbel
    {
82 144137a1 Alex Zirbel
        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 a2e6bd4c Alex
        sonar_pub = node.advertise<sonar::sonar_distance>("sonar_distance", 1);
90 144137a1 Alex Zirbel
        set_pen_srv = node.advertiseService("set_pen",
91 266ae7f2 Alex Zirbel
                                            &Scout::setPenCallback,
92
                                            this);
93 9f547ef7 Alex Zirbel
        query_encoders_srv =
94
            node.advertiseService("query_encoders",
95
                                  &Scout::query_encoders_callback,
96
                                  this);
97 266ae7f2 Alex Zirbel
98 af0d9743 Alex
        query_linesensor_srv =
99
            node.advertiseService("query_linesensor",
100
                                  &Scout::query_linesensor_callback,
101
                                  this);
102
103 ade1b7f9 Alex
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
104
        {
105
            linesensor_readings.push_back(0);
106
        }
107
108 144137a1 Alex Zirbel
        meter = scout.GetHeight();
109 a2e6bd4c Alex
110
        // Initialize sonar
111
        sonar_position = 0;
112
        sonar_stop_l = 0;
113
        sonar_stop_r = 23;
114
        sonar_direction = 1;
115
        sonar_tick_time = ros::Duration(scoutsim::SONAR_HALF_SPIN_TIME / 24.0);
116 266ae7f2 Alex Zirbel
    }
117
118 a8480867 Alex Zirbel
    /**
119
     * A callback function that sets velocity based on a set_motors
120
     * request.
121 c492be62 Alex Zirbel
     * @todo Use "callback" in all callback function names? Or remove?
122 a8480867 Alex Zirbel
     */
123
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
124
    {
125 144137a1 Alex Zirbel
        last_command_time = ros::WallTime::now();
126 a8480867 Alex Zirbel
127
        if(msg->fl_set)
128
        {
129 144137a1 Alex Zirbel
            motor_fl_speed = msg->fl_speed;
130 a8480867 Alex Zirbel
        }
131
        if(msg->fr_set)
132
        {
133 144137a1 Alex Zirbel
            motor_fr_speed = msg->fr_speed;
134 a8480867 Alex Zirbel
        }
135
        if(msg->bl_set)
136
        {
137 144137a1 Alex Zirbel
            motor_bl_speed = msg->bl_speed;
138 a8480867 Alex Zirbel
        }
139
        if(msg->br_set)
140
        {
141 144137a1 Alex Zirbel
            motor_br_speed = msg->br_speed;
142 a8480867 Alex Zirbel
        }
143
144 266ae7f2 Alex Zirbel
    }
145
146
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
147
                               scoutsim::SetPen::Response&)
148
    {
149 144137a1 Alex Zirbel
        pen_on = !req.off;
150 266ae7f2 Alex Zirbel
        if (req.off)
151
        {
152
            return true;
153
        }
154
155
        wxPen pen(wxColour(req.r, req.g, req.b));
156
        if (req.width != 0)
157
        {
158
            pen.SetWidth(req.width);
159
        }
160
161 144137a1 Alex Zirbel
        pen = pen;
162 266ae7f2 Alex Zirbel
        return true;
163
    }
164
165 9f547ef7 Alex Zirbel
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
166
                                        encoders::query_encoders::Response& res)
167
    {
168
        res.fl_distance = fl_ticks;
169
        res.fr_distance = fr_ticks;
170
        res.bl_distance = bl_ticks;
171
        res.br_distance = br_ticks;
172
173
        return true;
174
    }
175
176 af0d9743 Alex
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
177
                                          linesensor::query_linesensor::Response& res)
178
    {
179 ade1b7f9 Alex
        res.readings = linesensor_readings;
180
181
        return true;
182
    }
183 af0d9743 Alex
184 ade1b7f9 Alex
    // Scale to linesensor value
185 a2e6bd4c Alex
    unsigned int Scout::rgb_to_grey(unsigned char r,
186
                                    unsigned char g,
187
                                    unsigned char b)
188 ade1b7f9 Alex
    {
189
        // Should be 0 to 255
190
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
191
192
        /// @todo Convert to the proper range
193
        return 255 - grey;
194
    }
195
196 a2e6bd4c Alex
    unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y,
197 68b23184 viki
                                    double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
198 a2e6bd4c Alex
    {
199 71e1154d Alex
        double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2;
200 a2e6bd4c Alex
        unsigned int d = 0;
201
202
        unsigned int reading = 0;
203 6639ce9c viki
        int d_x = 0;
204
        int d_y = 0;
205 a2e6bd4c Alex
        do
206
        {
207 6639ce9c viki
            d_x = x + (int) floor(d * cos(angle));
208
            d_y = y + (int) floor(d * sin(angle));
209 a2e6bd4c Alex
210
            // Out of image boundary
211
            if (d_x < 0 || d_x >= walls_image.GetWidth() ||
212
                d_y < 0 || d_y >= walls_image.GetHeight())
213
            {
214
                return d;
215
            }
216
217
            // Max range
218
            if (d > scoutsim::SONAR_MAX_RANGE)
219
            {
220
                return d;
221
            }
222
223
            // Get the sonar reading at the current position of the sonar
224
            unsigned char r = walls_image.GetRed(d_x, d_y);
225
            unsigned char g = walls_image.GetGreen(d_x, d_y);
226
            unsigned char b = walls_image.GetBlue(d_x, d_y);
227 6639ce9c viki
        
228 a2e6bd4c Alex
            reading = rgb_to_grey(r, g, b);
229 6639ce9c viki
            
230 a2e6bd4c Alex
231
            d++;
232
        }
233 71e1154d Alex
        /// @todo Consider using different cutoffs for different features
234
        while (reading < 128); /// @todo Get rid of hardcoded stuff like this
235 6639ce9c viki
        
236 b1fdaaf6 Hui Jun Tay
        if(sonar_on)
237
        {       
238
                if(isFront)
239
                {
240
                        // draw a circle at the wall_x, wall_y where reading > 128
241
                        sonar_dc.SelectObject(*path_bitmap);
242
                        sonar_dc.SetBrush(*wxRED_BRUSH); //old value
243
                        sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
244
                        old_front_dx = d_x;
245
                        old_front_dy = d_y;
246
                }
247
                else
248
                {
249
                        // draw a circle at the wall_x, wall_y where reading > 128
250
                        sonar_dc.SelectObject(*path_bitmap);
251
                        sonar_dc.SetBrush(*wxRED_BRUSH); //old value
252
                        sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
253
                        old_back_dx = d_x;
254
                        old_back_dy = d_y;
255
                }
256 68b23184 viki
257
                sonar_dc.SetBrush(*wxGREEN_BRUSH);  //newest value
258
                sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
259 b1fdaaf6 Hui Jun Tay
                if (isFront) //for some reason isFront = (!isFront) is not working
260
                {
261
                        isFront = FALSE;
262
                }
263
                else
264
                {
265
                        isFront = TRUE;        
266
                }
267
                        
268 68b23184 viki
        }
269 a2e6bd4c Alex
        return d;
270
    }
271
272 71e1154d Alex
    // x and y is current position of the sonar
273 a2e6bd4c Alex
    void Scout::update_sonar(const wxImage& walls_image, int x, int y,
274 68b23184 viki
                             double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
275 a2e6bd4c Alex
    {
276
        // Only rotate the sonar at the correct rate.
277
        if (ros::Time::now() - last_sonar_time < sonar_tick_time)
278
        {
279
            return;
280
        }
281
        last_sonar_time = ros::Time::now();
282
283
        unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta,
284 68b23184 viki
                                           sonar_position,sonar_dc, sonar_on);
285 a2e6bd4c Alex
        unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta,
286 68b23184 viki
                                          sonar_position + 24,sonar_dc, sonar_on);
287 a2e6bd4c Alex
288
        // Publish
289
        sonar::sonar_distance msg;
290
        msg.pos = sonar_position;
291
        msg.distance0 = d_front;
292
        msg.distance1 = d_back;
293
294
        sonar_pub.publish(msg);
295
296
        // Update the sonar rotation
297
        if (sonar_position + sonar_direction <= sonar_stop_r &&
298
            sonar_position + sonar_direction >= sonar_stop_l)
299
        {
300
            sonar_position = sonar_position + sonar_direction;
301
        }
302
        else
303
        {
304
            sonar_direction = -sonar_direction;
305
        }
306
    }
307
308
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y,
309
                                  double robot_theta)
310 ade1b7f9 Alex
    {
311
        linesensor_readings.clear();
312
313
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
314
        for (int s = 0; s < NUM_LINESENSORS; s++)
315 af0d9743 Alex
        {
316 ade1b7f9 Alex
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
317 af0d9743 Alex
318 a2e6bd4c Alex
            int sensor_x = (int) (x - LNSNSR_D * cos(robot_theta) -
319
                                  offset * sin(robot_theta));
320
            int sensor_y = (int) (y + LNSNSR_D * sin(robot_theta) -
321
                                  offset * cos(robot_theta));
322 af0d9743 Alex
323 ade1b7f9 Alex
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
324
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
325
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
326
327 a2e6bd4c Alex
            unsigned int reading = rgb_to_grey(r, g, b);
328 ade1b7f9 Alex
329
            linesensor_readings.push_back(reading);
330
        }
331 af0d9743 Alex
    }
332
333 9b3564f3 Alex Zirbel
    /// Sends back the position of this scout so scoutsim can save
334
    /// the world state
335
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
336 6639ce9c viki
                                        wxMemoryDC& sonar_dc,
337 68b23184 viki
                                        bool sonar_on,
338 9b3564f3 Alex Zirbel
                                        const wxImage& path_image,
339 ade1b7f9 Alex
                                        const wxImage& lines_image,
340 a2e6bd4c Alex
                                        const wxImage& walls_image,
341 9b3564f3 Alex Zirbel
                                        wxColour background_color,
342 6639ce9c viki
                                        wxColour sonar_color,
343 9b3564f3 Alex Zirbel
                                        world_state state)
344 266ae7f2 Alex Zirbel
    {
345 9f547ef7 Alex Zirbel
        // Assume that the two motors on the same side will be set to
346
        // roughly the same speed. Does not account for slip conditions
347
        // when they are set to different speeds.
348
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
349
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
350
351
        // Set the linear and angular speeds
352
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
353
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
354
355 144137a1 Alex Zirbel
        Vector2 old_pos = pos;
356 266ae7f2 Alex Zirbel
357 9f547ef7 Alex Zirbel
        // Update encoders
358
        /// @todo replace
359
        fl_ticks += (unsigned int) motor_fl_speed;
360
        fr_ticks += (unsigned int) motor_fr_speed;
361
        bl_ticks += (unsigned int) motor_bl_speed;
362
        br_ticks += (unsigned int) motor_br_speed;
363
364 144137a1 Alex Zirbel
        orient = fmod(orient + ang_vel * dt, 2*PI);
365
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
366
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
367 266ae7f2 Alex Zirbel
368
        // Clamp to screen size
369 e3f69e61 Alex
        if (pos.x < 0 || pos.x >= state.canvas_width
370
                || pos.y < 0 || pos.y >= state.canvas_height)
371 266ae7f2 Alex Zirbel
        {
372 144137a1 Alex Zirbel
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
373 266ae7f2 Alex Zirbel
        }
374
375 af0d9743 Alex
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
376
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
377 266ae7f2 Alex Zirbel
378 144137a1 Alex Zirbel
        int canvas_x = pos.x * meter;
379
        int canvas_y = pos.y * meter;
380 266ae7f2 Alex Zirbel
381
        {
382
            wxImage rotated_image =
383 144137a1 Alex Zirbel
                scout_image.Rotate(orient - PI/2.0,
384
                                   wxPoint(scout_image.GetWidth() / 2,
385 ade1b7f9 Alex
                                           scout_image.GetHeight() / 2),
386 5e2c3dc1 Alex
                                   false);
387 266ae7f2 Alex Zirbel
388
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
389
            {
390
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
391
                {
392
                    if (rotated_image.GetRed(x, y) == 255
393
                            && rotated_image.GetBlue(x, y) == 255
394
                            && rotated_image.GetGreen(x, y) == 255)
395
                    {
396
                        rotated_image.SetAlpha(x, y, 0);
397
                    }
398
                }
399
            }
400
401 144137a1 Alex Zirbel
            scout = wxBitmap(rotated_image);
402 266ae7f2 Alex Zirbel
        }
403
404
        Pose p;
405 144137a1 Alex Zirbel
        p.x = pos.x;
406 e3f69e61 Alex
        p.y = state.canvas_height - pos.y;
407 144137a1 Alex Zirbel
        p.theta = orient;
408
        p.linear_velocity = lin_vel;
409
        p.angular_velocity = ang_vel;
410
        pose_pub.publish(p);
411 266ae7f2 Alex Zirbel
412 ade1b7f9 Alex
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
413 a2e6bd4c Alex
        update_sonar(walls_image,
414
                     canvas_x + scoutsim::SCOUT_SONAR_X,
415
                     canvas_y + scoutsim::SCOUT_SONAR_Y,
416 68b23184 viki
                     p.theta,sonar_dc,sonar_on);
417 ade1b7f9 Alex
418 266ae7f2 Alex Zirbel
        // Figure out (and publish) the color underneath the scout
419
        {
420 23b8119f Alex
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
421 266ae7f2 Alex Zirbel
            Color color;
422
            color.r = path_image.GetRed(canvas_x, canvas_y);
423
            color.g = path_image.GetGreen(canvas_x, canvas_y);
424
            color.b = path_image.GetBlue(canvas_x, canvas_y);
425 144137a1 Alex Zirbel
            color_pub.publish(color);
426 266ae7f2 Alex Zirbel
        }
427
428
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
429 144137a1 Alex Zirbel
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
430 266ae7f2 Alex Zirbel
431 144137a1 Alex Zirbel
        if (pen_on)
432 266ae7f2 Alex Zirbel
        {
433 144137a1 Alex Zirbel
            if (pos != old_pos)
434 266ae7f2 Alex Zirbel
            {
435 6639ce9c viki
                path_dc.SelectObject(*path_bitmap);
436 144137a1 Alex Zirbel
                path_dc.SetPen(pen);
437
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
438
                                 old_pos.x * meter, old_pos.y * meter);
439 266ae7f2 Alex Zirbel
            }
440
        }
441 e3f69e61 Alex
442
        geometry_msgs::Pose2D my_pose;
443
        my_pose.x = pos.x;
444
        my_pose.y = pos.y;
445
        my_pose.theta = orient;
446
447
        return my_pose;
448 266ae7f2 Alex Zirbel
    }
449
450
    void Scout::paint(wxDC& dc)
451
    {
452 144137a1 Alex Zirbel
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
453
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
454
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
455 266ae7f2 Alex Zirbel
    }
456
457
}