pub current goal

Signed-off-by: fukimiyazaki <[email protected]>

cabot_ui/cabot_ui/navigation.py

@@ -363,6 +363,7 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo
         self.path_all_pub = node.create_publisher(nav_msgs.msg.Path, path_all_output, transient_local_qos, callback_group=MutuallyExclusiveCallbackGroup())
 
         self.updated_goal_sub = node.create_subscription(geometry_msgs.msg.PoseStamped, "/updated_goal", self._goal_updated_callback, 10, callback_group=MutuallyExclusiveCallbackGroup())
+        self._eventPub = self._node.create_publisher(std_msgs.msg.String, "/cabot/debug", 10, callback_group=MutuallyExclusiveCallbackGroup())
 
         self.current_queue_msg = None
         self.need_queue_start_arrived_info = False
@@ -662,6 +663,9 @@ def _navigate_next_sub_goal(self):
             self.delegate.activity_log("cabot/navigation", "next_sub_goal")
             self._current_goal = self._sub_goals.pop(0)
             self._current_goal.reset()
+            msg = std_msgs.msg.String()
+            msg.data = F"set current goal:{self._current_goal}"
+            self._eventPub.publish(msg)
             self._navigate_sub_goal(self._current_goal)
             return
 
@@ -939,6 +943,9 @@ def _check_goal(self, current_pose):
             return
 
         goal.exit()
+
+        msg = std_msgs.msg.String()
+        msg.data = F"complete current goal:{self._current_goal}"
         self._current_goal = None
         if goal.need_to_announce_arrival:
             self.delegate.activity_log("cabot/navigation", "navigation", "arrived")