Enable goals to be sent in other frames than the pre-configured globa…

…l_frame (#3917)

* nav2_behavior_tree/nav2_bt_navigator: enable goals to be sent in other frames than the pre-configured global_frame (map)

Addresses #3894, at least partially.

* Address review comments (#3917 (review))

* Address more review comments (#3917 (review))

* Make uncrustify happy using cuddle braces for single line if statements...

* Fix removed global_frame input for the GoalReached node in the unit test

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp

@@ -55,7 +55,7 @@ class RemovePassedGoals : public BT::ActionNodeBase
   double viapoint_achieved_radius_;
   double transform_tolerance_;
   std::shared_ptr<tf2_ros::Buffer> tf_;
-  std::string global_frame_, robot_base_frame_;
+  std::string robot_base_frame_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp

@@ -92,7 +92,7 @@ class GoalReachedCondition : public BT::ConditionNode
   bool initialized_;
   double goal_reached_tol_;
   double transform_tolerance_;
-  std::string global_frame_, robot_base_frame_;
+  std::string robot_base_frame_;
 };
 
 }  // namespace nav2_behavior_tree

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

@@ -37,8 +37,6 @@ RemovePassedGoals::RemovePassedGoals(
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   node->get_parameter("transform_tolerance", transform_tolerance_);
 
-  global_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
-    node, "global_frame", this);
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, RemovePassedGoals>(
     node, "robot_base_frame", this);
 }
@@ -59,7 +57,7 @@ inline BT::NodeStatus RemovePassedGoals::tick()
 
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, global_frame_, robot_base_frame_,
+      current_pose, *tf_, goal_poses[0].header.frame_id, robot_base_frame_,
       transform_tolerance_))
   {
     return BT::NodeStatus::FAILURE;

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -33,8 +33,6 @@ GoalReachedCondition::GoalReachedCondition(
 {
   auto node = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
 
-  global_frame_ = BT::deconflictPortAndParamFrame<std::string, GoalReachedCondition>(
-    node, "global_frame", this);
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string, GoalReachedCondition>(
     node, "robot_base_frame", this);
 }
@@ -73,17 +71,17 @@ void GoalReachedCondition::initialize()
 
 bool GoalReachedCondition::isGoalReached()
 {
-  geometry_msgs::msg::PoseStamped current_pose;
+  geometry_msgs::msg::PoseStamped goal;
+  getInput("goal", goal);
 
+  geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_))
+      current_pose, *tf_, goal.header.frame_id, robot_base_frame_, transform_tolerance_))
   {
     RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
     return false;
   }
 
-  geometry_msgs::msg::PoseStamped goal;
-  getInput("goal", goal);
   double dx = goal.pose.position.x - current_pose.pose.position.x;
   double dy = goal.pose.position.y - current_pose.pose.position.y;
 

nav2_behavior_tree/test/plugins/condition/test_goal_reached.cpp

@@ -45,7 +45,7 @@ class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeT
       R"(
       <root main_tree_to_execute = "MainTree" >
         <BehaviorTree ID="MainTree">
-            <GoalReached goal="{goal}" global_frame="map" robot_base_frame="base_link"/>
+            <GoalReached goal="{goal}" robot_base_frame="base_link"/>
         </BehaviorTree>
       </root>)";
 

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -104,8 +104,9 @@ class NavigateThroughPosesNavigator
 
   /**
    * @brief Goal pose initialization on the blackboard
+   * @return bool if goal was initialized successfully to be processed
    */
-  void initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal);
+  bool initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal);
 
   rclcpp::Time start_time_;
   std::string goals_blackboard_id_;

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

@@ -116,8 +116,9 @@ class NavigateToPoseNavigator
   /**
    * @brief Goal pose initialization on the blackboard
    * @param goal Action template's goal message to process
+   * @return bool if goal was initialized successfully to be processed
    */
-  void initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
+  bool initializeGoalPose(ActionT::Goal::ConstSharedPtr goal);
 
   rclcpp::Time start_time_;
 

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -81,9 +81,7 @@ NavigateThroughPosesNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
     return false;
   }
 
-  initializeGoalPoses(goal);
-
-  return true;
+  return initializeGoalPoses(goal);
 }
 
 void
@@ -113,10 +111,14 @@ NavigateThroughPosesNavigator::onLoop()
   }
 
   geometry_msgs::msg::PoseStamped current_pose;
-  nav2_util::getCurrentPose(
-    current_pose, *feedback_utils_.tf,
-    feedback_utils_.global_frame, feedback_utils_.robot_frame,
-    feedback_utils_.transform_tolerance);
+  if (!nav2_util::getCurrentPose(
+      current_pose, *feedback_utils_.tf,
+      feedback_utils_.global_frame, feedback_utils_.robot_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    RCLCPP_ERROR(logger_, "Robot pose is not available.");
+    return;
+  }
 
   try {
     // Get current path points
@@ -185,7 +187,13 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
     // if pending goal requests the same BT as the current goal, accept the pending goal
     // if pending goal has an empty behavior_tree field, it requests the default BT file
     // accept the pending goal if the current goal is running the default BT file
-    initializeGoalPoses(bt_action_server_->acceptPendingGoal());
+    if (!initializeGoalPoses(bt_action_server_->acceptPendingGoal())) {
+      RCLCPP_WARN(
+        logger_,
+        "Preemption request was rejected since the goal poses could not be "
+        "transformed. For now, continuing to track the last goal until completion.");
+      bt_action_server_->terminatePendingGoal();
+    }
   } else {
     RCLCPP_WARN(
       logger_,
@@ -198,13 +206,27 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
   }
 }
 
-void
+bool
 NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal)
 {
-  if (goal->poses.size() > 0) {
+  Goals goal_poses = goal->poses;
+  for (auto & goal_pose : goal_poses) {
+    if (!nav2_util::transformPoseInTargetFrame(
+        goal_pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
+        feedback_utils_.transform_tolerance))
+    {
+      RCLCPP_ERROR(
+        logger_,
+        "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.",
+        goal_pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str());
+      return false;
+    }
+  }
+
+  if (goal_poses.size() > 0) {
     RCLCPP_INFO(
       logger_, "Begin navigating from current location through %zu poses to (%.2f, %.2f)",
-      goal->poses.size(), goal->poses.back().pose.position.x, goal->poses.back().pose.position.y);
+      goal_poses.size(), goal_poses.back().pose.position.x, goal_poses.back().pose.position.y);
   }
 
   // Reset state for new action feedback
@@ -213,7 +235,9 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
   blackboard->set<int>("number_recoveries", 0);  // NOLINT
 
   // Update the goal pose on the blackboard
-  blackboard->set<Goals>(goals_blackboard_id_, goal->poses);
+  blackboard->set<Goals>(goals_blackboard_id_, std::move(goal_poses));
+
+  return true;
 }
 
 }  // namespace nav2_bt_navigator

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -94,9 +94,7 @@ NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
     return false;
   }
 
-  initializeGoalPose(goal);
-
-  return true;
+  return initializeGoalPose(goal);
 }
 
 void
@@ -114,10 +112,14 @@ NavigateToPoseNavigator::onLoop()
   auto feedback_msg = std::make_shared<ActionT::Feedback>();
 
   geometry_msgs::msg::PoseStamped current_pose;
-  nav2_util::getCurrentPose(
-    current_pose, *feedback_utils_.tf,
-    feedback_utils_.global_frame, feedback_utils_.robot_frame,
-    feedback_utils_.transform_tolerance);
+  if (!nav2_util::getCurrentPose(
+      current_pose, *feedback_utils_.tf,
+      feedback_utils_.global_frame, feedback_utils_.robot_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    RCLCPP_ERROR(logger_, "Robot pose is not available.");
+    return;
+  }
 
   auto blackboard = bt_action_server_->getBlackboard();
 
@@ -187,7 +189,13 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
     // if pending goal requests the same BT as the current goal, accept the pending goal
     // if pending goal has an empty behavior_tree field, it requests the default BT file
     // accept the pending goal if the current goal is running the default BT file
-    initializeGoalPose(bt_action_server_->acceptPendingGoal());
+    if (!initializeGoalPose(bt_action_server_->acceptPendingGoal())) {
+      RCLCPP_WARN(
+        logger_,
+        "Preemption request was rejected since the goal pose could not be "
+        "transformed. For now, continuing to track the last goal until completion.");
+      bt_action_server_->terminatePendingGoal();
+    }
   } else {
     RCLCPP_WARN(
       logger_,
@@ -200,27 +208,45 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
   }
 }
 
-void
+bool
 NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  nav2_util::getCurrentPose(
-    current_pose, *feedback_utils_.tf,
-    feedback_utils_.global_frame, feedback_utils_.robot_frame,
-    feedback_utils_.transform_tolerance);
+  if (!nav2_util::getCurrentPose(
+      current_pose, *feedback_utils_.tf,
+      feedback_utils_.global_frame, feedback_utils_.robot_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    RCLCPP_ERROR(logger_, "Initial robot pose is not available.");
+    return false;
+  }
+
+  geometry_msgs::msg::PoseStamped goal_pose;
+  if (!nav2_util::transformPoseInTargetFrame(
+      goal->pose, goal_pose, *feedback_utils_.tf, feedback_utils_.global_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    RCLCPP_ERROR(
+      logger_,
+      "Failed to transform a goal pose provided with frame_id '%s' to the global frame '%s'.",
+      goal->pose.header.frame_id.c_str(), feedback_utils_.global_frame.c_str());
+    return false;
+  }
 
   RCLCPP_INFO(
     logger_, "Begin navigating from current location (%.2f, %.2f) to (%.2f, %.2f)",
     current_pose.pose.position.x, current_pose.pose.position.y,
-    goal->pose.pose.position.x, goal->pose.pose.position.y);
+    goal_pose.pose.position.x, goal_pose.pose.position.y);
 
   // Reset state for new action feedback
   start_time_ = clock_->now();
   auto blackboard = bt_action_server_->getBlackboard();
   blackboard->set<int>("number_recoveries", 0);  // NOLINT
 
   // Update the goal pose on the blackboard
-  blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
+  blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal_pose);
+
+  return true;
 }
 
 void