include more testing

Signed-off-by: stevedan <[email protected]>

nav2_smac_planner/src/a_star.cpp

@@ -262,7 +262,7 @@ void AStarAlgorithm<NodeT>::setGoal(
   // we just have to check whether the x and y are the same because the dim3 is not used
   // in the computation of the obstacle heuristic
   if (!_search_info.cache_obstacle_heuristic ||
-    goals_coordinates != _goals_coordinates)
+    (goals_coordinates != _goals_coordinates))
   {
     if (!_start) {
       throw std::runtime_error("Start must be set before goal.");

nav2_smac_planner/test/test_a_star.cpp

@@ -400,10 +400,17 @@ TEST(AStarTest, test_se2_single_pose_path)
     };
   // functional case testing
   a_star.setCollisionChecker(checker.get());
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+
+  // test with no goals set nor start
+  EXPECT_THROW(
+    a_star.createPath(
+      path, num_it, tolerance,
+      dummy_cancel_checker), std::runtime_error);
+
   a_star.setStart(10u, 10u, 0u);
   // Goal is one costmap cell away
   a_star.setGoal(12u, 10u, 0u);
-  nav2_smac_planner::NodeHybrid::CoordinateVector path;
   EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker));
 
   // Check that the path is length one
@@ -491,9 +498,10 @@ TEST(AStarTest, test_goal_heading_mode)
   // UNKNOWN goal heading mode
   a_star.setCollisionChecker(checker.get());
   a_star.setStart(10u, 10u, 0u);
+
   EXPECT_THROW(
     a_star.setGoal(
-      80u, 80u, 40u,
+      80u, 80u, 10u,
       nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error);
 }