Revision 68b23184
Scoutsim: Added Sonar Toggle
Sonar display can now be toggled via Menu->Sim->Sonar
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 63 | 63 |
wxBitmap *path_bitmap, |
| 64 | 64 |
float orient) |
| 65 | 65 |
: path_bitmap(path_bitmap) |
| 66 |
, sonar_on(sonar_on) |
|
| 66 | 67 |
, node (nh) |
| 67 | 68 |
, scout_image(scout_image) |
| 68 | 69 |
, pos(pos) |
| ... | ... | |
| 193 | 194 |
} |
| 194 | 195 |
|
| 195 | 196 |
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, |
| 196 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc) |
|
| 197 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
|
|
| 197 | 198 |
{
|
| 198 | 199 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2; |
| 199 | 200 |
unsigned int d = 0; |
| ... | ... | |
| 232 | 233 |
/// @todo Consider using different cutoffs for different features |
| 233 | 234 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
| 234 | 235 |
|
| 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; |
|
| 244 |
|
|
| 236 |
if(sonar_on == TRUE) |
|
| 237 |
{
|
|
| 238 |
// draw a circle at the wall_x, wall_y where reading > 128 |
|
| 239 |
sonar_dc.SelectObject(*path_bitmap); |
|
| 240 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value |
|
| 241 |
sonar_dc.DrawCircle(wxPoint(old_dx,old_dy), 2); |
|
| 242 |
|
|
| 243 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value |
|
| 244 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2); |
|
| 245 |
old_dx = d_x; |
|
| 246 |
old_dy = d_y; |
|
| 247 |
} |
|
| 245 | 248 |
return d; |
| 246 | 249 |
} |
| 247 | 250 |
|
| 248 | 251 |
// x and y is current position of the sonar |
| 249 | 252 |
void Scout::update_sonar(const wxImage& walls_image, int x, int y, |
| 250 |
double robot_theta,wxMemoryDC& sonar_dc) |
|
| 253 |
double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
|
|
| 251 | 254 |
{
|
| 252 | 255 |
// Only rotate the sonar at the correct rate. |
| 253 | 256 |
if (ros::Time::now() - last_sonar_time < sonar_tick_time) |
| ... | ... | |
| 257 | 260 |
last_sonar_time = ros::Time::now(); |
| 258 | 261 |
|
| 259 | 262 |
unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta, |
| 260 |
sonar_position,sonar_dc); |
|
| 263 |
sonar_position,sonar_dc, sonar_on);
|
|
| 261 | 264 |
unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta, |
| 262 |
sonar_position + 24,sonar_dc); |
|
| 265 |
sonar_position + 24,sonar_dc, sonar_on);
|
|
| 263 | 266 |
|
| 264 | 267 |
// Publish |
| 265 | 268 |
sonar::sonar_distance msg; |
| ... | ... | |
| 310 | 313 |
/// the world state |
| 311 | 314 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| 312 | 315 |
wxMemoryDC& sonar_dc, |
| 316 |
bool sonar_on, |
|
| 313 | 317 |
const wxImage& path_image, |
| 314 | 318 |
const wxImage& lines_image, |
| 315 | 319 |
const wxImage& walls_image, |
| ... | ... | |
| 388 | 392 |
update_sonar(walls_image, |
| 389 | 393 |
canvas_x + scoutsim::SCOUT_SONAR_X, |
| 390 | 394 |
canvas_y + scoutsim::SCOUT_SONAR_Y, |
| 391 |
p.theta,sonar_dc); |
|
| 395 |
p.theta,sonar_dc,sonar_on);
|
|
| 392 | 396 |
|
| 393 | 397 |
// Figure out (and publish) the color underneath the scout |
| 394 | 398 |
{
|
Also available in: Unified diff