diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 2596c073ba..bc101fd732 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -74,11 +74,18 @@ VelocitySmoother::on_configure(const rclcpp_lifecycle::State &) node->get_parameter("max_accel", max_accels_); node->get_parameter("max_decel", max_decels_); - for (unsigned int i = 0; i != max_decels_.size(); i++) { + for (unsigned int i = 0; i != 3; i++) { if (max_decels_[i] > 0.0) { - RCLCPP_WARN( - get_logger(), - "Positive values set of deceleration! These should be negative to slow down!"); + throw std::runtime_error( + "Positive values set of deceleration! These should be negative to slow down!"); + } + if (max_accels_[i] < 0.0) { + throw std::runtime_error( + "Negative values set of acceleration! These should be positive to speed up!"); + } + if (min_velocities_[i] > max_velocities_[i]) { + throw std::runtime_error( + "Min velocities are higher than max velocities!"); } } @@ -358,8 +365,24 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } else if (name == "min_velocity") { min_velocities_ = parameter.as_double_array(); } else if (name == "max_accel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] < 0.0) { + RCLCPP_WARN( + get_logger(), + "Negative values set of acceleration! These should be positive to speed up!"); + result.successful = false; + } + } max_accels_ = parameter.as_double_array(); } else if (name == "max_decel") { + for (unsigned int i = 0; i != 3; i++) { + if (parameter.as_double_array()[i] > 0.0) { + RCLCPP_WARN( + get_logger(), + "Positive values set of deceleration! These should be negative to slow down!"); + result.successful = false; + } + } max_decels_ = parameter.as_double_array(); } else if (name == "deadband_velocity") { deadband_velocities_ = parameter.as_double_array(); diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index a1b2f5454a..c58e20b22c 100644 --- a/nav2_velocity_smoother/test/test_velocity_smoother.cpp +++ b/nav2_velocity_smoother/test/test_velocity_smoother.cpp @@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams) EXPECT_THROW(smoother->configure(state), std::runtime_error); } +TEST(VelocitySmootherTest, testInvalidParamsAccelDecel) +{ + auto smoother = + std::make_shared(); + + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; + std::vector bad_test_min_vel{10.0, 10.0, 10.0}; + std::vector bad_test_max_vel{-10.0, -10.0, -10.0}; + + smoother->declare_parameter("max_velocity", rclcpp::ParameterValue(bad_test_max_vel)); + smoother->declare_parameter("min_velocity", rclcpp::ParameterValue(bad_test_min_vel)); + rclcpp_lifecycle::State state; + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_accel", rclcpp::ParameterValue(bad_test_accel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); + + smoother->set_parameter(rclcpp::Parameter("max_decel", rclcpp::ParameterValue(bad_test_decel))); + EXPECT_THROW(smoother->configure(state), std::runtime_error); +} + TEST(VelocitySmootherTest, testDynamicParameter) { auto smoother = @@ -613,6 +635,8 @@ TEST(VelocitySmootherTest, testDynamicParameter) std::vector min_accel{0.0, 0.0, 0.0}; std::vector deadband{0.0, 0.0, 0.0}; std::vector bad_test{0.0, 0.0}; + std::vector bad_test_accel{-10.0, -10.0, -10.0}; + std::vector bad_test_decel{10.0, 10.0, 10.0}; auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("smoothing_frequency", 100.0), @@ -662,6 +686,16 @@ TEST(VelocitySmootherTest, testDynamicParameter) rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); EXPECT_FALSE(results.get().successful); + // Test invalid accel / decel + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_accel", bad_test_accel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("max_decel", bad_test_decel)}); + rclcpp::spin_until_future_complete(smoother->get_node_base_interface(), results); + EXPECT_FALSE(results.get().successful); + // test full state after major changes smoother->deactivate(state); smoother->cleanup(state);