Add again velocity state_interface for gripper

Wiktor-99 · Feb 10, 2024 · 6c2d928 · 6c2d928
1 parent 32912a6
commit 6c2d928
Showing 1 changed file with 13 additions and 16 deletions.
diff --git a/ros2_grasp_service/ros2_grasp_service/grasp_service.py b/ros2_grasp_service/ros2_grasp_service/grasp_service.py
@@ -142,6 +142,7 @@ def declare_initial_parameters(self):
                 ("time_to_wait_on_target", Parameter.Type.INTEGER),
                 ("gripper_close", Parameter.Type.DOUBLE),
                 ("gripper_open", Parameter.Type.DOUBLE),
+                ("gripper_controller_name", Parameter.Type.STRING),
             ],
         )
 
@@ -183,30 +184,26 @@ def create_gripper_msg(self, gripper_value):
 
         return msg
 
-    def grasp(self, request, response):
-        self.get_logger().info("Trigger received. Grasp sequence will be started.")
-
-        self.joint_trajectory_action_client.send_goal(
-            self.create_trajectory_goal(self.positions_to_target, self.times_for_positions_to_target)
-        )
+    def go_to_pose(self, array_of_points, times_in_sec):
+        self.joint_trajectory_action_client.send_goal(self.create_trajectory_goal(array_of_points, times_in_sec))
         while self.joint_trajectory_action_client.status != GoalStatus.STATUS_SUCCEEDED:
             spin_once(self.joint_trajectory_action_client)
 
-        self.gripper_action.send_goal(self.create_gripper_msg(self.gripper_close_position))
+    def set_gripper_postion(self, gripper_postion):
+        self.gripper_action.send_goal(self.create_gripper_msg(gripper_postion))
         while self.gripper_action.status != GoalStatus.STATUS_SUCCEEDED:
             spin_once(self.gripper_action)
 
-        sleep(self.time_to_wait_on_target)
+    def grasp(self, request, response):
+        self.get_logger().error("Trigger received. Grasp sequence will be started.")
 
-        self.joint_trajectory_action_client.send_goal(
-            self.create_trajectory_goal(self.positions_to_home, self.times_for_positions_to_home)
-        )
-        while self.joint_trajectory_action_client.status != GoalStatus.STATUS_SUCCEEDED:
-            spin_once(self.joint_trajectory_action_client)
+        self.go_to_pose(self.positions_to_target, self.times_for_positions_to_target)
+        self.set_gripper_postion(self.gripper_close_position)
 
-        self.gripper_action.send_goal(self.create_gripper_msg(self.gripper_open_position))
-        while self.gripper_action.status != GoalStatus.STATUS_SUCCEEDED:
-            spin_once(self.gripper_action)
+        sleep(self.time_to_wait_on_target)
+
+        self.go_to_pose(self.positions_to_home, self.times_for_positions_to_home)
+        self.set_gripper_postion(self.gripper_open_position)
 
         return response