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 |
} |
Also available in: Unified diff