odometry.h
Go to the documentation of this file.
1 
9 #ifndef ODOMETRY_H_
10 #define ODOMETRY_H_
11 
12 #include <ros/time.h>
13 #include <boost/accumulators/accumulators.hpp>
14 #include <boost/accumulators/statistics/stats.hpp>
15 #include <boost/accumulators/statistics/rolling_mean.hpp>
16 #include <boost/function.hpp>
18 
19 namespace ackermann_controller
20 {
21 namespace bacc = boost::accumulators;
22 
27 class Odometry
28 {
29 public:
30 
32  typedef boost::function<void(double, double)> IntegrationFunction;
33 
40  Odometry(size_t velocity_rolling_window_size = 10);
41 
46  void init(const ros::Time &time);
47 
53  bool update(
54  const std::vector<ActuatedJoint>& steering_joints,
55  const std::vector<Wheel>& odometry_joints,
56  const ros::Time &time);
57 
64  void updateOpenLoop(double linear, double angular, const ros::Time &time);
65 
70  double getHeading() const
71  {
72  return heading_;
73  }
74 
79  double getX() const
80  {
81  return x_ + wheelbase_ * (1.0 - cos(heading_));
82  }
83 
88  double getY() const
89  {
90  return y_ - wheelbase_ * sin(heading_);
91  }
92 
97  double getLinear() const
98  {
99  return linear_;
100  }
101 
106  double getAngular() const
107  {
108  return angular_;
109  }
110 
111  void setWheelbase(double wheelbase)
112  {
113  wheelbase_ = wheelbase;
114  }
115 
120  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
121 
122 private:
123 
125  typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
126  typedef bacc::tag::rolling_window RollingWindow;
127 
133  void integrateRungeKutta2(double linear, double angular);
134 
140  void integrateExact(double linear, double angular);
141 
145  void resetAccumulators();
146 
149 
151  double x_; // [m]
152  double y_; // [m]
153  double heading_; // [rad]
154 
156  double linear_; // [m/s]
157  double angular_; // [rad/s]
158 
159  double wheelbase_;
160 
162  std::map<std::string, double> wheels_old_pos_;
163 
166  RollingMeanAcc linear_acc_;
167  RollingMeanAcc angular_acc_;
168 
170  IntegrationFunction integrate_fun_;
171 };
172 }
173 
174 #endif /* ODOMETRY_H_ */
double getY() const
y position getter
Definition: odometry.h:88
bool update(const std::vector< ActuatedJoint > &steering_joints, const std::vector< Wheel > &odometry_joints, const ros::Time &time)
Updates the odometry class with latest wheels position.
Definition: odometry.cpp:39
void setWheelbase(double wheelbase)
Definition: odometry.h:111
IntegrationFunction integrate_fun_
Integration funcion, used to integrate the odometry:
Definition: odometry.h:170
ros::Time timestamp_
Current timestamp:
Definition: odometry.h:148
double x_
Current pose:
Definition: odometry.h:151
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
Velocity rolling window size setter.
Definition: odometry.cpp:110
size_t velocity_rolling_window_size_
Rolling mean accumulators for the linar and angular velocities:
Definition: odometry.h:165
double linear_
Current velocity:
Definition: odometry.h:156
RollingMeanAcc angular_acc_
Definition: odometry.h:167
double getHeading() const
heading getter
Definition: odometry.h:70
void integrateExact(double linear, double angular)
Integrates the velocities (linear and angular) using exact method.
Definition: odometry.cpp:132
std::map< std::string, double > wheels_old_pos_
Previou wheel position/state [rad]:
Definition: odometry.h:162
void updateOpenLoop(double linear, double angular, const ros::Time &time)
Updates the odometry class with latest velocity command.
Definition: odometry.cpp:98
double getLinear() const
linear velocity getter
Definition: odometry.h:97
boost::function< void(double, double)> IntegrationFunction
Integration function, used to integrate the odometry:
Definition: odometry.h:32
Odometry(size_t velocity_rolling_window_size=10)
Constructor Timestamp will get the current time value Value will be set to zero.
Definition: odometry.cpp:17
double getX() const
x position getter
Definition: odometry.h:79
bacc::accumulator_set< double, bacc::stats< bacc::tag::rolling_mean > > RollingMeanAcc
Rolling mean accumulator and window:
Definition: odometry.h:125
void integrateRungeKutta2(double linear, double angular)
Integrates the velocities (linear and angular) using 2nd order Runge-Kutta.
Definition: odometry.cpp:117
void init(const ros::Time &time)
Initialize the odometry.
Definition: odometry.cpp:32
void resetAccumulators()
Reset linear and angular accumulators.
Definition: odometry.cpp:147
bacc::tag::rolling_window RollingWindow
Definition: odometry.h:126
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:27
double getAngular() const
angular velocity getter
Definition: odometry.h:106
RollingMeanAcc linear_acc_
Definition: odometry.h:166


ackermann_controller
Author(s): GĂ©rald Lelong
autogenerated on Mon Jun 10 2019 12:44:49