.. _program_listing_file__tmp_ws_src_kobuki_core_include_kobuki_core_modules_acceleration_limiter.hpp:

Program Listing for File acceleration_limiter.hpp
=================================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_kobuki_core_include_kobuki_core_modules_acceleration_limiter.hpp>` (``/tmp/ws/src/kobuki_core/include/kobuki_core/modules/acceleration_limiter.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   /*****************************************************************************
   ** Ifdefs
   *****************************************************************************/
   
   #ifndef KOBUKI_CORE_ACCELERATION_LIMITER_HPP_
   #define KOBUKI_CORE_ACCELERATION_LIMITER_HPP_
   
   /*****************************************************************************
   ** Includes
   *****************************************************************************/
   
   #include <vector>
   #include <iomanip>
   #include <sstream>
   #include <iostream>
   #include <stdint.h>
   #include <ecl/time.hpp>
   
   /*****************************************************************************
   ** Namespaces
   *****************************************************************************/
   
   namespace kobuki {
   
   /*****************************************************************************
   ** Interfaces
   *****************************************************************************/
   
   class AccelerationLimiter {
   public:
     AccelerationLimiter() :
       is_enabled(true),
       last_timestamp(ecl::TimeStamp()),
       last_vx(0.0),
       last_wz(0.0)
     {}
     void init(bool enable_acceleration_limiter
       , double linear_acceleration_max_= 0.5, double angular_acceleration_max_= 3.5
       , double linear_deceleration_max_=-0.5*1.2, double angular_deceleration_max_=-3.5*1.2)
     {
       is_enabled = enable_acceleration_limiter;
       linear_acceleration_max  = linear_acceleration_max_ ;
       linear_deceleration_max  = linear_deceleration_max_ ;
       angular_acceleration_max = angular_acceleration_max_;
       angular_deceleration_max = angular_deceleration_max_;
     }
   
     bool isEnabled() const { return is_enabled; }
   
     std::vector<double> limit(const std::vector<double> &command) { return limit(command[0], command[1]); }
   
     std::vector<double> limit(const double &vx, const double &wz)
     {
       std::vector<double> ret_val;
       if( is_enabled ) {
         //get current time
         ecl::TimeStamp curr_timestamp;
         //get time difference
         ecl::TimeStamp duration = curr_timestamp - last_timestamp;
         //calculate acceleration
         double linear_acceleration = ((double)(vx - last_vx)) / duration; // in [m/s^2]
         double angular_acceleration = ((double)(wz - last_wz)) / duration; // in [rad/s^2]
   
         //std::ostringstream oss;
         //oss << std::fixed << std::setprecision(4);
         //oss << "[" << std::setw(6) << (double)duration << "]";
         //oss << "[" << std::setw(6) << last_vx << ", " << std::setw(6) << last_wz << "]";
         //oss << "[" << std::setw(6) << vx << ", " << std::setw(6) << wz << "]";
         //oss << "[" << std::setw(6) << linear_acceleration << ", " << std::setw(6) << angular_acceleration << "]";
   
         if( linear_acceleration > linear_acceleration_max )
           command_vx = last_vx + linear_acceleration_max * duration;
         else if( linear_acceleration < linear_deceleration_max )
           command_vx = last_vx + linear_deceleration_max * duration;
         else
           command_vx = vx;
         last_vx = command_vx;
   
         if( angular_acceleration > angular_acceleration_max )
           command_wz = last_wz + angular_acceleration_max * duration;
         else if( angular_acceleration < angular_deceleration_max )
           command_wz = last_wz + angular_deceleration_max * duration;
         else
           command_wz = wz;
         last_wz = command_wz;
   
         last_timestamp = curr_timestamp;
   
         //oss << "[" << std::setw(6) << command_vx << ", " << std::setw(6) << command_wz << "]";
         //std::cout << oss.str() << std::endl;
   
         ret_val.push_back(command_vx);
         ret_val.push_back(command_wz);
       } else {
         ret_val.push_back(0.0);
         ret_val.push_back(0.0);
       }
       return ret_val;
     }
   
   private:
     bool is_enabled;
     ecl::TimeStamp last_timestamp;
   
     double last_vx, last_wz; // In [m/s] and [rad/s]
     double command_vx, command_wz; // In [m/s] and [rad/s]
     double linear_acceleration_max, linear_deceleration_max; // In [m/s^2]
     double angular_acceleration_max, angular_deceleration_max; // In [rad/s^2]
   };
   
   } // namespace kobuki
   
   #endif /* KOBUKI_ACCELERATION_LIMITER__HPP_ */