NAV2-Velocity Smoother 速度平滑器理解

1、Velocity Smoother简介

The nav2_velocity_smoother is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners’ control efforts.（nav2_velocity_smoother 是一个包含 lifecycle-component（生命周期）节点的用于平滑由Nav2框架发送给机器人控制器速度的功能包。该软件包的目的是实现速度、加速度和死区平滑，通过平滑加速来减少机器人电机和硬件控制器的磨损，可以用于一些局部轨迹规划器的控制）

个人理解：其一，保证发布的速度在容许的速度区间内；其二，保证指令速度和机器人当前速度所计算得到的加速度在容许的区间内；其三，[x,y,theta]考虑是否需要按照同比例变化；其四，是否考虑闭环控制（是否使用odom数据）

2、Velocity Smoother Parameters（速度平滑参数）

1、smoothing_frequency （平滑频率）

Type	Default
double	20.0

Frequency (Hz) to use the last received velocity command to smooth by velocity, acceleration, and deadband constraints. If set approximately to the rate of your local trajectory planner, it should smooth by acceleration constraints velocity commands. If set much higher, it will interpolate and provide a smooth set of commands to the hardware controller.（频率(Hz)使用最后接收到的速度命令按速度、加速度和死区约束进行平滑。如果将其近似设置为局部轨迹规划器的速率，则应通过加速度约束使其速度平滑。如果设置得比局部规划器频率高，它将向硬件控制器提供一组插入后的平滑的命令。）

2、scale_velocities

Type	Default
bool	false

Whether or not to adjust other components of velocity proportionally to a component’s required changes due to acceleration limits. This will try to adjust all components to follow the same direction, but still enforces acceleration limits to guarantee compliance, even if it means deviating off commanded trajectory slightly.（是否按照同比例进行约束变化，选择能让x,y,z都满足条件的变化量）

3、feedback

Type	Default
string	“OPEN_LOOP”

Type of feedback to use for the current state of the robot’s velocity. In OPEN_LOOP, it will use the last commanded velocity as the next iteration’s current velocity. When acceleration limits are set appropriately, this is a good assumption. In CLOSED_LOOP, it will use the odometry from the odom topic to estimate the robot’s current speed. In closed loop mode, it is important that the odometry is high rate and low latency, relative to the smoothing frequency.（主要是用于机器人当前速度状态的反馈，OPEN_LOOP使用上一次发布的速度作为机器人当前速度，属于开环控制；CLOSED_LOOP使用里程计计算得到的速度作为当前速度，属于闭环控制，闭环控制需要低延迟的里程计）

4、max_velocity

Type	Default
vector<double>	[0.5, 0.0, 2.5]

Maximum velocities (m/s) in [x, y, theta] axes.（最大速度）

5、min_velocity

Type	Default
vector<double>	[-0.5, 0.0, -2.5]

Minimum velocities (m/s) in [x, y, theta] axes. This is signed and thus must be negative to reverse. Note: rotational velocities negative direction is a right-hand turn, so this should always be negative regardless of reversing preference.（最小速度）

6、deadband_velocity

Type	Default
vector<double>	[0.0, 0.0, 0.0]

Minimum velocities (m/s) to send to the robot hardware controllers, to prevent small commands from damaging hardware controllers if that speed cannot be achieved due to stall torque.（死区速度，以防止由于电机无法达到该速度而损坏硬件控制器）

7、velocity_timeout

Type	Default
double	1.0

Timeout (s) after which the velocity smoother will send a zero-ed out Twist command and stop publishing.（速度接收的超时时间，即速度平滑之前，原始速度的接收频率）

8、 max_accel

Type	Default
vector<double>	[2.5, 0.0, 3.2]

Maximum acceleration to apply to each axis [x, y, theta].（最大加速度）

9、max_decel

Type	Default
vector<double>	[-2.5, 0.0, -3.2]

Minimum acceleration to apply to each axis [x, y, theta]. This is signed and thus these should generally all be negative.（最大减速度）

10、odom_topic

Type	Default
string	“odom”

1. Topic to find robot odometry, if in CLOSED_LOOP operational mode.（odom消息话题，用于闭环控制中获取odom速度）

11、odom_duration

Type	Default
double	0.1

Time (s) to buffer odometry commands to estimate the robot speed, if in CLOSED_LOOP operational mode.（在闭环控制中，容许odom的获取超时时间）

// for example
velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.26, 0.0, 1.0]
    min_velocity: [-0.26, 0.0, -1.0]
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

3、源码注解

