diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 1c42ff44fb2..04a32081a2a 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -182,9 +182,21 @@ double VelocitySmoother::findEtaConstraint( const double v_curr, const double v_cmd, const double accel, const double decel) { // Exploiting vector scaling properties - const double v_component_max = accel / smoothing_frequency_; - const double v_component_min = decel / smoothing_frequency_; - const double dv = v_cmd - v_curr; + 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; @@ -202,8 +214,21 @@ double VelocitySmoother::applyConstraints( const double accel, const double decel, const double eta) { double dv = v_cmd - v_curr; - const double v_component_max = accel / smoothing_frequency_; - const double v_component_min = decel / smoothing_frequency_; + + 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); } diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index dcc82c31ac7..a1b2f5454af 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -179,17 +179,56 @@ TEST(VelocitySmootherTest, testfindEtaConstraint) // default frequency is 20.0 smoother->configure(state); + double accel = 0.1; // dv 0.005 + double decel = -1.0; // dv 0.05 + // In range - EXPECT_EQ(smoother->findEtaConstraint(1.0, 1.0, 1.5, -2.0), -1); - EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.55, 1.5, -2.0), -1); - EXPECT_EQ(smoother->findEtaConstraint(0.5, 0.45, 1.5, -2.0), -1); - // Too high - EXPECT_EQ(smoother->findEtaConstraint(1.0, 2.0, 1.5, -2.0), 0.075); - // Too low - EXPECT_EQ(smoother->findEtaConstraint(1.0, 0.0, 1.5, -2.0), 0.1); - - // In a more realistic situation accelerating linear axis - EXPECT_NEAR(smoother->findEtaConstraint(0.40, 0.50, 1.5, -2.0), 0.75, 0.001); + // 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) @@ -217,6 +256,300 @@ TEST(VelocitySmootherTest, testapplyConstraints) 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(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(); + 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(); + 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(); + 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(); + 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 =