clear start cell for planning (#3980)

* clear start cell for planning

* clean up

* add headers

* added comment

* start blocked

* code review

* code review

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) + ", " +

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -245,6 +245,11 @@ class AStarAlgorithm
   inline void populateExpansionsLog(
     const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
 
+  /**
+   * @brief Clear Start
+   */
+  void clearStart();
+
   int _timing_interval = 5000;
 
   bool _traverse_unknown;

nav2_smac_planner/src/a_star.cpp

@@ -234,10 +234,8 @@ bool AStarAlgorithm<NodeT>::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<NodeT>::getSizeDim3()
   return _dim3_size;
 }
 
+template<>
+void AStarAlgorithm<Node2D>::clearStart()
+{
+  auto coords = Node2D::getCoords(_start->getIndex());
+  _costmap->setCost(coords.x, coords.y, nav2_costmap_2d::FREE_SPACE);
+}
+
+template<typename NodeT>
+void AStarAlgorithm<NodeT>::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<Node2D>;
 template class AStarAlgorithm<NodeHybrid>;

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<float>(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 {

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 {

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<float>(_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 {

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;

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:

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<unsigned int>(src_.x);
+  unsigned int my_start = static_cast<unsigned int>(src_.y);
+  costmap_->setCost(mx_start, my_start, nav2_costmap_2d::FREE_SPACE);
+}
+
 }  //  namespace theta_star

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)",