Revision eb9cff77
ID | eb9cff771c06a2ed3f37c591ed4c1ee0a20df76d |
Fixed sonar bug (corrected y direction)
Added sonar_toggle and set_scan command and callbacks
Cleaned up sonar_visual toggle code
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 |
, sonar_visual_on(sonar_visual_on)
|
|
67 | 67 |
, node (nh) |
68 | 68 |
, scout_image(scout_image) |
69 | 69 |
, pos(pos) |
... | ... | |
90 | 90 |
set_pen_srv = node.advertiseService("set_pen", |
91 | 91 |
&Scout::setPenCallback, |
92 | 92 |
this); |
93 |
toggle_sonar_srv = node.advertiseService("sonar_toggle", |
|
94 |
&Scout::handle_sonar_toggle, |
|
95 |
this); |
|
96 |
set_sonar_srv = node.advertiseService("sonar_set_scan", |
|
97 |
&Scout::handle_sonar_set_scan, |
|
98 |
this); |
|
93 | 99 |
query_encoders_srv = |
94 | 100 |
node.advertiseService("query_encoders", |
95 | 101 |
&Scout::query_encoders_callback, |
... | ... | |
142 | 148 |
} |
143 | 149 |
|
144 | 150 |
} |
151 |
//scout sonar callback |
|
152 |
|
|
153 |
bool Scout::handle_sonar_toggle(sonar::sonar_toggle::Request &req, |
|
154 |
sonar::sonar_toggle::Response &res) |
|
155 |
{ |
|
156 |
if (req.set_on && !sonar_on) |
|
157 |
{ |
|
158 |
ROS_INFO("Turning on the sonar"); |
|
159 |
sonar_on = true; |
|
160 |
|
|
161 |
} |
|
162 |
else if (!req.set_on && sonar_on) |
|
163 |
{ |
|
164 |
ROS_INFO("Turning off the sonar"); |
|
165 |
sonar_on = false; |
|
166 |
} |
|
167 |
else |
|
168 |
{ |
|
169 |
ROS_INFO("Sonar state remains unchanged"); |
|
170 |
} |
|
171 |
|
|
172 |
res.ack = true; |
|
173 |
return true; |
|
174 |
} |
|
175 |
|
|
176 |
|
|
177 |
bool Scout::handle_sonar_set_scan(sonar::sonar_set_scan::Request &req, |
|
178 |
sonar::sonar_set_scan::Response &res) |
|
179 |
{ |
|
180 |
// Code to set the sonar to scan from |
|
181 |
// req.stop_l to req.stop_r |
|
182 |
if (req.stop_l>=0 and req.stop_r<=23 and req.stop_l<=req.stop_r) |
|
183 |
{ |
|
184 |
ROS_INFO("Setting sonar scan range to [%i, %i]", |
|
185 |
req.stop_l, |
|
186 |
req.stop_r); |
|
187 |
sonar_stop_l = req.stop_l; |
|
188 |
sonar_stop_r = req.stop_r; |
|
189 |
sonar_position = req.stop_l; |
|
190 |
sonar_direction = 1; |
|
191 |
res.ack = true; |
|
192 |
} |
|
193 |
else |
|
194 |
{ |
|
195 |
ROS_INFO("Bad Input: Input should be integers 0-23, stop_l<stop_r"); |
|
196 |
} |
|
197 |
return true; |
|
198 |
} |
|
145 | 199 |
|
146 | 200 |
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, |
147 | 201 |
scoutsim::SetPen::Response&) |
... | ... | |
194 | 248 |
} |
195 | 249 |
|
196 | 250 |
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, |
197 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc, bool sonar_on)
|
|
251 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc) |
|
198 | 252 |
{ |
199 | 253 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2; |
200 | 254 |
unsigned int d = 0; |
201 | 255 |
|
202 | 256 |
unsigned int reading = 0; |
203 |
int d_x = 0; |
|
204 |
int d_y = 0; |
|
257 |
int d_x = 0;
|
|
258 |
int d_y = 0;
|
|
205 | 259 |
do |
206 | 260 |
{ |
207 | 261 |
d_x = x + (int) floor(d * cos(angle)); |
208 |
d_y = y + (int) floor(d * sin(angle));
|
|
262 |
d_y = y - (int) floor(d * sin(angle));
|
|
209 | 263 |
|
210 | 264 |
// Out of image boundary |
211 | 265 |
if (d_x < 0 || d_x >= walls_image.GetWidth() || |
... | ... | |
233 | 287 |
/// @todo Consider using different cutoffs for different features |
234 | 288 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
235 | 289 |
|
236 |
if(sonar_on) |
|
237 |
{
|
|
290 |
if(sonar_visual_on)
|
|
291 |
{ |
|
238 | 292 |
if(isFront) |
239 | 293 |
{ |
240 | 294 |
// draw a circle at the wall_x, wall_y where reading > 128 |
... | ... | |
271 | 325 |
|
272 | 326 |
// x and y is current position of the sonar |
273 | 327 |
void Scout::update_sonar(const wxImage& walls_image, int x, int y, |
274 |
double robot_theta,wxMemoryDC& sonar_dc, bool sonar_on)
|
|
328 |
double robot_theta,wxMemoryDC& sonar_dc) |
|
275 | 329 |
{ |
276 | 330 |
// Only rotate the sonar at the correct rate. |
277 | 331 |
if (ros::Time::now() - last_sonar_time < sonar_tick_time) |
... | ... | |
281 | 335 |
last_sonar_time = ros::Time::now(); |
282 | 336 |
|
283 | 337 |
unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta, |
284 |
sonar_position,sonar_dc, sonar_on);
|
|
338 |
sonar_position,sonar_dc); |
|
285 | 339 |
unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta, |
286 |
sonar_position + 24,sonar_dc, sonar_on);
|
|
340 |
sonar_position + 24,sonar_dc); |
|
287 | 341 |
|
288 | 342 |
// Publish |
289 | 343 |
sonar::sonar_distance msg; |
... | ... | |
334 | 388 |
/// the world state |
335 | 389 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
336 | 390 |
wxMemoryDC& sonar_dc, |
337 |
bool sonar_on,
|
|
391 |
bool sonar_visual,
|
|
338 | 392 |
const wxImage& path_image, |
339 | 393 |
const wxImage& lines_image, |
340 | 394 |
const wxImage& walls_image, |
... | ... | |
342 | 396 |
wxColour sonar_color, |
343 | 397 |
world_state state) |
344 | 398 |
{ |
399 |
|
|
400 |
sonar_visual_on = sonar_visual; |
|
345 | 401 |
// Assume that the two motors on the same side will be set to |
346 | 402 |
// roughly the same speed. Does not account for slip conditions |
347 | 403 |
// when they are set to different speeds. |
... | ... | |
410 | 466 |
pose_pub.publish(p); |
411 | 467 |
|
412 | 468 |
update_linesensor(lines_image, canvas_x, canvas_y, p.theta); |
413 |
update_sonar(walls_image, |
|
414 |
canvas_x + scoutsim::SCOUT_SONAR_X, |
|
415 |
canvas_y + scoutsim::SCOUT_SONAR_Y, |
|
416 |
p.theta,sonar_dc,sonar_on); |
|
469 |
if (sonar_on) |
|
470 |
{ |
|
471 |
update_sonar(walls_image, |
|
472 |
canvas_x + scoutsim::SCOUT_SONAR_X, |
|
473 |
canvas_y + scoutsim::SCOUT_SONAR_Y, |
|
474 |
p.theta,sonar_dc); |
|
417 | 475 |
|
476 |
} |
|
418 | 477 |
// Figure out (and publish) the color underneath the scout |
419 | 478 |
{ |
420 | 479 |
//wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); |
scout/scoutsim/src/scout.h | ||
---|---|---|
56 | 56 |
#include <encoders/query_encoders.h> |
57 | 57 |
#include <linesensor/query_linesensor.h> |
58 | 58 |
#include <sonar/sonar_distance.h> |
59 |
#include <sonar/sonar_toggle.h> |
|
60 |
#include <sonar/sonar_set_scan.h> |
|
59 | 61 |
|
60 | 62 |
#include <scoutsim/Pose.h> |
61 | 63 |
#include <scoutsim/SetPen.h> |
... | ... | |
114 | 116 |
|
115 | 117 |
geometry_msgs::Pose2D update(double dt, wxMemoryDC& path_dc, |
116 | 118 |
wxMemoryDC& sonar_dc, |
117 |
bool sonar_on,
|
|
119 |
bool sonar_visual,
|
|
118 | 120 |
const wxImage& path_image, |
119 | 121 |
const wxImage& lines_image, |
120 | 122 |
const wxImage& walls_image, |
... | ... | |
131 | 133 |
encoders::query_encoders::Response&); |
132 | 134 |
bool query_linesensor_callback(linesensor::query_linesensor::Request&, |
133 | 135 |
linesensor::query_linesensor::Response&); |
136 |
bool handle_sonar_toggle(sonar::sonar_toggle::Request &req, |
|
137 |
sonar::sonar_toggle::Response &res); |
|
138 |
bool handle_sonar_set_scan(sonar::sonar_set_scan::Request &req, |
|
139 |
sonar::sonar_set_scan::Response &res); |
|
134 | 140 |
unsigned int rgb_to_grey(unsigned char r, |
135 | 141 |
unsigned char g, |
136 | 142 |
unsigned char b); |
137 | 143 |
unsigned int trace_sonar(const wxImage& walls_image, int x, int y, |
138 |
double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc, bool sonar_on);
|
|
144 |
double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc); |
|
139 | 145 |
void update_sonar(const wxImage& walls_image, int x, int y, |
140 |
double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on);
|
|
146 |
double robot_theta, wxMemoryDC& sonar_dc); |
|
141 | 147 |
void update_linesensor(const wxImage& lines_image, int x, int y, |
142 | 148 |
double theta); |
143 | 149 |
int old_front_dx; |
... | ... | |
147 | 153 |
bool isFront; |
148 | 154 |
|
149 | 155 |
wxBitmap *path_bitmap; |
150 |
bool sonar_on; |
|
156 |
bool sonar_visual_on; |
|
157 |
bool sonar_on; |
|
151 | 158 |
|
152 | 159 |
ros::NodeHandle node; |
153 | 160 |
|
... | ... | |
195 | 202 |
ros::ServiceServer set_pen_srv; |
196 | 203 |
ros::ServiceServer query_encoders_srv; |
197 | 204 |
ros::ServiceServer query_linesensor_srv; |
205 |
ros::ServiceServer toggle_sonar_srv; |
|
206 |
ros::ServiceServer set_sonar_srv; |
|
198 | 207 |
|
199 | 208 |
ros::WallTime last_command_time; |
200 | 209 |
|
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 |
sonar_visual_on = TRUE;
|
|
107 | 107 |
|
108 | 108 |
// Try to load the file; if it fails, make a new blank file |
109 | 109 |
if (!lines_bitmap.LoadFile(wxString::FromAscii(map_lines_name.c_str()))) |
... | ... | |
272 | 272 |
|
273 | 273 |
void SimFrame::showSonar(wxCommandEvent& WXUNUSED(event)) |
274 | 274 |
{ |
275 |
sonar_on = not sonar_on;
|
|
275 |
sonar_visual_on = not sonar_visual_on;
|
|
276 | 276 |
clear(); |
277 | 277 |
} |
278 | 278 |
|
... | ... | |
489 | 489 |
|
490 | 490 |
for (; it != end; ++it) |
491 | 491 |
{ |
492 |
it->second->update(0.016, path_dc,sonar_dc,sonar_on, |
|
492 |
it->second->update(0.016, path_dc,sonar_dc,sonar_visual_on,
|
|
493 | 493 |
path_image, lines_image, walls_image, |
494 | 494 |
path_dc.GetBackground().GetColour(), |
495 | 495 |
sonar_dc.GetBackground().GetColour(), |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
166 | 166 |
float width_in_meters; |
167 | 167 |
float height_in_meters; |
168 | 168 |
|
169 |
bool sonar_on; |
|
169 |
bool sonar_visual_on;
|
|
170 | 170 |
|
171 | 171 |
std::string map_base_name; |
172 | 172 |
std::string map_lines_name; |
Also available in: Unified diff