adding checks on config and dynamic parameters for proper velocity an…

…d acceleration limits (#3731)
ros-navigation · Aug 2, 2023 · 78893d4 · 78893d4
1 parent afda75f
commit 78893d4
Show file tree

Hide file tree

Showing 2 changed files with 61 additions and 4 deletions.
diff --git 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<rclcpp::Parameter> 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
@@ -593,6 +593,28 @@ TEST(VelocitySmootherTest, testInvalidParams)
   EXPECT_THROW(smoother->configure(state), std::runtime_error);
 }
 
+TEST(VelocitySmootherTest, testInvalidParamsAccelDecel)
+{
+  auto smoother =
+    std::make_shared<VelSmootherShim>();
+
+  std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
+  std::vector<double> bad_test_decel{10.0, 10.0, 10.0};
+  std::vector<double> bad_test_min_vel{10.0, 10.0, 10.0};
+  std::vector<double> 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<double> min_accel{0.0, 0.0, 0.0};
   std::vector<double> deadband{0.0, 0.0, 0.0};
   std::vector<double> bad_test{0.0, 0.0};
+  std::vector<double> bad_test_accel{-10.0, -10.0, -10.0};
+  std::vector<double> 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);