主函数为 void VelocitySmoother::smootherTimer()，它接收由局部路径规划器或运动控制器发布的速度指令，发布经由平滑后的速度。

// velocity_smoother.hpp

/**
 * @file velocity_smoother.hpp
 * @author annotation by mce_jun
 * @date 2023-07-10
*/

#ifndef NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
#define NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_

#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"

namespace nav2_velocity_smoother {

/**
 * @class nav2_velocity_smoother::VelocitySmoother
 * @brief This class that smooths cmd_vel velocities for robot bases
 */
class VelocitySmoother : public nav2_util::LifecycleNode {
public:
    /**
     * @brief A constructor for nav2_velocity_smoother::VelocitySmoother
     * @param options Additional options to control creation of the node.
     */
    explicit VelocitySmoother(const rclcpp::NodeOptions &options = rclcpp::NodeOptions());

    /**
     * @brief Destructor for nav2_velocity_smoother::VelocitySmoother
     */
    ~VelocitySmoother();

    /**
     * @brief Find the scale factor, eta, which scales axis into acceleration 寻找比例因子
     * range
     * @param v_curr current velocity
     * @param v_cmd commanded velocity
     * @param accel maximum acceleration
     * @param decel maximum deceleration
     * @return Scale factor, eta
     */
    double findEtaConstraint(const double v_curr, const double v_cmd, const double accel, const double decel);

    /**
     * @brief Apply acceleration and scale factor constraints
     * @param v_curr current velocity
     * @param v_cmd commanded velocity
     * @param accel maximum acceleration
     * @param decel maximum deceleration
     * @param eta Scale factor
     * @return Velocity command
     */
    double applyConstraints(const double v_curr, const double v_cmd, const double accel, const double decel, const double eta);

protected:
    /**
     * @brief Configures parameters and member variables
     * 配置参数和成员变量
     * @param state LifeCycle Node's state
     * @return Success or Failure
     */
    nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override;

    /**
     * @brief Activates member variables
     * 成员变量还活跃着
     * @param state LifeCycle Node's state
     * @return Success or Failure
     */
    nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override;

    /**
     * @brief Deactivates member variables
     * 停用成员变量
     * @param state LifeCycle Node's state
     * @return Success or Failure
     */
    nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override;

    /**
     * @brief Calls clean up states and resets member variables.
     * @param state LifeCycle Node's state
     * @return Success or Failure
     */
    nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &state) override;

    /**
     * @brief Called when in Shutdown state
     * @param state LifeCycle Node's state
     * @return Success or Failure
     */
    nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override;

    /**
     * @brief Callback for incoming velocity commands
     * @param msg Twist message
     */
    void inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg);

    /**
     * @brief Main worker timer function
     * 速度平滑器（功能主函数）
     */
    void smootherTimer();

    /**
     * @brief Dynamic reconfigure callback
     * 动态参数回调
     * @param parameters Parameter list to change
     */
    rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

    // Network interfaces
    std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
    rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr smoothed_cmd_pub_;
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Clock::SharedPtr clock_;
    // 上一时刻的运动速度指令
    geometry_msgs::msg::Twist last_cmd_;
    // 接收的速度指令
    geometry_msgs::msg::Twist::SharedPtr command_;

    // Parameters
    double smoothing_frequency_; //平滑频率 20
    double odom_duration_;       // 0.1
    std::string odom_topic_;
    bool open_loop_; // 开环控制 或者 闭环控制
    bool stopped_{true};
    bool scale_velocities_;
    std::vector<double> max_velocities_;      // 最大速度 [0.26, 0.0, 1.0]
    std::vector<double> min_velocities_;      // 最小速度 [-0.26, 0.0, -1.0]
    std::vector<double> max_accels_;          // 最大加速度 [2.5, 0.0, 3.2]
    std::vector<double> max_decels_;          // 最小加速度 [-2.5, 0.0, -3.2]
    std::vector<double> deadband_velocities_; // [0.0, 0.0, 0.0]
    rclcpp::Duration velocity_timeout_{0, 0};
    rclcpp::Time last_command_time_;

    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};

} // namespace nav2_velocity_smoother

#endif // NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_

// velocity_smoother.cpp

/**
 * @file velocity_smoother.hpp
 * @author annotation by mce_jun
 * @date 2023-07-10
*/

#include "nav2_velocity_smoother/velocity_smoother.hpp"
#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

using namespace std::chrono_literals;
using nav2_util::declare_parameter_if_not_declared;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;

namespace nav2_velocity_smoother {

VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions &options)
    : LifecycleNode("velocity_smoother", "", options), last_command_time_{0, 0, get_clock()->get_clock_type()} {}

