From de2716fc32ef9c92e800aa1cbc543b4da5562d83 Mon Sep 17 00:00:00 2001 From: stevedan Date: Mon, 25 Mar 2024 21:07:48 +0100 Subject: [PATCH] minor imporvement --- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/test/test_a_star.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index ce823210aa2..e9529bbdca1 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -297,7 +297,7 @@ bool AStarAlgorithm::areInputsValid() // Check if ending point is valid if (getToleranceHeuristic() < 0.001) { // if any goal is valid, then all goals are valid - auto goal = *_goalsSet.begin(); + auto goal = *_goalsSet.begin(); if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 4c772debcaa..55c42a43662 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -351,7 +351,7 @@ TEST(AStarTest, test_se2_single_pose_path) delete costmapA; } -TEST(AStarTest, test_goal_heading_goals) +TEST(AStarTest, test_goal_heading_mode) { auto lnode = std::make_shared("test"); nav2_smac_planner::SearchInfo info; @@ -373,6 +373,7 @@ TEST(AStarTest, test_goal_heading_goals) double max_planning_time = 120.0; int num_it = 0; + // BIDIRECTIONAL goal heading mode a_star.initialize( false, max_iterations, it_on_approach, terminal_checking_interval, max_planning_time, 401, size_theta, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); @@ -390,7 +391,6 @@ TEST(AStarTest, test_goal_heading_goals) std::make_unique(costmap_ros, size_theta, lnode); checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - // BIDIRECTIONAL goal heading mode a_star.setCollisionChecker(checker.get()); a_star.setStart(10u, 10u, 0u); a_star.setGoal(80u, 80u, 40u);