Skip to content

Add 6dof support for velocity smoother #5243

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 7 commits into from
Jul 10, 2025
Merged

Add 6dof support for velocity smoother #5243

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
bcc7f3a
Add 6dof support for velocity smoother
mini-1235 Jun 6, 2025
5f2d817
Fix formatting and logic error
mini-1235 Jul 3, 2025
06e8b70
Check size first
mini-1235 Jul 3, 2025
f28e59e
Add tests for param size
mini-1235 Jul 3, 2025
5f74c17
Revert param changes
mini-1235 Jul 3, 2025
c7d9177
Add more tests
mini-1235 Jul 3, 2025
3f8375b
Fix as suggested
mini-1235 Jul 8, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_velocity_smoother/include/nav2_velocity_smoother/velocity_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,7 @@ class VelocitySmoother : public nav2::LifecycleNode
bool open_loop_;
bool stopped_{true};
bool scale_velocities_;
bool is_6dof_;
std::vector<double> max_velocities_;
std::vector<double> min_velocities_;
std::vector<double> max_accels_;
Expand Down
234 changes: 167 additions & 67 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,37 @@ 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++) {
// 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);

// 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 ||
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(
get_logger(),
Expand Down Expand Up @@ -108,29 +138,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<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)
{
RCLCPP_ERROR(
get_logger(),
"Invalid setting of kinematic and/or deadband limits!"
" All limits must be size of 3 representing (x, y, theta).");
on_cleanup(state);
return nav2::CallbackReturn::FAILURE;
}

// Get control type
if (feedback_type == "OPEN_LOOP") {
open_loop_ = true;
Expand Down Expand Up @@ -332,15 +339,36 @@ void VelocitySmoother::smootherTimer()
}

// Apply absolute velocity restrictions to the command
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.angular.z = std::clamp(
command_->twist.angular.z, min_velocities_[2],
max_velocities_[2]);
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, min_velocities_[1],
max_velocities_[1]);
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 <dvx, dvy, dvw>, eta, to reduce all axes
Expand All @@ -350,42 +378,113 @@ void VelocitySmoother::smootherTimer()
double eta = 1.0;
if (scale_velocities_) {
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]);
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.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.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]);
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.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;
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(
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.angular.z = applyConstraints(
current_.twist.angular.z, command_->twist.angular.z, max_accels_[2], max_decels_[2], eta);
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(
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);
} 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));
}

Expand Down Expand Up @@ -423,15 +522,16 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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() != 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(),
Expand All @@ -443,7 +543,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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(),
Expand All @@ -455,7 +555,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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(),
Expand All @@ -467,7 +567,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector<rclcpp::Parameter> 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(),
Expand Down
Loading
Loading