Revision 6639ce9c
Edited Sonar to display points on simulator.
Changed rate of sonar scan to 0.5s
Made a new wxDC graphics object for sonar
scout/scoutsim/src/scout.cpp | ||
---|---|---|
60 | 60 |
Scout::Scout(const ros::NodeHandle& nh, |
61 | 61 |
const wxImage& scout_image, |
62 | 62 |
const Vector2& pos, |
63 |
wxBitmap *path_bitmap, |
|
63 | 64 |
float orient) |
64 |
: node (nh) |
|
65 |
: path_bitmap(path_bitmap) |
|
66 |
, node (nh) |
|
65 | 67 |
, scout_image(scout_image) |
66 | 68 |
, pos(pos) |
67 | 69 |
, orient(orient) |
... | ... | |
191 | 193 |
} |
192 | 194 |
|
193 | 195 |
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, |
194 |
double robot_theta, int sonar_pos) |
|
196 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
|
|
195 | 197 |
{ |
196 | 198 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2; |
197 | 199 |
unsigned int d = 0; |
198 | 200 |
|
199 | 201 |
unsigned int reading = 0; |
202 |
int d_x = 0; |
|
203 |
int d_y = 0; |
|
200 | 204 |
do |
201 | 205 |
{ |
202 |
int d_x = x + (int) floor(d * cos(angle));
|
|
203 |
int d_y = y + (int) floor(d * sin(angle));
|
|
206 |
d_x = x + (int) floor(d * cos(angle)); |
|
207 |
d_y = y + (int) floor(d * sin(angle)); |
|
204 | 208 |
|
205 | 209 |
// Out of image boundary |
206 | 210 |
if (d_x < 0 || d_x >= walls_image.GetWidth() || |
... | ... | |
219 | 223 |
unsigned char r = walls_image.GetRed(d_x, d_y); |
220 | 224 |
unsigned char g = walls_image.GetGreen(d_x, d_y); |
221 | 225 |
unsigned char b = walls_image.GetBlue(d_x, d_y); |
222 |
|
|
226 |
|
|
223 | 227 |
reading = rgb_to_grey(r, g, b); |
228 |
|
|
224 | 229 |
|
225 | 230 |
d++; |
226 | 231 |
} |
227 | 232 |
/// @todo Consider using different cutoffs for different features |
228 | 233 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
234 |
|
|
235 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
236 |
sonar_dc.SelectObject(*path_bitmap); |
|
237 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
238 |
sonar_dc.DrawCircle(wxPoint(old_dx,old_dy), 2); |
|
239 |
|
|
240 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value |
|
241 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2); |
|
242 |
old_dx = d_x; |
|
243 |
old_dy = d_y; |
|
229 | 244 |
|
230 | 245 |
return d; |
231 | 246 |
} |
232 | 247 |
|
233 | 248 |
// x and y is current position of the sonar |
234 | 249 |
void Scout::update_sonar(const wxImage& walls_image, int x, int y, |
235 |
double robot_theta) |
|
250 |
double robot_theta,wxMemoryDC& sonar_dc)
|
|
236 | 251 |
{ |
237 | 252 |
// Only rotate the sonar at the correct rate. |
238 | 253 |
if (ros::Time::now() - last_sonar_time < sonar_tick_time) |
... | ... | |
242 | 257 |
last_sonar_time = ros::Time::now(); |
243 | 258 |
|
244 | 259 |
unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta, |
245 |
sonar_position); |
|
260 |
sonar_position,sonar_dc);
|
|
246 | 261 |
unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta, |
247 |
sonar_position + 24); |
|
262 |
sonar_position + 24,sonar_dc);
|
|
248 | 263 |
|
249 | 264 |
// Publish |
250 | 265 |
sonar::sonar_distance msg; |
... | ... | |
294 | 309 |
/// Sends back the position of this scout so scoutsim can save |
295 | 310 |
/// the world state |
296 | 311 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
312 |
wxMemoryDC& sonar_dc, |
|
297 | 313 |
const wxImage& path_image, |
298 | 314 |
const wxImage& lines_image, |
299 | 315 |
const wxImage& walls_image, |
300 | 316 |
wxColour background_color, |
317 |
wxColour sonar_color, |
|
301 | 318 |
world_state state) |
302 | 319 |
{ |
303 | 320 |
// Assume that the two motors on the same side will be set to |
... | ... | |
371 | 388 |
update_sonar(walls_image, |
372 | 389 |
canvas_x + scoutsim::SCOUT_SONAR_X, |
373 | 390 |
canvas_y + scoutsim::SCOUT_SONAR_Y, |
374 |
p.theta); |
|
391 |
p.theta,sonar_dc);
|
|
375 | 392 |
|
376 | 393 |
// Figure out (and publish) the color underneath the scout |
377 | 394 |
{ |
... | ... | |
390 | 407 |
{ |
391 | 408 |
if (pos != old_pos) |
392 | 409 |
{ |
410 |
path_dc.SelectObject(*path_bitmap); |
|
393 | 411 |
path_dc.SetPen(pen); |
394 | 412 |
path_dc.DrawLine(pos.x * meter, pos.y * meter, |
395 | 413 |
old_pos.x * meter, old_pos.y * meter); |
Also available in: Unified diff