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  */
00041 
00042 #include <diff_drive_controller/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)
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   }
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 left_pos, double right_pos, const ros::Time &time)
00076   {
00078     const double left_wheel_cur_pos  = left_pos  * wheel_radius_;
00079     const double right_wheel_cur_pos = right_pos * wheel_radius_;
00080 
00082     const double left_wheel_est_vel  = left_wheel_cur_pos  - left_wheel_old_pos_;
00083     const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
00084 
00086     left_wheel_old_pos_  = left_wheel_cur_pos;
00087     right_wheel_old_pos_ = right_wheel_cur_pos;
00088 
00090     const double linear  = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
00091     const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
00092 
00094     integrate_fun_(linear, angular);
00095 
00097     const double dt = (time - timestamp_).toSec();
00098     if (dt < 0.0001)
00099       return false; // Interval too small to integrate with
00100 
00101     timestamp_ = time;
00102 
00104     linear_acc_(linear/dt);
00105     angular_acc_(angular/dt);
00106 
00107     linear_ = bacc::rolling_mean(linear_acc_);
00108     angular_ = bacc::rolling_mean(angular_acc_);
00109 
00110     return true;
00111   }
00112 
00113   void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
00114   {
00116     linear_ = linear;
00117     angular_ = angular;
00118 
00120     const double dt = (time - timestamp_).toSec();
00121     timestamp_ = time;
00122     integrate_fun_(linear * dt, angular * dt);
00123   }
00124 
00125   void Odometry::setWheelParams(double wheel_separation, double wheel_radius)
00126   {
00127     wheel_separation_ = wheel_separation;
00128     wheel_radius_     = wheel_radius;
00129   }
00130 
00131   void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
00132   {
00133     velocity_rolling_window_size_ = velocity_rolling_window_size;
00134 
00135     resetAccumulators();
00136   }
00137 
00138   void Odometry::integrateRungeKutta2(double linear, double angular)
00139   {
00140     const double direction = heading_ + angular * 0.5;
00141 
00143     x_       += linear * cos(direction);
00144     y_       += linear * sin(direction);
00145     heading_ += angular;
00146   }
00147 
00153   void Odometry::integrateExact(double linear, double angular)
00154   {
00155     if (fabs(angular) < 1e-6)
00156       integrateRungeKutta2(linear, angular);
00157     else
00158     {
00160       const double heading_old = heading_;
00161       const double r = linear/angular;
00162       heading_ += angular;
00163       x_       +=  r * (sin(heading_) - sin(heading_old));
00164       y_       += -r * (cos(heading_) - cos(heading_old));
00165     }
00166   }
00167 
00168   void Odometry::resetAccumulators()
00169   {
00170     linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00171     angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
00172   }
00173 
00174 } // namespace diff_drive_controller


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Sat Aug 13 2016 04:20:35