VelocitySmoother::~VelocitySmoother() {
    if (timer_) {
        timer_->cancel();
        timer_.reset();
    }
}

// 加载参数中
nav2_util::CallbackReturn VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(get_logger(), "Configuring velocity smoother");
    auto node = shared_from_this();
    std::string feedback_type;
    double velocity_timeout_dbl;

    // Smoothing metadata
    declare_parameter_if_not_declared(node, "smoothing_frequency", rclcpp::ParameterValue(20.0));
    declare_parameter_if_not_declared(node, "feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP")));
    declare_parameter_if_not_declared(node, "scale_velocities", rclcpp::ParameterValue(false));
    node->get_parameter("smoothing_frequency", smoothing_frequency_);
    node->get_parameter("feedback", feedback_type);
    node->get_parameter("scale_velocities", scale_velocities_);

    // Kinematics
    declare_parameter_if_not_declared(node, "max_velocity", rclcpp::ParameterValue(std::vector<double>{0.50, 0.0, 2.5}));
    declare_parameter_if_not_declared(node, "min_velocity", rclcpp::ParameterValue(std::vector<double>{-0.50, 0.0, -2.5}));
    declare_parameter_if_not_declared(node, "max_accel", rclcpp::ParameterValue(std::vector<double>{2.5, 0.0, 3.2}));
    declare_parameter_if_not_declared(node, "max_decel", rclcpp::ParameterValue(std::vector<double>{-2.5, 0.0, -3.2}));
    node->get_parameter("max_velocity", max_velocities_);
    node->get_parameter("min_velocity", min_velocities_);
    node->get_parameter("max_accel", max_accels_);
    node->get_parameter("max_decel", max_decels_);

    for (unsigned int i = 0; i != max_decels_.size(); i++) {
        if (max_decels_[i] > 0.0) {
            RCLCPP_WARN(get_logger(),
                        "Positive values set of deceleration! These "
                        "should be negative to slow down!");
        }
    }

    // Get feature parameters
    declare_parameter_if_not_declared(node, "odom_topic", rclcpp::ParameterValue("odom"));
    declare_parameter_if_not_declared(node, "odom_duration", rclcpp::ParameterValue(0.1));
    declare_parameter_if_not_declared(node, "deadband_velocity", rclcpp::ParameterValue(std::vector<double>{0.0, 0.0, 0.0}));
    declare_parameter_if_not_declared(node, "velocity_timeout", rclcpp::ParameterValue(1.0));
    node->get_parameter("odom_topic", odom_topic_);
    node->get_parameter("odom_duration", odom_duration_);
    node->get_parameter("deadband_velocity", deadband_velocities_);
    node->get_parameter("velocity_timeout", velocity_timeout_dbl);
    velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl);

    if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || max_accels_.size() != 3 || max_decels_.size() != 3 ||
        deadband_velocities_.size() != 3) {
        throw std::runtime_error(
            "Invalid setting of kinematic and/or deadband limits!"
            " All limits must be size of 3 representing (x, y, theta).");
    }

    // Get control type
    if (feedback_type == "OPEN_LOOP") {
        open_loop_ = true;
    } else if (feedback_type == "CLOSED_LOOP") {
        open_loop_ = false;
        odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, odom_duration_, odom_topic_);
    } else {
        throw std::runtime_error("Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
    }

    // Setup inputs / outputs
    // 发布平滑后的速度指令
    smoothed_cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel_smoothed", 1);
    // 接收原始速度指令
    cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>("cmd_vel", rclcpp::QoS(1),
                                                              std::bind(&VelocitySmoother::inputCommandCallback, this, std::placeholders::_1));

    return nav2_util::CallbackReturn::SUCCESS;
}

// 活跃状态
nav2_util::CallbackReturn VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(get_logger(), "Activating");
    smoothed_cmd_pub_->on_activate();
    double timer_duration_ms = 1000.0 / smoothing_frequency_;
    timer_ = create_wall_timer(std::chrono::milliseconds(static_cast<int>(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this));

    dyn_params_handler_ = this->add_on_set_parameters_callback(std::bind(&VelocitySmoother::dynamicParametersCallback, this, _1));

    // create bond connection
    createBond();
    return nav2_util::CallbackReturn::SUCCESS;
}

// 销毁状态中
nav2_util::CallbackReturn VelocitySmoother::on_deactivate(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(get_logger(), "Deactivating");
    if (timer_) {
        timer_->cancel();
        timer_.reset();
    }
    smoothed_cmd_pub_->on_deactivate();
    dyn_params_handler_.reset();

    // destroy bond connection
    destroyBond();
    return nav2_util::CallbackReturn::SUCCESS;
}

