check getCurrentPose in initialiseGoalPoses (#4341)

This mirrors the check done in NavigateToPose::initializeGoalPose checking that we can get a current pose in order to not accept the goal request. Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Jul 11, 2024 · c9b4d28 · c9b4d28
1 parent ccc0a4b
commit c9b4d28
Showing 1 changed file with 12 additions and 0 deletions.
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -212,6 +212,18 @@ NavigateThroughPosesNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
 bool
 NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal)
 {
+  geometry_msgs::msg::PoseStamped current_pose;
+  if (!nav2_util::getCurrentPose(
+      current_pose, *feedback_utils_.tf,
+      feedback_utils_.global_frame, feedback_utils_.robot_frame,
+      feedback_utils_.transform_tolerance))
+  {
+    current_error_code_ = ActionT::Result::POSE_NOT_AVAILABLE;
+    current_error_msg_ = "Initial robot pose is not available.";
+    RCLCPP_ERROR(logger_, current_error_msg_.c_str());
+    return false;
+  }
+
   Goals goal_poses = goal->poses;
   int i = 0;
   for (auto & goal_pose : goal_poses) {