diff --git a/src/gripper_controller/gripper_controller.py b/src/gripper_controller/gripper_controller.py index 8fd1ef1..7b72a0c 100755 --- a/src/gripper_controller/gripper_controller.py +++ b/src/gripper_controller/gripper_controller.py @@ -20,30 +20,20 @@ def __init__(self): self.success = True # server - self.srv_safe_joint_change_left = actionlib.SimpleActionServer('left_arm/gripper/action', - GripperCommandAction, - execute_cb=self.gripper_left, - auto_start=False) - self.srv_safe_joint_change_right = actionlib.SimpleActionServer('right_arm/gripper/action', - GripperCommandAction, - execute_cb=self.gripper_right, - auto_start=False) - self.srv_safe_joint_change_left.start() - self.srv_safe_joint_change_right.start() + self.srv_safe_joint_change = actionlib.SimpleActionServer('gripper/action', + GripperCommandAction, + execute_cb=self.gripper, + auto_start=False) + self.srv_safe_joint_change.start() # clients self.client_safe_joint_change = rospy.ServiceProxy('safe_pose_changer/change_joint', SafeJointChange) self._grasp_client = actionlib.SimpleActionClient('gripper_controller/grasp', GripperApplyEffortAction) - def gripper_left(self, goal): + def gripper(self, goal): self.success = self.safe_joint_change_srv(goal) if self.success: - self.srv_safe_joint_change_left.set_succeeded() - - def gripper_right(self, goal): - self.success = self.safe_joint_change_srv(goal) - if self.success: - self.srv_safe_joint_change_right.set_succeeded() + self.srv_safe_joint_change.set_succeeded() def safe_joint_change_srv(self, goal): """ diff --git a/src/handover_node/handover_detection.py b/src/handover_node/handover_detection.py index 7b77d96..151065a 100755 --- a/src/handover_node/handover_detection.py +++ b/src/handover_node/handover_detection.py @@ -22,13 +22,13 @@ def __init__(self, side): self.force_z = 0 # server - self.sub_r2h = rospy.Subscriber("handoverdetector_" + side + "/toggle_robot2human", + self.sub_r2h = rospy.Subscriber("handover_detector/toggle_robot2human", Bool, self.detect_handover) - self.sub_h2r = rospy.Subscriber("handoverdetector_" + side + "/toggle_human2robot", + self.sub_h2r = rospy.Subscriber("handover_detector/toggle_human2robot", Bool, self.detect_handover) self.sub_wrist_wrench = rospy.Subscriber("wrist_wrench/raw", WrenchStamped, self.update_forces) - self.pub_result = rospy.Publisher("handoverdetector_" + side + "/result", Bool, queue_size=1) + self.pub_result = rospy.Publisher("handover_detector/result", Bool, queue_size=1) def detect_handover(self, data): if data.data: diff --git a/src/plan_node/manipulation_bridge.py b/src/plan_node/manipulation_bridge.py index 9baf5fc..79658db 100755 --- a/src/plan_node/manipulation_bridge.py +++ b/src/plan_node/manipulation_bridge.py @@ -26,28 +26,17 @@ def __init__(self): self.whole_body = self.robot.try_get('whole_body') # server - self.srv_manipulation_left = actionlib.SimpleActionServer('left_arm/grasp_precompute', + self.srv_manipulation = actionlib.SimpleActionServer('arm_center/grasp_precompute', GraspPrecomputeAction, - execute_cb=self.manipulation_srv_left, + execute_cb=self.manipulation_srv_inst, auto_start=False) - self.srv_manipulation_right = actionlib.SimpleActionServer('right_arm/grasp_precompute', - GraspPrecomputeAction, - execute_cb=self.manipulation_srv_right, - auto_start=False) - self.srv_manipulation_left.start() - self.srv_manipulation_right.start() - - def manipulation_srv_left(self, action): - success = self.manipulation_srv(action) - if success: - rospy.loginfo('Manipulation bridge: Succeeded') - self.srv_manipulation_left.set_succeeded() + self.srv_manipulation.start() - def manipulation_srv_right(self, action): + def manipulation_srv_inst(self, action): success = self.manipulation_srv(action) if success: rospy.loginfo('Manipulation bridge: Succeeded') - self.srv_manipulation_right.set_succeeded() + self.srv_manipulation.set_succeeded() def manipulation_srv(self, action): """ diff --git a/src/tf_ghost/tf_ghost.py b/src/tf_ghost/tf_ghost.py index c3c70b4..6404e19 100755 --- a/src/tf_ghost/tf_ghost.py +++ b/src/tf_ghost/tf_ghost.py @@ -19,10 +19,6 @@ def create_ghosts(self): self.remap_frame("head_rgbd_sensor_gazebo_frame", "top_kinect/openni_camera") self.remap_frame("base_range_sensor_link", self.prefix + "/base_laser") - self.sendTransform((0, 0, 0), tf_conversions.transformations.quaternion_from_euler(pi, -pi/2, 0), - rospy.Time.now(), self.prefix + "/grippoint_left", "hand_palm_link") - self.sendTransform((0, 0, 0), tf_conversions.transformations.quaternion_from_euler(pi, -pi/2, 0), - rospy.Time.now(), self.prefix + "/grippoint_right", "hand_palm_link") self.sendTransform((0, 0, 0), tf_conversions.transformations.quaternion_from_euler(pi, -pi/2, 0), rospy.Time.now(), "head_mount", "torso_lift_link")