// 重置变量中
nav2_util::CallbackReturn VelocitySmoother::on_cleanup(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(get_logger(), "Cleaning up");
    smoothed_cmd_pub_.reset();
    odom_smoother_.reset();
    cmd_sub_.reset();
    return nav2_util::CallbackReturn::SUCCESS;
}

// 关机中
nav2_util::CallbackReturn VelocitySmoother::on_shutdown(const rclcpp_lifecycle::State &) {
    RCLCPP_INFO(get_logger(), "Shutting down");
    return nav2_util::CallbackReturn::SUCCESS;
}

// 速度命令回调函数
void VelocitySmoother::inputCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
    // If message contains NaN or Inf, ignore
    if (!nav2_util::validateTwist(*msg)) {
        RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!");
        return;
    }
    // geometry_msgs/Vector3 linear
    // geometry_msgs/Vector3 angular
    command_ = msg;
    last_command_time_ = now();
}

// 返回比例因子  容许控制量/实际变化量
double VelocitySmoother::findEtaConstraint(const double v_curr, const double v_cmd, const double accel, const double decel) {
    // Exploiting vector scaling properties
    double dv = v_cmd - v_curr;
    double v_component_max;
    double v_component_min;

    //  smoothing_frequency_平滑频率，默认为20
    //  命令速度大 且 速度同向
    if (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
        v_component_max = accel / smoothing_frequency_;
        v_component_min = -accel / smoothing_frequency_;
    } else {
        // 减速 或者 反向
        v_component_max = -decel / smoothing_frequency_;
        v_component_min = decel / smoothing_frequency_;
    }

    // 如果 速度变化量 > 容许变化量
    if (dv > v_component_max) {
        // 0< <1
        return v_component_max / dv;
    }
    if (dv < v_component_min) {
        // 0< <1
        return v_component_min / dv;
    }

    // 输出 -1 则满足要求
    return -1.0;
}

// 返回平滑后的速度 当前速度、命令速度、最大加速度、最大减速度、比例因子（容许变化量/命令速度变化量）
double VelocitySmoother::applyConstraints(const double v_curr, const double v_cmd, const double accel, const double decel, const double eta) {
    // 速度变化量
    double dv = v_cmd - v_curr;
    double v_component_max;
    double v_component_min;

    // Accelerating if magnitude of v_cmd is above magnitude of v_curr
    // and if v_cmd and v_curr have the same sign (i.e. speed is NOT passing
    // through 0.0) Decelerating otherwise。 smoothing_frequency_ 默认 20
    // 命令速度大 且 速度同向
    if (abs(v_cmd) >= abs(v_curr) && v_curr * v_cmd >= 0.0) {
        v_component_max = accel / smoothing_frequency_;
        v_component_min = -accel / smoothing_frequency_;
    } else {
        v_component_max = -decel / smoothing_frequency_;
        v_component_min = decel / smoothing_frequency_;
    }
    // eta 比例因子（容许变化量/命令速度变化量）
    return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);
}

