Created service for gripper pose

estherRay · Mar 2, 2022 · bca7a15 · bca7a15
1 parent 3d52804
commit bca7a15
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py
@@ -38,6 +38,7 @@
 from spot_msgs.srv import ArmJointMovement, ArmJointMovementResponse, ArmJointMovementRequest
 from spot_msgs.srv import GripperAngleMove, GripperAngleMoveResponse, GripperAngleMoveRequest
 from spot_msgs.srv import ArmForceTrajectory, ArmForceTrajectoryResponse, ArmForceTrajectoryRequest
+from spot_msgs.srv import HandPose, HandPoseResponse, HandPoseRequest
 
 
 from .ros_helpers import *
@@ -556,6 +557,11 @@ def handle_body_follow_arm(self, srv_data):
         """ROS service to send a pose to the end effector"""
         resp = self.spot_wrapper.hand_position_3d()
         return TriggerResponse(resp[0], resp[1])
+
+    def handle_hand_pose(self, srv_data: HandPoseRequest):
+        """ROS service to give a position to the gripper"""
+        resp = self.spot_wrapper.hand_pose(pose_points = srv_data.pose_point)
+        return HandPoseResponse(resp[0], resp[1])
 
     ##################################################################
 
@@ -696,6 +702,7 @@ def main(self):
             rospy.Service("arm_joint_move", ArmJointMovement, self.handle_arm_joint_move)
             rospy.Service("force_trajectory", ArmForceTrajectory, self.handle_force_trajectory)
             rospy.Service("body_follow_hand", Trigger, self.handle_body_follow_arm)
+            rospy.Service("gripper_pose", HandPose, self.handle_hand_pose)
             #########################################################
 
             self.navigate_as = actionlib.SimpleActionServer('navigate_to', NavigateToAction,