report index of failed initializeGoalPoses (#4341)

Add the index of the pose that cannot be transformed in NavigateThroughPosesNavigator::initializeGoalPoses to the error_msg Signed-off-by: Mike Wake <[email protected]>
ros-navigation · Jul 10, 2024 · 2fdbaea · 2fdbaea
1 parent 9371c21
commit 2fdbaea
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp b/nav2_bt_navigator/src/navigators/navigate_through_poses.cpp
@@ -231,14 +231,15 @@ bool
 NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr goal)
 {
   Goals goal_poses = goal->poses;
+  int i = 0;
   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))
     {
       current_error_code_ = ActionT::Result::TF_ERROR;
       current_error_msg_ =
-        "Failed to transform a goal pose provided with frame_id '" +
+        "Failed to transform a goal pose (" + std::to_string(i) + ") provided with frame_id '" +
         goal_pose.header.frame_id +
         "' to the global frame '" +
         feedback_utils_.global_frame +
@@ -247,6 +248,7 @@ NavigateThroughPosesNavigator::initializeGoalPoses(ActionT::Goal::ConstSharedPtr
       RCLCPP_ERROR(logger_, current_error_msg_.c_str());
       return false;
     }
+    i++;
   }
 
   if (goal_poses.size() > 0) {