diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index c65119698a..2ee8d69f53 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -147,12 +147,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (tolerance_ == 0 && costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 650d8d4adf..e4ecf38f03 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -245,6 +245,11 @@ class AStarAlgorithm inline void populateExpansionsLog( const NodePtr & node, std::vector> * expansions_log); + /** + * @brief Clear Start + */ + void clearStart(); + int _timing_interval = 5000; bool _traverse_unknown; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index d6c9db69bb..55fe35e531 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -234,10 +234,8 @@ bool AStarAlgorithm::areInputsValid() throw nav2_core::GoalOccupied("Goal was in lethal cost"); } - // Check if starting point is valid - if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } + // Note: We do not check the if the start is valid because it is cleared + clearStart(); return true; } @@ -465,6 +463,20 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template<> +void AStarAlgorithm::clearStart() +{ + auto coords = Node2D::getCoords(_start->getIndex()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + +template +void AStarAlgorithm::clearStart() +{ + auto coords = NodeT::getCoords(_start->getIndex(), _costmap->getSizeInCellsX(), getSizeDim3()); + _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 94b8ee1306..77650a61f4 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -240,9 +240,6 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( // Corner case of start and goal beeing on the same cell if (mx_start == mx_goal && my_start == my_goal) { - if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied("Start was in lethal cost"); - } pose.pose = start.pose; // if we have a different start and goal orientation, set the unique path pose to the goal // orientation, unless use_final_approach_orientation=true where we need it to be the start @@ -262,6 +259,11 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan( path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index d623ab6110..fc016ac545 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -399,6 +399,12 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } _expansions_publisher->publish(msg); } + + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 8b46fc3ae7..d3ddf5bdef 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -287,6 +287,11 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( if (!_a_star->createPath( path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) { + // Note: If the start is blocked only one iteration will occur before failure + if (num_iterations == 1) { + throw nav2_core::StartOccupied("Start occupied"); + } + if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index b6107b141d..f048cc5550 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -92,10 +92,6 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setCollisionChecker(checker.get()); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); - a_star_2.setStart(50, 50, 0); // invalid - a_star_2.setGoal(0, 0, 0); // valid - num_it = 0; - EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); a_star_2.setStart(0, 0, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid num_it = 0; diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index d1ddf7354c..a58a4f87ed 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -116,6 +116,11 @@ class ThetaStar return !(isSafe(src_.x, src_.y)) || !(isSafe(dst_.x, dst_.y)); } + /** + * @brief Clear Start + */ + void clearStart(); + int nodes_opened = 0; protected: diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index 9432f867a4..f83b745b22 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -258,4 +258,12 @@ void ThetaStar::initializePosn(int size_inc) node_position_.push_back(nullptr); } } + +void ThetaStar::clearStart() +{ + unsigned int mx_start = static_cast(src_.x); + unsigned int my_start = static_cast(src_.y); + costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE); +} + } // namespace theta_star diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index f93eaa44d7..ca9391111b 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -111,12 +111,6 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( std::to_string(goal.pose.position.y) + ") was outside bounds"); } - if (planner_->costmap_->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) { - throw nav2_core::StartOccupied( - "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " + - std::to_string(start.pose.position.y) + ") was in lethal cost"); - } - if (planner_->costmap_->getCost(mx_goal, my_goal) == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav2_core::GoalOccupied( "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " + @@ -141,6 +135,7 @@ nav_msgs::msg::Path ThetaStarPlanner::createPlan( return global_path; } + planner_->clearStart(); planner_->setStartAndGoal(start, goal); RCLCPP_DEBUG( logger_, "Got the src and dst... (%i, %i) && (%i, %i)",