From 89fa7b2cad53cbe5c2bea3b3a3c1d31efcc7804c Mon Sep 17 00:00:00 2001 From: rayvburn Date: Sat, 12 Feb 2022 02:52:55 +0100 Subject: [PATCH] hubero_ros - NavigationRos: added logging with plan computations data --- hubero_ros/src/navigation_ros.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/hubero_ros/src/navigation_ros.cpp b/hubero_ros/src/navigation_ros.cpp index 615b70fb..06292013 100644 --- a/hubero_ros/src/navigation_ros.cpp +++ b/hubero_ros/src/navigation_ros.cpp @@ -618,6 +618,18 @@ nav_msgs::Path NavigationRos::computePlan( ); return nav_msgs::Path(); } + HUBERO_LOG( + "[%s].[NavigationRos] Computed plan with %lu poses (goal: {%2.2f, %2.2f}, " + "lastElem: {%2.2f, %2.2f}, secToLastElem: {%2.2f, %2.2f}\r\n", + actor_name_.c_str(), + resp.plan.poses.size(), + pose_goal_global_ref_plane.Pos().X(), + pose_goal_global_ref_plane.Pos().Y(), + resp.plan.poses.back().pose.position.x, + resp.plan.poses.back().pose.position.y, + resp.plan.poses.end()[-2].pose.position.x, + resp.plan.poses.end()[-2].pose.position.y + ); return resp.plan; }