add humble-navigation2

navigation2/nav2_velocity_smoother/CMakeLists.txt

@@ -0,0 +1,84 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_velocity_smoother)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(executable_name velocity_smoother)
+set(library_name ${executable_name}_core)
+
+set(dependencies
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+  nav2_util
+)
+
+# Main library
+add_library(${library_name} SHARED
+  src/velocity_smoother.cpp
+)
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+# Main executable
+add_executable(${executable_name}
+  src/main.cpp
+)
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+target_link_libraries(${executable_name} ${library_name})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+rclcpp_components_register_nodes(${library_name} "nav2_velocity_smoother::VelocitySmoother")
+
+install(
+  TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name})
+ament_export_dependencies(${dependencies})
+ament_package()

navigation2/nav2_velocity_smoother/README.md

@@ -0,0 +1,87 @@
+# 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.
+
+It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). 
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions.
+
+## Features
+
+This package was created to do the following:
+
+- Limit velocity commands by kinematic constraints, including velocity and acceleration
+- Limit velocities based on deadband regions
+- Stop sending velocities after a given timeout duration of no new commands (due to stopped navigation)
+- Send a zero-velocity command at velocity timeout to stop the robot, in case not properly handled
+- Support Omni and differential drive robots (e.g. X, Y, Theta)
+- Smooth velocities proportionally in the same direction as commanded, whenever possible within kinematic limits
+- Provide open loop and closed loop options
+- Component nodes for use in single-process systems and stand-alone node format
+- Dynamically reconfigurable parameters
+
+## Design
+
+This is a lifecycle-component node, using the lifecycle manager for state management and composition for process management.
+It is designed to take in a command from Nav2's controller server and smooth it for use on robot hardware controllers.
+Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed output on `smoothed_cmd_vel`.
+
+The node is designed on a regular timer running at a configurable rate.
+This is in contrast to simply computing a smoothed velocity command in the callback of each `cmd_vel` input from Nav2.
+This allows us to interpolate commands at a higher frequency than Nav2's local trajectory planners can provide.
+For example, if a local trajectory planner is running at 20hz, the velocity smoother can run at 100hz to provide approximately 5 messages to a robot controller which will be smoothed by kinematic limits at each timestep.
+This provides a more regular stream of commands to a robot base and interpolates commands between the current velocity and the desired velocity more finely for smoother acceleration / motion profiles.
+While this is not required, it is a nice design feature.
+It is possible to also simply run the smoother at `cmd_vel` rate to smooth velocities alone without interpolation.
+
+There are two primary operation modes: open and closed loop.
+In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly).
+This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop.
+In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed.
+This will be used to determine the robot's current velocity and therefore achievable velocity targets by the velocity, acceleration, and deadband constraints using live data.
+
+## Parameters
+
+See inline description of parameters in the `VelocitySmoother`.
+
+```
+velocity_smoother:
+  ros__parameters:
+  	smoothing_frequency: 20.0  # Rate to run smoother
+  	scale_velocities: false  # scale velocities proportionally if any axis is outside of acceleration range to follow same vector, if possible
+  	feedback: "OPEN_LOOP"  # Type of feedback for current speed. Open loop uses the last smoothed output. Closed loop uses robot odometry
+  	max_velocity: [0.5, 0.0, 2.5]  # Maximum velocities, ordered [Vx, Vy, Vw]
+   	min_velocity: [-0.5, 0.0, -2.5]  # Minimum velocities, ordered [Vx, Vy, Vw]
+   	deadband_velocity: [0.0, 0.0, 0.0]  # A deadband of velocities below which they should be zero-ed out for sending to the robot base controller, ordered [Vx, Vy, Vw]
+   	velocity_timeout: 1.0  # Time (s) after which if no new velocity commands are received to zero out and stop
+   	max_accel: [2.5, 0.0, 3.2]  # Maximum acceleration, ordered [Ax, Ay, Aw]
+   	max_decel: [-2.5, 0.0, -3.2]  # Maximum deceleration, ordered [Ax, Ay, Aw]
+   	odom_topic: "odom"  # Topic of odometry to use for estimating current velocities
+   	odom_duration: 0.1  # Period of time (s) to sample odometry information in for velocity estimation
+```
+
+## Topics
+
+| Topic            | Type                    | Use                           |
+|------------------|-------------------------|-------------------------------|
+| smoothed_cmd_vel | geometry_msgs/Twist     | Publish smoothed velocities   |
+| cmd_vel          | geometry_msgs/Twist     | Subscribe to input velocities |
+
+
+## Install
+
+```
+sudo apt-get install ros-<ros2-distro>-nav2-velocity-smoother
+```
+
+## Etc (Important Side Notes)
+
+Typically: if you have low rate odometry, you should use open-loop mode or set the smoothing frequency relatively similar to that of your `cmd_vel` topic. If you have high rate odometry, you can use closed-loop mode with a higher smoothing frequency since you'll have more up to date information to smooth based off of. Do not exceed the smoothing frequency to your odometry frequency in closed loop mode. Also consider the ``odom_duration`` to use relative to your odometry publication rate and noise characteristics.
+If the smoothing frequency out paces odometry or poorly selected ``odom_duration``s are used, the robot can oscillate and/or accelerate slowly due to latency in closed loop mode.
+
+When in doubt, open-loop is a reasonable choice for most users.
+
+The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and right turns. While we make it possible to specify these separately, most users would be wise to set these values the same (but signed) for rotation. Additionally, the parameters are signed, so it is important to specify maximum deceleration with negative signs to represent deceleration. Minimum velocities with negatives when moving backward, so backward movement can be restricted by setting this to ``0``.
+
+Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current.

