diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py new file mode 100644 index 0000000000..59c43b5466 --- /dev/null +++ b/hero_skills/src/hero_skills/Suction_cup.py @@ -0,0 +1,69 @@ +#ROS +import rospy +from actionlib import GoalStatus + +#General +import PyKDL as kdl + +# TUe Robotics +from robot_skills.arm.gripper import Gripper +# Toyota +from tmc_suction.msg import SuctionControlAction, SuctionControlGoal + + +class SuctionGripper(Gripper): + """ + A Suction gripper that has a vacuum suction cup to grasp objects + """ + def __init__(self, robot_name, tf_buffer, gripper_name): + """ + constructor + :param robot_name: robot_name + :param tf_buffer: tf2_ros.Buffer + :param gripper_name: string to identify the gripper + """ + super(SuctionGripper, self).__init__(robot_name=robot_name, tf_listener=tf_buffer) + self.gripper_name = gripper_name + offset = self.load_param('skills/' + self.gripper_name + '/grasp_offset/') + self.offset = kdl.Frame(kdl.Rotation.RPY(offset["roll"], offset["pitch"], offset["yaw"]),kdl.Vector(offset["x"], offset["y"], offset["z"])) + + # Init gripper actionlib for SuctionGripper + self._ac_suction = self.create_simple_action_client("/" + robot_name + "/suction_control", SuctionControlAction) + + # Waits until the action server has started up and started + # listening for goals + self._ac_suction.wait_for_server() + + def send_gripper_goal(self, sucking, timeout=0.0): + """ + Send a GripperCommand to the gripper and wait for finishing + :param sucking: turn suction on or off + :type sucking: bool + :param timeout: timeout in seconds; timeout of 0.0 is not allowed + :type timeout: float + :return: True of False + :rtype: bool + """ + + if not isinstance(sucking, bool): + rospy.logerr('State should be true or false, now it is {0}'.format(sucking)) + return False + + goal = SuctionControlGoal() + + goal.suction_on.data = sucking + + self._ac_suction.send_goal(goal) + + if timeout != 0.0: + self._ac_suction.wait_for_result(rospy.Duration(timeout)) + goal_status = self._ac_suction.get_state() + goal_status_bool = goal_status == GoalStatus.SUCCEEDED + else: + rospy.logerr("A timeout value of 0.0 is given, which is not allowed!") + goal_status = self._ac_suction.get_state() + while goal_status == GoalStatus.PENDING: + goal_status = self._ac_suction.get_state() + goal_status_bool = goal_status == GoalStatus.SUCCEEDED + + return goal_status_bool diff --git a/hero_skills/src/hero_skills/hero.py b/hero_skills/src/hero_skills/hero.py index a781c7da69..c25a8846aa 100644 --- a/hero_skills/src/hero_skills/hero.py +++ b/hero_skills/src/hero_skills/hero.py @@ -8,6 +8,7 @@ from robot_skills import api, base, ebutton, head, ears, lights, perception, robot, speech, torso, world_model_ed from robot_skills.arm import arms, force_sensor, gripper, handover_detector from robot_skills.simulation import is_sim_mode, SimEButton +from hero_skills import Suction_cup class Hero(robot.Robot): @@ -31,6 +32,7 @@ def __init__(self, wait_services=False): hero_arm = arms.Arm(self.robot_name, self.tf_buffer, self.get_joint_states, "arm_center") hero_arm.add_part('force_sensor', force_sensor.ForceSensor(self.robot_name, self.tf_buffer, "/" + self.robot_name + "/wrist_wrench/raw")) hero_arm.add_part('gripper', gripper.ParrallelGripper(self.robot_name, self.tf_buffer, 'gripper')) + hero_arm.add_part('suction_gripper', Suction_cup.SuctionGripper(self.robot_name, self.tf_buffer, 'Suction_gripper')) hero_arm.add_part('handover_detector', handover_detector.HandoverDetector(self.robot_name, self.tf_buffer, 'handover_detector')) self.add_arm_part('arm_center', hero_arm) diff --git a/robot_skills/src/robot_skills/arm/gripper.py b/robot_skills/src/robot_skills/arm/gripper.py index 0767c7500d..238771a8a6 100644 --- a/robot_skills/src/robot_skills/arm/gripper.py +++ b/robot_skills/src/robot_skills/arm/gripper.py @@ -47,7 +47,7 @@ def occupied_by(self): @occupied_by.setter def occupied_by(self, value): """ - Set the entity which occupies the arm. + Set the entity which occupies the Gripper. :param value: robot_skills.util.entity, ED entity :return: no return