Project

General

Profile

Statistics
| Branch: | Revision:

root / scout / scoutsim / src / scout.cpp @ 9b3564f3

History | View | Annotate | Download (8.86 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
namespace scoutsim
57
{
58
    Scout::Scout(const ros::NodeHandle& nh,
59
                 const wxImage& scout_image,
60
                 const Vector2& pos,
61
                 float orient)
62
        : node (nh)
63
          , scout_image(scout_image)
64
          , pos(pos)
65
          , orient(orient)
66
          , motor_fl_speed(0)
67
          , motor_fr_speed(0)
68
          , motor_bl_speed(0)
69
          , motor_br_speed(0)
70
          , fl_ticks(0)
71
          , fr_ticks(0)
72
          , bl_ticks(0)
73
          , br_ticks(0)
74
          , pen_on(true)
75
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
76
    {
77
        pen.SetWidth(3);
78
        scout = wxBitmap(scout_image);
79

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

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

    
92
        meter = scout.GetHeight();
93
    }
94

    
95
    /**
96
     * A callback function that sets velocity based on a set_motors
97
     * request.
98
     * @todo Use "callback" in all callback function names? Or remove?
99
     */
100
    void Scout::setMotors(const motors::set_motors::ConstPtr& msg)
101
    {
102
        last_command_time = ros::WallTime::now();
103

    
104
        if(msg->fl_set)
105
        {
106
            motor_fl_speed = msg->fl_speed;
107
        }
108
        if(msg->fr_set)
109
        {
110
            motor_fr_speed = msg->fr_speed;
111
        }
112
        if(msg->bl_set)
113
        {
114
            motor_bl_speed = msg->bl_speed;
115
        }
116
        if(msg->br_set)
117
        {
118
            motor_br_speed = msg->br_speed;
119
        }
120

    
121
    }
122

    
123
    float Scout::getSonar(float angle)
124
    {
125

    
126
        return 0.0;
127
    }
128

    
129
    bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
130
                               scoutsim::SetPen::Response&)
131
    {
132
        pen_on = !req.off;
133
        if (req.off)
134
        {
135
            return true;
136
        }
137

    
138
        wxPen pen(wxColour(req.r, req.g, req.b));
139
        if (req.width != 0)
140
        {
141
            pen.SetWidth(req.width);
142
        }
143

    
144
        pen = pen;
145
        return true;
146
    }
147

    
148
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
149
                                        encoders::query_encoders::Response& res)
150
    {
151
        res.fl_distance = fl_ticks;
152
        res.fr_distance = fr_ticks;
153
        res.bl_distance = bl_ticks;
154
        res.br_distance = br_ticks;
155

    
156
        return true;
157
    }
158

    
159
    /// Sends back the position of this scout so scoutsim can save
160
    /// the world state
161
    geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc,
162
                                        const wxImage& path_image,
163
                                        wxColour background_color,
164
                                        world_state state)
165
    {
166
        // Assume that the two motors on the same side will be set to
167
        // roughly the same speed. Does not account for slip conditions
168
        // when they are set to different speeds.
169
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
170
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
171

    
172
        // Set the linear and angular speeds
173
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
174
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
175

    
176
        Vector2 old_pos = pos;
177

    
178
        // Update encoders
179
        /// @todo replace
180
        fl_ticks += (unsigned int) motor_fl_speed;
181
        fr_ticks += (unsigned int) motor_fr_speed;
182
        bl_ticks += (unsigned int) motor_bl_speed;
183
        br_ticks += (unsigned int) motor_br_speed;
184

    
185
        orient = fmod(orient + ang_vel * dt, 2*PI);
186
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
187
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;
188

    
189
        // Clamp to screen size
190
        if (pos.x < 0 || pos.x >= canvas_width
191
                || pos.y < 0 || pos.y >= canvas_height)
192
        {
193
            ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
194
        }
195

    
196
        pos.x = std::min(std::max(pos.x, 0.0f), canvas_width);
197
        pos.y = std::min(std::max(pos.y, 0.0f), canvas_height);
198

    
199
        int canvas_x = pos.x * meter;
200
        int canvas_y = pos.y * meter;
201

    
202
        {
203
            wxImage rotated_image =
204
                scout_image.Rotate(orient - PI/2.0,
205
                                   wxPoint(scout_image.GetWidth() / 2,
206
                                            scout_image.GetHeight() / 2),
207
                                    false);
208

    
209
            for (int y = 0; y < rotated_image.GetHeight(); ++y)
210
            {
211
                for (int x = 0; x < rotated_image.GetWidth(); ++x)
212
                {
213
                    if (rotated_image.GetRed(x, y) == 255
214
                            && rotated_image.GetBlue(x, y) == 255
215
                            && rotated_image.GetGreen(x, y) == 255)
216
                    {
217
                        rotated_image.SetAlpha(x, y, 0);
218
                    }
219
                }
220
            }
221

    
222
            scout = wxBitmap(rotated_image);
223
        }
224

    
225
        Pose p;
226
        p.x = pos.x;
227
        p.y = canvas_height - pos.y;
228
        p.theta = orient;
229
        p.linear_velocity = lin_vel;
230
        p.angular_velocity = ang_vel;
231
        pose_pub.publish(p);
232

    
233
        // Figure out (and publish) the color underneath the scout
234
        {
235
            wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
236
            Color color;
237
            color.r = path_image.GetRed(canvas_x, canvas_y);
238
            color.g = path_image.GetGreen(canvas_x, canvas_y);
239
            color.b = path_image.GetBlue(canvas_x, canvas_y);
240
            color_pub.publish(color);
241
        }
242

    
243
        ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
244
                  node.getNamespace().c_str(), pos.x, pos.y, orient);
245

    
246
        if (pen_on)
247
        {
248
            if (pos != old_pos)
249
            {
250
                path_dc.SetPen(pen);
251
                path_dc.DrawLine(pos.x * meter, pos.y * meter,
252
                                 old_pos.x * meter, old_pos.y * meter);
253
            }
254
        }
255
    }
256

    
257
    void Scout::paint(wxDC& dc)
258
    {
259
        wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight());
260
        dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
261
                      pos.y * meter - (scout_size.GetHeight() / 2), true);
262
    }
263

    
264
}