navigation2/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp

@@ -0,0 +1,161 @@
+// Copyright (c) 2022 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#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_;
+  double odom_duration_;
+  std::string odom_topic_;
+  bool open_loop_;
+  bool stopped_{true};
+  bool scale_velocities_;
+  std::vector<double> max_velocities_;
+  std::vector<double> min_velocities_;
+  std::vector<double> max_accels_;
+  std::vector<double> max_decels_;
+  std::vector<double> deadband_velocities_;
+  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_

navigation2/nav2_velocity_smoother/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_velocity_smoother</name>
+  <version>1.1.18</version>
+  <description>Nav2's Output velocity smoother</description>
+  <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav2_util</depend>
+
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_velocity_smoother/src/main.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) 2022 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+
+#include "nav2_velocity_smoother/velocity_smoother.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<nav2_velocity_smoother::VelocitySmoother>();
+  rclcpp::spin(node->get_node_base_interface());
+  rclcpp::shutdown();
+
+  return 0;
+}

navigation2/nav2_velocity_smoother/src/velocity_smoother.cpp

@@ -0,0 +1,421 @@
+// Copyright (c) 2022 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <chrono>
+#include <limits>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "nav2_velocity_smoother/velocity_smoother.hpp"
+
+using namespace std::chrono_literals;
+using nav2_util::declare_parameter_if_not_declared;
+using std::placeholders::_1;
+using rcl_interfaces::msg::ParameterType;
+
+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 != 3; i++) {
+    if (max_decels_[i] > 0.0) {
+      throw std::runtime_error(
+              "Positive values set of deceleration! These should be negative to slow down!");
+    }
+    if (max_accels_[i] < 0.0) {
+      throw std::runtime_error(
+              "Negative values set of acceleration! These should be positive to speed up!");
+    }
+    if (min_velocities_[i] > max_velocities_[i]) {
+      throw std::runtime_error(
+              "Min velocities are higher than max velocities!");
+    }
+  }
+
+  // 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_ = this->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;
+  }
+
+  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;
+
+  // 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
+  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) {
+    return v_component_max / dv;
+  }
+
+  if (dv < v_component_min) {
+    return v_component_min / dv;
+  }
+
+  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
+  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_;
+  }
+
+  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
+  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 {
+    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;
+
+    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]);
+    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;
+    }
+  }
+
+  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);
+  last_cmd_ = *cmd_vel;
+
+  // 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;
+
+  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_ = this->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") {
+        for (unsigned int i = 0; i != 3; i++) {
+          if (parameter.as_double_array()[i] < 0.0) {
+            RCLCPP_WARN(
+              get_logger(),
+              "Negative values set of acceleration! These should be positive to speed up!");
+            result.successful = false;
+          }
+        }
+        max_accels_ = parameter.as_double_array();
+      } else if (name == "max_decel") {
+        for (unsigned int i = 0; i != 3; i++) {
+          if (parameter.as_double_array()[i] > 0.0) {
+            RCLCPP_WARN(
+              get_logger(),
+              "Positive values set of deceleration! These should be negative to slow down!");
+            result.successful = false;
+          }
+        }
+        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)

