Program Listing for File velocity_smoother.hpp
↰ Return to documentation for file (include/kobuki_velocity_smoother/velocity_smoother.hpp
)
/*****************************************************************************
** 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_