Revision 43811241
| ID | 43811241b4b22dba6e0abbbbb16af2e6ef041c0d |
Added services to set individual control for visualization tools.
However, there is a bug in the sonar viz for multiple scouts, looking into it now.
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 63 | 63 |
wxBitmap *path_bitmap, |
| 64 | 64 |
float orient) |
| 65 | 65 |
: path_bitmap(path_bitmap) |
| 66 |
, sonar_visual_on(sonar_visual_on)
|
|
| 67 |
, node (nh) |
|
| 66 |
, sonar_visual_on(false)
|
|
| 67 |
, node (nh)
|
|
| 68 | 68 |
, scout_image(scout_image) |
| 69 | 69 |
, pos(pos) |
| 70 | 70 |
, orient(orient) |
| ... | ... | |
| 312 | 312 |
} |
| 313 | 313 |
|
| 314 | 314 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value |
| 315 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
|
|
| 316 |
if (isFront) //for some reason isFront = (!isFront) is not working |
|
| 315 |
sonar_dc.DrawCircle(wxPoint(d_x,d_y), 2); |
|
| 316 |
if (isFront) // @todo for some reason isFront = (!isFront) is not working
|
|
| 317 | 317 |
{
|
| 318 | 318 |
isFront = FALSE; |
| 319 | 319 |
} |
| ... | ... | |
| 321 | 321 |
{
|
| 322 | 322 |
isFront = TRUE; |
| 323 | 323 |
} |
| 324 |
|
|
| 325 | 324 |
} |
| 326 | 325 |
|
| 327 | 326 |
return d; |
| ... | ... | |
| 339 | 338 |
last_sonar_time = ros::Time::now(); |
| 340 | 339 |
|
| 341 | 340 |
unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta, |
| 342 |
sonar_position,sonar_dc); |
|
| 341 |
sonar_position, sonar_dc);
|
|
| 343 | 342 |
unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta, |
| 344 |
sonar_position + 24,sonar_dc); |
|
| 343 |
sonar_position + 24, sonar_dc);
|
|
| 345 | 344 |
|
| 346 | 345 |
// Publish |
| 347 | 346 |
sonar::sonar_distance msg; |
| ... | ... | |
| 393 | 392 |
/// TODO remove dt param |
| 394 | 393 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| 395 | 394 |
wxMemoryDC& sonar_dc, |
| 396 |
bool sonar_visual, |
|
| 397 | 395 |
const wxImage& path_image, |
| 398 | 396 |
const wxImage& lines_image, |
| 399 | 397 |
const wxImage& walls_image, |
| ... | ... | |
| 401 | 399 |
wxColour sonar_color, |
| 402 | 400 |
world_state state) |
| 403 | 401 |
{
|
| 404 |
sonar_visual_on = sonar_visual; |
|
| 405 |
|
|
| 406 | 402 |
// Assume that the two motors on the same side will be set to |
| 407 | 403 |
// roughly the same speed. Does not account for slip conditions |
| 408 | 404 |
// when they are set to different speeds. |
| ... | ... | |
| 483 | 479 |
p.theta,sonar_dc); |
| 484 | 480 |
|
| 485 | 481 |
} |
| 482 |
|
|
| 486 | 483 |
// Figure out (and publish) the color underneath the scout |
| 487 | 484 |
{
|
| 488 | 485 |
//wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); |
| ... | ... | |
| 521 | 518 |
dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2), |
| 522 | 519 |
pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true); |
| 523 | 520 |
} |
| 521 |
|
|
| 522 |
void Scout::set_sonar_visual(bool on) |
|
| 523 |
{
|
|
| 524 |
/// @todo Remove |
|
| 525 |
ROS_INFO("Sonar visual on set.");
|
|
| 526 |
sonar_visual_on = on; |
|
| 527 |
} |
|
| 524 | 528 |
} |
Also available in: Unified diff