Revision 36701fcd
ID | 36701fcdf70230a47c0a3b1c60c190811451d20e |
added new Odometry in test behaviors, done some simple testing in scoutsim
scout/libscout/src/test_behaviors/Odometry_new.cpp | ||
---|---|---|
1 |
|
|
2 |
#include "Odometry_new.h" |
|
3 |
|
|
4 |
using namespace std; |
|
5 |
|
|
6 |
/** Set up the odometry node and prepare communications over ROS */ |
|
7 |
Odometry_new::Odometry_new(string scoutname, Sensors* sensors): |
|
8 |
Behavior(scoutname, "odometry", sensors) |
|
9 |
{ |
|
10 |
name = scoutname; |
|
11 |
scout_pos = new posi; |
|
12 |
motor_fl_ticks_last = motor_fr_ticks_last = 0; |
|
13 |
motor_bl_ticks_last = motor_br_ticks_last = 0; |
|
14 |
} |
|
15 |
|
|
16 |
|
|
17 |
/** Query encoders and integrate using RK4 to estimate position */ |
|
18 |
void Odometry_new::get_position(double loop_time) |
|
19 |
{ |
|
20 |
float avg_left_ticks, avg_right_ticks; |
|
21 |
float ang_vl, ang_vr, lin_vl, lin_vr; |
|
22 |
float robot_v, robot_w; |
|
23 |
float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32; |
|
24 |
// figure out the encoder ticks during the time |
|
25 |
encoder_readings scout_enc = encoders->query(); |
|
26 |
motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last; |
|
27 |
motor_fr_ticks_iter = scout_enc.fr_ticks - motor_fr_ticks_last; |
|
28 |
motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last; |
|
29 |
motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last; |
|
30 |
|
|
31 |
|
|
32 |
avg_left_ticks = (float) motor_fl_ticks_iter; |
|
33 |
avg_right_ticks = (float) motor_fr_ticks_iter; |
|
34 |
// right now the back encoders does not work so uncomment the next |
|
35 |
// two lines when the work!!! |
|
36 |
/* |
|
37 |
avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0); |
|
38 |
avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0); |
|
39 |
*/ |
|
40 |
|
|
41 |
// figure out the angular velocity in radians per second |
|
42 |
ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK; |
|
43 |
ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK; |
|
44 |
|
|
45 |
// figure out the linear velosity of robot |
|
46 |
lin_vl = ang_vl * WHEEL_R; |
|
47 |
lin_vr = ang_vr * WHEEL_R; |
|
48 |
|
|
49 |
// figure out how the robot speed and turning speed |
|
50 |
robot_v = (lin_vl/2.0 + lin_vr/2.0); |
|
51 |
robot_w = (lin_vr - lin_vl)/WHEEL_B; |
|
52 |
|
|
53 |
// now use RK4 to integrade the velocities to get the the move in pos |
|
54 |
k00 = robot_v * cos(scout_pos->theta); |
|
55 |
k01 = robot_v * sin(scout_pos->theta); |
|
56 |
k02 = robot_w; |
|
57 |
|
|
58 |
k10 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k02); |
|
59 |
k11 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k02); |
|
60 |
k12 = robot_w; |
|
61 |
|
|
62 |
k20 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k12); |
|
63 |
k21 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k12); |
|
64 |
k22 = robot_w; |
|
65 |
|
|
66 |
k30 = robot_v * cos(scout_pos->theta + LOOP_TIME * k22); |
|
67 |
k31 = robot_v * sin(scout_pos->theta + LOOP_TIME * k22); |
|
68 |
k32 = robot_w; |
|
69 |
|
|
70 |
scout_pos->x += LOOP_TIME/6 * (k00+2*(k10+k20)+k30); |
|
71 |
scout_pos->y += LOOP_TIME/6 * (k01+2*(k11+k21)+k31); |
|
72 |
scout_pos->theta += LOOP_TIME/6 * (k02+2*(k12+k22)+k32); |
|
73 |
|
|
74 |
// update the |
|
75 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
|
76 |
motor_fr_ticks_last = scout_enc.fr_ticks; |
|
77 |
motor_bl_ticks_last = scout_enc.bl_ticks; |
|
78 |
motor_br_ticks_last = scout_enc.br_ticks; |
|
79 |
|
|
80 |
} |
|
81 |
|
|
82 |
void Odometry_new::run() |
|
83 |
{ |
|
84 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000); |
|
85 |
double last_time = Time::now().toSec(); |
|
86 |
double loop_time, current_time; |
|
87 |
motors->set_sides(50,50, MOTOR_ABSOLUTE); |
|
88 |
while(ok()) |
|
89 |
{ |
|
90 |
//Rate r(LOOP_RATE); |
|
91 |
Duration r = Duration(LOOP_TIME); |
|
92 |
current_time = Time::now().toSec(); |
|
93 |
loop_time = current_time - last_time; |
|
94 |
last_time = current_time; |
|
95 |
get_position(loop_time); |
|
96 |
position.name = name; |
|
97 |
position.x = scout_pos->x; |
|
98 |
position.y = scout_pos->y; |
|
99 |
position.theta = scout_pos->theta; |
|
100 |
scout_position.publish(position); |
|
101 |
ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta); |
|
102 |
r.sleep(); |
|
103 |
} |
|
104 |
|
|
105 |
} |
scout/libscout/src/test_behaviors/Odometry_new.h | ||
---|---|---|
1 |
// a smarter Odometry the use RK4 integration |
|
2 |
|
|
3 |
#ifndef _ODOMETRY_NEW_H_ |
|
4 |
#define _ODOMETRY_NEW_H_ |
|
5 |
|
|
6 |
#include "../Behavior.h" |
|
7 |
#include "../Sensors.h" |
|
8 |
#include "messages/ScoutPosition.h" |
|
9 |
|
|
10 |
#define WHEEL_R 1 //this is a virtual unit now... use real values |
|
11 |
#define WHEEL_B 1 // this is a virtual unit now... use real values |
|
12 |
#define TICKS_PER_REV 48 |
|
13 |
#define PI 3.1415926 |
|
14 |
#define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0) |
|
15 |
#define LOOP_RATE 10 // Hz, integrate 10 times per second |
|
16 |
#define LOOP_TIME (1.0/LOOP_RATE) // secs |
|
17 |
|
|
18 |
typedef struct{ |
|
19 |
float x; |
|
20 |
float y; |
|
21 |
float theta; |
|
22 |
} posi; |
|
23 |
|
|
24 |
class Odometry_new : Behavior{ |
|
25 |
|
|
26 |
public: |
|
27 |
|
|
28 |
/** Set up the odometry node and prepare communications over ROS */ |
|
29 |
Odometry_new(std::string scoutname, Sensors* sensors); |
|
30 |
|
|
31 |
/** Query encoders and estimate position based on encoder reading */ |
|
32 |
void get_position(double loop_time); |
|
33 |
|
|
34 |
/** Gets scout position and prints to screen */ |
|
35 |
void run(); |
|
36 |
|
|
37 |
private: |
|
38 |
|
|
39 |
/** ROS publisher and client declaration */ |
|
40 |
ros::NodeHandle node; |
|
41 |
ros::Publisher scout_position; |
|
42 |
ros::ServiceClient query_encoders_client; |
|
43 |
messages::ScoutPosition position; |
|
44 |
|
|
45 |
std::string name; |
|
46 |
|
|
47 |
// FIXME: not sure whether these would overflow.... |
|
48 |
int motor_fl_ticks_iter; |
|
49 |
int motor_fr_ticks_iter; |
|
50 |
int motor_bl_ticks_iter; |
|
51 |
int motor_br_ticks_iter; |
|
52 |
|
|
53 |
int motor_fl_ticks_last; |
|
54 |
int motor_fr_ticks_last; |
|
55 |
int motor_bl_ticks_last; |
|
56 |
int motor_br_ticks_last; |
|
57 |
int scout_theta; |
|
58 |
|
|
59 |
posi* scout_pos; |
|
60 |
|
|
61 |
}; |
|
62 |
|
|
63 |
#endif |
Also available in: Unified diff