Statistics
| Branch: | Revision:

root / scout / scoutsim / src / scout.cpp @ 23b8119f

History | View | Annotate | Download (11 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
                 float orient)
64 144137a1 Alex Zirbel
        : node (nh)
65
          , scout_image(scout_image)
66
          , pos(pos)
67
          , orient(orient)
68 9f547ef7 Alex Zirbel
          , 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 144137a1 Alex Zirbel
          , pen_on(true)
77
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
78 266ae7f2 Alex Zirbel
    {
79 144137a1 Alex Zirbel
        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
        set_pen_srv = node.advertiseService("set_pen",
87 266ae7f2 Alex Zirbel
                                            &Scout::setPenCallback,
88
                                            this);
89 9f547ef7 Alex Zirbel
        query_encoders_srv =
90
            node.advertiseService("query_encoders",
91
                                  &Scout::query_encoders_callback,
92
                                  this);
93 266ae7f2 Alex Zirbel
94 af0d9743 Alex
        query_linesensor_srv =
95
            node.advertiseService("query_linesensor",
96
                                  &Scout::query_linesensor_callback,
97
                                  this);
98
99 ade1b7f9 Alex
        for (unsigned int i = 0; i < NUM_LINESENSORS; i++)
100
        {
101
            linesensor_readings.push_back(0);
102
        }
103
104 144137a1 Alex Zirbel
        meter = scout.GetHeight();
105 266ae7f2 Alex Zirbel
    }
106
107 a8480867 Alex Zirbel
    /**
108
     * A callback function that sets velocity based on a set_motors
109
     * request.
110 c492be62 Alex Zirbel
     * @todo Use "callback" in all callback function names? Or remove?
111 a8480867 Alex Zirbel
     */
112
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
113
    {
114 144137a1 Alex Zirbel
        last_command_time = ros::WallTime::now();
115 a8480867 Alex Zirbel
116
        if(msg->fl_set)
117
        {
118 144137a1 Alex Zirbel
            motor_fl_speed = msg->fl_speed;
119 a8480867 Alex Zirbel
        }
120
        if(msg->fr_set)
121
        {
122 144137a1 Alex Zirbel
            motor_fr_speed = msg->fr_speed;
123 a8480867 Alex Zirbel
        }
124
        if(msg->bl_set)
125
        {
126 144137a1 Alex Zirbel
            motor_bl_speed = msg->bl_speed;
127 a8480867 Alex Zirbel
        }
128
        if(msg->br_set)
129
        {
130 144137a1 Alex Zirbel
            motor_br_speed = msg->br_speed;
131 a8480867 Alex Zirbel
        }
132
133 266ae7f2 Alex Zirbel
    }
134
135 c492be62 Alex Zirbel
    float Scout::getSonar(float angle)
136
    {
137
138
        return 0.0;
139
    }
140
141 266ae7f2 Alex Zirbel
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
142
                               scoutsim::SetPen::Response&)
143
    {
144 144137a1 Alex Zirbel
        pen_on = !req.off;
145 266ae7f2 Alex Zirbel
        if (req.off)
146
        {
147
            return true;
148
        }
149
150
        wxPen pen(wxColour(req.r, req.g, req.b));
151
        if (req.width != 0)
152
        {
153
            pen.SetWidth(req.width);
154
        }
155
156 144137a1 Alex Zirbel
        pen = pen;
157 266ae7f2 Alex Zirbel
        return true;
158
    }
159
160 9f547ef7 Alex Zirbel
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
161
                                        encoders::query_encoders::Response& res)
162
    {
163
        res.fl_distance = fl_ticks;
164
        res.fr_distance = fr_ticks;
165
        res.bl_distance = bl_ticks;
166
        res.br_distance = br_ticks;
167
168
        return true;
169
    }
170
171 af0d9743 Alex
    bool Scout::query_linesensor_callback(linesensor::query_linesensor::Request&,
172
                                          linesensor::query_linesensor::Response& res)
173
    {
174 ade1b7f9 Alex
        res.readings = linesensor_readings;
175
176
        return true;
177
    }
178 af0d9743 Alex
179 ade1b7f9 Alex
    // Scale to linesensor value
180
    unsigned int Scout::rgb_to_linesensor_val(unsigned char r,
181
                                              unsigned char g,
182
                                              unsigned char b)
183
    {
184
        // Should be 0 to 255
185
        unsigned int grey = ((unsigned int) r + (unsigned int) g + (unsigned int) b) / 3;
186
187
        /// @todo Convert to the proper range
188
        return 255 - grey;
189
    }
190
191
    void Scout::update_linesensor(const wxImage& lines_image, int x, int y, double theta)
192
    {
193
        linesensor_readings.clear();
194
195
        double spacing = scout_image.GetWidth() / (NUM_LINESENSORS - 1);
196
        for (int s = 0; s < NUM_LINESENSORS; s++)
197 af0d9743 Alex
        {
198 ade1b7f9 Alex
            double offset = -(scout_image.GetWidth() / 2) + s * spacing;
199 af0d9743 Alex
200 ade1b7f9 Alex
            int sensor_x = (int) (x - LNSNSR_D * cos(theta) - offset * sin(theta));
201
            int sensor_y = (int) (y + LNSNSR_D * sin(theta) - offset * cos(theta));
202 af0d9743 Alex
203 ade1b7f9 Alex
            unsigned char r = lines_image.GetRed(sensor_x, sensor_y);
204
            unsigned char g = lines_image.GetGreen(sensor_x, sensor_y);
205
            unsigned char b = lines_image.GetBlue(sensor_x, sensor_y);
206
207
            unsigned int reading = rgb_to_linesensor_val(r, g, b);
208
209
            linesensor_readings.push_back(reading);
210
        }
211 af0d9743 Alex
    }
