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 counter-clock-wise, 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