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]>

Author: aosmw

Diff for: 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) {