212
213 9b3564f3 Alex Zirbel
    /// Sends back the position of this scout so scoutsim can save
214
    /// the world state
215
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
216
                                        const wxImage& path_image,
217 ade1b7f9 Alex
                                        const wxImage& lines_image,
218 9b3564f3 Alex Zirbel
                                        wxColour background_color,
219
                                        world_state state)
220 266ae7f2 Alex Zirbel
    {
221 9f547ef7 Alex Zirbel
        // Assume that the two motors on the same side will be set to
222
        // roughly the same speed. Does not account for slip conditions
223
        // when they are set to different speeds.
224
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
225
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
226
227
        // Set the linear and angular speeds
228
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
229
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
230
231 144137a1 Alex Zirbel
        Vector2 old_pos = pos;
232 266ae7f2 Alex Zirbel
233 9f547ef7 Alex Zirbel
        // Update encoders
234
        /// @todo replace
235
        fl_ticks += (unsigned int) motor_fl_speed;
236
        fr_ticks += (unsigned int) motor_fr_speed;
237
        bl_ticks += (unsigned int) motor_bl_speed;
238
        br_ticks += (unsigned int) motor_br_speed;
239
240 144137a1 Alex Zirbel
        orient = fmod(orient + ang_vel * dt, 2*PI);
241
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
242
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
243 266ae7f2 Alex Zirbel
244
        // Clamp to screen size
245 e3f69e61 Alex
        if (pos.x < 0 || pos.x >= state.canvas_width
246
                || pos.y < 0 || pos.y >= state.canvas_height)
247 266ae7f2 Alex Zirbel
        {
248 144137a1 Alex Zirbel
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
249 266ae7f2 Alex Zirbel
        }
250
251 af0d9743 Alex
        pos.x = min(max(pos.x, 0.0f), state.canvas_width);
252
        pos.y = min(max(pos.y, 0.0f), state.canvas_height);
253 266ae7f2 Alex Zirbel
254 144137a1 Alex Zirbel
        int canvas_x = pos.x * meter;
255
        int canvas_y = pos.y * meter;
256 266ae7f2 Alex Zirbel
257
        {
258
            wxImage rotated_image =
259 144137a1 Alex Zirbel
                scout_image.Rotate(orient - PI/2.0,
260
                                   wxPoint(scout_image.GetWidth() / 2,
261 ade1b7f9 Alex
                                           scout_image.GetHeight() / 2),
262 5e2c3dc1 Alex
                                   false);
263 266ae7f2 Alex Zirbel
264
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
265
            {
266
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
267
                {
268
                    if (rotated_image.GetRed(x, y) == 255
269
                            && rotated_image.GetBlue(x, y) == 255
270
                            && rotated_image.GetGreen(x, y) == 255)
271
                    {
272
                        rotated_image.SetAlpha(x, y, 0);
273
                    }
274
                }
275
            }
276
277 144137a1 Alex Zirbel
            scout = wxBitmap(rotated_image);
278 266ae7f2 Alex Zirbel
        }
279
280
        Pose p;
281 144137a1 Alex Zirbel
        p.x = pos.x;
282 e3f69e61 Alex
        p.y = state.canvas_height - pos.y;
283 144137a1 Alex Zirbel
        p.theta = orient;
284
        p.linear_velocity = lin_vel;
285
        p.angular_velocity = ang_vel;
286
        pose_pub.publish(p);
287 266ae7f2 Alex Zirbel
288 ade1b7f9 Alex
        update_linesensor(lines_image, canvas_x, canvas_y, p.theta);
289
290 266ae7f2 Alex Zirbel
        // Figure out (and publish) the color underneath the scout
291
        {
292 23b8119f Alex
            //wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
293 266ae7f2 Alex Zirbel
            Color color;
294
            color.r = path_image.GetRed(canvas_x, canvas_y);
295
            color.g = path_image.GetGreen(canvas_x, canvas_y);
296
            color.b = path_image.GetBlue(canvas_x, canvas_y);
297 144137a1 Alex Zirbel
            color_pub.publish(color);
298 266ae7f2 Alex Zirbel
        }
299
300
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
301 144137a1 Alex Zirbel
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
302 266ae7f2 Alex Zirbel
303 144137a1 Alex Zirbel
        if (pen_on)
304 266ae7f2 Alex Zirbel
        {
305 144137a1 Alex Zirbel
            if (pos != old_pos)
306 266ae7f2 Alex Zirbel
            {
307 144137a1 Alex Zirbel
                path_dc.SetPen(pen);
308
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
309
                                 old_pos.x * meter, old_pos.y * meter);
310 266ae7f2 Alex Zirbel
            }
311
        }
312 e3f69e61 Alex
313
        geometry_msgs::Pose2D my_pose;
314
        my_pose.x = pos.x;
315
        my_pose.y = pos.y;
316
        my_pose.theta = orient;
317
318
        return my_pose;
319 266ae7f2 Alex Zirbel
    }
320
321
    void Scout::paint(wxDC& dc)
322
    {
323 144137a1 Alex Zirbel
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
324
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
325
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
326 266ae7f2 Alex Zirbel
    }
327
328
}