include testing

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
ros-navigation · Nov 18, 2024 · 44ec788 · 44ec788
1 parent 38b94ec
commit 44ec788
Show file tree

Hide file tree

Showing 4 changed files with 39 additions and 1 deletion.
diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -672,6 +672,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "Unable to get GoalHeader type. Given '%s', "
             "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
             goal_heading_type.c_str());
+          throw nav2_core::PlannerException("Invalid GoalHeadingMode type");
         } else {
           _goal_heading_mode = goal_heading_mode;
         }

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -566,6 +566,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
             "Unable to get GoalHeader type. Given '%s', "
             "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ",
             goal_heading_type.c_str());
+          throw nav2_core::PlannerException("Invalid GoalHeadingMode type");
         } else {
           _goal_heading_mode = goal_heading_mode;
         }

diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp
@@ -37,6 +37,16 @@ class RclCppFixture
 };
 RclCppFixture g_rclcppfixture;
 
+// Simple wrapper to be able to call a private member
+class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid
+{
+public:
+  void callDynamicParams(std::vector<rclcpp::Parameter> parameters)
+  {
+    dynamicParametersCallback(parameters);
+  }
+};
+
 // SMAC smoke tests for plugin-level issues rather than algorithms
 // (covered by more extensively testing in other files)
 // System tests in nav2_system_tests will actually plan with this work
@@ -68,6 +78,14 @@ TEST(SmacTest, test_smac_se2)
   goal.pose.position.y = 1.0;
   goal.pose.orientation.w = 1.0;
   auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+
+  // invalid goal heading mode
+  nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN"));
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error);
+
+  // valid goal heading mode
+  nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -94,7 +112,7 @@ TEST(SmacTest, test_smac_se2_reconfigure)
     std::make_shared<nav2_costmap_2d::Costmap2DROS>("global_costmap");
   costmap_ros->on_configure(rclcpp_lifecycle::State());
 
-  auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>();
+  auto planner = std::make_unique<HybridWrap>();
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
@@ -166,4 +184,9 @@ TEST(SmacTest, test_smac_se2_reconfigure)
     nodeSE2->get_node_base_interface(),
     results2);
   EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2);
+
+  // Test reconfigure with invalid goal heading mode
+  std::vector<rclcpp::Parameter> parameters;
+  parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
 }
diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp
@@ -73,6 +73,14 @@ TEST(SmacTest, test_smac_lattice)
   goal.pose.orientation.w = 1.0;
   auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>();
   try {
+    // invalid goal heading mode
+    nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN"));
+    nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+    EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error);
+
+    // valid goal heading mode
+    nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT")));
+
     // Expect to throw due to invalid prims file in param
     planner->configure(nodeLattice, "test", nullptr, costmap_ros);
   } catch (...) {
@@ -150,4 +158,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
   std::vector<rclcpp::Parameter> parameters;
   parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI")));
   EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
+
+  // same with invalid goal heading mode
+  parameters.clear();
+  parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN")));
+  EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error);
 }