Revision fa857318
ID  fa857318c5cf70645335548760f19fed4703af1b 
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