Merged Fix navfn_planner from humble PR #5087

nav2_navfn_planner/src/navfn_planner.cpp

@@ -115,10 +115,6 @@ NavfnPlanner::deactivate()
   RCLCPP_INFO(
     logger_, "Deactivating plugin %s of type NavfnPlanner",
     name_.c_str());
-  auto node = node_.lock();
-  if (dyn_params_handler_ && node) {
-    node->remove_on_set_parameters_callback(dyn_params_handler_.get());
-  }
   dyn_params_handler_.reset();
 }
 
@@ -133,30 +129,11 @@ NavfnPlanner::cleanup()
 
 nav_msgs::msg::Path NavfnPlanner::createPlan(
   const geometry_msgs::msg::PoseStamped & start,
-  const geometry_msgs::msg::PoseStamped & goal,
-  std::function<bool()> cancel_checker)
+  const geometry_msgs::msg::PoseStamped & goal)
 {
 #ifdef BENCHMARK_TESTING
   steady_clock::time_point a = steady_clock::now();
 #endif
-  unsigned int mx_start, my_start, mx_goal, my_goal;
-  if (!costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start)) {
-    throw nav2_core::StartOutsideMapBounds(
-            "Start Coordinates of(" + std::to_string(start.pose.position.x) + ", " +
-            std::to_string(start.pose.position.y) + ") was outside bounds");
-  }
-
-  if (!costmap_->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) {
-    throw nav2_core::GoalOutsideMapBounds(
-            "Goal Coordinates of(" + std::to_string(goal.pose.position.x) + ", " +
-            std::to_string(goal.pose.position.y) + ") was outside bounds");
-  }
-
-  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) + ", " +
-            std::to_string(goal.pose.position.y) + ") was in lethal cost");
-  }
 
   // Update planner based on the new costmap size
   if (isPlannerOutOfDate()) {
@@ -171,6 +148,12 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
   if (start.pose.position.x == goal.pose.position.x &&
     start.pose.position.y == goal.pose.position.y)
   {
+    unsigned int mx, my;
+    costmap_->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
+    if (costmap_->getCost(mx, my) == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      RCLCPP_WARN(logger_, "Failed to create a unique pose path because of obstacles");
+      return path;
+    }
     path.header.stamp = clock_->now();
     path.header.frame_id = global_frame_;
     geometry_msgs::msg::PoseStamped pose;
@@ -188,9 +171,10 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
     return path;
   }
 
-  if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) {
-    throw nav2_core::NoValidPathCouldBeFound(
-            "Failed to create plan with tolerance of: " + std::to_string(tolerance_) );
+  if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
+    RCLCPP_WARN(
+      logger_, "%s: failed to create plan with "
+      "tolerance %.2f.", name_.c_str(), tolerance_);
   }
 
 
@@ -219,7 +203,6 @@ bool
 NavfnPlanner::makePlan(
   const geometry_msgs::msg::Pose & start,
   const geometry_msgs::msg::Pose & goal, double tolerance,
-  std::function<bool()> cancel_checker,
   nav_msgs::msg::Path & plan)
 {
   // clear the plan, just in case
@@ -236,7 +219,14 @@ NavfnPlanner::makePlan(
     start.position.x, start.position.y, goal.position.x, goal.position.y);
 
   unsigned int mx, my;
-  worldToMap(wx, wy, mx, my);
+  if (!worldToMap(wx, wy, mx, my)) {
+    RCLCPP_WARN(
+      logger_,
+      "Cannot create a plan: the robot's start position is off the global"
+      " costmap. Planning will always fail, are you sure"
+      " the robot has been properly localized?");
+    return false;
+  }
 
   // clear the starting cell within the costmap because we know it can't be an obstacle
   clearRobotCell(mx, my);
@@ -259,17 +249,24 @@ NavfnPlanner::makePlan(
   wx = goal.position.x;
   wy = goal.position.y;
 
-  worldToMap(wx, wy, mx, my);
+  if (!worldToMap(wx, wy, mx, my)) {
+    RCLCPP_WARN(
+      logger_,
+      "The goal sent to the planner is off the global costmap."
+      " Planning will always fail to this goal.");
+    return false;
+  }
+
   int map_goal[2];
   map_goal[0] = mx;
   map_goal[1] = my;
 
   planner_->setStart(map_goal);
   planner_->setGoal(map_start);
   if (use_astar_) {
-    planner_->calcNavFnAstar(cancel_checker);
+    planner_->calcNavFnAstar();
   } else {
-    planner_->calcNavFnDijkstra(cancel_checker, true);
+    planner_->calcNavFnDijkstra(true);
   }
 
   double resolution = costmap_->getResolution();
@@ -293,13 +290,11 @@ NavfnPlanner::makePlan(
       p.position.x = goal.position.x - tolerance;
       while (p.position.x <= goal.position.x + tolerance) {
         potential = getPointPotential(p.position);
-        if (potential < POT_HIGH) {
-          double sdist = squared_distance(p, goal);
-          if (sdist < best_sdist) {
-            best_sdist = sdist;
-            best_pose = p;
-            found_legal = true;
-          }
+        double sdist = squared_distance(p, goal);
+        if (potential < POT_HIGH && sdist < best_sdist) {
+          best_sdist = sdist;
+          best_pose = p;
+          found_legal = true;
         }
         p.position.x += resolution;
       }
@@ -368,6 +363,7 @@ NavfnPlanner::smoothApproachToGoal(
   }
   geometry_msgs::msg::PoseStamped goal_copy;
   goal_copy.pose = goal;
+  goal_copy.header = plan.header;
   plan.poses.push_back(goal_copy);
 }
 
@@ -385,7 +381,13 @@ NavfnPlanner::getPlanFromPotential(
 
   // the potential has already been computed, so we won't update our copy of the costmap
   unsigned int mx, my;
-  worldToMap(wx, wy, mx, my);
+  if (!worldToMap(wx, wy, mx, my)) {
+    RCLCPP_WARN(
+      logger_,
+      "The goal sent to the navfn planner is off the global costmap."
+      " Planning will always fail to this goal.");
+    return false;
+  }
 
   int map_goal[2];
   map_goal[0] = mx;
@@ -417,6 +419,7 @@ NavfnPlanner::getPlanFromPotential(
     mapToWorld(x[i], y[i], world_x, world_y);
 
     geometry_msgs::msg::PoseStamped pose;
+    pose.header = plan.header;
     pose.pose.position.x = world_x;
     pose.pose.position.y = world_y;
     pose.pose.position.z = 0.0;