Colony » Colony Scout

#ifndef _ODOMETRY_H_

#define _ODOMETRY_H_

#include "../Behavior.h"

#include "../Sensors.h"

#define WHEEL_RADIUS  2

#define WHEEL_CIRCUM  (2*M_PI*WHEEL_RADIUS)

#define WHEEL_BASE    5

#define ENCODER_COUNT (4*1200*M_PI/5)

#define DIST_PER_TICK (WHEEL_CIRCUM/ENCODER_COUNT)

typedef struct{

  float x;

  float y;

  float theta;

} pos;

class Odometry : Behavior{

  public:

  /** Set up the odometry node and prepare communcations over ROS */

  Odometry(std::string scoutname, Sensors* sensors);

  /** Query encoders and estimate position based on encoder reading */

  void get_position();

  /** Gets scout position and prints to screen */

  void run();

  private:

    /** ROS publisher and client declaration */

    ros::NodeHandle node;

    //TODO: ros::Publisher scout_position;

    ros::ServiceClient query_encoders_client;

    float msg_time_in;

    int motor_fl_dist;

    int motor_fr_dist;

    int motor_bl_dist;

    int motor_br_dist;

    unsigned int motor_fl_ticks;

    unsigned int motor_fr_ticks;

    unsigned int motor_bl_ticks;

    unsigned int motor_br_ticks;

    float scout_theta;

    pos* scout_pos;

};

#endif