Revision ddfeb111
ID  ddfeb11183e90127649a5f69425bedc926bda71c 
Fixing bugs with ghost scout (these were problems in odometry related
to constants in the simulator and coordinate frame and theta representation
in the simulator). Also made ghost scout translucent in order to differentiate
it.
scout/libscout/src/behaviors/Odometry.cpp  

23  23 
motor_bl_dist = scout_enc.bl_ticks  motor_bl_ticks; 
24  24 
motor_br_dist = scout_enc.br_ticks  motor_br_ticks; 
25  25  
26 
ROS_INFO("fl %d fr %d bl %d br %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks); 

27 
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks); 

28 
ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist); 

29  
30  26 
// Get Left and Right distance 
31  27 
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + 
32  28 
((float)(motor_bl_dist))/2.0); 
...  ...  
34  30 
((float)(motor_br_dist))/2.0); 
35  31  
36  32 
total_dist = (left_dist)/2.0 + (right_dist)/2.0; 
37 
theta = scout_pos>theta + atan2(left_dist  right_dist, WHEEL_BASE); 

38  
39 
ROS_INFO("l: %f r: %f theta: %f", left_dist, right_dist, theta); 

40  
41 
scout_pos>x += total_dist*sin(theta); 

42 
scout_pos>y += total_dist*cos(theta); 

33 
theta = scout_pos>theta + atan2(right_dist  left_dist, WHEEL_BASE); 

34  
35 
//Use negative theta because we measure theta from 0 on the 

36 
//right to 180 on the left counterclockwise, but this is 

37 
//negative theta in the coordinate frame. 

38 
//Also, subtract the delta from y because positive y is down. 

39 
scout_pos>x += total_dist*cos(theta); 

40 
scout_pos>y = total_dist*sin(theta); 

43  41 
scout_pos>theta = fmod(theta, 2*M_PI); 
44  42  
45  43 
//Save state for next time in. 
...  ...  
65  63 
position.y = scout_pos>y; 
66  64 
position.theta = scout_pos>theta; 
67  65 
scout_position.publish(position); 
68  
69 
ROS_INFO("Scout is at x: %f y: %f theta: %f\n", scout_pos>x, scout_pos>y, scout_pos>theta); 

70  66 
} 
71  67 
} 
Also available in: Unified diff