Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 #ifndef ODOMETRY_H_
00043 #define ODOMETRY_H_
00044
00045 #include <ros/time.h>
00046 #include <boost/accumulators/accumulators.hpp>
00047 #include <boost/accumulators/statistics/stats.hpp>
00048 #include <boost/accumulators/statistics/rolling_mean.hpp>
00049 #include <boost/function.hpp>
00050
00051 namespace diff_drive_controller
00052 {
00053 namespace bacc = boost::accumulators;
00054
00059 class Odometry
00060 {
00061 public:
00062
00064 typedef boost::function<void(double, double)> IntegrationFunction;
00065
00072 Odometry(size_t velocity_rolling_window_size, double slipFactor);
00073
00078 void init(const ros::Time &time);
00079
00087 bool update(double left_pos, double right_pos, const ros::Time &time);
00088
00095 void updateOpenLoop(double linear, double angular, const ros::Time &time);
00096
00101 double getHeading() const
00102 {
00103 return heading_;
00104 }
00105
00110 double getX() const
00111 {
00112 return x_;
00113 }
00114
00119 double getY() const
00120 {
00121 return y_;
00122 }
00123
00128 double getLinear() const
00129 {
00130 return linear_;
00131 }
00132
00137 double getAngular() const
00138 {
00139 return angular_;
00140 }
00141
00142 double getSlipFactor() {return _slipFactor;}
00143
00149 void setWheelParams(double wheel_separation, double wheel_radius);
00150
00155 void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
00156
00157 void setSlipFactor(double slipFactor);
00158
00159 private:
00160 double _slipFactor;
00162 typedef bacc::accumulator_set<double, bacc::stats<bacc::tag::rolling_mean> > RollingMeanAcc;
00163 typedef bacc::tag::rolling_window RollingWindow;
00164
00170 void integrateRungeKutta2(double linear, double angular);
00171
00177 void integrateExact(double linear, double angular);
00178
00182 void resetAccumulators();
00183
00185 ros::Time timestamp_;
00186
00188 double x_;
00189 double y_;
00190 double heading_;
00191
00193 double linear_;
00194 double angular_;
00195
00197 double wheel_separation_;
00198 double wheel_radius_;
00199
00201 double left_wheel_old_pos_;
00202 double right_wheel_old_pos_;
00203
00205 size_t velocity_rolling_window_size_;
00206 RollingMeanAcc linear_acc_;
00207 RollingMeanAcc angular_acc_;
00208
00210 IntegrationFunction integrate_fun_;
00211 };
00212 }
00213
00214 #endif
00215