From 9724b5d1f5419aed844849f51497f72438e0f962 Mon Sep 17 00:00:00 2001
From: brettpac <brettpac@users.noreply.github.com>
Date: Thu, 21 Nov 2024 00:47:09 -0800
Subject: [PATCH] Brettpac branch (#567)

* Modifying DEBUG to INFO for undo_path_global_planner

* undoing if statement

---------

Co-authored-by: brettpac <brettpac@system76-pc.localdomain>
---
 .../undo_path_global_planner.cpp              | 21 ++++++++++---------
 1 file changed, 11 insertions(+), 10 deletions(-)

diff --git a/smacc2_client_library/nav2z_client/custom_planners/undo_path_global_planner/src/undo_path_global_planner/undo_path_global_planner.cpp b/smacc2_client_library/nav2z_client/custom_planners/undo_path_global_planner/src/undo_path_global_planner/undo_path_global_planner.cpp
index 37d450362..8354a677a 100644
--- a/smacc2_client_library/nav2z_client/custom_planners/undo_path_global_planner/src/undo_path_global_planner/undo_path_global_planner.cpp
+++ b/smacc2_client_library/nav2z_client/custom_planners/undo_path_global_planner/src/undo_path_global_planner/undo_path_global_planner.cpp
@@ -112,7 +112,7 @@ void UndoPathGlobalPlanner::configure(
 void UndoPathGlobalPlanner::onForwardTrailMsg(const nav_msgs::msg::Path::SharedPtr forwardPath)
 {
   lastForwardPathMsg_ = *forwardPath;
-  RCLCPP_DEBUG_STREAM(
+  RCLCPP_INFO_STREAM(
     nh_->get_logger(), "[UndoPathGlobalPlanner] received backward path msg poses ["
                          << lastForwardPathMsg_.poses.size() << "]");
 }
@@ -236,14 +236,14 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
       linear_mindist = dist;
       startPositionProjected = pose.pose;
 
-      RCLCPP_DEBUG_STREAM(
+      RCLCPP_INFO_STREAM(
         nh_->get_logger(), "[UndoPathGlobalPlanner] initial start point search, NEWBEST_LINEAR= "
                              << i << ". error, linear: " << linear_mindist
                              << ", angular: " << angleError);
     }
     else
     {
-      RCLCPP_DEBUG_STREAM(
+      RCLCPP_INFO_STREAM(
         nh_->get_logger(), "[UndoPathGlobalPlanner] initial start point search, skipped= "
                              << i << ". best linear error: " << linear_mindist
                              << ". current error, linear: " << dist << " angular: " << angleError);
@@ -257,7 +257,7 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
   // close angular targets, (accepting a larger linear minerror of 1.5 besterror. That is, more or less in the same
   // point).
 
-  RCLCPP_DEBUG(nh_->get_logger(), "[UndoPathGlobalPlanner] second angular pass");
+  RCLCPP_INFO(nh_->get_logger(), "[UndoPathGlobalPlanner] second angular pass");
   double angularMinDist = std::numeric_limits<double>::max();
 
   if (mindistindex >= (int)transformedPlan.poses.size())
@@ -271,12 +271,12 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
     }
 
     // ------- FULL FORWARD PASS TO FIND THE STARTING POIINT OF THE FORWARD MOTION ------
-    RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[UndoPathGlobalPlanner] second pass loop");
+    RCLCPP_INFO_STREAM(nh_->get_logger(), "[UndoPathGlobalPlanner] second pass loop");
     for (int i = mindistindex; i >= 0; i--)
     {
       // warning this index, i refers to some inverse interpretation from the previous loop,
       // (last indexes in this path corresponds to the poses closer to our current position)
-      RCLCPP_DEBUG_STREAM(
+      RCLCPP_INFO_STREAM(
         nh_->get_logger(), "[UndoPathGlobalPlanner] " << i << "/" << transformedPlan.poses.size());
       auto index = (int)transformedPlan.poses.size() - i - 1;
       if (index < 0 || (size_t)index >= transformedPlan.poses.size())
@@ -289,7 +289,7 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
       geometry_msgs::msg::PoseStamped pose =
         transformedPlan.poses[transformedPlan.poses.size() - i - 1];
 
-      RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[UndoPathGlobalPlanner] global frame");
+      RCLCPP_INFO_STREAM(nh_->get_logger(), "[UndoPathGlobalPlanner] global frame");
       pose.header.frame_id = costmap_ros_->getGlobalFrameID();
 
       double dx = pose.pose.position.x - start.pose.position.x;
@@ -305,7 +305,7 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
         {
           angularMinDist = angleError;
           mindistindex = i;
-          RCLCPP_DEBUG_STREAM(
+          RCLCPP_INFO_STREAM(
             nh_->get_logger(),
             "[UndoPathGlobalPlanner] initial start point search (angular update), NEWBEST_ANGULAR= "
               << i << ". error, linear: " << dist << "(" << linear_mindist << ")"
@@ -313,7 +313,7 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
         }
         else
         {
-          RCLCPP_DEBUG_STREAM(
+          RCLCPP_INFO_STREAM(
             nh_->get_logger(),
             "[UndoPathGlobalPlanner] initial start point search (angular update), skipped= "
               << i << ". error, linear: " << dist << "(" << linear_mindist << ")"
@@ -322,7 +322,7 @@ void UndoPathGlobalPlanner::createDefaultUndoPathPlan(
       }
       else
       {
-        RCLCPP_DEBUG_STREAM(
+        RCLCPP_INFO_STREAM(
           nh_->get_logger(),
           "[UndoPathGlobalPlanner] initial start point search (angular update) not in linear "
           "range, skipped= "
@@ -384,6 +384,7 @@ nav_msgs::msg::Path UndoPathGlobalPlanner::createPlan(
   RCLCPP_INFO_STREAM(
     nh_->get_logger(),
     "[UndoPathGlobalPlanner] last forward path msg size: " << lastForwardPathMsg_.poses.size());
+
   RCLCPP_INFO_STREAM(
     nh_->get_logger(), "[UndoPathGlobalPlanner] last forward path frame id: "
                          << lastForwardPathMsg_.poses.front().header.frame_id);