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   enum RobotFeedbackType
00051   {
00052     NONE,
00053     ODOMETRY,
00054     COMMANDS
00055   } robot_feedback;  
00057   std::string name;
00058   double speed_lim_v, accel_lim_v, decel_lim_v;
00059   double speed_lim_w, accel_lim_w, decel_lim_w;
00060   double decel_factor;
00061 
00062   double frequency;
00063 
00064   geometry_msgs::Twist last_cmd_vel;
00065   geometry_msgs::Twist  current_vel;
00066   geometry_msgs::Twist   target_vel;
00067 
00068   bool                 shutdown_req; 
00069   bool                 input_active;
00070   double                cb_avg_time;
00071   ros::Time            last_cb_time;
00072   std::vector<double> period_record; 
00073   unsigned int             pr_next; 
00075   ros::Subscriber odometry_sub;    
00076   ros::Subscriber current_vel_sub; 
00077   ros::Subscriber raw_in_vel_sub;  
00078   ros::Publisher  smooth_vel_pub;  
00080   void velocityCB(const geometry_msgs::Twist::ConstPtr& msg);
00081   void robotVelCB(const geometry_msgs::Twist::ConstPtr& msg);
00082   void odometryCB(const nav_msgs::Odometry::ConstPtr& msg);
00083 
00084   double sign(double x)  { return x < 0.0 ? -1.0 : +1.0; };
00085 
00086   double median(std::vector<double> values) {
00087     // Return the median element of an doubles vector
00088     nth_element(values.begin(), values.begin() + values.size()/2, values.end());
00089     return values[values.size()/2];
00090   };
00091 
00092   /*********************
00093   ** Dynamic Reconfigure
00094   **********************/
00095   dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig> *             dynamic_reconfigure_server;
00096   dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>::CallbackType dynamic_reconfigure_callback;
00097   void reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t unused_level);
00098 };
00099 
00100 } // yocs_namespace velocity_smoother
00101 
00102 #endif /* YUJIN_OCS_VELOCITY_SMOOTHER_HPP_ */