From 060ac031bdddae09a2b72cc2d2954528efe56fd9 Mon Sep 17 00:00:00 2001 From: Dylan De Coeyer Date: Fri, 28 Jul 2023 13:20:58 +0200 Subject: [PATCH] nav2_bt_navigator: log current location on navigate_to_pose action initialization It is very useful to know the current location considered by the bt_navigator for debug purposes. --- nav2_bt_navigator/src/navigators/navigate_to_pose.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index afae6ca0857..cbd686cc040 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -203,8 +203,15 @@ NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal) void 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); + RCLCPP_INFO( - logger_, "Begin navigating from current location to (%.2f, %.2f)", + 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); // Reset state for new action feedback