// 速度平滑器主函数
void VelocitySmoother::smootherTimer() {
    // Wait until the first command is received  等待接受命令
    if (!command_) {
        return;
    }

    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();

    // Check for velocity timeout. If nothing received, publish zeros to apply deceleration
    // 检查速度超时。如果没有收到任何消息，则发布零以应用减速(默认1.0s)
    if (now() - last_command_time_ > velocity_timeout_) {
        if (last_cmd_ == geometry_msgs::msg::Twist() || stopped_) {
            stopped_ = true;
            return;
        }
        *command_ = geometry_msgs::msg::Twist();
    }

    stopped_ = false;

    // Get current velocity based on feedback type
    geometry_msgs::msg::Twist current_;
    // 开环控制，直接使用上一次输出的速度
    if (open_loop_) {
        current_ = last_cmd_;
    } else {
        // 闭环控制, 使用odom的实际速度
        current_ = odom_smoother_->getTwist();
    }

    // Apply absolute velocity restrictions to the command
    command_->linear.x = std::clamp(command_->linear.x, min_velocities_[0], max_velocities_[0]);
    command_->linear.y = std::clamp(command_->linear.y, min_velocities_[1], max_velocities_[1]);
    command_->angular.z = std::clamp(command_->angular.z, min_velocities_[2], max_velocities_[2]);

    // Find if any component is not within the acceleration constraints. If so,
    // store the most significant scale factor to apply to the vector <dvx, dvy,
    // dvw>, eta, to reduce all axes proportionally to follow the same direction,
    // within change of velocity bounds. In case eta reduces another axis out of
    // its own limit, apply accel constraint to guarantee output is within limits,
    // even if it deviates from requested command slightly.
    double eta = 1.0;
    if (scale_velocities_) {
        double curr_eta = -1.0;
        // current_为上一次发布的速度（即当前速度）、command_为当前时刻指令速度
        // 如果 curr_eta == -1 则输出速度符合条件
        // curr_eta(0< <1) , 越小则越不符合条件
        curr_eta = findEtaConstraint(current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0]);
        if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
            eta = curr_eta;
        }

        curr_eta = findEtaConstraint(current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1]);
        // 满足此条件则证明 y的速度相比于x 更不符合条件
        if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
            eta = curr_eta;
        }

        curr_eta = findEtaConstraint(current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2]);
        if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) {
            eta = curr_eta;
        }
    }

    // 理解：其实就是为了找到能够满足x,y,角度三个条件的比例因子， 然后按照最小的比例增加因子去改变速度
    cmd_vel->linear.x = applyConstraints(current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0], eta);
    cmd_vel->linear.y = applyConstraints(current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta);
    cmd_vel->angular.z = applyConstraints(current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta);

    // Apply deadband restrictions & publish
    cmd_vel->linear.x = fabs(cmd_vel->linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x;
    cmd_vel->linear.y = fabs(cmd_vel->linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y;
    cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z;
    last_cmd_ = *cmd_vel;
    smoothed_cmd_pub_->publish(std::move(cmd_vel));
}

// 动态参数回调
rcl_interfaces::msg::SetParametersResult VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters) {
    rcl_interfaces::msg::SetParametersResult result;
    result.successful = true;

    for (auto parameter : parameters) {
        const auto &type = parameter.get_type();
        const auto &name = parameter.get_name();

        if (type == ParameterType::PARAMETER_DOUBLE) {
            if (name == "smoothing_frequency") {
                smoothing_frequency_ = parameter.as_double();
                if (timer_) {
                    timer_->cancel();
                    timer_.reset();
                }

                double timer_duration_ms = 1000.0 / smoothing_frequency_;
                timer_ = create_wall_timer(std::chrono::milliseconds(static_cast<int>(timer_duration_ms)),
                                           std::bind(&VelocitySmoother::smootherTimer, this));
            } else if (name == "velocity_timeout") {
                velocity_timeout_ = rclcpp::Duration::from_seconds(parameter.as_double());
            } else if (name == "odom_duration") {
                odom_duration_ = parameter.as_double();
                odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), odom_duration_, odom_topic_);
            }
        } else if (type == ParameterType::PARAMETER_DOUBLE_ARRAY) {
            if (parameter.as_double_array().size() != 3) {
                RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size 3", name.c_str());
                result.successful = false;
                break;
            }

            if (name == "max_velocity") {
                max_velocities_ = parameter.as_double_array();
            } else if (name == "min_velocity") {
                min_velocities_ = parameter.as_double_array();
            } else if (name == "max_accel") {
                max_accels_ = parameter.as_double_array();
            } else if (name == "max_decel") {
                max_decels_ = parameter.as_double_array();
            } else if (name == "deadband_velocity") {
                deadband_velocities_ = parameter.as_double_array();
            }
        } else if (type == ParameterType::PARAMETER_STRING) {
            if (name == "feedback") {
                if (parameter.as_string() == "OPEN_LOOP") {
                    open_loop_ = true;
                    odom_smoother_.reset();
                } else if (parameter.as_string() == "CLOSED_LOOP") {
                    open_loop_ = false;
                    odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), odom_duration_, odom_topic_);
                } else {
                    RCLCPP_WARN(get_logger(), "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP.");
                    result.successful = false;
                    break;
                }
            } else if (name == "odom_topic") {
                odom_topic_ = parameter.as_string();
                odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), odom_duration_, odom_topic_);
            }
        }
    }
    return result;
}

} // namespace nav2_velocity_smoother

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(nav2_velocity_smoother::VelocitySmoother)

目录

1、Velocity Smoother简介

2、Velocity Smoother Parameters（速度平滑参数）

1、smoothing_frequency （平滑频率）

2、scale_velocities

3、feedback

4、max_velocity

5、min_velocity

6、deadband_velocity

7、velocity_timeout

8、 max_accel

9、max_decel

10、odom_topic

11、odom_duration

3、源码注解

---