Revision 6639ce9c
| ID | 6639ce9c5b5ca4644e9f66e71b7d79bc24cf4fb0 |
Edited Sonar to display points on simulator.
Changed rate of sonar scan to 0.5s
Made a new wxDC graphics object for sonar
| scout/scoutsim/src/scout.cpp | ||
|---|---|---|
| 60 | 60 |
Scout::Scout(const ros::NodeHandle& nh, |
| 61 | 61 |
const wxImage& scout_image, |
| 62 | 62 |
const Vector2& pos, |
| 63 |
wxBitmap *path_bitmap, |
|
| 63 | 64 |
float orient) |
| 64 |
: node (nh) |
|
| 65 |
: path_bitmap(path_bitmap) |
|
| 66 |
, node (nh) |
|
| 65 | 67 |
, scout_image(scout_image) |
| 66 | 68 |
, pos(pos) |
| 67 | 69 |
, orient(orient) |
| ... | ... | |
| 191 | 193 |
} |
| 192 | 194 |
|
| 193 | 195 |
unsigned int Scout::trace_sonar(const wxImage& walls_image, int x, int y, |
| 194 |
double robot_theta, int sonar_pos) |
|
| 196 |
double robot_theta, int sonar_pos, wxMemoryDC& sonar_dc)
|
|
| 195 | 197 |
{
|
| 196 | 198 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2; |
| 197 | 199 |
unsigned int d = 0; |
| 198 | 200 |
|
| 199 | 201 |
unsigned int reading = 0; |
| 202 |
int d_x = 0; |
|
| 203 |
int d_y = 0; |
|
| 200 | 204 |
do |
| 201 | 205 |
{
|
| 202 |
int d_x = x + (int) floor(d * cos(angle));
|
|
| 203 |
int d_y = y + (int) floor(d * sin(angle));
|
|
| 206 |
d_x = x + (int) floor(d * cos(angle)); |
|
| 207 |
d_y = y + (int) floor(d * sin(angle)); |
|
| 204 | 208 |
|
| 205 | 209 |
// Out of image boundary |
| 206 | 210 |
if (d_x < 0 || d_x >= walls_image.GetWidth() || |
| ... | ... | |
| 219 | 223 |
unsigned char r = walls_image.GetRed(d_x, d_y); |
| 220 | 224 |
unsigned char g = walls_image.GetGreen(d_x, d_y); |
| 221 | 225 |
unsigned char b = walls_image.GetBlue(d_x, d_y); |
| 222 |
|
|
| 226 |
|
|
| 223 | 227 |
reading = rgb_to_grey(r, g, b); |
| 228 |
|
|
| 224 | 229 |
|
| 225 | 230 |
d++; |
| 226 | 231 |
} |
| 227 | 232 |
/// @todo Consider using different cutoffs for different features |
| 228 | 233 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
| 234 |
|
|
| 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; |
|
| 229 | 244 |
|
| 230 | 245 |
return d; |
| 231 | 246 |
} |
| 232 | 247 |
|
| 233 | 248 |
// x and y is current position of the sonar |
| 234 | 249 |
void Scout::update_sonar(const wxImage& walls_image, int x, int y, |
| 235 |
double robot_theta) |
|
| 250 |
double robot_theta,wxMemoryDC& sonar_dc)
|
|
| 236 | 251 |
{
|
| 237 | 252 |
// Only rotate the sonar at the correct rate. |
| 238 | 253 |
if (ros::Time::now() - last_sonar_time < sonar_tick_time) |
| ... | ... | |
| 242 | 257 |
last_sonar_time = ros::Time::now(); |
| 243 | 258 |
|
| 244 | 259 |
unsigned int d_front = trace_sonar(walls_image, x, y, robot_theta, |
| 245 |
sonar_position); |
|
| 260 |
sonar_position,sonar_dc);
|
|
| 246 | 261 |
unsigned int d_back = trace_sonar(walls_image, x, y, robot_theta, |
| 247 |
sonar_position + 24); |
|
| 262 |
sonar_position + 24,sonar_dc);
|
|
| 248 | 263 |
|
| 249 | 264 |
// Publish |
| 250 | 265 |
sonar::sonar_distance msg; |
| ... | ... | |
| 294 | 309 |
/// Sends back the position of this scout so scoutsim can save |
| 295 | 310 |
/// the world state |
| 296 | 311 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
| 312 |
wxMemoryDC& sonar_dc, |
|
| 297 | 313 |
const wxImage& path_image, |
| 298 | 314 |
const wxImage& lines_image, |
| 299 | 315 |
const wxImage& walls_image, |
| 300 | 316 |
wxColour background_color, |
| 317 |
wxColour sonar_color, |
|
| 301 | 318 |
world_state state) |
| 302 | 319 |
{
|
| 303 | 320 |
// Assume that the two motors on the same side will be set to |
| ... | ... | |
| 371 | 388 |
update_sonar(walls_image, |
| 372 | 389 |
canvas_x + scoutsim::SCOUT_SONAR_X, |
| 373 | 390 |
canvas_y + scoutsim::SCOUT_SONAR_Y, |
| 374 |
p.theta); |
|
| 391 |
p.theta,sonar_dc);
|
|
| 375 | 392 |
|
| 376 | 393 |
// Figure out (and publish) the color underneath the scout |
| 377 | 394 |
{
|
| ... | ... | |
| 390 | 407 |
{
|
| 391 | 408 |
if (pos != old_pos) |
| 392 | 409 |
{
|
| 410 |
path_dc.SelectObject(*path_bitmap); |
|
| 393 | 411 |
path_dc.SetPen(pen); |
| 394 | 412 |
path_dc.DrawLine(pos.x * meter, pos.y * meter, |
| 395 | 413 |
old_pos.x * meter, old_pos.y * meter); |
| scout/scoutsim/src/scout.h | ||
|---|---|---|
| 109 | 109 |
class Scout |
| 110 | 110 |
{
|
| 111 | 111 |
public: |
| 112 |
Scout(const ros::NodeHandle& nh, const wxImage& scout_image,
|
|
| 113 |
const Vector2& pos, float orient); |
|
| 112 |
Scout(const ros::NodeHandle& nh,const wxImage& scout_image, |
|
| 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 | 117 |
const wxImage& path_image, |
| 117 | 118 |
const wxImage& lines_image, |
| 118 | 119 |
const wxImage& walls_image, |
| 119 | 120 |
wxColour background_color, |
| 121 |
wxColour sonar_color, |
|
| 120 | 122 |
world_state state); |
| 121 | 123 |
void paint(wxDC& dc); |
| 122 | 124 |
|
| ... | ... | |
| 132 | 134 |
unsigned char g, |
| 133 | 135 |
unsigned char b); |
| 134 | 136 |
unsigned int trace_sonar(const wxImage& walls_image, int x, int y, |
| 135 |
double robot_theta, int sonar_pos); |
|
| 137 |
double robot_theta, int sonar_pos,wxMemoryDC& sonar_dc);
|
|
| 136 | 138 |
void update_sonar(const wxImage& walls_image, int x, int y, |
| 137 |
double robot_theta); |
|
| 139 |
double robot_theta, wxMemoryDC& sonar_dc);
|
|
| 138 | 140 |
void update_linesensor(const wxImage& lines_image, int x, int y, |
| 139 | 141 |
double theta); |
| 142 |
int old_dx; |
|
| 143 |
int old_dy; |
|
| 144 |
wxBitmap *path_bitmap; |
|
| 140 | 145 |
|
| 141 | 146 |
ros::NodeHandle node; |
| 142 | 147 |
|
| scout/scoutsim/src/scout_constants.h | ||
|---|---|---|
| 59 | 59 |
|
| 60 | 60 |
const unsigned int SONAR_MAX_RANGE = 255; |
| 61 | 61 |
// Time it takes for the sonar to spin from position 0 to position 23 |
| 62 |
const float SONAR_HALF_SPIN_TIME = 24.0;
|
|
| 62 |
const float SONAR_HALF_SPIN_TIME = 0.5;
|
|
| 63 | 63 |
} |
| scout/scoutsim/src/sim_frame.cpp | ||
|---|---|---|
| 217 | 217 |
|
| 218 | 218 |
ScoutPtr t(new Scout(ros::NodeHandle(real_name), |
| 219 | 219 |
scout_images[rand() % SCOUTSIM_NUM_SCOUTS], |
| 220 |
Vector2(x, y), angle)); |
|
| 220 |
Vector2(x, y), &path_bitmap,angle));
|
|
| 221 | 221 |
scouts[real_name] = t; |
| 222 | 222 |
|
| 223 | 223 |
ROS_INFO("Spawning scout [%s] at x=[%f], y=[%f], theta=[%f]",
|
| ... | ... | |
| 269 | 269 |
path_dc.SetBackground(wxBrush(wxColour(100, 100, 100))); |
| 270 | 270 |
path_dc.Clear(); |
| 271 | 271 |
|
| 272 |
sonar_dc.SetBackground(wxBrush(wxColour(255, 0, 0))); |
|
| 273 |
sonar_dc.Clear(); |
|
| 274 |
|
|
| 275 |
sonar_dc.SelectObject(path_bitmap); |
|
| 276 |
|
|
| 272 | 277 |
path_bitmap.LoadFile(wxString::FromAscii(display_map_name.c_str())); |
| 273 | 278 |
path_dc.SelectObject(path_bitmap); |
| 274 | 279 |
SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight())); |
| ... | ... | |
| 323 | 328 |
|
| 324 | 329 |
for (; it != end; ++it) |
| 325 | 330 |
{
|
| 326 |
it->second->update(0.016, path_dc, |
|
| 331 |
it->second->update(0.016, path_dc,sonar_dc,
|
|
| 327 | 332 |
path_image, lines_image, walls_image, |
| 328 | 333 |
path_dc.GetBackground().GetColour(), |
| 334 |
sonar_dc.GetBackground().GetColour(), |
|
| 329 | 335 |
state); |
| 330 | 336 |
} |
| 331 | 337 |
|
| scout/scoutsim/src/sim_frame.h | ||
|---|---|---|
| 114 | 114 |
wxImage lines_image; |
| 115 | 115 |
wxImage walls_image; |
| 116 | 116 |
wxMemoryDC path_dc; |
| 117 |
wxMemoryDC sonar_dc; |
|
| 117 | 118 |
|
| 118 | 119 |
uint64_t frame_count; |
| 119 | 120 |
|
Also available in: Unified diff