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()); |
Also available in: Unified diff