.. _program_listing_file__tmp_ws_src_kobuki_velocity_smoother_include_kobuki_velocity_smoother_velocity_smoother.hpp:

Program Listing for File velocity_smoother.hpp
==============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_kobuki_velocity_smoother_include_kobuki_velocity_smoother_velocity_smoother.hpp>` (``/tmp/ws/src/kobuki_velocity_smoother/include/kobuki_velocity_smoother/velocity_smoother.hpp``)

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

.. code-block:: cpp

   
   /*****************************************************************************
    ** Ifdefs
    *****************************************************************************/
   
   #ifndef KOBUKI_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
   #define KOBUKI_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
   
   /*****************************************************************************
    ** Includes
    *****************************************************************************/
   
   #include <algorithm>
   #include <vector>
   
   #include "geometry_msgs/msg/twist.hpp"
   #include "nav_msgs/msg/odometry.hpp"
   #include "rcl_interfaces/msg/set_parameters_result.hpp"
   #include "rclcpp/rclcpp.hpp"
   
   /*****************************************************************************
   ** Namespaces
   *****************************************************************************/
   
   namespace kobuki_velocity_smoother
   {
   
   /*****************************************************************************
   ** VelocitySmoother
   *****************************************************************************/
   
   class VelocitySmoother final : public rclcpp::Node
   {
   public:
     explicit VelocitySmoother(const rclcpp::NodeOptions & options);
   
     ~VelocitySmoother() override;
     VelocitySmoother(VelocitySmoother && c) = delete;
     VelocitySmoother & operator=(VelocitySmoother && c) = delete;
     VelocitySmoother(const VelocitySmoother & c) = delete;
     VelocitySmoother & operator=(const VelocitySmoother & c) = delete;
   
   private:
     enum RobotFeedbackType
     {
       NONE,
       ODOMETRY,
       COMMANDS
     } feedback_;  
     double accel_lim_w_;
   
     geometry_msgs::msg::Twist current_vel_;
     geometry_msgs::msg::Twist target_vel_;
     double last_cmd_vel_linear_x_{0.0};
     double last_cmd_vel_angular_z_{0.0};
   
     double period_;
     bool input_active_;
     double cb_avg_time_;
     rclcpp::Time last_velocity_cb_time_;
     std::vector<double> period_record_;  
     unsigned int pr_next_;  
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr current_vel_sub_;
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr raw_in_vel_sub_;
     rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr smooth_vel_pub_;
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
   
     void timerCB();
     void velocityCB(const geometry_msgs::msg::Twist::SharedPtr msg);
     void robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg);
     void odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg);
   
     rcl_interfaces::msg::SetParametersResult parameterUpdate(
       const std::vector<rclcpp::Parameter> & parameters);
   
     double sign(double x) {return x < 0.0 ? -1.0 : +1.0;}
   
     double median(std::vector<double> & values)
     {
       // Return the median element of an doubles vector
       std::nth_element(values.begin(), values.begin() + values.size() / 2, values.end());
       return values[values.size() / 2];
     }
   };
   
   }  // namespace kobuki_velocity_smoother
   
   #endif  // KOBUKI_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_