From c805c78569509e790ddfa5ade41326039f31c29d Mon Sep 17 00:00:00 2001 From: curt Date: Mon, 22 Sep 2025 11:31:21 +0200 Subject: [PATCH 1/5] make velocity smoother keep curvature Signed-off-by: Dominik Moss --- .../src/velocity_smoother.cpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 56e9c880a94..dc719a0fc28 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -274,12 +274,16 @@ double VelocitySmoother::findEtaConstraint( v_component_min = decel / smoothing_frequency_; } + if (v_cmd == 0.0) { + return -1.0; + } + if (dv > v_component_max) { - return v_component_max / dv; + return ( v_component_max + v_curr ) / v_cmd; } if (dv < v_component_min) { - return v_component_min / dv; + return ( v_component_min + v_curr ) / v_cmd; } return -1.0; @@ -289,7 +293,8 @@ 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 scaled = eta * v_cmd; + double diff = scaled - v_curr; double v_component_max; double v_component_min; @@ -305,7 +310,14 @@ double VelocitySmoother::applyConstraints( v_component_min = decel / smoothing_frequency_; } - return v_curr + std::clamp(eta * dv, v_component_min, v_component_max); + auto clamped_diff = std::clamp(diff, v_component_min, v_component_max); + auto restricted_command = v_curr + clamped_diff; + + if(std::abs(scaled - restricted_command) > 0.0001){ + RCLCPP_WARN(get_logger(), "Clamped velocity change: eta * v_cmd = %f, clamped to %f", scaled, restricted_command); + } + + return restricted_command; } void VelocitySmoother::smootherTimer() @@ -338,6 +350,9 @@ void VelocitySmoother::smootherTimer() current_ = odom_smoother_->getTwistStamped(); } + RCLCPP_INFO(get_logger(), "current (odom) velocity: [%f, %f, %f]", current_.twist.linear.x, current_.twist.linear.y, current_.twist.angular.z); + RCLCPP_INFO(get_logger(), "input velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); + // Apply absolute velocity restrictions to the command if(!is_6dof_) { command_->twist.linear.x = std::clamp( @@ -370,12 +385,15 @@ void VelocitySmoother::smootherTimer() max_velocities_[5]); } + RCLCPP_INFO(get_logger(), "clamped velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); + // 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 // 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; + RCLCPP_INFO(get_logger(), "scale_velocities_ = %d", (int) scale_velocities_); if (scale_velocities_) { double curr_eta = -1.0; if(!is_6dof_) { From c77cfd2e004f468bf254ff1036225d7b2e3b5f4e Mon Sep 17 00:00:00 2001 From: Dominik Moss Date: Mon, 13 Oct 2025 13:22:01 +0200 Subject: [PATCH 2/5] Cleanup and log levels Signed-off-by: Dominik Moss --- .../src/velocity_smoother.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index dc719a0fc28..570c74b1e37 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -293,8 +293,8 @@ double VelocitySmoother::applyConstraints( const double v_curr, const double v_cmd, const double accel, const double decel, const double eta) { - double scaled = eta * v_cmd; - double diff = scaled - v_curr; + double v_scaled = eta * v_cmd; + double v_diff = v_scaled - v_curr; double v_component_max; double v_component_min; @@ -310,14 +310,14 @@ double VelocitySmoother::applyConstraints( v_component_min = decel / smoothing_frequency_; } - auto clamped_diff = std::clamp(diff, v_component_min, v_component_max); - auto restricted_command = v_curr + clamped_diff; + auto v_diff_clamped = std::clamp(v_diff, v_component_min, v_component_max); + auto v_cmd_restricted = v_curr + v_diff_clamped; - if(std::abs(scaled - restricted_command) > 0.0001){ - RCLCPP_WARN(get_logger(), "Clamped velocity change: eta * v_cmd = %f, clamped to %f", scaled, restricted_command); + if(std::abs(v_scaled - v_cmd_restricted) > 0.0001){ + RCLCPP_DEBUG(get_logger(), "Clamped velocity change: eta * v_cmd = %f, clamped to %f", v_scaled, v_cmd_restricted); } - return restricted_command; + return v_cmd_restricted; } void VelocitySmoother::smootherTimer() @@ -350,8 +350,8 @@ void VelocitySmoother::smootherTimer() current_ = odom_smoother_->getTwistStamped(); } - RCLCPP_INFO(get_logger(), "current (odom) velocity: [%f, %f, %f]", current_.twist.linear.x, current_.twist.linear.y, current_.twist.angular.z); - RCLCPP_INFO(get_logger(), "input velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); + RCLCPP_DEBUG(get_logger(), "Current (odom) velocity: [%f, %f, %f]", current_.twist.linear.x, current_.twist.linear.y, current_.twist.angular.z); + RCLCPP_DEBUG(get_logger(), "Input velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); // Apply absolute velocity restrictions to the command if(!is_6dof_) { @@ -385,7 +385,7 @@ void VelocitySmoother::smootherTimer() max_velocities_[5]); } - RCLCPP_INFO(get_logger(), "clamped velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); + RCLCPP_DEBUG(get_logger(), "Clamped velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); // 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 @@ -393,7 +393,6 @@ void VelocitySmoother::smootherTimer() // 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; - RCLCPP_INFO(get_logger(), "scale_velocities_ = %d", (int) scale_velocities_); if (scale_velocities_) { double curr_eta = -1.0; if(!is_6dof_) { @@ -452,6 +451,7 @@ void VelocitySmoother::smootherTimer() } } } + RCLCPP_DEBUG(get_logger(), "Eta = %f", eta); if(!is_6dof_) { cmd_vel->twist.linear.x = applyConstraints( From ccd288ac5abb05919aa7682f368a0cf8dcbe4178 Mon Sep 17 00:00:00 2001 From: Dominik Moss Date: Tue, 14 Oct 2025 10:13:57 +0200 Subject: [PATCH 3/5] Removed debugging output, used epsilon instead of zero check Signed-off-by: Dominik Moss --- nav2_velocity_smoother/src/velocity_smoother.cpp | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 570c74b1e37..b954c94cd7d 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -274,7 +274,7 @@ double VelocitySmoother::findEtaConstraint( v_component_min = decel / smoothing_frequency_; } - if (v_cmd == 0.0) { + if (std::abs(v_cmd) < 1e-6) { return -1.0; } @@ -313,10 +313,6 @@ double VelocitySmoother::applyConstraints( auto v_diff_clamped = std::clamp(v_diff, v_component_min, v_component_max); auto v_cmd_restricted = v_curr + v_diff_clamped; - if(std::abs(v_scaled - v_cmd_restricted) > 0.0001){ - RCLCPP_DEBUG(get_logger(), "Clamped velocity change: eta * v_cmd = %f, clamped to %f", v_scaled, v_cmd_restricted); - } - return v_cmd_restricted; } @@ -350,9 +346,6 @@ void VelocitySmoother::smootherTimer() current_ = odom_smoother_->getTwistStamped(); } - RCLCPP_DEBUG(get_logger(), "Current (odom) velocity: [%f, %f, %f]", current_.twist.linear.x, current_.twist.linear.y, current_.twist.angular.z); - RCLCPP_DEBUG(get_logger(), "Input velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); - // Apply absolute velocity restrictions to the command if(!is_6dof_) { command_->twist.linear.x = std::clamp( @@ -385,8 +378,6 @@ void VelocitySmoother::smootherTimer() max_velocities_[5]); } - RCLCPP_DEBUG(get_logger(), "Clamped velocity: [%f, %f, %f]", command_->twist.linear.x, command_->twist.linear.y, command_->twist.angular.z); - // 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 // proportionally to follow the same direction, within change of velocity bounds. @@ -451,7 +442,6 @@ void VelocitySmoother::smootherTimer() } } } - RCLCPP_DEBUG(get_logger(), "Eta = %f", eta); if(!is_6dof_) { cmd_vel->twist.linear.x = applyConstraints( From c23a8f09d7bfc83cfa37f6328122ca42ae4248fa Mon Sep 17 00:00:00 2001 From: DMO Date: Sun, 19 Oct 2025 21:09:30 +0200 Subject: [PATCH 4/5] Updated comments and return positive 1 in eta calculation Signed-off-by: DMO --- nav2_velocity_smoother/src/velocity_smoother.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index b954c94cd7d..23f44a7734f 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -275,7 +275,9 @@ double VelocitySmoother::findEtaConstraint( } if (std::abs(v_cmd) < 1e-6) { - return -1.0; + // This avoids generating an eta of inf, which would result in sudden accelerations + // of the other factors. + return 1.0; } if (dv > v_component_max) { @@ -285,8 +287,8 @@ double VelocitySmoother::findEtaConstraint( if (dv < v_component_min) { return ( v_component_min + v_curr ) / v_cmd; } - - return -1.0; + // no acceleration limit exceeded, so return "no change" + return 1.0; } double VelocitySmoother::applyConstraints( @@ -386,6 +388,14 @@ void VelocitySmoother::smootherTimer() double eta = 1.0; if (scale_velocities_) { double curr_eta = -1.0; + // When a deceleration is limited, eta would be > 1.0. + // This would lead to a acceleration of the other vector elements, + // which is not intended with this smoother at the moment. + // When the commanded twist is changing the curvature sign, but the acceleration + // limits prevents this sign switch, eta would be negative. + // This would lead to surprising accelerations or deceleration of the other vector elements, + // which is also not intended with this smoother. + // So eta is limited to [0.0 : 1.0] if(!is_6dof_) { curr_eta = findEtaConstraint( current_.twist.linear.x, command_->twist.linear.x, max_accels_[0], max_decels_[0]); From f1aa29333280aaf2b2d0bde46d72a77e006d4e27 Mon Sep 17 00:00:00 2001 From: DMO Date: Sun, 19 Oct 2025 21:33:29 +0200 Subject: [PATCH 5/5] Added tests for curvature scaling Signed-off-by: DMO --- .../test/test_velocity_smoother.cpp | 287 ++++++++++++++++-- 1 file changed, 261 insertions(+), 26 deletions(-) diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 8de8bd7e9a7..f545d9ce903 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -263,51 +263,51 @@ TEST(VelocitySmootherTest, testfindEtaConstraint) // In range // Constant positive - EXPECT_EQ(smoother->findEtaConstraint(1.0, 1.0, accel, decel), -1); + EXPECT_EQ(smoother->findEtaConstraint(1.0, 1.0, accel, decel), 1); // Constant negative - EXPECT_EQ(smoother->findEtaConstraint(-1.0, -1.0, accel, decel), -1); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + 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); + EXPECT_NEAR(smoother->findEtaConstraint(0.5, 1.5, accel, decel), 0.3366, 0.01); // Positive To Positive Decel - EXPECT_EQ(smoother->findEtaConstraint(1.5, 0.5, accel, decel), 0.05); + EXPECT_NEAR(smoother->findEtaConstraint(1.5, 0.5, accel, decel), 2.9, 0.01); // 0 To Positive Accel - EXPECT_EQ(smoother->findEtaConstraint(0.0, 1.0, accel, decel), 0.005); + EXPECT_NEAR(smoother->findEtaConstraint(0.0, 1.0, accel, decel), 0.005, 0.01); // Positive To 0 Decel - EXPECT_EQ(smoother->findEtaConstraint(1.0, 0.0, accel, decel), 0.05); + EXPECT_EQ(smoother->findEtaConstraint(1.0, 0.0, accel, decel), 1); // Negative To Negative Accel - EXPECT_EQ(smoother->findEtaConstraint(-0.5, -1.5, accel, decel), 0.005); + EXPECT_NEAR(smoother->findEtaConstraint(-0.5, -1.5, accel, decel), 0.3366, 0.01); // Negative To Negative Decel - EXPECT_EQ(smoother->findEtaConstraint(-1.5, -0.5, accel, decel), 0.05); + EXPECT_NEAR(smoother->findEtaConstraint(-1.5, -0.5, accel, decel), 2.9, 0.01); // 0 To Negative Accel - EXPECT_EQ(smoother->findEtaConstraint(0.0, -1.0, accel, decel), 0.005); + EXPECT_NEAR(smoother->findEtaConstraint(0.0, -1.0, accel, decel), 0.005, 0.01); // Negative To 0 Decel - EXPECT_EQ(smoother->findEtaConstraint(-1.0, 0.0, accel, decel), 0.05); + EXPECT_EQ(smoother->findEtaConstraint(-1.0, 0.0, accel, decel), 1); // Negative to Positive - EXPECT_EQ(smoother->findEtaConstraint(-0.2, 0.8, accel, decel), 0.05); + EXPECT_NEAR(smoother->findEtaConstraint(-0.2, 0.8, accel, decel), -0.1875, 0.01); // Positive to Negative - EXPECT_EQ(smoother->findEtaConstraint(0.2, -0.8, accel, decel), 0.05); + EXPECT_NEAR(smoother->findEtaConstraint(0.2, -0.8, accel, decel), -0.1875, 0.01); } TEST(VelocitySmootherTest, testapplyConstraints) @@ -327,12 +327,14 @@ TEST(VelocitySmootherTest, testapplyConstraints) // 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); + EXPECT_NEAR(smoother->applyConstraints(1.0, 2.0, 1.5, -2.0, 0.75), 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); + EXPECT_NEAR(smoother->applyConstraints(0.5, 0.75, 1.5, -2.0, 0.75), 0.5625, 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); + // In a more realistic situation, applied to angular, too high + EXPECT_NEAR(smoother->applyConstraints(0.8, 2.0, 3.2, -3.2, 0.75), 0.96, 0.01); + // In a more realistic situation, applied to angular, OK + EXPECT_NEAR(smoother->applyConstraints(0.8, 1.2, 3.2, -3.2, 0.75), 0.9, 0.01); } TEST(VelocitySmootherTest, testapplyConstraintsPositiveToPositiveAccel) @@ -816,6 +818,239 @@ TEST(VelocitySmootherTest, testDynamicParameter) smoother.reset(); } +TEST(VelocitySmootherTest, testCurvatureUpdatingOmegaLimit) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + // default frequency is 20.0 + smoother->configure(state); + double eta = 1.0; + double curr_eta = 1.0; + double vel_accel_max = 2000.0; + double vel_accel_min = -2000.0; + double omega_accel_max = 10.0; + double omega_accel_min = -10.0; + double vel_limited; + double omega_limited; + + // Omega_curr is zero + double vel_curr = 1.5; + double omega_curr = 0.0; + double vel_cmd = 2.0; + double omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + EXPECT_NEAR(eta, 0.3333, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 0.6666, 0.001); + EXPECT_NEAR(omega_limited, 0.5, 0.001); + + EXPECT_NEAR(vel_cmd / omega_cmd, vel_limited / omega_limited, 0.001); + EXPECT_NEAR(omega_cmd / vel_cmd, omega_limited / vel_limited, 0.001); + + // Omega_cmd is zero + eta = 1.0; + vel_curr = 2.0; + omega_curr = -1.5; + vel_cmd = 1.5; + omega_cmd = 0.0; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + // No limitation, because eta is limited (<= 1.0) + EXPECT_NEAR(eta, 1.0, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 1.5, 0.001); + EXPECT_NEAR(omega_limited, -1.0, 0.001); + // Curvature is not fully updated, since eta is limited. + EXPECT_GT(vel_cmd / omega_cmd, vel_limited / omega_limited); + EXPECT_GT(omega_cmd / vel_cmd, omega_limited / vel_limited); + // Velocity sign switch + eta = 1.0; + vel_curr = -2.0; + omega_curr = 0.0; + vel_cmd = 2.0; + omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + EXPECT_NEAR(eta, 0.3333, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 0.6666, 0.001); + EXPECT_NEAR(omega_limited, 0.5, 0.001); + + EXPECT_NEAR(vel_cmd / omega_cmd, vel_limited / omega_limited, 0.001); + EXPECT_NEAR(omega_cmd / vel_cmd, omega_limited / vel_limited, 0.001); + + // Omega sign switch + eta = 1.0; + vel_curr = 2.0; + omega_curr = -1.0; + vel_cmd = 1.0; + omega_cmd = 1.0; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + // No limitations, since eta is limited (>= 0.0) + EXPECT_NEAR(eta, 1.0, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 1.0, 0.001); + EXPECT_NEAR(omega_limited, -0.5, 0.001); + + // Curvature is not fully updated, since eta is limited. + EXPECT_TRUE(vel_limited / omega_limited < 0.0); + EXPECT_TRUE(omega_limited / vel_limited < 0.0); +} + +TEST(VelocitySmootherTest, testCurvatureUpdatingVelocityLimit) +{ + auto smoother = + std::make_shared(); + rclcpp_lifecycle::State state; + // default frequency is 20.0 + smoother->configure(state); + double eta = 1.0; + double curr_eta = 1.0; + double vel_accel_max = 2.0; + double vel_accel_min = -2.0; + double omega_accel_max = 20.0; + double omega_accel_min = -20.0; + double vel_limited; + double omega_limited; + + // Vel_curr is zero + double vel_curr = 0.0; + double omega_curr = 0.5; + double vel_cmd = 2.0; + double omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + EXPECT_NEAR(eta, 0.05, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 0.1, 0.001); + EXPECT_NEAR(omega_limited, 0.075, 0.001); + + EXPECT_NEAR(vel_cmd / omega_cmd, vel_limited / omega_limited, 0.001); + EXPECT_NEAR(omega_cmd / vel_cmd, omega_limited / vel_limited, 0.001); + + // Vel_cmd is zero + eta = 1.0; + vel_curr = -1.0; + omega_curr = 0.5; + vel_cmd = 0.0; + omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + // No limitation, because eta is limited (<= 1.0) + EXPECT_NEAR(eta, 1.0, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, -0.90, 0.001); + EXPECT_NEAR(omega_limited, 1.5, 0.001); + // No limitation, so curvature too low and negative + EXPECT_GT(vel_cmd / omega_cmd, vel_limited / omega_limited); + EXPECT_GT(omega_cmd / vel_cmd, omega_limited / vel_limited); + + // velocity sign change + eta = 1.0; + vel_curr = -0.05; + omega_curr = 0.5; + vel_cmd = 0.1; + omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + EXPECT_NEAR(eta, 0.5, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 0.05, 0.001); + EXPECT_NEAR(omega_limited, 0.75, 0.001); + + EXPECT_NEAR(vel_cmd / omega_cmd, vel_limited / omega_limited, 0.01); + EXPECT_NEAR(omega_cmd / vel_cmd, omega_limited / vel_limited, 0.01); + + // Velocity sign change + eta = 1.0; + vel_curr = 2.0; + omega_curr = 0.5; + vel_cmd = -2.0; + omega_cmd = 1.5; + + curr_eta = smoother->findEtaConstraint(vel_curr, vel_cmd, vel_accel_max, vel_accel_min); + EXPECT_LT(curr_eta, 0.0); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + curr_eta = smoother->findEtaConstraint(omega_curr, omega_cmd, omega_accel_max, omega_accel_min); + if (curr_eta > 0.0 && std::fabs(1.0 - curr_eta) > std::fabs(1.0 - eta)) { + eta = curr_eta; + } + + EXPECT_NEAR(eta, 1.0, 0.001); + vel_limited = smoother->applyConstraints(vel_curr, vel_cmd, vel_accel_max, vel_accel_min, eta); + omega_limited = smoother->applyConstraints(omega_curr, omega_cmd, omega_accel_max, omega_accel_min, eta); + EXPECT_NEAR(vel_limited, 1.9, 0.001); + EXPECT_NEAR(omega_limited, 1.5, 0.001); + // Curvature cannot be maintained + EXPECT_LT(vel_cmd / omega_cmd, vel_limited / omega_limited); + EXPECT_LT(omega_cmd / vel_cmd, omega_limited / vel_limited); +} + + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv);