Add message when the goal is reverted

at-wat · Jul 31, 2023 · d509fe3 · d509fe3
1 parent d7f414b
commit d509fe3
Showing 1 changed file with 11 additions and 0 deletions.
diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp
@@ -553,6 +553,17 @@ class Planner3dNode
         ROS_INFO("Goal moved. Metric: (%f, %f, %f), Grid: (%d, %d, %d)", x, y, yaw, e[0], e[1], e[2]);
         break;
       default:
+        Astar::Vec e_prev;
+        grid_metric_converter::metric2Grid(
+            map_info_, e_prev[0], e_prev[1], e_prev[2],
+            goal_.pose.position.x, goal_.pose.position.y,
+            tf2::getYaw(goal_.pose.orientation));
+        if (e[0] != e_prev[0] || e[1] != e_prev[1] || e[2] != e_prev[2])
+        {
+          ROS_INFO("Goal reverted. Metric: (%f, %f, %f), Grid: (%d, %d, %d)",
+                   goal_raw_.pose.position.x, goal_raw_.pose.position.y, tf2::getYaw(goal_raw_.pose.orientation),
+                   e[0], e[1], e[2]);
+        }
         goal_ = goal_raw_;
         break;
     }