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 #include <robotican_controllers/odometry.h>
00043
00044 #include <boost/bind.hpp>
00045
00046 namespace diff_drive_controller
00047 {
00048 namespace bacc = boost::accumulators;
00049
00050 Odometry::Odometry(size_t velocity_rolling_window_size, double slipFactor)
00051 : timestamp_(0.0)
00052 , x_(0.0)
00053 , y_(0.0)
00054 , heading_(0.0)
00055 , linear_(0.0)
00056 , angular_(0.0)
00057 , wheel_separation_(0.0)
00058 , wheel_radius_(0.0)
00059 , left_wheel_old_pos_(0.0)
00060 , right_wheel_old_pos_(0.0)
00061 , velocity_rolling_window_size_(velocity_rolling_window_size)
00062 , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
00063 , angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
00064 , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
00065 {
00066 _slipFactor = slipFactor;
00067 }
00068
00069 void Odometry::init(const ros::Time& time)
00070 {
00071
00072 resetAccumulators();
00073 timestamp_ = time;
00074 }
00075
00076 bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
00077 {
00079 const double left_wheel_cur_pos = left_pos * wheel_radius_;
00080 const double right_wheel_cur_pos = right_pos * wheel_radius_;
00081
00083 const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
00084 const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
00085
00087 left_wheel_old_pos_ = left_wheel_cur_pos;
00088 right_wheel_old_pos_ = right_wheel_cur_pos;
00089
00091 const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
00092 const double angular = _slipFactor*((right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_);
00094
00095 integrate_fun_(linear, (angular));
00096
00098 const double dt = (time - timestamp_).toSec();
00099 if (dt < 0.0001)
00100 return false;
00101
00102 timestamp_ = time;
00103
00105 linear_acc_(linear/dt);
00106 angular_acc_(angular/dt);
00107
00108 linear_ = bacc::rolling_mean(linear_acc_);
00109 angular_ = bacc::rolling_mean(angular_acc_);
00110
00111 return true;
00112 }
00113
00114 void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
00115 {
00117 linear_ = linear;
00118 angular_ = angular;
00119
00121 const double dt = (time - timestamp_).toSec();
00122 timestamp_ = time;
00123 integrate_fun_(linear * dt, angular * dt);
00124 }
00125
00126 void Odometry::setWheelParams(double wheel_separation, double wheel_radius)
00127 {
00128 wheel_separation_ = wheel_separation;
00129 wheel_radius_ = wheel_radius;
00130 }
00131
00132 void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
00133 {
00134 velocity_rolling_window_size_ = velocity_rolling_window_size;
00135
00136 resetAccumulators();
00137 }
00138
00139 void Odometry::integrateRungeKutta2(double linear, double angular)
00140 {
00141 const double direction = heading_ + angular * 0.5;
00142
00144 x_ += linear * cos(direction);
00145 y_ += linear * sin(direction);
00146 heading_ += angular;
00147 }
00148
00154 void Odometry::integrateExact(double linear, double angular)
00155 {
00156 if (fabs(angular) < 1e-6)
00157 integrateRungeKutta2(linear, angular);
00158 else
00159 {
00161 const double heading_old = heading_;
00162 const double r = linear/angular;
00163 heading_ += angular;
00164 x_ += r * (sin(heading_) - sin(heading_old));
00165 y_ += -r * (cos(heading_) - cos(heading_old));
00166 }
00167 }
00168
00169 void Odometry::resetAccumulators()
00170 {
00171 linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00172 angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00173 }
00174
00175 void Odometry::setSlipFactor(double slipFactor) {
00176 _slipFactor = slipFactor;
00177 }
00178 }
00179