From dd7f67b15f85eea4c6f46fdc7048c23d1d3fa495 Mon Sep 17 00:00:00 2001 From: Mike Wake Date: Thu, 11 Jul 2024 19:09:38 +1000 Subject: [PATCH] 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 --- .../src/navigators/navigate_through_poses.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp index 8b145192f67..51809b14d12 100644 --- a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp @@ -211,6 +211,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) {