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 |
{ |
scout/scoutsim/src/scout.h | ||
---|---|---|
113 | 113 |
const Vector2& pos, wxBitmap *path_bitmap, float orient); |
114 | 114 |
|
115 | 115 |
geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc, |
116 |
wxMemoryDC& sonar_dc, |
|
116 |
wxMemoryDC& sonar_dc, |
|
117 |
bool sonar_on, |
|
117 | 118 |
const wxImage& path_image, |
118 | 119 |
const wxImage& lines_image, |
119 | 120 |
const wxImage& walls_image, |
... | ... | |
134 | 135 |
unsigned char g, |
135 | 136 |
unsigned char b); |
136 | 137 |
unsigned int trace_sonar(const wxImage& walls_image, int x, int y, |
137 |
double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc); |
|
138 |
double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
|
|
138 | 139 |
void update_sonar(const wxImage& walls_image, int x, int y, |
139 |
double robot_theta, wxMemoryDC& sonar_dc); |
|
140 |
double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
|
|
140 | 141 |
void update_linesensor(const wxImage& lines_image, int x, int y, |
141 | 142 |
double theta); |
142 | 143 |
int old_dx; |
143 | 144 |
int old_dy; |
144 | 145 |
wxBitmap *path_bitmap; |
146 |
bool sonar_on; |
|
145 | 147 |
|
146 | 148 |
ros::NodeHandle node; |
147 | 149 |
|
scout/scoutsim/src/scoutsim.cpp | ||
---|---|---|
137 | 137 |
EVT_MENU(ID_TELEOP_NONE, scoutsim::SimFrame::stopTeleop) |
138 | 138 |
EVT_MENU(ID_TELEOP_PRECISE, scoutsim::SimFrame::startTeleopPrecise) |
139 | 139 |
EVT_MENU(ID_TELEOP_FLUID, scoutsim::SimFrame::startTeleopFluid) |
140 |
EVT_MENU(ID_SONAR, scoutsim::SimFrame::showSonar) |
|
140 | 141 |
END_EVENT_TABLE() |
141 | 142 |
|
142 | 143 |
DECLARE_APP(ScoutApp); |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
103 | 103 |
wxBitmap lines_bitmap; |
104 | 104 |
wxBitmap walls_bitmap; |
105 | 105 |
path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str())); |
106 |
sonar_on = TRUE; |
|
106 | 107 |
|
107 | 108 |
// Try to load the file; if it fails, make a new blank file |
108 | 109 |
if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str()))) |
... | ... | |
150 | 151 |
menuFile->Append(ID_QUIT, _("E&xit")); |
151 | 152 |
|
152 | 153 |
wxMenu *menuSim = new wxMenu; |
154 |
menuSim->Append(ID_SONAR, _("S&onar")); |
|
153 | 155 |
menuSim->Append(ID_CLEAR, _("&Clear")); |
154 | 156 |
|
155 | 157 |
wxMenu *menuView = new wxMenu; |
... | ... | |
268 | 270 |
clear(); |
269 | 271 |
} |
270 | 272 |
|
273 |
void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event)) |
|
274 |
{ |
|
275 |
sonar_on = not sonar_on; |
|
276 |
clear(); |
|
277 |
} |
|
278 |
|
|
279 |
|
|
271 | 280 |
void SimFrame::showMap(wxCommandEvent& WXUNUSED(event)) |
272 | 281 |
{ |
273 | 282 |
display_map_name = map_base_name; |
... | ... | |
480 | 489 |
|
481 | 490 |
for (; it != end; ++it) |
482 | 491 |
{ |
483 |
it->second->update(0.016, path_dc,sonar_dc, |
|
492 |
it->second->update(0.016, path_dc,sonar_dc,sonar_on,
|
|
484 | 493 |
path_image, lines_image, walls_image, |
485 | 494 |
path_dc.GetBackground().GetColour(), |
486 | 495 |
sonar_dc.GetBackground().GetColour(), |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
73 | 73 |
#define ID_TELEOP_NONE 7 |
74 | 74 |
#define ID_TELEOP_PRECISE 8 |
75 | 75 |
#define ID_TELEOP_FLUID 9 |
76 |
#define ID_SONAR 10 |
|
76 | 77 |
|
77 | 78 |
#define TELEOP_PRECISE_SPEED 30 |
78 | 79 |
#define TELEOP_FLUID_MAX_SPEED 80 |
... | ... | |
102 | 103 |
void stopTeleop(wxCommandEvent& event); |
103 | 104 |
void startTeleopPrecise(wxCommandEvent& event); |
104 | 105 |
void startTeleopFluid(wxCommandEvent& event); |
106 |
void showSonar(wxCommandEvent& event); |
|
105 | 107 |
|
106 | 108 |
DECLARE_EVENT_TABLE() |
107 | 109 |
|
... | ... | |
164 | 166 |
float width_in_meters; |
165 | 167 |
float height_in_meters; |
166 | 168 |
|
169 |
bool sonar_on; |
|
170 |
|
|
167 | 171 |
std::string map_base_name; |
168 | 172 |
std::string map_lines_name; |
169 | 173 |
std::string map_walls_name; |
Also available in: Unified diff