From bcc7f3a0161fd06c3596903609083b8a0f6d9338 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Fri, 6 Jun 2025 15:22:32 +0000 Subject: [PATCH 1/7] Add 6dof support for velocity smoother Signed-off-by: mini-1235 --- .../velocity_smoother.hpp | 1 + .../src/velocity_smoother.cpp | 168 ++++++++++++++---- 2 files changed, 134 insertions(+), 35 deletions(-) diff --git a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp index a5756ee12ba..3506a2dffa8 100644 --- a/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp +++ b/nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp @@ -149,6 +149,7 @@ class VelocitySmoother : public nav2::LifecycleNode bool open_loop_; bool stopped_{true}; bool scale_velocities_; + bool is_6dof_; std::vector max_velocities_; std::vector min_velocities_; std::vector max_accels_; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 20547c53e12..63ce9032d2b 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -119,18 +119,23 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) node->get_parameter("deadband_velocity", deadband_velocities_); node->get_parameter("velocity_timeout", velocity_timeout_dbl); velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl); + size_t size = max_velocities_.size(); - if (max_velocities_.size() != 3 || min_velocities_.size() != 3 || - max_accels_.size() != 3 || max_decels_.size() != 3 || deadband_velocities_.size() != 3) + if ((size != 3 && size != 6) || + min_velocities_.size() != size || + max_accels_.size() != size || + max_decels_.size() != size || + deadband_velocities_.size() != size) { RCLCPP_ERROR( get_logger(), "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 representing (x, y, theta)."); + " All limits must be size of 3 or 6"); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } + is_6dof_ = (size == 6); // Get control type if (feedback_type == "OPEN_LOOP") { open_loop_ = true; @@ -332,15 +337,36 @@ void VelocitySmoother::smootherTimer() } // Apply absolute velocity restrictions to the command - command_->twist.linear.x = std::clamp( + if(!is_6dof_) { + command_->twist.linear.x = std::clamp( command_->twist.linear.x, min_velocities_[0], max_velocities_[0]); - command_->twist.linear.y = std::clamp( + command_->twist.linear.y = std::clamp( command_->twist.linear.y, min_velocities_[1], max_velocities_[1]); - command_->twist.angular.z = std::clamp( + command_->twist.angular.z = std::clamp( command_->twist.angular.z, min_velocities_[2], max_velocities_[2]); + } else { + command_->twist.linear.x = std::clamp( + command_->twist.linear.x, min_velocities_[0], + max_velocities_[0]); + command_->twist.linear.y = std::clamp( + command_->twist.linear.y, min_velocities_[1], + max_velocities_[1]); + command_->twist.linear.z = std::clamp( + command_->twist.linear.z, min_velocities_[2], + max_velocities_[2]); + command_->twist.angular.x = std::clamp( + command_->twist.angular.x, min_velocities_[3], + max_velocities_[3]); + command_->twist.angular.y = std::clamp( + command_->twist.angular.y, min_velocities_[4], + max_velocities_[4]); + command_->twist.angular.z = std::clamp( + command_->twist.angular.z, min_velocities_[5], + max_velocities_[5]); + } // Find if any component is not within the acceleration constraints. If so, store the most // significant scale factor to apply to the vector , eta, to reduce all axes @@ -350,42 +376,113 @@ void VelocitySmoother::smootherTimer() double eta = 1.0; if (scale_velocities_) { double curr_eta = -1.0; - - curr_eta = findEtaConstraint( + if(!is_6dof_) { + curr_eta = findEtaConstraint( current_.twist.linear.x, command_->twist.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; - } + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } - curr_eta = findEtaConstraint( + curr_eta = findEtaConstraint( current_.twist.linear.y, command_->twist.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; - } + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } - curr_eta = findEtaConstraint( + curr_eta = findEtaConstraint( current_.twist.angular.z, command_->twist.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; + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + } else { + curr_eta = findEtaConstraint( + current_.twist.linear.x, command_->twist.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_.twist.linear.y, command_->twist.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_.twist.linear.z, command_->twist.linear.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; + } + + curr_eta = findEtaConstraint( + current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + curr_eta = findEtaConstraint( + current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + curr_eta = findEtaConstraint( + current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5]); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } } } - cmd_vel->twist.linear.x = applyConstraints( + if(!is_6dof_) { + cmd_vel->twist.linear.x = applyConstraints( current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); - cmd_vel->twist.linear.y = applyConstraints( + cmd_vel->twist.linear.y = applyConstraints( current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); - cmd_vel->twist.angular.z = applyConstraints( + cmd_vel->twist.angular.z = applyConstraints( current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta); + } else { + cmd_vel->twist.linear.x = applyConstraints( + current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); + cmd_vel->twist.linear.y = applyConstraints( + current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); + cmd_vel->twist.linear.z = applyConstraints( + current_.twist.linear.z, command_->twist.linear.z, max_accels_[2], max_decels_[2], eta); + cmd_vel->twist.angular.x = applyConstraints( + current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3], eta); + cmd_vel->twist.angular.y = applyConstraints( + current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4], eta); + cmd_vel->twist.angular.z = applyConstraints( + current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5], eta); + } + last_cmd_ = *cmd_vel; - // Apply deadband restrictions & publish - cmd_vel->twist.linear.x = - fabs(cmd_vel->twist.linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; - cmd_vel->twist.linear.y = - fabs(cmd_vel->twist.linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; - cmd_vel->twist.angular.z = - fabs(cmd_vel->twist.angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z; + // Apply deadband restrictions & publish + if(!is_6dof_) { + cmd_vel->twist.linear.x = + fabs(cmd_vel->twist.linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; + cmd_vel->twist.linear.y = + fabs(cmd_vel->twist.linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; + cmd_vel->twist.linear.z = command_->twist.linear.z; + cmd_vel->twist.angular.x = command_->twist.angular.x; + cmd_vel->twist.angular.y = command_->twist.angular.y; + cmd_vel->twist.angular.z = + fabs(cmd_vel->twist.angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->twist.angular.z; + } else { + cmd_vel->twist.linear.x = + fabs(cmd_vel->twist.linear.x) < deadband_velocities_[0] ? 0.0 : cmd_vel->twist.linear.x; + cmd_vel->twist.linear.y = + fabs(cmd_vel->twist.linear.y) < deadband_velocities_[1] ? 0.0 : cmd_vel->twist.linear.y; + cmd_vel->twist.linear.z = + fabs(cmd_vel->twist.linear.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->twist.linear.z; + cmd_vel->twist.angular.x = + fabs(cmd_vel->twist.angular.x) < deadband_velocities_[3] ? 0.0 : cmd_vel->twist.angular.x; + cmd_vel->twist.angular.y = + fabs(cmd_vel->twist.angular.y) < deadband_velocities_[4] ? 0.0 : cmd_vel->twist.angular.y; + cmd_vel->twist.angular.z = + fabs(cmd_vel->twist.angular.z) < deadband_velocities_[5] ? 0.0 : cmd_vel->twist.angular.z; + } smoothed_cmd_pub_->publish(std::move(cmd_vel)); } @@ -423,15 +520,16 @@ VelocitySmoother::dynamicParametersCallback(std::vector param shared_from_this(), odom_duration_, odom_topic_); } } else if (param_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", - param_name.c_str()); + size_t size = is_6dof_ ? 6 : 3; + if (parameter.as_double_array().size() != 3 && parameter.as_double_array().size() != size) { + RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size %ld", + param_name.c_str(), size); result.successful = false; break; } if (param_name == "max_velocity") { - for (unsigned int i = 0; i != 3; i++) { + for (unsigned int i = 0; i != size; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( get_logger(), @@ -443,7 +541,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param max_velocities_ = parameter.as_double_array(); } } else if (param_name == "min_velocity") { - for (unsigned int i = 0; i != 3; i++) { + for (unsigned int i = 0; i != size; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( get_logger(), @@ -455,7 +553,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param min_velocities_ = parameter.as_double_array(); } } else if (param_name == "max_accel") { - for (unsigned int i = 0; i != 3; i++) { + for (unsigned int i = 0; i != size; i++) { if (parameter.as_double_array()[i] < 0.0) { RCLCPP_WARN( get_logger(), @@ -467,7 +565,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param max_accels_ = parameter.as_double_array(); } } else if (param_name == "max_decel") { - for (unsigned int i = 0; i != 3; i++) { + for (unsigned int i = 0; i != size; i++) { if (parameter.as_double_array()[i] > 0.0) { RCLCPP_WARN( get_logger(), From 5f2d8171e61c1a87b5a73e00f11787c1f5a7b732 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 3 Jul 2025 11:03:24 +0000 Subject: [PATCH 2/7] Fix formatting and logic error Signed-off-by: mini-1235 --- .../src/velocity_smoother.cpp | 80 +++++++++---------- 1 file changed, 40 insertions(+), 40 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 63ce9032d2b..67155610701 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,7 +74,8 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); - for (unsigned int i = 0; i != 3; i++) { + size_t size = max_velocities_.size(); + for (unsigned int i = 0; i != size; i++) { if (max_decels_[i] > 0.0) { RCLCPP_ERROR( get_logger(), @@ -119,7 +120,6 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) node->get_parameter("deadband_velocity", deadband_velocities_); node->get_parameter("velocity_timeout", velocity_timeout_dbl); velocity_timeout_ = rclcpp::Duration::from_seconds(velocity_timeout_dbl); - size_t size = max_velocities_.size(); if ((size != 3 && size != 6) || min_velocities_.size() != size || @@ -130,7 +130,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_ERROR( get_logger(), "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 or 6"); + " All limits must be size of 3 (x, y, theta) or 6 (x, y, z, r, p, y)"); on_cleanup(state); return nav2::CallbackReturn::FAILURE; } @@ -339,33 +339,33 @@ void VelocitySmoother::smootherTimer() // Apply absolute velocity restrictions to the command if(!is_6dof_) { command_->twist.linear.x = std::clamp( - command_->twist.linear.x, min_velocities_[0], - max_velocities_[0]); + command_->twist.linear.x, min_velocities_[0], + max_velocities_[0]); command_->twist.linear.y = std::clamp( - command_->twist.linear.y, min_velocities_[1], - max_velocities_[1]); + command_->twist.linear.y, min_velocities_[1], + max_velocities_[1]); command_->twist.angular.z = std::clamp( - command_->twist.angular.z, min_velocities_[2], - max_velocities_[2]); + command_->twist.angular.z, min_velocities_[2], + max_velocities_[2]); } else { command_->twist.linear.x = std::clamp( - command_->twist.linear.x, min_velocities_[0], - max_velocities_[0]); + command_->twist.linear.x, min_velocities_[0], + max_velocities_[0]); command_->twist.linear.y = std::clamp( - command_->twist.linear.y, min_velocities_[1], - max_velocities_[1]); + command_->twist.linear.y, min_velocities_[1], + max_velocities_[1]); command_->twist.linear.z = std::clamp( - command_->twist.linear.z, min_velocities_[2], - max_velocities_[2]); + command_->twist.linear.z, min_velocities_[2], + max_velocities_[2]); command_->twist.angular.x = std::clamp( - command_->twist.angular.x, min_velocities_[3], - max_velocities_[3]); + command_->twist.angular.x, min_velocities_[3], + max_velocities_[3]); command_->twist.angular.y = std::clamp( - command_->twist.angular.y, min_velocities_[4], - max_velocities_[4]); + command_->twist.angular.y, min_velocities_[4], + max_velocities_[4]); command_->twist.angular.z = std::clamp( - command_->twist.angular.z, min_velocities_[5], - max_velocities_[5]); + command_->twist.angular.z, min_velocities_[5], + max_velocities_[5]); } // Find if any component is not within the acceleration constraints. If so, store the most @@ -378,55 +378,55 @@ void VelocitySmoother::smootherTimer() double curr_eta = -1.0; if(!is_6dof_) { curr_eta = findEtaConstraint( - current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0]); + current_.twist.linear.x, command_->twist.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_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1]); + current_.twist.linear.y, command_->twist.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_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2]); + current_.twist.angular.z, command_->twist.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; } } else { curr_eta = findEtaConstraint( - current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0]); + current_.twist.linear.x, command_->twist.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_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1]); + current_.twist.linear.y, command_->twist.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_.twist.linear.z, command_->twist.linear.z, max_accels_[2], max_decels_[2]); + current_.twist.linear.z, command_->twist.linear.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; } curr_eta = findEtaConstraint( - current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3]); + current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } curr_eta = findEtaConstraint( - current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4]); + current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } curr_eta = findEtaConstraint( - current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5]); + current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5]); if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { eta = curr_eta; } @@ -435,24 +435,24 @@ void VelocitySmoother::smootherTimer() if(!is_6dof_) { cmd_vel->twist.linear.x = applyConstraints( - current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); + current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); cmd_vel->twist.linear.y = applyConstraints( - current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); + current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); cmd_vel->twist.angular.z = applyConstraints( - current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta); + current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta); } else { cmd_vel->twist.linear.x = applyConstraints( - current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); + current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0], eta); cmd_vel->twist.linear.y = applyConstraints( - current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); + current_.twist.linear.y, command_->twist.linear.y, max_accels_[1], max_decels_[1], eta); cmd_vel->twist.linear.z = applyConstraints( - current_.twist.linear.z, command_->twist.linear.z, max_accels_[2], max_decels_[2], eta); + current_.twist.linear.z, command_->twist.linear.z, max_accels_[2], max_decels_[2], eta); cmd_vel->twist.angular.x = applyConstraints( - current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3], eta); + current_.twist.angular.x, command_->twist.angular.x, max_accels_[3], max_decels_[3], eta); cmd_vel->twist.angular.y = applyConstraints( - current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4], eta); + current_.twist.angular.y, command_->twist.angular.y, max_accels_[4], max_decels_[4], eta); cmd_vel->twist.angular.z = applyConstraints( - current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5], eta); + current_.twist.angular.z, command_->twist.angular.z, max_accels_[5], max_decels_[5], eta); } last_cmd_ = *cmd_vel; @@ -521,7 +521,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } } else if (param_type == ParameterType::PARAMETER_DOUBLE_ARRAY) { size_t size = is_6dof_ ? 6 : 3; - if (parameter.as_double_array().size() != 3 && parameter.as_double_array().size() != size) { + if (parameter.as_double_array().size() != size) { RCLCPP_WARN(get_logger(), "Invalid size of parameter %s. Must be size %ld", param_name.c_str(), size); result.successful = false; From 06e8b709658a43cf38e325429ec42da0e2da3eb2 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 3 Jul 2025 11:11:57 +0000 Subject: [PATCH 3/7] Check size first Signed-off-by: mini-1235 --- .../src/velocity_smoother.cpp | 54 ++++++++++--------- 1 file changed, 28 insertions(+), 26 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 67155610701..36d4c3bf72f 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,7 +74,35 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); + // 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{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); + + // Check if parameters are properly set size_t size = max_velocities_.size(); + + if ((size != 3 && size != 6) || + min_velocities_.size() != size || + max_accels_.size() != size || + max_decels_.size() != size || + deadband_velocities_.size() != size) + { + RCLCPP_ERROR( + get_logger(), + "Invalid setting of kinematic and/or deadband limits!" + " All limits must be size of 3 (x, y, theta) or 6 (x, y, z, r, p, y)"); + on_cleanup(state); + return nav2::CallbackReturn::FAILURE; + } + for (unsigned int i = 0; i != size; i++) { if (max_decels_[i] > 0.0) { RCLCPP_ERROR( @@ -109,32 +137,6 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) } } - // 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{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 ((size != 3 && size != 6) || - min_velocities_.size() != size || - max_accels_.size() != size || - max_decels_.size() != size || - deadband_velocities_.size() != size) - { - RCLCPP_ERROR( - get_logger(), - "Invalid setting of kinematic and/or deadband limits!" - " All limits must be size of 3 (x, y, theta) or 6 (x, y, z, r, p, y)"); - on_cleanup(state); - return nav2::CallbackReturn::FAILURE; - } - is_6dof_ = (size == 6); // Get control type if (feedback_type == "OPEN_LOOP") { From f28e59ef8bd65e4388d4183323b89457c4092c32 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 3 Jul 2025 11:37:43 +0000 Subject: [PATCH 4/7] Add tests for param size Signed-off-by: mini-1235 --- nav2_bringup/params/nav2_params.yaml | 211 ++++++++++-------- .../test/test_velocity_smoother.cpp | 32 +++ 2 files changed, 151 insertions(+), 92 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index cfbcdc858bf..6bf5b746d93 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -113,99 +113,126 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 FollowPath: - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - ay_min: -3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - publish_optimal_trajectory: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - AckermannConstraints: - min_turning_r: 0.2 - critics: - [ - "ConstraintCritic", - "CostCritic", - "GoalCritic", - "GoalAngleCritic", - "PathAlignCritic", - "PathFollowCritic", - "PathAngleCritic", - "PreferForwardCritic", - ] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - near_collision_cost: 253 - critical_cost: 300.0 - consider_footprint: false - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.25 + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + allow_reversing: false + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 + max_robot_pose_search_dist: 10.0 + stateful: true + + # FollowPath: + # plugin: "nav2_mppi_controller::MPPIController" + # time_steps: 56 + # model_dt: 0.05 + # batch_size: 2000 + # ax_max: 3.0 + # ax_min: -3.0 + # ay_max: 3.0 + # ay_min: -3.0 + # az_max: 3.5 + # vx_std: 0.2 + # vy_std: 0.2 + # wz_std: 0.4 + # vx_max: 0.5 + # vx_min: -0.35 + # vy_max: 0.5 + # wz_max: 1.9 + # iteration_count: 1 + # prune_distance: 1.7 + # transform_tolerance: 0.1 + # temperature: 0.3 + # gamma: 0.015 + # motion_model: "DiffDrive" + # visualize: true + # publish_optimal_trajectory: true + # regenerate_noises: true + # TrajectoryVisualizer: + # trajectory_step: 5 + # time_step: 3 + # AckermannConstraints: + # min_turning_r: 0.2 + # critics: + # [ + # "ConstraintCritic", + # "CostCritic", + # "GoalCritic", + # "GoalAngleCritic", + # "PathAlignCritic", + # "PathFollowCritic", + # "PathAngleCritic", + # "PreferForwardCritic", + # ] + # ConstraintCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 4.0 + # GoalCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # threshold_to_consider: 1.4 + # GoalAngleCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.0 + # threshold_to_consider: 0.5 + # PreferForwardCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # threshold_to_consider: 0.5 + # CostCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 3.81 + # near_collision_cost: 253 + # critical_cost: 300.0 + # consider_footprint: false + # collision_cost: 1000000.0 + # near_goal_distance: 1.0 + # trajectory_point_step: 2 + # PathAlignCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 14.0 + # max_path_occupancy_ratio: 0.05 + # trajectory_point_step: 4 + # threshold_to_consider: 0.5 + # offset_from_furthest: 20 + # use_path_orientations: false + # PathFollowCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 5.0 + # offset_from_furthest: 5 + # threshold_to_consider: 1.4 + # PathAngleCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 2.0 + # offset_from_furthest: 4 + # threshold_to_consider: 0.5 + # max_angle_to_furthest: 1.0 + # mode: 0 # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 190b7b5ef3e..f0773f613ff 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -618,6 +618,38 @@ TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) EXPECT_EQ(smoother->configure(state), nav2::CallbackReturn::FAILURE); } +TEST(VelocitySmootherTest, testDifferentParamsSize) { + auto smoother = + std::make_shared(); + + std::vector max_vel{0.5, 0.5, 0.5, 2.5, 2.5, 2.5}; + std::vector bad_min_vel{0.0, 0.0, 0.5, 2.5}; + std::vector accel{2.5, 2.5, 2.5, 5.0, 5.0, 5.0}; + std::vector decel{-2.5, -2.5, -2.5, -5.0, -5.0, -5.0}; + std::vector deadband_vel{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_min_vel)); + smoother->declare_parameter("max_accel", rclcpp::ParameterValue(accel)); + smoother->declare_parameter("min_decel", rclcpp::ParameterValue(decel)); + smoother->declare_parameter("deadband_velocity", rclcpp::ParameterValue(deadband_vel)); + + rclcpp_lifecycle::State state; + EXPECT_EQ(smoother->configure(state), nav2::CallbackReturn::FAILURE); +} + +TEST(VelocitySmootherTest, testInvalidParamsSize) { + auto smoother = + std::make_shared(); + + std::vector bad_max_vel{0.5, 0.5, 0.5, 2.5}; + std::vector bad_min_vel{0, 5, 0.5}; + + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_min_vel)); + rclcpp_lifecycle::State state; + EXPECT_EQ(smoother->configure(state), nav2::CallbackReturn::FAILURE); +} + TEST(VelocitySmootherTest, testDynamicParameter) { auto smoother = From 5f74c177fb01e9512f367c19b266137af8d476c7 Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 3 Jul 2025 13:51:24 +0000 Subject: [PATCH 5/7] Revert param changes Signed-off-by: mini-1235 --- nav2_bringup/params/nav2_params.yaml | 211 ++++++++++++--------------- 1 file changed, 92 insertions(+), 119 deletions(-) diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 6bf5b746d93..cfbcdc858bf 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -113,126 +113,99 @@ controller_server: xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 FollowPath: - plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 0.5 - lookahead_dist: 0.6 - min_lookahead_dist: 0.3 - max_lookahead_dist: 0.9 - lookahead_time: 1.5 - rotate_to_heading_angular_vel: 1.8 + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 56 + model_dt: 0.05 + batch_size: 2000 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + ay_min: -3.0 + az_max: 3.5 + vx_std: 0.2 + vy_std: 0.2 + wz_std: 0.4 + vx_max: 0.5 + vx_min: -0.35 + vy_max: 0.5 + wz_max: 1.9 + iteration_count: 1 + prune_distance: 1.7 transform_tolerance: 0.1 - use_velocity_scaled_lookahead_dist: false - min_approach_linear_velocity: 0.05 - approach_velocity_scaling_dist: 0.6 - use_collision_detection: true - max_allowed_time_to_collision_up_to_carrot: 1.0 - use_regulated_linear_velocity_scaling: true - use_fixed_curvature_lookahead: false - curvature_lookahead_dist: 0.25 - use_cost_regulated_linear_velocity_scaling: false - regulated_linear_scaling_min_radius: 0.9 - regulated_linear_scaling_min_speed: 0.25 - use_rotate_to_heading: true - allow_reversing: false - rotate_to_heading_min_angle: 0.785 - max_angular_accel: 3.2 - max_robot_pose_search_dist: 10.0 - stateful: true - - # FollowPath: - # plugin: "nav2_mppi_controller::MPPIController" - # time_steps: 56 - # model_dt: 0.05 - # batch_size: 2000 - # ax_max: 3.0 - # ax_min: -3.0 - # ay_max: 3.0 - # ay_min: -3.0 - # az_max: 3.5 - # vx_std: 0.2 - # vy_std: 0.2 - # wz_std: 0.4 - # vx_max: 0.5 - # vx_min: -0.35 - # vy_max: 0.5 - # wz_max: 1.9 - # iteration_count: 1 - # prune_distance: 1.7 - # transform_tolerance: 0.1 - # temperature: 0.3 - # gamma: 0.015 - # motion_model: "DiffDrive" - # visualize: true - # publish_optimal_trajectory: true - # regenerate_noises: true - # TrajectoryVisualizer: - # trajectory_step: 5 - # time_step: 3 - # AckermannConstraints: - # min_turning_r: 0.2 - # critics: - # [ - # "ConstraintCritic", - # "CostCritic", - # "GoalCritic", - # "GoalAngleCritic", - # "PathAlignCritic", - # "PathFollowCritic", - # "PathAngleCritic", - # "PreferForwardCritic", - # ] - # ConstraintCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 4.0 - # GoalCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 5.0 - # threshold_to_consider: 1.4 - # GoalAngleCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 3.0 - # threshold_to_consider: 0.5 - # PreferForwardCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 5.0 - # threshold_to_consider: 0.5 - # CostCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 3.81 - # near_collision_cost: 253 - # critical_cost: 300.0 - # consider_footprint: false - # collision_cost: 1000000.0 - # near_goal_distance: 1.0 - # trajectory_point_step: 2 - # PathAlignCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 14.0 - # max_path_occupancy_ratio: 0.05 - # trajectory_point_step: 4 - # threshold_to_consider: 0.5 - # offset_from_furthest: 20 - # use_path_orientations: false - # PathFollowCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 5.0 - # offset_from_furthest: 5 - # threshold_to_consider: 1.4 - # PathAngleCritic: - # enabled: true - # cost_power: 1 - # cost_weight: 2.0 - # offset_from_furthest: 4 - # threshold_to_consider: 0.5 - # max_angle_to_furthest: 1.0 - # mode: 0 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: true + publish_optimal_trajectory: true + regenerate_noises: true + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.2 + critics: + [ + "ConstraintCritic", + "CostCritic", + "GoalCritic", + "GoalAngleCritic", + "PathAlignCritic", + "PathFollowCritic", + "PathAngleCritic", + "PreferForwardCritic", + ] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + near_collision_cost: 253 + critical_cost: 300.0 + consider_footprint: false + collision_cost: 1000000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + mode: 0 # TwirlingCritic: # enabled: true # twirling_cost_power: 1 From c7d91778eeb2b6a29f26a22ac33f782d3896cf6c Mon Sep 17 00:00:00 2001 From: mini-1235 Date: Thu, 3 Jul 2025 15:13:14 +0000 Subject: [PATCH 6/7] Add more tests Signed-off-by: mini-1235 --- .../test/test_velocity_smoother.cpp | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index f0773f613ff..ae3f36aa1b7 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -51,6 +51,76 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother } }; +TEST(VelocitySmootherTest, openLoopTestTimer6dof) +{ + auto smoother = + std::make_shared(); + std::vector deadbands{0.2, 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector min_velocity{-0.5, -0.5, -0.5, -2.5, -2.5, -2.5}; + std::vector max_velocity{0.5, 0.5, 0.5, 2.5, 2.5, 2.5}; + std::vector max_decel{-2.5, -2.5, -2.5, -3.2, -3.2, -3.2}; + std::vector max_accel{2.5, 2.5, 2.5, 3.2, 3.2, 3.2}; + + 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)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(min_velocity)); + smoother->set_parameter(rclcpp::Parameter("min_velocity", min_velocity)); + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(max_velocity)); + smoother->set_parameter(rclcpp::Parameter("max_velocity", max_velocity)); + smoother->declare_parameter("max_decel", rclcpp::ParameterValue(max_decel)); + smoother->set_parameter(rclcpp::Parameter("max_decel", max_decel)); + smoother->declare_parameter("max_accel", rclcpp::ParameterValue(max_accel)); + smoother->set_parameter(rclcpp::Parameter("max_accel", max_accel)); + + rclcpp_lifecycle::State state; + smoother->configure(state); + smoother->activate(state); + + std::vector linear_vels; + auto subscription = nav2_util::TwistSubscriber( + smoother, + "cmd_vel_smoothed", + [&](geometry_msgs::msg::Twist::SharedPtr msg) { + linear_vels.push_back(msg->linear.x); + }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) { + linear_vels.push_back(msg->twist.linear.x); + }); + + // Send a velocity command + auto cmd = std::make_shared(); + cmd->twist.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, openLoopTestTimer) { auto smoother = From 3f8375b9fd735dd0df424bc6b84f2a54eea61be9 Mon Sep 17 00:00:00 2001 From: Maurice Date: Tue, 8 Jul 2025 17:08:28 +0800 Subject: [PATCH 7/7] Fix as suggested Signed-off-by: Maurice --- nav2_velocity_smoother/src/velocity_smoother.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 36d4c3bf72f..56e9c880a94 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -88,6 +88,7 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) // Check if parameters are properly set size_t size = max_velocities_.size(); + is_6dof_ = (size == 6); if ((size != 3 && size != 6) || min_velocities_.size() != size || @@ -137,7 +138,6 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State & state) } } - is_6dof_ = (size == 6); // Get control type if (feedback_type == "OPEN_LOOP") { open_loop_ = true;