Revision fa857318
Fixed bugs with how the simulator was doing math for pose estimation.
scout/scoutsim/src/scout.cpp | ||
---|---|---|
415 | 415 |
|
416 | 416 |
// Find linear and angular movement in m |
417 | 417 |
float lin_dist = SIM_TIME_REFRESH_RATE * (l_speed + r_speed) / 2; |
418 |
float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed); |
|
418 |
float ang_dist = SIM_TIME_REFRESH_RATE * (r_speed - l_speed) / SCOUT_WIDTH;
|
|
419 | 419 |
|
420 | 420 |
//store currently teleop'd scoutname |
421 | 421 |
std::stringstream ss; |
... | ... | |
436 | 436 |
|
437 | 437 |
orient = fmod((float) (orient + ang_dist), (float) (2.0 * PI)); |
438 | 438 |
|
439 |
pos.x += sin(orient + PI/2.0) * lin_dist;
|
|
440 |
pos.y += cos(orient + PI/2.0) * lin_dist;
|
|
439 |
pos.x += cos(orient) * lin_dist;
|
|
440 |
pos.y -= sin(orient) * lin_dist; //Subtract because of the way the simulator handles y directions.
|
|
441 | 441 |
|
442 | 442 |
// Clamp to screen size |
443 | 443 |
if (pos.x < 0 || pos.x >= state.canvas_width |
scout/scoutsim/src/scout_constants.h | ||
---|---|---|
66 | 66 |
const float PIX_PER_METER = 200.0; |
67 | 67 |
|
68 | 68 |
// Scout dimensions, in meters |
69 |
const float SCOUT_WIDTH = 0.05;
|
|
70 |
const float SCOUT_LENGTH = 0.2; |
|
69 |
const float SCOUT_WIDTH = 0.125;
|
|
70 |
const float SCOUT_LENGTH = 0.23;
|
|
71 | 71 |
|
72 | 72 |
// Position of sonar relative to robot center |
73 | 73 |
const float SCOUT_SONAR_X = -0.01; |
Also available in: Unified diff