acceleration_limiter.hpp
Go to the documentation of this file.
1 
9 /*****************************************************************************
10 ** Ifdefs
11 *****************************************************************************/
12 
13 #ifndef KOBUKI_ACCELERATION_LIMITER_HPP_
14 #define KOBUKI_ACCELERATION_LIMITER_HPP_
15 
16 /*****************************************************************************
17 ** Includes
18 *****************************************************************************/
19 
20 #include <vector>
21 #include <iomanip>
22 #include <sstream>
23 #include <iostream>
24 #include <stdint.h>
25 #include <ecl/time.hpp>
26 
27 /*****************************************************************************
28 ** Namespaces
29 *****************************************************************************/
30 
31 namespace kobuki {
32 
33 /*****************************************************************************
34 ** Interfaces
35 *****************************************************************************/
36 
51 public:
53  is_enabled(true),
54  last_speed(0),
55  last_timestamp(ecl::TimeStamp())
56  {}
57  void init(bool enable_acceleration_limiter
58  , double linear_acceleration_max_= 0.5, double angular_acceleration_max_= 3.5
59  , double linear_deceleration_max_=-0.5*1.2, double angular_deceleration_max_=-3.5*1.2)
60  {
61  is_enabled = enable_acceleration_limiter;
62  linear_acceleration_max = linear_acceleration_max_ ;
63  linear_deceleration_max = linear_deceleration_max_ ;
64  angular_acceleration_max = angular_acceleration_max_;
65  angular_deceleration_max = angular_deceleration_max_;
66  }
67 
68  bool isEnabled() const { return is_enabled; }
69 
77  std::vector<double> limit(const std::vector<double> &command) { return limit(command[0], command[1]); }
78 
79  std::vector<double> limit(const double &vx, const double &wz)
80  {
81  if( is_enabled ) {
82  //get current time
83  ecl::TimeStamp curr_timestamp;
84  //get time difference
85  ecl::TimeStamp duration = curr_timestamp - last_timestamp;
86  //calculate acceleration
87  double linear_acceleration = ((double)(vx - last_vx)) / duration; // in [m/s^2]
88  double angular_acceleration = ((double)(wz - last_wz)) / duration; // in [rad/s^2]
89 
90  //std::ostringstream oss;
91  //oss << std::fixed << std::setprecision(4);
92  //oss << "[" << std::setw(6) << (double)duration << "]";
93  //oss << "[" << std::setw(6) << last_vx << ", " << std::setw(6) << last_wz << "]";
94  //oss << "[" << std::setw(6) << vx << ", " << std::setw(6) << wz << "]";
95  //oss << "[" << std::setw(6) << linear_acceleration << ", " << std::setw(6) << angular_acceleration << "]";
96 
97  if( linear_acceleration > linear_acceleration_max )
99  else if( linear_acceleration < linear_deceleration_max )
101  else
102  command_vx = vx;
104 
105  if( angular_acceleration > angular_acceleration_max )
107  else if( angular_acceleration < angular_deceleration_max )
109  else
110  command_wz = wz;
112 
113  last_timestamp = curr_timestamp;
114 
115  //oss << "[" << std::setw(6) << command_vx << ", " << std::setw(6) << command_wz << "]";
116  //std::cout << oss.str() << std::endl;
117 
118  std::vector<double> ret_val;
119  ret_val.push_back(command_vx);
120  ret_val.push_back(command_wz);
121  return ret_val;
122  }
123  }
124 
125 private:
127  short last_speed;
128  short last_radius;
129 // unsigned short last_timestamp;
130  ecl::TimeStamp last_timestamp;
131 
132  double last_vx, last_wz; // In [m/s] and [rad/s]
133  double command_vx, command_wz; // In [m/s] and [rad/s]
136 };
137 
138 } // namespace kobuki
139 
140 #endif /* KOBUKI_ACCELERATION_LIMITER__HPP_ */
std::vector< double > limit(const std::vector< double > &command)
Limits the input velocity commands if gatekeeper is enabled.
std::vector< double > limit(const double &vx, const double &wz)
An acceleration limiter for the kobuki.
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)


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Mon Mar 27 2017 01:02:59