root / scout / scoutsim / src / scout.cpp @ 266ae7f2
History | View | Annotate | Download (7.38 KB)
1 | 266ae7f2 | Alex Zirbel | /*
|
---|---|---|---|
2 | * This package was developed by the CMU Robotics Club project using code
|
||
3 | * from Willow Garage, Inc. Please see licensing.txt for details.
|
||
4 | *
|
||
5 | * All rights reserved.
|
||
6 | *
|
||
7 | * @brief Keeps track of a single scout robot.
|
||
8 | * @file scout.cpp
|
||
9 | * @author Colony Project, CMU Robotics Club
|
||
10 | * @author Alex Zirbel
|
||
11 | */
|
||
12 | |||
13 | #include "scoutsim/scout.h" |
||
14 | |||
15 | #include <wx/wx.h> |
||
16 | |||
17 | #define DEFAULT_PEN_R 0xb3 |
||
18 | #define DEFAULT_PEN_G 0xb8 |
||
19 | #define DEFAULT_PEN_B 0xff |
||
20 | |||
21 | namespace scoutsim
|
||
22 | { |
||
23 | |||
24 | Scout::Scout(const ros::NodeHandle& nh,
|
||
25 | const wxImage& scout_image,
|
||
26 | const Vector2& pos,
|
||
27 | float orient)
|
||
28 | : nh_(nh) |
||
29 | , scout_image_(scout_image) |
||
30 | , pos_(pos) |
||
31 | , orient_(orient) |
||
32 | , lin_vel_(0.0) |
||
33 | , ang_vel_(0.0) |
||
34 | , pen_on_(true)
|
||
35 | , pen_(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B)) |
||
36 | { |
||
37 | pen_.SetWidth(3);
|
||
38 | scout_ = wxBitmap(scout_image_); |
||
39 | |||
40 | velocity_sub_ = |
||
41 | nh_.subscribe("command_velocity",
|
||
42 | 1,
|
||
43 | &Scout::velocityCallback, this);
|
||
44 | pose_pub_ = nh_.advertise<Pose>("pose", 1); |
||
45 | color_pub_ = nh_.advertise<Color>("color_sensor", 1); |
||
46 | set_pen_srv_ = nh_.advertiseService("set_pen",
|
||
47 | &Scout::setPenCallback, |
||
48 | this);
|
||
49 | teleport_relative_srv_ = |
||
50 | nh_.advertiseService("teleport_relative",
|
||
51 | &Scout::teleportRelativeCallback, |
||
52 | this);
|
||
53 | teleport_absolute_srv_ = |
||
54 | nh_.advertiseService("teleport_absolute",
|
||
55 | &Scout::teleportAbsoluteCallback, |
||
56 | this);
|
||
57 | |||
58 | meter_ = scout_.GetHeight(); |
||
59 | } |
||
60 | |||
61 | |||
62 | void Scout::velocityCallback(const VelocityConstPtr& vel) |
||
63 | { |
||
64 | last_command_time_ = ros::WallTime::now(); |
||
65 | lin_vel_ = vel->linear; |
||
66 | ang_vel_ = vel->angular; |
||
67 | } |
||
68 | |||
69 | bool Scout::setPenCallback(scoutsim::SetPen::Request& req,
|
||
70 | scoutsim::SetPen::Response&) |
||
71 | { |
||
72 | pen_on_ = !req.off; |
||
73 | if (req.off)
|
||
74 | { |
||
75 | return true; |
||
76 | } |
||
77 | |||
78 | wxPen pen(wxColour(req.r, req.g, req.b)); |
||
79 | if (req.width != 0) |
||
80 | { |
||
81 | pen.SetWidth(req.width); |
||
82 | } |
||
83 | |||
84 | pen_ = pen; |
||
85 | return true; |
||
86 | } |
||
87 | |||
88 | bool Scout::teleportRelativeCallback(scoutsim::TeleportRelative::Request& req,
|
||
89 | scoutsim::TeleportRelative::Response&) |
||
90 | { |
||
91 | teleport_requests_.push_back(TeleportRequest(0,
|
||
92 | 0,
|
||
93 | req.angular, |
||
94 | req.linear, |
||
95 | true));
|
||
96 | return true; |
||
97 | } |
||
98 | |||
99 | bool Scout::teleportAbsoluteCallback(scoutsim::TeleportAbsolute::Request& req,
|
||
100 | scoutsim::TeleportAbsolute::Response&) |
||
101 | { |
||
102 | teleport_requests_.push_back(TeleportRequest(req.x, |
||
103 | req.y, |
||
104 | req.theta, |
||
105 | 0,
|
||
106 | false));
|
||
107 | return true; |
||
108 | } |
||
109 | |||
110 | void Scout::update(double dt, wxMemoryDC& path_dc, |
||
111 | const wxImage& path_image, wxColour background_color,
|
||
112 | float canvas_width, float canvas_height) |
||
113 | { |
||
114 | // first process any teleportation requests, in order
|
||
115 | V_TeleportRequest::iterator it = teleport_requests_.begin(); |
||
116 | V_TeleportRequest::iterator end = teleport_requests_.end(); |
||
117 | for (; it != end; ++it)
|
||
118 | { |
||
119 | const TeleportRequest& req = *it;
|
||
120 | |||
121 | Vector2 old_pos = pos_; |
||
122 | if (req.relative)
|
||
123 | { |
||
124 | orient_ += req.theta; |
||
125 | pos_.x += sin(orient_ + PI/2.0) * req.linear; |
||
126 | pos_.y += cos(orient_ + PI/2.0) * req.linear; |
||
127 | } |
||
128 | else
|
||
129 | { |
||
130 | pos_.x = req.pos.x; |
||
131 | pos_.y = std::max(0.0f, canvas_height - req.pos.y); |
||
132 | orient_ = req.theta; |
||
133 | } |
||
134 | |||
135 | path_dc.SetPen(pen_); |
||
136 | path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_, |
||
137 | old_pos.x * meter_, old_pos.y * meter_); |
||
138 | } |
||
139 | |||
140 | teleport_requests_.clear(); |
||
141 | |||
142 | if (ros::WallTime::now() - last_command_time_ > ros::WallDuration(1.0)) |
||
143 | { |
||
144 | lin_vel_ = 0.0f; |
||
145 | ang_vel_ = 0.0f; |
||
146 | } |
||
147 | |||
148 | Vector2 old_pos = pos_; |
||
149 | |||
150 | orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);
|
||
151 | pos_.x += sin(orient_ + PI/2.0) * lin_vel_ * dt; |
||
152 | pos_.y += cos(orient_ + PI/2.0) * lin_vel_ * dt; |
||
153 | |||
154 | // Clamp to screen size
|
||
155 | if (pos_.x < 0 || pos_.x >= canvas_width |
||
156 | || pos_.y < 0 || pos_.y >= canvas_height)
|
||
157 | { |
||
158 | ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos_.x, pos_.y);
|
||
159 | } |
||
160 | |||
161 | pos_.x = std::min(std::max(pos_.x, 0.0f), canvas_width); |
||
162 | pos_.y = std::min(std::max(pos_.y, 0.0f), canvas_height); |
||
163 | |||
164 | int canvas_x = pos_.x * meter_;
|
||
165 | int canvas_y = pos_.y * meter_;
|
||
166 | |||
167 | { |
||
168 | wxImage rotated_image = |
||
169 | scout_image_.Rotate(orient_ - PI/2.0, |
||
170 | wxPoint(scout_image_.GetWidth() / 2,
|
||
171 | scout_image_.GetHeight() / 2),
|
||
172 | false);
|
||
173 | |||
174 | for (int y = 0; y < rotated_image.GetHeight(); ++y) |
||
175 | { |
||
176 | for (int x = 0; x < rotated_image.GetWidth(); ++x) |
||
177 | { |
||
178 | if (rotated_image.GetRed(x, y) == 255 |
||
179 | && rotated_image.GetBlue(x, y) == 255
|
||
180 | && rotated_image.GetGreen(x, y) == 255)
|
||
181 | { |
||
182 | rotated_image.SetAlpha(x, y, 0);
|
||
183 | } |
||
184 | } |
||
185 | } |
||
186 | |||
187 | scout_ = wxBitmap(rotated_image); |
||
188 | } |
||
189 | |||
190 | Pose p; |
||
191 | p.x = pos_.x; |
||
192 | p.y = canvas_height - pos_.y; |
||
193 | p.theta = orient_; |
||
194 | p.linear_velocity = lin_vel_; |
||
195 | p.angular_velocity = ang_vel_; |
||
196 | pose_pub_.publish(p); |
||
197 | |||
198 | // Figure out (and publish) the color underneath the scout
|
||
199 | { |
||
200 | wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight()); |
||
201 | Color color; |
||
202 | color.r = path_image.GetRed(canvas_x, canvas_y); |
||
203 | color.g = path_image.GetGreen(canvas_x, canvas_y); |
||
204 | color.b = path_image.GetBlue(canvas_x, canvas_y); |
||
205 | color_pub_.publish(color); |
||
206 | } |
||
207 | |||
208 | ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f",
|
||
209 | nh_.getNamespace().c_str(), pos_.x, pos_.y, orient_); |
||
210 | |||
211 | if (pen_on_)
|
||
212 | { |
||
213 | if (pos_ != old_pos)
|
||
214 | { |
||
215 | path_dc.SetPen(pen_); |
||
216 | path_dc.DrawLine(pos_.x * meter_, pos_.y * meter_, |
||
217 | old_pos.x * meter_, old_pos.y * meter_); |
||
218 | } |
||
219 | } |
||
220 | } |
||
221 | |||
222 | void Scout::paint(wxDC& dc)
|
||
223 | { |
||
224 | wxSize scout_size = wxSize(scout_.GetWidth(), scout_.GetHeight()); |
||
225 | dc.DrawBitmap(scout_, pos_.x * meter_ - (scout_size.GetWidth() / 2),
|
||
226 | pos_.y * meter_ - (scout_size.GetHeight() / 2), true); |
||
227 | } |
||
228 | |||
229 | } |