navigation2/nav2_velocity_smoother/test/CMakeLists.txt

@@ -0,0 +1,10 @@
+# Tests
+ament_add_gtest(velocity_smoother_tests
+  test_velocity_smoother.cpp
+)
+ament_target_dependencies(velocity_smoother_tests
+  ${dependencies}
+)
+target_link_libraries(velocity_smoother_tests
+  ${library_name}
+)

navigation2/nav2_velocity_smoother/test/test_velocity_smoother.cpp

@@ -0,0 +1,704 @@
+// Copyright (c) 2022 Samsung Research
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <math.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "gtest/gtest.h"
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_velocity_smoother/velocity_smoother.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+using namespace std::chrono_literals;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
+{
+public:
+  VelSmootherShim()
+  : VelocitySmoother() {}
+  void configure(const rclcpp_lifecycle::State & state) {this->on_configure(state);}
+  void activate(const rclcpp_lifecycle::State & state) {this->on_activate(state);}
+  void deactivate(const rclcpp_lifecycle::State & state) {this->on_deactivate(state);}
+  void cleanup(const rclcpp_lifecycle::State & state) {this->on_cleanup(state);}
+  void shutdown(const rclcpp_lifecycle::State & state) {this->on_shutdown(state);}
+
+  bool isOdomSmoother() {return odom_smoother_ ? true : false;}
+  bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;}
+  geometry_msgs::msg::Twist::SharedPtr lastCommandMsg() {return command_;}
+
+  void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);}
+};
+
+TEST(VelocitySmootherTest, openLoopTestTimer)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  std::vector<double> deadbands{0.2, 0.0, 0.0};
+  smoother->declare_parameter("scale_velocities", rclcpp::ParameterValue(true));
+  smoother->set_parameter(rclcpp::Parameter("scale_velocities", true));
+  smoother->declare_parameter("deadband_velocity", rclcpp::ParameterValue(deadbands));
+  smoother->set_parameter(rclcpp::Parameter("deadband_velocity", deadbands));
+  rclcpp_lifecycle::State state;
+  smoother->configure(state);
+  smoother->activate(state);
+
+  std::vector<double> linear_vels;
+  auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel_smoothed",
+    1,
+    [&](geometry_msgs::msg::Twist::SharedPtr msg) {
+      linear_vels.push_back(msg->linear.x);
+    });
+
+  // Send a velocity command
+  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
+  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  smoother->sendCommandMsg(cmd);
+
+  // Process velocity smoothing and send updated odometry based on commands
+  auto start = smoother->now();
+  while (smoother->now() - start < 1.5s) {
+    rclcpp::spin_some(smoother->get_node_base_interface());
+  }
+
+  // Sanity check we have the approximately right number of messages for the timespan and timeout
+  EXPECT_GT(linear_vels.size(), 19u);
+  EXPECT_LT(linear_vels.size(), 30u);
+
+  // Should have last command be a stop since we timed out the command stream
+  EXPECT_EQ(linear_vels.back(), 0.0);
+
+  // From deadband, first few should be 0 until above 0.2
+  for (unsigned int i = 0; i != linear_vels.size(); i++) {
+    if (linear_vels[i] != 0) {
+      EXPECT_GT(linear_vels[i], 0.2);
+      break;
+    }
+  }
+
+  // Process to make sure stops at limit in velocity,
+  // doesn't exceed acceleration
+  for (unsigned int i = 0; i != linear_vels.size(); i++) {
+    EXPECT_TRUE(linear_vels[i] <= 0.5);
+  }
+}
+
+TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("CLOSED_LOOP")));
+  smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP")));
+  rclcpp_lifecycle::State state;
+  smoother->configure(state);
+  smoother->activate(state);
+
+  std::vector<double> linear_vels;
+  auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel_smoothed",
+    1,
+    [&](geometry_msgs::msg::Twist::SharedPtr msg) {
+      linear_vels.push_back(msg->linear.x);
+    });
+
+  auto odom_pub = smoother->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+  odom_pub->on_activate();
+  nav_msgs::msg::Odometry odom_msg;
+  odom_msg.header.frame_id = "odom";
+  odom_msg.child_frame_id = "base_link";
+
+  // Fill buffer with 0 twisted-commands
+  for (unsigned int i = 0; i != 30; i++) {
+    odom_msg.header.stamp = smoother->now() + rclcpp::Duration::from_seconds(i * 0.01);
+    odom_pub->publish(odom_msg);
+  }
+
+  // Send a velocity command
+  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
+  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  smoother->sendCommandMsg(cmd);
+
+  // Process velocity smoothing and send updated odometry based on commands
+  auto start = smoother->now();
+  while (smoother->now() - start < 1.5s) {
+    odom_msg.header.stamp = smoother->now();
+    if (linear_vels.size() > 0) {
+      odom_msg.twist.twist.linear.x = linear_vels.back();
+    }
+    odom_pub->publish(odom_msg);
+    rclcpp::spin_some(smoother->get_node_base_interface());
+  }
+
+  // Sanity check we have the approximately right number of messages for the timespan and timeout
+  EXPECT_GT(linear_vels.size(), 19u);
+  EXPECT_LT(linear_vels.size(), 30u);
+
+  // Should have last command be a stop since we timed out the command stream
+  EXPECT_EQ(linear_vels.back(), 0.0);
+
+  // Process to make sure stops at limit in velocity,
+  // doesn't exceed acceleration
+  for (unsigned int i = 0; i != linear_vels.size(); i++) {
+    if (i > 0) {
+      double diff = linear_vels[i] - linear_vels[i - 1];
+      EXPECT_LT(diff, 0.126);  // default accel of 0.5 / 20 hz = 0.125
+    }
+
+    EXPECT_TRUE(linear_vels[i] <= 0.5);
+  }
+}
+
+TEST(VelocitySmootherTest, testfindEtaConstraint)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+
+  double accel = 0.1;    // dv 0.005
+  double decel = -1.0;  // dv 0.05
+
+  // In range
+  // Constant positive
+  EXPECT_EQ(smoother->findEtaConstraint(1.0, 1.0, accel, decel), -1);
+  // Constant negative
+  EXPECT_EQ(smoother->findEtaConstraint(-1.0, -1.0, accel, decel), -1);
+  // Positive To Positive Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.504, accel, decel), -1);
+  // Positive To Positive Decel
+  EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.46, accel, decel), -1);
+  // 0 To Positive Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.0, 0.004, accel, decel), -1);
+  // Positive To 0 Decel
+  EXPECT_EQ(smoother->findEtaConstraint(0.04, 0.0, accel, decel), -1);
+  // Negative To Negative Accel
+  EXPECT_EQ(smoother->findEtaConstraint(-0.5, -0.504, accel, decel), -1);
+  // Negative To Negative Decel
+  EXPECT_EQ(smoother->findEtaConstraint(-0.5, -0.46, accel, decel), -1);
+  // 0 To Negative Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.0, -0.004, accel, decel), -1);
+  // Negative To 0 Decel
+  EXPECT_EQ(smoother->findEtaConstraint(-0.04, 0.0, accel, decel), -1);
+  // Negative to Positive
+  EXPECT_EQ(smoother->findEtaConstraint(-0.02, 0.02, accel, decel), -1);
+  // Positive to Negative
+  EXPECT_EQ(smoother->findEtaConstraint(0.02, -0.02, accel, decel), -1);
+
+  // Faster than limit
+  // Positive To Positive Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.5, 1.5, accel, decel), 0.005);
+  // Positive To Positive Decel
+  EXPECT_EQ(smoother->findEtaConstraint(1.5, 0.5, accel, decel), 0.05);
+  // 0 To Positive Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.0, 1.0, accel, decel), 0.005);
+  // Positive To 0 Decel
+  EXPECT_EQ(smoother->findEtaConstraint(1.0, 0.0, accel, decel), 0.05);
+  // Negative To Negative Accel
+  EXPECT_EQ(smoother->findEtaConstraint(-0.5, -1.5, accel, decel), 0.005);
+  // Negative To Negative Decel
+  EXPECT_EQ(smoother->findEtaConstraint(-1.5, -0.5, accel, decel), 0.05);
+  // 0 To Negative Accel
+  EXPECT_EQ(smoother->findEtaConstraint(0.0, -1.0, accel, decel), 0.005);
+  // Negative To 0 Decel
+  EXPECT_EQ(smoother->findEtaConstraint(-1.0, 0.0, accel, decel), 0.05);
+  // Negative to Positive
+  EXPECT_EQ(smoother->findEtaConstraint(-0.2, 0.8, accel, decel), 0.05);
+  // Positive to Negative
+  EXPECT_EQ(smoother->findEtaConstraint(0.2, -0.8, accel, decel), 0.05);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraints)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+
+  // Apply examples from testfindEtaConstraint
+  // In range, so no eta or acceleration limit impact
+  EXPECT_EQ(smoother->applyConstraints(1.0, 1.0, 1.5, -2.0, no_eta), 1.0);
+  EXPECT_EQ(smoother->applyConstraints(0.5, 0.55, 1.5, -2.0, no_eta), 0.55);
+  EXPECT_EQ(smoother->applyConstraints(0.5, 0.45, 1.5, -2.0, no_eta), 0.45);
+  // Too high, without eta
+  EXPECT_NEAR(smoother->applyConstraints(1.0, 2.0, 1.5, -2.0, no_eta), 1.075, 0.01);
+  // Too high, with eta applied on its own axis
+  EXPECT_NEAR(smoother->applyConstraints(1.0, 2.0, 1.5, -2.0, 0.075), 1.075, 0.01);
+  // On another virtual axis that is OK
+  EXPECT_NEAR(smoother->applyConstraints(0.5, 0.55, 1.5, -2.0, 0.075), 0.503, 0.01);
+
+  // In a more realistic situation, applied to angular
+  EXPECT_NEAR(smoother->applyConstraints(0.8, 1.0, 3.2, -3.2, 0.75), 1.075, 0.95);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsPositiveToPositiveAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = 1.0;
+  target_vel = 2.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel + i * dv_accel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsZeroToPositiveAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = 0.0;
+  target_vel = 2.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel + i * dv_accel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsNegativeToPositiveDecelAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_below_zero, steps_above_zero;
+
+  init_vel = -1.0;
+  target_vel = 2.0;
+  steps_below_zero = -init_vel / dv_decel;
+  steps_above_zero = target_vel / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_below_zero + steps_above_zero + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    if (v_curr > 0) {
+      EXPECT_NEAR(v_curr, (i - steps_below_zero) * dv_accel, 0.001);
+    } else {
+      EXPECT_NEAR(v_curr, init_vel + i * dv_decel, 0.001);
+    }
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsNegativeToNegativeAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = -1.0;
+  target_vel = -2.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel - i * dv_accel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsZeroToNegativeAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = 0.0;
+  target_vel = -2.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel - i * dv_accel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsPositiveToNegativeDecelAccel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_accel = accel / 20.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_below_zero, steps_above_zero;
+
+  init_vel = 1.0;
+  target_vel = -2.0;
+  steps_above_zero = init_vel / dv_decel;
+  steps_below_zero = -target_vel / dv_accel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_below_zero + steps_above_zero + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    if (v_curr < 0) {
+      EXPECT_NEAR(v_curr, -static_cast<int>(i - steps_above_zero) * dv_accel, 0.001);
+    } else {
+      EXPECT_NEAR(v_curr, init_vel - i * dv_decel, 0.001);
+    }
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsPositiveToPositiveDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+
+  // Test asymetric accel/decel use cases
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = 2.0;
+  target_vel = 1.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_decel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel - i * dv_decel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsPositiveToZeroDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = 2.0;
+  target_vel = 0.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_decel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel - i * dv_decel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsNegativeToNegativeDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = -2.0;
+  target_vel = -1.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_decel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel + i * dv_decel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testapplyConstraintsNegativeToZeroDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  // default frequency is 20.0
+  smoother->configure(state);
+  double no_eta = 1.0;
+  double accel = 0.1;
+  double decel = -1.0;
+  double dv_decel = -decel / 20.0;
+  double init_vel, target_vel, v_curr;
+  uint steps_to_target;
+
+  init_vel = -2.0;
+  target_vel = 0.0;
+  steps_to_target = abs(target_vel - init_vel) / dv_decel;
+
+  v_curr = init_vel;
+  for (size_t i = 1; i < steps_to_target + 1; i++) {
+    v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+    EXPECT_NEAR(v_curr, init_vel + i * dv_decel, 0.001);
+  }
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+  v_curr = smoother->applyConstraints(v_curr, target_vel, accel, decel, no_eta);
+  EXPECT_NEAR(v_curr, target_vel, 0.001);
+}
+
+TEST(VelocitySmootherTest, testCommandCallback)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  smoother->configure(state);
+  smoother->activate(state);
+
+  auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+  pub->on_activate();
+  auto msg = std::make_unique<geometry_msgs::msg::Twist>();
+  msg->linear.x = 100.0;
+  pub->publish(std::move(msg));
+  rclcpp::spin_some(smoother->get_node_base_interface());
+
+  EXPECT_TRUE(smoother->hasCommandMsg());
+  EXPECT_EQ(smoother->lastCommandMsg()->linear.x, 100.0);
+}
+
+TEST(VelocitySmootherTest, testClosedLoopSub)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  smoother->declare_parameter("feedback", rclcpp::ParameterValue(std::string("OPEN_LOOP")));
+  smoother->set_parameter(rclcpp::Parameter("feedback", std::string("CLOSED_LOOP")));
+  rclcpp_lifecycle::State state;
+  smoother->configure(state);
+  EXPECT_TRUE(smoother->isOdomSmoother());
+}
+
+TEST(VelocitySmootherTest, testInvalidParams)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  std::vector<double> max_vels{0.0, 0.0};  // invalid size
+  smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vels));
+  rclcpp_lifecycle::State state;
+  EXPECT_THROW(smoother->configure(state), std::runtime_error);
+
+  smoother->set_parameter(rclcpp::Parameter("feedback", std::string("LAWLS")));
+  EXPECT_THROW(smoother->configure(state), std::runtime_error);
+}
+
+TEST(VelocitySmootherTest, testInvalidParamsAccelDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+
+  std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
+  std::vector<double> bad_test_decel{10.0, 10.0, 10.0};
+  std::vector<double> bad_test_min_vel{10.0, 10.0, 10.0};
+  std::vector<double> bad_test_max_vel{-10.0, -10.0, -10.0};
+
+  smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel));
+  smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel));
+  rclcpp_lifecycle::State state;
+  EXPECT_THROW(smoother->configure(state), std::runtime_error);
+
+  smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel)));
+  EXPECT_THROW(smoother->configure(state), std::runtime_error);
+
+  smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel)));
+  EXPECT_THROW(smoother->configure(state), std::runtime_error);
+}
+
+TEST(VelocitySmootherTest, testDynamicParameter)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+  rclcpp_lifecycle::State state;
+  smoother->configure(state);
+  smoother->activate(state);
+  EXPECT_FALSE(smoother->isOdomSmoother());
+
+  auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
+    smoother->get_node_base_interface(), smoother->get_node_topics_interface(),
+    smoother->get_node_graph_interface(),
+    smoother->get_node_services_interface());
+
+  std::vector<double> max_vel{10.0, 10.0, 10.0};
+  std::vector<double> min_vel{0.0, 0.0, 0.0};
+  std::vector<double> max_accel{10.0, 10.0, 10.0};
+  std::vector<double> min_accel{0.0, 0.0, 0.0};
+  std::vector<double> deadband{0.0, 0.0, 0.0};
+  std::vector<double> bad_test{0.0, 0.0};
+  std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
+  std::vector<double> bad_test_decel{10.0, 10.0, 10.0};
+
+  auto results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("smoothing_frequency", 100.0),
+      rclcpp::Parameter("feedback", std::string("CLOSED_LOOP")),
+      rclcpp::Parameter("scale_velocities", true),
+      rclcpp::Parameter("max_velocity", max_vel),
+      rclcpp::Parameter("min_velocity", min_vel),
+      rclcpp::Parameter("max_accel", max_accel),
+      rclcpp::Parameter("max_decel", min_accel),
+      rclcpp::Parameter("odom_topic", std::string("TEST")),
+      rclcpp::Parameter("odom_duration", 2.0),
+      rclcpp::Parameter("velocity_timeout", 4.0),
+      rclcpp::Parameter("deadband_velocity", deadband)});
+
+  rclcpp::spin_until_future_complete(
+    smoother->get_node_base_interface(),
+    results);
+
+  EXPECT_EQ(smoother->get_parameter("smoothing_frequency").as_double(), 100.0);
+  EXPECT_EQ(smoother->get_parameter("feedback").as_string(), std::string("CLOSED_LOOP"));
+  EXPECT_EQ(smoother->get_parameter("scale_velocities").as_bool(), true);
+  EXPECT_EQ(smoother->get_parameter("max_velocity").as_double_array(), max_vel);
+  EXPECT_EQ(smoother->get_parameter("min_velocity").as_double_array(), min_vel);
+  EXPECT_EQ(smoother->get_parameter("max_accel").as_double_array(), max_accel);
+  EXPECT_EQ(smoother->get_parameter("max_decel").as_double_array(), min_accel);
+  EXPECT_EQ(smoother->get_parameter("odom_topic").as_string(), std::string("TEST"));
+  EXPECT_EQ(smoother->get_parameter("odom_duration").as_double(), 2.0);
+  EXPECT_EQ(smoother->get_parameter("velocity_timeout").as_double(), 4.0);
+  EXPECT_EQ(smoother->get_parameter("deadband_velocity").as_double_array(), deadband);
+
+  // Test reverting
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("feedback", std::string("OPEN_LOOP"))});
+  rclcpp::spin_until_future_complete(
+    smoother->get_node_base_interface(), results);
+  EXPECT_EQ(smoother->get_parameter("feedback").as_string(), std::string("OPEN_LOOP"));
+
+  // Test invalid change
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("feedback", std::string("LAWLS"))});
+  rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
+  EXPECT_FALSE(results.get().successful);
+
+  // Test invalid size
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("max_velocity", bad_test)});
+  rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
+  EXPECT_FALSE(results.get().successful);
+
+  // Test invalid accel / decel
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("max_accel", bad_test_accel)});
+  rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
+  EXPECT_FALSE(results.get().successful);
+  results = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("max_decel", bad_test_decel)});
+  rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results);
+  EXPECT_FALSE(results.get().successful);
+
+  // test full state after major changes
+  smoother->deactivate(state);
+  smoother->cleanup(state);
+  smoother->shutdown(state);
+  smoother.reset();
+}