Revision c63c9752
ID | c63c97520e4ba4810364a2b18151db6f250300b8 |
Scoutsim now has consistent units of m and m/s.
This will cause some problems.
- Behaviors which relied on the old units will have to change. We need to change the Control classes to scale things right.
- Encoder ticks are almost fixed, but not quite right (I think because of a rounding problem).
- Speeds are now all in m/s, which is much different than before - we'll have to change the Control class somehow.
I'm afraid I'm out of time to make the rest of these changes now, but I'll do them ASAP.
scout/scoutsim/src/scout.cpp | ||
---|---|---|
105 | 105 |
linesensor_readings.push_back(0); |
106 | 106 |
} |
107 | 107 |
|
108 |
meter = scout.GetHeight(); |
|
109 |
|
|
110 | 108 |
// Initialize sonar |
111 | 109 |
sonar_position = 0; |
112 | 110 |
sonar_stop_l = 0; |
... | ... | |
140 | 138 |
{ |
141 | 139 |
motor_br_speed = msg->br_speed; |
142 | 140 |
} |
143 |
|
|
144 | 141 |
} |
145 | 142 |
|
146 | 143 |
bool Scout::setPenCallback(scoutsim::SetPen::Request& req, |
... | ... | |
194 | 191 |
} |
195 | 192 |
|
196 | 193 |
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) |
|
194 |
double robot_theta, int sonar_pos, |
|
195 |
wxMemoryDC& sonar_dc, bool sonar_on) |
|
198 | 196 |
{ |
199 | 197 |
double angle = robot_theta + (PI * ((float) sonar_pos) / 24.0) - PI / 2; |
200 | 198 |
unsigned int d = 0; |
201 | 199 |
|
202 | 200 |
unsigned int reading = 0; |
203 |
int d_x = 0; |
|
204 |
int d_y = 0; |
|
201 |
int d_x = 0; |
|
202 |
int d_y = 0; |
|
203 |
|
|
205 | 204 |
do |
206 | 205 |
{ |
207 | 206 |
d_x = x + (int) floor(d * cos(angle)); |
... | ... | |
233 | 232 |
/// @todo Consider using different cutoffs for different features |
234 | 233 |
while (reading < 128); /// @todo Get rid of hardcoded stuff like this |
235 | 234 |
|
236 |
if(sonar_on)
|
|
237 |
{
|
|
238 |
if(isFront)
|
|
239 |
{
|
|
240 |
// draw a circle at the wall_x, wall_y where reading > 128
|
|
241 |
sonar_dc.SelectObject(*path_bitmap);
|
|
242 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value
|
|
243 |
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
|
|
244 |
old_front_dx = d_x;
|
|
245 |
old_front_dy = d_y;
|
|
246 |
}
|
|
247 |
else
|
|
248 |
{
|
|
249 |
// draw a circle at the wall_x, wall_y where reading > 128
|
|
250 |
sonar_dc.SelectObject(*path_bitmap);
|
|
251 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value
|
|
252 |
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
|
|
253 |
old_back_dx = d_x;
|
|
254 |
old_back_dy = d_y;
|
|
255 |
}
|
|
256 |
|
|
257 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value
|
|
258 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
|
|
259 |
if (isFront) //for some reason isFront = (!isFront) is not working
|
|
260 |
{
|
|
261 |
isFront = FALSE;
|
|
262 |
}
|
|
263 |
else
|
|
264 |
{
|
|
265 |
isFront = TRUE;
|
|
266 |
}
|
|
267 |
|
|
268 |
}
|
|
235 |
if(sonar_on)
|
|
236 |
{
|
|
237 |
if(isFront)
|
|
238 |
{
|
|
239 |
// draw a circle at the wall_x, wall_y where reading > 128
|
|
240 |
sonar_dc.SelectObject(*path_bitmap);
|
|
241 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value
|
|
242 |
sonar_dc.DrawCircle(wxPoint(old_front_dx,old_front_dy), 2);
|
|
243 |
old_front_dx = d_x;
|
|
244 |
old_front_dy = d_y;
|
|
245 |
}
|
|
246 |
else
|
|
247 |
{
|
|
248 |
// draw a circle at the wall_x, wall_y where reading > 128
|
|
249 |
sonar_dc.SelectObject(*path_bitmap);
|
|
250 |
sonar_dc.SetBrush(*wxRED_BRUSH); //old value
|
|
251 |
sonar_dc.DrawCircle(wxPoint(old_back_dx,old_back_dy), 2);
|
|
252 |
old_back_dx = d_x;
|
|
253 |
old_back_dy = d_y;
|
|
254 |
}
|
|
255 |
|
|
256 |
sonar_dc.SetBrush(*wxGREEN_BRUSH); //newest value
|
|
257 |
sonar_dc.DrawCircle( wxPoint(d_x,d_y), 2);
|
|
258 |
if (isFront) //for some reason isFront = (!isFront) is not working
|
|
259 |
{
|
|
260 |
isFront = FALSE;
|
|
261 |
}
|
|
262 |
else
|
|
263 |
{
|
|
264 |
isFront = TRUE;
|
|
265 |
}
|
|
266 |
|
|
267 |
}
|
|
269 | 268 |
return d; |
270 | 269 |
} |
271 | 270 |
|
... | ... | |
333 | 332 |
/// Sends back the position of this scout so scoutsim can save |
334 | 333 |
/// the world state |
335 | 334 |
geometry_msgs::Pose2D Scout::update(double dt, wxMemoryDC& path_dc, |
336 |
wxMemoryDC& sonar_dc,
|
|
337 |
bool sonar_on,
|
|
335 |
wxMemoryDC& sonar_dc,
|
|
336 |
bool sonar_on,
|
|
338 | 337 |
const wxImage& path_image, |
339 | 338 |
const wxImage& lines_image, |
340 | 339 |
const wxImage& walls_image, |
341 | 340 |
wxColour background_color, |
342 |
wxColour sonar_color,
|
|
341 |
wxColour sonar_color,
|
|
343 | 342 |
world_state state) |
344 | 343 |
{ |
345 | 344 |
// Assume that the two motors on the same side will be set to |
... | ... | |
348 | 347 |
float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2; |
349 | 348 |
float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2; |
350 | 349 |
|
351 |
// Set the linear and angular speeds
|
|
352 |
float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
|
|
353 |
float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
|
|
350 |
// Find linear and angular movement in m
|
|
351 |
float lin_dist = SCOUTSIM_REFRESH_RATE * (l_speed + r_speed) / 2;
|
|
352 |
float ang_dist = SCOUTSIM_REFRESH_RATE * (r_speed - l_speed);
|
|
354 | 353 |
|
355 | 354 |
Vector2 old_pos = pos; |
356 | 355 |
|
357 | 356 |
// Update encoders |
358 |
/// @todo replace |
|
359 |
fl_ticks += (unsigned int) motor_fl_speed; |
|
360 |
fr_ticks += (unsigned int) motor_fr_speed; |
|
361 |
bl_ticks += (unsigned int) motor_bl_speed; |
|
362 |
br_ticks += (unsigned int) motor_br_speed; |
|
357 |
fl_ticks += (unsigned int) (motor_fl_speed * SCOUTSIM_REFRESH_RATE * |
|
358 |
ENCODER_TICKS_PER_METER); |
|
359 |
fr_ticks += (unsigned int) (motor_fr_speed * SCOUTSIM_REFRESH_RATE * |
|
360 |
ENCODER_TICKS_PER_METER); |
|
361 |
bl_ticks += (unsigned int) (motor_bl_speed * SCOUTSIM_REFRESH_RATE * |
|
362 |
ENCODER_TICKS_PER_METER); |
|
363 |
br_ticks += (unsigned int) (motor_br_speed * SCOUTSIM_REFRESH_RATE * |
|
364 |
ENCODER_TICKS_PER_METER); |
|
365 |
|
|
366 |
orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI)); |
|
363 | 367 |
|
364 |
orient = fmod(orient + ang_vel * dt, 2*PI);
|
|
365 |
pos.x += sin(orient + PI/2.0) * lin_vel * dt;
|
|
366 |
pos.y += cos(orient + PI/2.0) * lin_vel * dt;
|
|
368 |
// Multiply by dt, the time passed (SCOUTSIM_REFRESH_RATE)
|
|
369 |
pos.x += sin(orient + PI/2.0) * lin_dist;
|
|
370 |
pos.y += cos(orient + PI/2.0) * lin_dist;
|
|
367 | 371 |
|
368 | 372 |
// Clamp to screen size |
369 | 373 |
if (pos.x < 0 || pos.x >= state.canvas_width |
370 | 374 |
|| pos.y < 0 || pos.y >= state.canvas_height) |
371 | 375 |
{ |
372 |
ROS_WARN("Oh no! I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y);
|
|
376 |
ROS_WARN("I hit the wall! (Clamping from [x=%f, y=%f])", pos.x, pos.y); |
|
373 | 377 |
} |
374 | 378 |
|
375 | 379 |
pos.x = min(max(pos.x, 0.0f), state.canvas_width); |
376 | 380 |
pos.y = min(max(pos.y, 0.0f), state.canvas_height); |
377 | 381 |
|
378 |
int canvas_x = pos.x * meter;
|
|
379 |
int canvas_y = pos.y * meter;
|
|
382 |
int canvas_x = pos.x * PIX_PER_METER;
|
|
383 |
int canvas_y = pos.y * PIX_PER_METER;
|
|
380 | 384 |
|
381 | 385 |
{ |
382 | 386 |
wxImage rotated_image = |
... | ... | |
405 | 409 |
p.x = pos.x; |
406 | 410 |
p.y = state.canvas_height - pos.y; |
407 | 411 |
p.theta = orient; |
408 |
p.linear_velocity = lin_vel;
|
|
409 |
p.angular_velocity = ang_vel;
|
|
412 |
p.linear_velocity = l_speed;
|
|
413 |
p.angular_velocity = r_speed;
|
|
410 | 414 |
pose_pub.publish(p); |
411 | 415 |
|
412 | 416 |
update_linesensor(lines_image, canvas_x, canvas_y, p.theta); |
... | ... | |
434 | 438 |
{ |
435 | 439 |
path_dc.SelectObject(*path_bitmap); |
436 | 440 |
path_dc.SetPen(pen); |
437 |
path_dc.DrawLine(pos.x * meter, pos.y * meter,
|
|
438 |
old_pos.x * meter, old_pos.y * meter);
|
|
441 |
path_dc.DrawLine(pos.x * PIX_PER_METER, pos.y * PIX_PER_METER,
|
|
442 |
old_pos.x * PIX_PER_METER, old_pos.y * PIX_PER_METER);
|
|
439 | 443 |
} |
440 | 444 |
} |
441 | 445 |
|
... | ... | |
450 | 454 |
void Scout::paint(wxDC& dc) |
451 | 455 |
{ |
452 | 456 |
wxSize scout_size = wxSize(scout.GetWidth(), scout.GetHeight()); |
453 |
dc.DrawBitmap(scout, pos.x * meter - (scout_size.GetWidth() / 2),
|
|
454 |
pos.y * meter - (scout_size.GetHeight() / 2), true);
|
|
457 |
dc.DrawBitmap(scout, pos.x * PIX_PER_METER - (scout_size.GetWidth() / 2),
|
|
458 |
pos.y * PIX_PER_METER - (scout_size.GetHeight() / 2), true);
|
|
455 | 459 |
} |
456 |
|
|
457 | 460 |
} |
scout/scoutsim/src/scout.h | ||
---|---|---|
70 | 70 |
|
71 | 71 |
#define PI 3.14159265 |
72 | 72 |
|
73 |
/// The scale factor so the speed of scout robots is reasonable for the sim |
|
74 |
#define SPEED_SCALE_FACTOR 0.05 |
|
75 |
|
|
76 | 73 |
#define NUM_LINESENSORS 8 |
77 | 74 |
|
78 | 75 |
// Distance, pixels, from center of robot to the linesensors. |
... | ... | |
135 | 132 |
unsigned char g, |
136 | 133 |
unsigned char b); |
137 | 134 |
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); |
|
135 |
double robot_theta, int sonar_pos, |
|
136 |
wxMemoryDC& sonar_dc, bool sonar_on); |
|
139 | 137 |
void update_sonar(const wxImage& walls_image, int x, int y, |
140 | 138 |
double robot_theta, wxMemoryDC& sonar_dc,bool sonar_on); |
141 | 139 |
void update_linesensor(const wxImage& lines_image, int x, int y, |
142 | 140 |
double theta); |
143 |
int old_front_dx;
|
|
144 |
int old_front_dy;
|
|
145 |
int old_back_dx;
|
|
146 |
int old_back_dy;
|
|
147 |
bool isFront;
|
|
141 |
int old_front_dx;
|
|
142 |
int old_front_dy;
|
|
143 |
int old_back_dx;
|
|
144 |
int old_back_dy;
|
|
145 |
bool isFront;
|
|
148 | 146 |
|
149 |
wxBitmap *path_bitmap;
|
|
150 |
bool sonar_on;
|
|
147 |
wxBitmap *path_bitmap;
|
|
148 |
bool sonar_on;
|
|
151 | 149 |
|
152 | 150 |
ros::NodeHandle node; |
153 | 151 |
|
... | ... | |
175 | 173 |
int sonar_stop_l; |
176 | 174 |
int sonar_stop_r; |
177 | 175 |
int sonar_direction; |
176 |
|
|
178 | 177 |
// The last time the sonar changed its position. |
179 | 178 |
ros::Time last_sonar_time; |
180 | 179 |
ros::Duration sonar_tick_time; |
... | ... | |
197 | 196 |
ros::ServiceServer query_linesensor_srv; |
198 | 197 |
|
199 | 198 |
ros::WallTime last_command_time; |
200 |
|
|
201 |
float meter; |
|
202 |
|
|
203 | 199 |
}; |
204 | 200 |
typedef boost::shared_ptr<Scout> ScoutPtr; |
205 |
|
|
206 | 201 |
} |
207 | 202 |
|
208 | 203 |
#endif |
scout/scoutsim/src/scout_constants.h | ||
---|---|---|
49 | 49 |
|
50 | 50 |
namespace scoutsim |
51 | 51 |
{ |
52 |
// Pixels in scoutsim per real-world meter. |
|
53 |
// Note that scout image size has been set based on this. |
|
54 |
const float PIX_PER_METER = 200.0; |
|
55 |
|
|
52 | 56 |
// Scout dimensions, in meters |
53 | 57 |
const float SCOUT_WIDTH = 0.05; |
54 | 58 |
const float SCOUT_LENGTH = 0.2; |
... | ... | |
57 | 61 |
const float SCOUT_SONAR_X = -0.01; |
58 | 62 |
const float SCOUT_SONAR_Y = 0.0; |
59 | 63 |
|
64 |
// @todo Inaccurate. Update |
|
65 |
const float ENCODER_TICKS_PER_METER = 363.78; |
|
66 |
|
|
60 | 67 |
const unsigned int SONAR_MAX_RANGE = 255; |
68 |
|
|
61 | 69 |
// Time it takes for the sonar to spin from position 0 to position 23 |
62 | 70 |
const float SONAR_HALF_SPIN_TIME = 0.5; |
71 |
|
|
72 |
const float SCOUTSIM_REFRESH_RATE = 0.01; // s |
|
63 | 73 |
} |
scout/scoutsim/src/sim_frame.cpp | ||
---|---|---|
68 | 68 |
srand(time(NULL)); |
69 | 69 |
|
70 | 70 |
update_timer = new wxTimer(this); |
71 |
update_timer->Start(16);
|
|
71 |
update_timer->Start(SCOUTSIM_REFRESH_RATE * 1000);
|
|
72 | 72 |
|
73 | 73 |
Connect(update_timer->GetId(), wxEVT_TIMER, |
74 | 74 |
wxTimerEventHandler(SimFrame::onUpdate), NULL, this); |
... | ... | |
77 | 77 |
|
78 | 78 |
images_path = ros::package::getPath("scoutsim") + "/images/"; |
79 | 79 |
|
80 |
// 200 pixels per meter. @todo make this a constant elsewhere. |
|
81 |
meter = 200; |
|
82 |
|
|
83 | 80 |
map_base_name = ros::package::getPath("scoutsim") + "/maps/" + |
84 | 81 |
map_name + ".bmp"; |
85 | 82 |
map_lines_name = ros::package::getPath("scoutsim") + "/maps/" + |
... | ... | |
160 | 157 |
|
161 | 158 |
SetMenuBar(menuBar); |
162 | 159 |
|
163 |
width_in_meters = GetSize().GetWidth() / meter;
|
|
164 |
height_in_meters = GetSize().GetHeight() / meter;
|
|
160 |
width_in_meters = GetSize().GetWidth() / PIX_PER_METER;
|
|
161 |
height_in_meters = GetSize().GetHeight() / PIX_PER_METER;
|
|
165 | 162 |
} |
166 | 163 |
|
167 | 164 |
SimFrame::~SimFrame() |
... | ... | |
314 | 311 |
SetSize(wxSize(path_bitmap.GetWidth(), path_bitmap.GetHeight())); |
315 | 312 |
} |
316 | 313 |
|
314 |
// Runs every SCOUTSIM_REFRESH_RATE. |
|
317 | 315 |
void SimFrame::onUpdate(wxTimerEvent& evt) |
318 | 316 |
{ |
319 | 317 |
ros::spinOnce(); |
... | ... | |
393 | 391 |
} |
394 | 392 |
else if (wxGetKeyState(WXK_LEFT)) |
395 | 393 |
{ |
396 |
teleop_l_speed = -TELEOP_PRECISE_SPEED; |
|
397 |
teleop_r_speed = TELEOP_PRECISE_SPEED; |
|
394 |
teleop_l_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
395 |
teleop_r_speed = TELEOP_PRECISE_SPEED * 2;
|
|
398 | 396 |
} |
399 | 397 |
else if (wxGetKeyState(WXK_RIGHT)) |
400 | 398 |
{ |
401 |
teleop_l_speed = TELEOP_PRECISE_SPEED; |
|
402 |
teleop_r_speed = -TELEOP_PRECISE_SPEED; |
|
399 |
teleop_l_speed = TELEOP_PRECISE_SPEED * 2;
|
|
400 |
teleop_r_speed = -TELEOP_PRECISE_SPEED * 2;
|
|
403 | 401 |
} |
404 | 402 |
} |
405 | 403 |
|
... | ... | |
407 | 405 |
{ |
408 | 406 |
if (wxGetKeyState(WXK_UP)) |
409 | 407 |
{ |
410 |
teleop_fluid_speed += 2; |
|
408 |
teleop_fluid_speed += TELEOP_FLUID_INC * 2;
|
|
411 | 409 |
} |
412 | 410 |
else if (wxGetKeyState(WXK_DOWN)) |
413 | 411 |
{ |
414 |
teleop_fluid_speed -= 2; |
|
412 |
teleop_fluid_speed -= TELEOP_FLUID_INC * 2;
|
|
415 | 413 |
} |
416 | 414 |
else if (teleop_fluid_speed > 0) |
417 | 415 |
{ |
418 |
teleop_fluid_speed -= 1;
|
|
416 |
teleop_fluid_speed -= TELEOP_FLUID_INC;
|
|
419 | 417 |
} |
420 | 418 |
else if (teleop_fluid_speed < 0) |
421 | 419 |
{ |
422 |
teleop_fluid_speed += 1;
|
|
420 |
teleop_fluid_speed += TELEOP_FLUID_INC;
|
|
423 | 421 |
} |
424 | 422 |
|
425 | 423 |
if (wxGetKeyState(WXK_LEFT)) |
426 | 424 |
{ |
427 |
teleop_fluid_omega -= 1;
|
|
425 |
teleop_fluid_omega -= TELEOP_FLUID_INC;
|
|
428 | 426 |
} |
429 | 427 |
else if (wxGetKeyState(WXK_RIGHT)) |
430 | 428 |
{ |
431 |
teleop_fluid_omega += 1;
|
|
429 |
teleop_fluid_omega += TELEOP_FLUID_INC;
|
|
432 | 430 |
} |
433 | 431 |
else |
434 | 432 |
{ |
... | ... | |
439 | 437 |
{ |
440 | 438 |
teleop_fluid_speed = TELEOP_FLUID_MAX_SPEED; |
441 | 439 |
} |
440 |
else if (teleop_fluid_speed < -TELEOP_FLUID_MAX_SPEED) |
|
441 |
{ |
|
442 |
teleop_fluid_speed = -TELEOP_FLUID_MAX_SPEED; |
|
443 |
} |
|
442 | 444 |
if (teleop_fluid_omega > TELEOP_FLUID_MAX_SPEED / 2) |
443 | 445 |
{ |
444 | 446 |
teleop_fluid_omega = TELEOP_FLUID_MAX_SPEED / 2; |
445 | 447 |
} |
448 |
else if (teleop_fluid_omega < -TELEOP_FLUID_MAX_SPEED / 2) |
|
449 |
{ |
|
450 |
teleop_fluid_omega = -TELEOP_FLUID_MAX_SPEED / 2; |
|
451 |
} |
|
446 | 452 |
|
447 | 453 |
teleop_l_speed = teleop_fluid_speed + teleop_fluid_omega; |
448 | 454 |
teleop_r_speed = teleop_fluid_speed - teleop_fluid_omega; |
scout/scoutsim/src/sim_frame.h | ||
---|---|---|
77 | 77 |
#define ID_TELEOP_FLUID 9 |
78 | 78 |
#define ID_SONAR 10 |
79 | 79 |
|
80 |
#define TELEOP_PRECISE_SPEED 30 |
|
81 |
#define TELEOP_FLUID_MAX_SPEED 80 |
|
80 |
#define TELEOP_PRECISE_SPEED 1.0f |
|
81 |
#define TELEOP_FLUID_MAX_SPEED 1.5f |
|
82 |
#define TELEOP_FLUID_INC 0.1f |
|
82 | 83 |
|
83 | 84 |
// Teleop types |
84 | 85 |
#define TELEOP_OFF 0 |
... | ... | |
134 | 135 |
ros::Publisher wireless_receive; |
135 | 136 |
|
136 | 137 |
// Teleop |
137 |
short teleop_l_speed;
|
|
138 |
short teleop_r_speed;
|
|
138 |
float teleop_l_speed;
|
|
139 |
float teleop_r_speed;
|
|
139 | 140 |
ros::Publisher teleop_pub; |
140 | 141 |
int teleop_type; |
141 | 142 |
|
142 |
short teleop_fluid_speed;
|
|
143 |
short teleop_fluid_omega;
|
|
143 |
float teleop_fluid_speed;
|
|
144 |
float teleop_fluid_omega;
|
|
144 | 145 |
|
145 | 146 |
ros::NodeHandle nh; |
146 | 147 |
wxTimer* update_timer; |
... | ... | |
166 | 167 |
|
167 | 168 |
std::string images_path; |
168 | 169 |
|
169 |
float meter; |
|
170 | 170 |
float width_in_meters; |
171 | 171 |
float height_in_meters; |
172 | 172 |
|
Also available in: Unified diff