7 |
7 |
Odometry::Odometry(string scoutname, Sensors* sensors)
|
8 |
8 |
: Behavior(scoutname, "odometry", sensors)
|
9 |
9 |
{
|
10 |
|
name = scoutname;
|
11 |
|
scout_pos = new pos;
|
12 |
|
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
|
10 |
name = scoutname;
|
|
11 |
scout_pos = new pos;
|
|
12 |
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
13 |
13 |
}
|
14 |
14 |
|
15 |
15 |
/** Set up the odometry node and prepare communcations over ROS */
|
16 |
|
Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
|
17 |
|
: Behavior(scoutname, behavior_name, sensors)
|
|
16 |
Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
|
|
17 |
: Behavior(scoutname, behavior_name, sensors)
|
18 |
18 |
{
|
19 |
|
name = scoutname;
|
20 |
|
scout_pos = new pos;
|
21 |
|
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
|
19 |
name = scoutname;
|
|
20 |
scout_pos = new pos;
|
|
21 |
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
|
22 |
}
|
|
23 |
|
|
24 |
void Odometry::wait(float duration)
|
|
25 |
{
|
|
26 |
ros::Rate r(WAIT_HZ);
|
|
27 |
int ticks = int(duration * WAIT_HZ);
|
|
28 |
for (int i = 0; i < ticks; i++)
|
|
29 |
{
|
|
30 |
get_position();
|
|
31 |
spinOnce();
|
|
32 |
r.sleep();
|
|
33 |
}
|
22 |
34 |
}
|
23 |
35 |
|
24 |
36 |
/** Query encoders and estimate position based on encoder reading */
|
25 |
37 |
void Odometry::get_position()
|
26 |
38 |
{
|
27 |
|
float left_dist, right_dist, total_dist, theta;
|
28 |
|
|
29 |
|
encoder_readings scout_enc = encoders->query();
|
30 |
|
motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
|
31 |
|
motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
|
32 |
|
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
|
33 |
|
motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
|
34 |
|
|
35 |
|
// Get Left and Right distance
|
36 |
|
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
|
37 |
|
((float)(motor_bl_dist))/2.0);
|
38 |
|
right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 +
|
39 |
|
((float)(motor_br_dist))/2.0);
|
40 |
|
|
41 |
|
total_dist = (left_dist)/2.0 + (right_dist)/2.0;
|
42 |
|
theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
|
43 |
|
|
44 |
|
//Use negative theta because we measure theta from 0 on the
|
45 |
|
//right to 180 on the left counter-clock-wise, but this is
|
46 |
|
//negative theta in the coordinate frame.
|
47 |
|
//Also, subtract the delta from y because positive y is down.
|
48 |
|
scout_pos->x += total_dist*cos(-theta);
|
49 |
|
scout_pos->y -= total_dist*sin(-theta);
|
50 |
|
scout_pos->theta = fmod(theta, (float)(2*M_PI));
|
51 |
|
|
52 |
|
//Save state for next time in.
|
53 |
|
motor_fl_ticks = scout_enc.fl_ticks;
|
54 |
|
motor_fr_ticks = scout_enc.fr_ticks;
|
55 |
|
motor_bl_ticks = scout_enc.bl_ticks;
|
56 |
|
motor_br_ticks = scout_enc.br_ticks;
|
57 |
|
|
58 |
|
|
59 |
|
return;
|
|
39 |
float left_dist, right_dist, total_dist, theta;
|
|
40 |
|
|
41 |
encoder_readings scout_enc = encoders->query();
|
|
42 |
motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
|
|
43 |
motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
|
|
44 |
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
|
|
45 |
motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
|
|
46 |
|
|
47 |
// Get Left and Right distance
|
|
48 |
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
|
|
49 |
((float)(motor_bl_dist))/2.0);
|
|
50 |
right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 +
|
|
51 |
((float)(motor_br_dist))/2.0);
|
|
52 |
|
|
53 |
total_dist = (left_dist)/2.0 + (right_dist)/2.0;
|
|
54 |
theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
|
|
55 |
|
|
56 |
//Use negative theta because we measure theta from 0 on the
|
|
57 |
//right to 180 on the left counter-clock-wise, but this is
|
|
58 |
//negative theta in the coordinate frame.
|
|
59 |
//Also, subtract the delta from y because positive y is down.
|
|
60 |
scout_pos->x += total_dist*cos(-theta);
|
|
61 |
scout_pos->y -= total_dist*sin(-theta);
|
|
62 |
scout_pos->theta = fmod(theta, (float)(2*M_PI));
|
|
63 |
|
|
64 |
//Save state for next time in.
|
|
65 |
motor_fl_ticks = scout_enc.fl_ticks;
|
|
66 |
motor_fr_ticks = scout_enc.fr_ticks;
|
|
67 |
motor_bl_ticks = scout_enc.bl_ticks;
|
|
68 |
motor_br_ticks = scout_enc.br_ticks;
|
|
69 |
|
|
70 |
|
|
71 |
return;
|
60 |
72 |
}
|
61 |
73 |
|
62 |
74 |
void Odometry::run()
|
63 |
75 |
{
|
64 |
|
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000);
|
65 |
|
|
66 |
|
while(ok())
|
67 |
|
{
|
68 |
|
get_position();
|
69 |
|
|
70 |
|
position.name = name;
|
71 |
|
position.x = scout_pos->x;
|
72 |
|
position.y = scout_pos->y;
|
73 |
|
position.theta = scout_pos->theta;
|
74 |
|
scout_position.publish(position);
|
75 |
|
}
|
|
76 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000);
|
|
77 |
|
|
78 |
while(ok())
|
|
79 |
{
|
|
80 |
get_position();
|
|
81 |
|
|
82 |
position.name = name;
|
|
83 |
position.x = scout_pos->x;
|
|
84 |
position.y = scout_pos->y;
|
|
85 |
position.theta = scout_pos->theta;
|
|
86 |
scout_position.publish(position);
|
|
87 |
}
|
76 |
88 |
}
|