Wait servo on off in actionlib

iory · Nov 1, 2024 · 82de8cc · 82de8cc
1 parent 4fdd009
commit 82de8cc
Showing 1 changed file with 4 additions and 0 deletions.
diff --git a/ros/kxr_controller/python/kxr_controller/kxr_interface.py b/ros/kxr_controller/python/kxr_controller/kxr_interface.py
@@ -85,6 +85,8 @@ def servo_on(self, joint_names=None):
         goal.joint_names = joint_names
         goal.servo_on_states = [True] * len(joint_names)
         client.send_goal(goal)
+        client.wait_for_result(timeout=rospy.Duration(10))
+        return client
 
     def servo_off(self, joint_names=None):
         if self.use_sim_time:
@@ -100,6 +102,8 @@ def servo_off(self, joint_names=None):
         goal.joint_names = joint_names
         goal.servo_on_states = [False] * len(joint_names)
         client.send_goal(goal)
+        client.wait_for_result(timeout=rospy.Duration(10))
+        return client
 
     def adjust_angle_vector(self, joint_names=None, error_threshold=None):
         if self.use_sim_time: