speed_limiter.cpp
Go to the documentation of this file.
00001 #include <algorithm>
00002 
00003 #include <four_wheel_steering_controller/speed_limiter.h>
00004 
00005 template<typename T>
00006 T clamp(T x, T min, T max)
00007 {
00008   return std::min(std::max(min, x), max);
00009 }
00010 
00011 namespace four_wheel_steering_controller
00012 {
00013 
00014   SpeedLimiter::SpeedLimiter(
00015     bool has_velocity_limits,
00016     bool has_acceleration_limits,
00017     bool has_jerk_limits,
00018     double min_velocity,
00019     double max_velocity,
00020     double min_acceleration,
00021     double max_acceleration,
00022     double min_jerk,
00023     double max_jerk
00024   )
00025   : has_velocity_limits(has_velocity_limits)
00026   , has_acceleration_limits(has_acceleration_limits)
00027   , has_jerk_limits(has_jerk_limits)
00028   , min_velocity(min_velocity)
00029   , max_velocity(max_velocity)
00030   , min_acceleration(min_acceleration)
00031   , max_acceleration(max_acceleration)
00032   , min_jerk(min_jerk)
00033   , max_jerk(max_jerk)
00034   {
00035   }
00036 
00037   double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
00038   {
00039     const double tmp = v;
00040 
00041     limit_jerk(v, v0, v1, dt);
00042     limit_acceleration(v, v0, dt);
00043     limit_velocity(v);
00044 
00045     return tmp != 0.0 ? v / tmp : 1.0;
00046   }
00047 
00048   double SpeedLimiter::limit_velocity(double& v)
00049   {
00050     const double tmp = v;
00051 
00052     if (has_velocity_limits)
00053     {
00054       v = clamp(v, min_velocity, max_velocity);
00055     }
00056 
00057     return tmp != 0.0 ? v / tmp : 1.0;
00058   }
00059 
00060   double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
00061   {
00062     const double tmp = v;
00063 
00064     if (has_acceleration_limits)
00065     {
00066       const double dv_min = min_acceleration * dt;
00067       const double dv_max = max_acceleration * dt;
00068 
00069       const double dv = clamp(v - v0, dv_min, dv_max);
00070 
00071       v = v0 + dv;
00072     }
00073 
00074     return tmp != 0.0 ? v / tmp : 1.0;
00075   }
00076 
00077   double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
00078   {
00079     const double tmp = v;
00080 
00081     if (has_jerk_limits)
00082     {
00083       const double dv  = v  - v0;
00084       const double dv0 = v0 - v1;
00085 
00086       const double dt2 = 2. * dt * dt;
00087 
00088       const double da_min = min_jerk * dt2;
00089       const double da_max = max_jerk * dt2;
00090 
00091       const double da = clamp(dv - dv0, da_min, da_max);
00092 
00093       v = v0 + dv0 + da;
00094     }
00095 
00096     return tmp != 0.0 ? v / tmp : 1.0;
00097   }
00098 
00099 } // namespace four_wheel_steering_controller


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Jun 8 2019 20:06:24