From a5b382c9a08a673538c1ae0f602fe9d3c462e856 Mon Sep 17 00:00:00 2001
From: Yoshiki Obinata <mqcmd196@hotmail.co.jp>
Date: Sun, 22 Oct 2023 16:52:19 +0900
Subject: [PATCH] stop bridging action. bridge action topic

---
 .../scripts/moveit_noetic_bridge.py           | 66 ++++++++++++-------
 1 file changed, 44 insertions(+), 22 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py
index 51bce5c46f..d0395801b5 100755
--- a/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py
+++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py
@@ -8,6 +8,9 @@
     AttachedCollisionObject,
     CollisionObject,
     MotionPlanResponse,
+    MoveGroupActionGoal,
+    MoveGroupActionResult,
+    MoveGroupActionFeedback,
     PlanningScene,
     PlanningSceneWorld,
     RobotState,
@@ -217,20 +220,38 @@ def __init__(self):
             "/plan_kinematic_path", self._melodic_get_motion_plan.GetMotionPlan
         )
 
-        # Actionlib
-        self.move_group_as = actionlib.SimpleActionServer(
-            "/move_group_noetic",
-            MoveGroupAction,
-            execute_cb=self._move_group_action_cb,
-            auto_start=False,
+        # Topic
+        self.move_group_goal_sub = rospy.Subscriber(
+            "/move_group_noetic/goal",
+            MoveGroupActionGoal,
+            self._move_group_goal_cb,
+        )
+        self.move_group_goal_pub = rospy.Publisher(
+            "/move_group/goal",
+            self._melodic_move_group_goal.MoveGroupActionGoal,
+            queue_size=1,
         )
-        self.move_group_as.start()
-        self.move_group_ac = actionlib.SimpleActionClient(
-            "/move_group", self._melodic_move_group_action.MoveGroupAction
+        self.move_group_result_sub = rospy.Subscriber(
+            "/move_group/result",
+            MoveGroupActionResult,
+            self._move_group_result_cb,
         )
-
-        # Topic
-        self.planning_scene_world_cb = rospy.Subscriber(
+        self.move_group_result_pub = rospy.Publisher(
+            "/move_group_noetic/result",
+            self._melodic_move_group_result.MoveGroupActionResult,
+            queue_size=1,
+        )
+        self.move_group_feedback_sub = rospy.Subscriber(
+            "/move_group/feedback",
+            self._melodic_move_group_action.MoveGroupActionFeedback,
+            self._move_group_feedback_cb,
+        )
+        self.move_group_feedback_pub = rospy.Publisher(
+            "/move_group_noetic/feedback",
+            MoveGroupActionFeedback,
+            queue_size=1,
+        )
+        self.planning_scene_world_sub = rospy.Subscriber(
             "/planning_scene_world_noetic",
             PlanningSceneWorld,
             self._planning_scene_world_cb,
@@ -305,19 +326,20 @@ def _plan_kinematic_path_srv_cb(self, request):
         )
         return response
 
-    def _move_group_action_cb(self, goal):
-        self.move_group_ac.send_goal(
-            self._convert_noetic_move_group_goal_msg_to_melodic(goal),
-            feedback_cb=self._move_group_feedback_cb,
+    def _move_group_goal_cb(self, action_goal):
+        action_goal.goal = self._convert_noetic_move_group_goal_msg_to_melodic(
+            action_goal.goal
         )
-        self.move_group_ac.wait_for_result()
-        result = self.move_group_ac.get_result()
-        self.move_group_as.set_succeeded(
-            self._convert_melodic_move_group_result_msg_to_noetic(result)
+        self.move_group_goal_pub.publish(action_goal)
+
+    def _move_group_result_cb(self, action_result):
+        action_result.result = self._convert_melodic_move_group_result_msg_to_noetic(
+            action_result.result
         )
+        self.move_group_result_pub.publish(action_result)
 
-    def _move_group_feedback_cb(self, feedback):
-        self.move_group_as.publish_feedback(feedback)
+    def _move_group_feedback_cb(self, action_feedback):
+        self.move_group_feedback_pub.publish(action_feedback)
 
     def _planning_scene_world_cb(self, msg):
         converted_msg = self._convert_noetic_planning_scene_world_msg_to_melodic(msg)