yocs_velocity_smoother: velocity_smoother

Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  ** Ifdefs
00011  *****************************************************************************/
00012 
00013 #ifndef YUJIN_OCS_VELOCITY_SMOOTHER_HPP_
00014 #define YUJIN_OCS_VELOCITY_SMOOTHER_HPP_
00015 
00016 /*****************************************************************************
00017  ** Includes
00018  *****************************************************************************/
00019 
00020 #include <ros/ros.h>
00021 #include <nav_msgs/Odometry.h>
00022 
00023 /*****************************************************************************
00024 ** Namespaces
00025 *****************************************************************************/
00026 
00027 namespace yocs_velocity_smoother {
00028 
00029 /*****************************************************************************
00030 ** VelocitySmoother
00031 *****************************************************************************/
00032 
00033 class VelocitySmoother
00034 {
00035 public:
00036   VelocitySmoother(const std::string &name)
00037    : name(name), shutdown_req(false), input_active(false), pr_next(0), dynamic_reconfigure_server(NULL) { };
00038 
00039   ~VelocitySmoother()
00040   {
00041     if (dynamic_reconfigure_server != NULL)
00042       delete dynamic_reconfigure_server;
00043   }
00044 
00045   bool init(ros::NodeHandle& nh);
00046   void spin();
00047   void shutdown() { shutdown_req = true; };
00048 
00049 private:
00050   std::string name;
00051   double speed_lim_v, accel_lim_v, decel_lim_v;
00052   double speed_lim_w, accel_lim_w, decel_lim_w;
00053   double decel_factor;
00054 
00055   double frequency;
00056 
00057   geometry_msgs::Twist odometry_vel;
00058   geometry_msgs::Twist last_cmd_vel;
00059   geometry_msgs::Twist   target_vel;
00060 
00061   bool                 shutdown_req; 
00062   bool                 input_active;
00063   double                cb_avg_time;
00064   ros::Time            last_cb_time;
00065   std::vector<double> period_record; 
00066   unsigned int              pr_next; 
00068   ros::Subscriber cur_vel_sub;  
00069   ros::Subscriber raw_vel_sub;  
00070   ros::Publisher  lim_vel_pub;  
00072   void velocityCB(const geometry_msgs::Twist::ConstPtr& msg);
00073   void odometryCB(const nav_msgs::Odometry::ConstPtr& msg);
00074 
00075   double sign(double x)  { return x < 0.0 ? -1.0 : +1.0; };
00076 
00077   double median(std::vector<double>& values) {
00078     // Return the median element of an doubles vector
00079     nth_element(values.begin(), values.begin() + values.size()/2, values.end());
00080     return values[values.size()/2];
00081   };
00082 
00083   /*********************
00084   ** Dynamic Reconfigure
00085   **********************/
00086   dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig> *             dynamic_reconfigure_server;
00087   dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>::CallbackType dynamic_reconfigure_callback;
00088   void reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t unused_level);
00089 };
00090 
00091 } // yocs_namespace velocity_smoother
00092 
00093 #endif /* YUJIN_OCS_VELOCITY_SMOOTHER_HPP_ */