Merge pull request #5 from angsa-robotics/AR-2212_Move_intermediate_goal_sampling_to_route_planner

AR-2212 Move intermediate goal sampling to route planner

Author: HovorunBh

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -66,13 +66,17 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
         BT::InputPort<int>(
           "nb_goals_to_consider", 5,
           "Number of goals to consider for collision checking"),
+        BT::InputPort<bool>(
+          "update_status", true,
+          "Whether to update the status of the goals"),
         BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
       });
   }
 
 private:
   bool use_footprint_;
   bool consider_unknown_as_obstacle_;
+  bool update_status_;
   double cost_threshold_;
   Goals input_goals_;
   int nb_goals_to_consider_;

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -39,6 +39,7 @@ void RemoveInCollisionGoals::on_tick()
   getInput("input_goals", input_goals_);
   getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
   getInput("nb_goals_to_consider", nb_goals_to_consider_);
+  getInput("update_status", update_status_);
 
   if (input_goals_.empty()) {
     setOutput("output_goals", input_goals_);
@@ -63,7 +64,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
   [[maybe_unused]] auto res = config().blackboard->get("goals_feedback", goals_feedback);
   for (int i = static_cast<int>(response->costs.size()) - 1; i >= 0; --i) {
     if ((response->costs[i] != 255 || consider_unknown_as_obstacle_) && response->costs[i] >= cost_threshold_) {
-      goals_feedback[input_goals_[i].index].status = nav2_msgs::msg::Waypoint::SKIPPED;
+      if (update_status_) {
+        goals_feedback[input_goals_[i].index].status = nav2_msgs::msg::Waypoint::SKIPPED;
+      }
       // if it's not valid then we erase it from input_goals_ and set the status to SKIPPED
       input_goals_.erase(input_goals_.begin() + i);
     }

nav2_planner/src/planner_server.cpp

@@ -433,10 +433,14 @@ void PlannerServer::computePlanThroughPoses()
           curr_start, curr_goal, goal->planner_id,
           cancel_checker);
       } catch (nav2_core::GoalOutsideMapBounds & ex) {
-        RCLCPP_WARN(
-          get_logger(), "Goal %d coordinates of (%.2f, %.2f) was outside bounds",
-          i, curr_goal.pose.position.x, curr_goal.pose.position.y);
-        continue;
+        if (i == 0) {
+          throw ex;
+        } else {
+          RCLCPP_WARN(
+            get_logger(), "Goal %d coordinates of (%.2f, %.2f) was outside bounds but there are still goals inside. Ignoring the rest",
+            i, curr_goal.pose.position.x, curr_goal.pose.position.y);
+          break;
+        }
       } catch (nav2_core::StartOccupied & ex) {
         RCLCPP_WARN(
           get_logger(), "Error START_OCCUPIED but probably 2 goals are just too close to each other.");