acceleration_limiter.hpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010 ** Ifdefs
00011 *****************************************************************************/
00012 
00013 #ifndef KOBUKI_ACCELERATION_LIMITER_HPP_
00014 #define KOBUKI_ACCELERATION_LIMITER_HPP_
00015 
00016 /*****************************************************************************
00017 ** Includes
00018 *****************************************************************************/
00019 
00020 #include <vector>
00021 #include <iomanip>
00022 #include <sstream>
00023 #include <iostream>
00024 #include <stdint.h>
00025 #include <ecl/time.hpp>
00026 
00027 /*****************************************************************************
00028 ** Namespaces
00029 *****************************************************************************/
00030 
00031 namespace kobuki {
00032 
00033 /*****************************************************************************
00034 ** Interfaces
00035 *****************************************************************************/
00036 
00050 class AccelerationLimiter {
00051 public:
00052   AccelerationLimiter() :
00053     is_enabled(true),
00054     last_speed(0),
00055     last_timestamp(ecl::TimeStamp())
00056   {}
00057   void init(bool enable_acceleration_limiter
00058     , double linear_acceleration_max_= 0.5, double angular_acceleration_max_= 3.5
00059     , double linear_deceleration_max_=-0.5*1.2, double angular_deceleration_max_=-3.5*1.2)
00060   {
00061     is_enabled = enable_acceleration_limiter;
00062     linear_acceleration_max  = linear_acceleration_max_ ;
00063     linear_deceleration_max  = linear_deceleration_max_ ;
00064     angular_acceleration_max = angular_acceleration_max_;
00065     angular_deceleration_max = angular_deceleration_max_;
00066   }
00067 
00068   bool isEnabled() const { return is_enabled; }
00069 
00077   std::vector<double> limit(const std::vector<double> &command) { return limit(command[0], command[1]); }
00078 
00079   std::vector<double> limit(const double &vx, const double &wz)
00080   {
00081     if( is_enabled ) {
00082       //get current time
00083       ecl::TimeStamp curr_timestamp;
00084       //get time difference
00085       ecl::TimeStamp duration = curr_timestamp - last_timestamp;
00086       //calculate acceleration
00087       double linear_acceleration = ((double)(vx - last_vx)) / duration; // in [m/s^2]
00088       double angular_acceleration = ((double)(wz - last_wz)) / duration; // in [rad/s^2]
00089 
00090       //std::ostringstream oss;
00091       //oss << std::fixed << std::setprecision(4);
00092       //oss << "[" << std::setw(6) << (double)duration << "]";
00093       //oss << "[" << std::setw(6) << last_vx << ", " << std::setw(6) << last_wz << "]";
00094       //oss << "[" << std::setw(6) << vx << ", " << std::setw(6) << wz << "]";
00095       //oss << "[" << std::setw(6) << linear_acceleration << ", " << std::setw(6) << angular_acceleration << "]";
00096 
00097       if( linear_acceleration > linear_acceleration_max )
00098         command_vx = last_vx + linear_acceleration_max * duration;
00099       else if( linear_acceleration < linear_deceleration_max )
00100         command_vx = last_vx + linear_deceleration_max * duration;
00101       else
00102         command_vx = vx;
00103       last_vx = command_vx;
00104 
00105       if( angular_acceleration > angular_acceleration_max )
00106         command_wz = last_wz + angular_acceleration_max * duration;
00107       else if( angular_acceleration < angular_deceleration_max )
00108         command_wz = last_wz + angular_deceleration_max * duration;
00109       else
00110         command_wz = wz;
00111       last_wz = command_wz;
00112 
00113       last_timestamp = curr_timestamp;
00114 
00115       //oss << "[" << std::setw(6) << command_vx << ", " << std::setw(6) << command_wz << "]";
00116       //std::cout << oss.str() << std::endl;
00117 
00118       std::vector<double> ret_val;
00119       ret_val.push_back(command_vx);
00120       ret_val.push_back(command_wz);
00121       return ret_val;
00122     }
00123   }
00124 
00125 private:
00126   bool is_enabled;
00127   short last_speed;
00128   short last_radius;
00129 //  unsigned short last_timestamp;
00130   ecl::TimeStamp last_timestamp;
00131 
00132   double last_vx, last_wz; // In [m/s] and [rad/s]
00133   double command_vx, command_wz; // In [m/s] and [rad/s]
00134   double linear_acceleration_max, linear_deceleration_max; // In [m/s^2]
00135   double angular_acceleration_max, angular_deceleration_max; // In [rad/s^2]
00136 };
00137 
00138 } // namespace kobuki
00139 
00140 #endif /* KOBUKI_ACCELERATION_LIMITER__HPP_ */


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37