odometry.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the PAL Robotics nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036  * Author: Luca Marchionni
00037  * Author: Bence Magyar
00038  * Author: Enrique Fernández
00039  * Author: Paul Mathieu
00040  * Author: Masaru Morita
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     // Reset accumulators and timestamp:
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; // Interval too small to integrate with
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 } // namespace diff_drive_controller


steer_drive_controller
Author(s): CIR-KIT , Masaru Morita
autogenerated on Sat Jun 8 2019 19:20:25