Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Odometry.h @ ddfeb111

History | View | Annotate | Download (1.22 KB)

1
#ifndef _ODOMETRY_H_
2
#define _ODOMETRY_H_
3

    
4
#include "../Behavior.h"
5
#include "../Sensors.h"
6
#include "messages/ScoutPosition.h"
7

    
8

    
9
#define WHEEL_RADIUS  .2
10
#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)
11
#define WHEEL_BASE    1.2
12
#define ENCODER_COUNT (2*WHEEL_RADIUS*350*M_PI/WHEEL_BASE)
13
#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)
14

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

    
21
class Odometry : Behavior{
22

    
23
  public:
24

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

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

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

    
36
    /** ROS publisher and client declaration */
37
    ros::NodeHandle node;
38
    ros::Publisher scout_position;
39
    ros::ServiceClient query_encoders_client;
40
    messages::ScoutPosition position;
41

    
42
    std::string name;
43

    
44
    float msg_time_in;
45

    
46
    int motor_fl_dist;
47
    int motor_fr_dist;
48
    int motor_bl_dist;
49
    int motor_br_dist;
50

    
51
    unsigned int motor_fl_ticks;
52
    unsigned int motor_fr_ticks;
53
    unsigned int motor_bl_ticks;
54
    unsigned int motor_br_ticks;
55
    float scout_theta;
56

    
57
    pos* scout_pos;
58
};
59

    
60
#endif