scoutos / scout / libscout / src / test_behaviors / Odometry_new.h @ 36701fcd
History | View | Annotate | Download (1.49 KB)
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
|