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
00043 #include <steer_drive_controller/odometry.h>
00044
00045 #include <boost/bind.hpp>
00046
00047 namespace steer_drive_controller
00048 {
00049 namespace bacc = boost::accumulators;
00050
00051 Odometry::Odometry(size_t velocity_rolling_window_size)
00052 : timestamp_(0.0)
00053 , x_(0.0)
00054 , y_(0.0)
00055 , heading_(0.0)
00056 , linear_(0.0)
00057 , angular_(0.0)
00058 , wheel_separation_h_(0.0)
00059 , wheel_radius_(0.0)
00060 , rear_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 }
00067
00068 void Odometry::init(const ros::Time& time)
00069 {
00070
00071 resetAccumulators();
00072 timestamp_ = time;
00073 }
00074
00075 bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time)
00076 {
00078 const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_;
00079
00081 const double rear_wheel_est_vel = rear_wheel_cur_pos - rear_wheel_old_pos_;
00082
00084 rear_wheel_old_pos_ = rear_wheel_cur_pos;
00085
00087 const double linear = rear_wheel_est_vel;
00088 const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
00089
00091 integrate_fun_(linear, angular);
00092
00094 const double dt = (time - timestamp_).toSec();
00095 if (dt < 0.0001)
00096 return false;
00097
00098 timestamp_ = time;
00099
00101 linear_acc_(linear/dt);
00102 angular_acc_(angular/dt);
00103
00104 linear_ = bacc::rolling_mean(linear_acc_);
00105 angular_ = bacc::rolling_mean(angular_acc_);
00106
00107 return true;
00108 }
00109
00110 void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
00111 {
00113 linear_ = linear;
00114 angular_ = angular;
00115
00117 const double dt = (time - timestamp_).toSec();
00118 timestamp_ = time;
00119 integrate_fun_(linear * dt, angular * dt);
00120 }
00121
00122 void Odometry::setWheelParams(double wheel_separation_h, double wheel_radius)
00123 {
00124 wheel_separation_h_ = wheel_separation_h;
00125 wheel_radius_ = wheel_radius;
00126 }
00127
00128 void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
00129 {
00130 velocity_rolling_window_size_ = velocity_rolling_window_size;
00131
00132 resetAccumulators();
00133 }
00134
00135 void Odometry::integrateRungeKutta2(double linear, double angular)
00136 {
00137 const double direction = heading_ + angular * 0.5;
00138
00140 x_ += linear * cos(direction);
00141 y_ += linear * sin(direction);
00142 heading_ += angular;
00143 }
00144
00150 void Odometry::integrateExact(double linear, double angular)
00151 {
00152 if (fabs(angular) < 1e-6)
00153 integrateRungeKutta2(linear, angular);
00154 else
00155 {
00157 const double heading_old = heading_;
00158 const double r = linear/angular;
00159 heading_ += angular;
00160 x_ += r * (sin(heading_) - sin(heading_old));
00161 y_ += -r * (cos(heading_) - cos(heading_old));
00162 }
00163 }
00164
00165 void Odometry::resetAccumulators()
00166 {
00167 linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00168 angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00169 }
00170
00171 }