scoutos / scout / libscout / src / behaviors / Odometry.cpp @ 30a3768e
History | View | Annotate | Download (2.31 KB)
1 |
|
---|---|
2 |
#include "Odometry.h" |
3 |
|
4 |
using namespace std; |
5 |
|
6 |
/** Set up the odometry node and prepare communcations over ROS */
|
7 |
Odometry::Odometry(string scoutname, Sensors* sensors):Behavior(scoutname,
|
8 |
"odometry", sensors)
|
9 |
{ |
10 |
name = scoutname; |
11 |
scout_pos = new pos;
|
12 |
motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
13 |
} |
14 |
|
15 |
/** Query encoders and estimate position based on encoder reading */
|
16 |
void Odometry::get_position()
|
17 |
{ |
18 |
float left_dist, right_dist, total_dist, theta;
|
19 |
|
20 |
encoder_readings scout_enc = encoders->query(); |
21 |
motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks; |
22 |
motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks; |
23 |
motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks; |
24 |
motor_br_dist = scout_enc.br_ticks - motor_br_ticks; |
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 |
// Get Left and Right distance
|
31 |
left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + |
32 |
((float)(motor_bl_dist))/2.0); |
33 |
right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + |
34 |
((float)(motor_br_dist))/2.0); |
35 |
|
36 |
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); |
43 |
scout_pos->theta = fmod(theta, 2*M_PI);
|
44 |
|
45 |
//Save state for next time in.
|
46 |
motor_fl_ticks = scout_enc.fl_ticks; |
47 |
motor_fr_ticks = scout_enc.fr_ticks; |
48 |
motor_bl_ticks = scout_enc.bl_ticks; |
49 |
motor_br_ticks = scout_enc.br_ticks; |
50 |
|
51 |
|
52 |
return;
|
53 |
} |
54 |
|
55 |
void Odometry::run()
|
56 |
{ |
57 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); |
58 |
|
59 |
while(ok())
|
60 |
{ |
61 |
get_position(); |
62 |
|
63 |
position.name = name; |
64 |
position.x = scout_pos->x; |
65 |
position.y = scout_pos->y; |
66 |
position.theta = scout_pos->theta; |
67 |
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 |
} |
71 |
} |