Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / Odometry.h @ d140fd71

History | View | Annotate | Download (1.09 KB)

1
#ifndef _ODOMETRY_H_
2
#define _ODOMETRY_H_
3

    
4
#include "../Behavior.h"
5
#include "../Sensors.h"
6

    
7

    
8
#define WHEEL_RADIUS  5
9
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
10
#define WHEEL_BASE    2
11
#define ENCODER_COUNT 8800000000
12
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
13

    
14
typedef struct{
15
  float x;
16
  float y;
17
  float theta;
18
} pos;
19

    
20
class Odometry : Behavior{
21

    
22
  public:
23

    
24
  /** Set up the odometry node and prepare communcations over ROS */
25
  Odometry(std::string scoutname, Sensors* sensors);
26

    
27
  /** Query encoders and estimate position based on encoder reading */
28
  void get_position();
29

    
30
  /** Gets scout position and prints to screen */
31
  void run();
32
  
33
  private:
34

    
35
    /** ROS publisher and client declaration */
36
    ros::NodeHandle node;
37
    //TODO: ros::Publisher scout_position;
38
    ros::ServiceClient query_encoders_client;
39

    
40
    float msg_time_in;
41

    
42
    float motor_fl_dist;
43
    float motor_fr_dist;
44
    float motor_bl_dist;
45
    float motor_br_dist;
46

    
47
    float motor_fl_ticks;
48
    float motor_fr_ticks;
49
    float motor_bl_ticks;
50
    float motor_br_ticks;
51
    float scout_theta;
52

    
53
    pos* scout_pos;
54
};
55

    
56
#endif