From fa08a4409b2440bfe653935d71d3dc1af3313c25 Mon Sep 17 00:00:00 2001
From: Shashank_Narla <s.narla@student.tue.nl>
Date: Tue, 4 May 2021 22:07:09 +0200
Subject: [PATCH 1/4] [hero_skills] add first version of suction cup gripper

---
 hero_skills/src/hero_skills/Suction_cup.py | 66 ++++++++++++++++++++++
 1 file changed, 66 insertions(+)
 create mode 100644 hero_skills/src/hero_skills/Suction_cup.py

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..a6b9514821
--- /dev/null
+++ b/hero_skills/src/hero_skills/Suction_cup.py
@@ -0,0 +1,66 @@
+#ROS
+import rospy
+
+import PyKDL as kdl
+from actionlib import GoalStatus
+from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal
+from tue_msgs.msg import GripperCommand
+
+
+from robot_skills.arms.gripper import Gripper
+# tmc_suction
+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_listener, gripper_name):
+        """
+        constructor
+        :param robot_name: robot_name
+        :param tf_listener: tf_server.TFClient()
+        :param gripper_name: string to identify the gripper
+        """
+        super(SuctionGripper, self).__init__(robot_name=robot_name, tf_listener=tf_listener)
+        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=5.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)
+
+        goal_status = GoalStatus.SUCCEEDED
+
+        if timeout != 0.0:
+            self._ac_suction.wait_for_result(rospy.Duration(timeout))
+            goal_status = self._ac_suction.get_state()
+
+        return goal_status == GoalStatus.SUCCEEDED
+

From 6d1be96c05ad2f4f04614e3d7c5345da09f57e1a Mon Sep 17 00:00:00 2001
From: Shashank_Narla <s.narla@student.tue.nl>
Date: Tue, 25 May 2021 22:12:18 +0200
Subject: [PATCH 2/4] Added goal to suction arm

---
 hero_skills/src/hero_skills/Suction_cup.py | 126 ++++++++++++++++++++-
 1 file changed, 124 insertions(+), 2 deletions(-)

diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py
index a6b9514821..aa28120f79 100644
--- a/hero_skills/src/hero_skills/Suction_cup.py
+++ b/hero_skills/src/hero_skills/Suction_cup.py
@@ -1,13 +1,14 @@
 #ROS
 import rospy
-
+import time
 import PyKDL as kdl
 from actionlib import GoalStatus
 from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal
+from tue_manipulation_msgs.msg import GraspPrecomputeGoal, GraspPrecomputeAction
 from tue_msgs.msg import GripperCommand
 
 
-from robot_skills.arms.gripper import Gripper
+from robot_skills.arm.gripper import Gripper
 # tmc_suction
 from tmc_suction.msg import SuctionControlAction, SuctionControlGoal
 
@@ -64,3 +65,124 @@ def send_gripper_goal(self, sucking, timeout=5.0):
 
         return goal_status == GoalStatus.SUCCEEDED
 
+    @property
+    def occupied_by_suction(self):
+        """
+        The 'occupied_by_suction' property will return the current entity that is in the suction cup of this arm.
+        :return: robot_skills.util.entity, ED entity
+        """
+        return self._occupied_by_suction
+
+    @occupied_by_suction.setter
+    def occupied_by_suction(self, value):
+        """
+        Set the entity which occupies the suction cup.
+        :param value: robot_skills.util.entity, ED entity
+        :return: no return
+        """
+        self._occupied_by_suction = value
+
+    def send_goal_suction(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_only=False,
+                          allowed_touch_objects=None):
+        """
+        Send a arm to a goal:
+        Using a combination of position and orientation: a kdl.Frame. A time
+        out time_out. pre_grasp means go to an offset that is normally needed
+        for things such as grasping. You can also specify the frame_id which
+        defaults to base_link
+        :param frameStamped: A FrameStamped to move the arm's end effector to
+        :param timeout: timeout in seconds; In case of 0.0, goal is executed without feedback and waiting
+        :param pre_grasp: Bool to use pre_grasp or not
+        :param first_joint_pos_only: Bool to only execute first joint position of whole trajectory
+        :param allowed_touch_objects: List of object names in the worldmodel, which are allowed to be touched
+        :return: True of False
+        """
+        if allowed_touch_objects is None:
+            allowed_touch_objects = list()
+
+        # save the arguments for debugging later
+        myargs = locals()
+
+        # If necessary, prefix frame_id
+        if frameStamped.frame_id.find(self.robot_name) < 0:
+            frameStamped.frame_id = "/" + self.robot_name + "/" + frameStamped.frame_id
+            rospy.loginfo("Grasp precompute frame id = {0}".format(frameStamped.frame_id))
+
+        # Convert to baselink, which is needed because the offset is defined in the base_link frame
+        frame_in_baselink = frameStamped.projectToFrame("/" + self.robot_name + "/base_link", self.tf_listener)
+
+        # TODO: Get rid of this custom message type
+        # Create goal:
+        grasp_precompute_goal = GraspPrecomputeGoal()
+        grasp_precompute_goal.goal.header.frame_id = frame_in_baselink.frame_id
+        grasp_precompute_goal.goal.header.stamp = rospy.Time.now()
+
+        grasp_precompute_goal.PERFORM_PRE_GRASP = pre_grasp
+        grasp_precompute_goal.FIRST_JOINT_POS_ONLY = first_joint_pos_only
+
+        grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects
+
+        grasp_precompute_goal.goal.x = frame_in_baselink.frame.p.x()
+        grasp_precompute_goal.goal.y = frame_in_baselink.frame.p.y()
+        grasp_precompute_goal.goal.z = frame_in_baselink.frame.p.z()
+
+        roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY()
+        grasp_precompute_goal.goal.roll = roll
+        grasp_precompute_goal.goal.pitch = pitch
+        grasp_precompute_goal.goal.yaw = yaw
+
+        self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point")
+
+        # Add tunable parameters
+        # todo: generalise send_goal in base arm so it can work with multiple gripper types
+        offset_frame = frame_in_baselink.frame * self.offset_suction
+
+        grasp_precompute_goal.goal.x = offset_frame.p.x()
+        grasp_precompute_goal.goal.y = offset_frame.p.y()
+        grasp_precompute_goal.goal.z = offset_frame.p.z()
+
+        roll, pitch, yaw = offset_frame.M.GetRPY()
+        grasp_precompute_goal.goal.roll = roll
+        grasp_precompute_goal.goal.pitch = pitch
+        grasp_precompute_goal.goal.yaw = yaw
+
+        # rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal))
+
+        self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected")
+
+        time.sleep(0.001)  # This is necessary: the rtt_actionlib in the hardware seems
+        # to only have a queue size of 1 and runs at 1000 hz. This
+        # means that if two goals are send approximately at the same
+        # time (e.g. an arm goal and a torso goal), one of the two
+        # goals probably won't make it. This sleep makes sure the
+        # goals will always arrive in different update hooks in the
+        # hardware TrajectoryActionLib server.
+
+        # Send goal:
+
+        if timeout == 0.0:
+            self._ac_grasp_precompute.send_goal(grasp_precompute_goal)
+            return True
+        else:
+            result = self._ac_grasp_precompute.send_goal_and_wait(
+                grasp_precompute_goal,
+                execute_timeout=rospy.Duration(timeout))
+            if result == GoalStatus.SUCCEEDED:
+
+                result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link",
+                                                               self.robot_name + "/grippoint_{}".format(self.side),
+                                                               rospy.Time(0))
+                dx = grasp_precompute_goal.goal.x - result_pose[0][0]
+                dy = grasp_precompute_goal.goal.y - result_pose[0][1]
+                dz = grasp_precompute_goal.goal.z - result_pose[0][2]
+
+                if abs(dx) > 0.005 or abs(dy) > 0.005 or abs(dz) > 0.005:
+                    rospy.logwarn("Grasp-precompute error too large: [{}, {}, {}]".format(
+                        dx, dy, dz))
+                return True
+            else:
+                # failure
+                rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs))
+                return False
+
+

From 887534c53cc3acc05de42028988d7bba0b1156e0 Mon Sep 17 00:00:00 2001
From: Shashank <shanki98@gmail.com>
Date: Tue, 22 Jun 2021 21:08:04 +0200
Subject: [PATCH 3/4] Added changes to Suction_cup

---
 hero_skills/src/hero_skills/Suction_cup.py   | 132 +------------------
 robot_skills/src/robot_skills/arm/gripper.py |   2 +-
 2 files changed, 5 insertions(+), 129 deletions(-)

diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py
index aa28120f79..dcc27a41cd 100644
--- a/hero_skills/src/hero_skills/Suction_cup.py
+++ b/hero_skills/src/hero_skills/Suction_cup.py
@@ -1,15 +1,13 @@
 #ROS
 import rospy
-import time
-import PyKDL as kdl
 from actionlib import GoalStatus
-from tue_manipulation_msgs.msg import GripperCommandAction, GripperCommandGoal
-from tue_manipulation_msgs.msg import GraspPrecomputeGoal, GraspPrecomputeAction
-from tue_msgs.msg import GripperCommand
 
+#General
+import PyKDL as kdl
 
+# TUe Robotics
 from robot_skills.arm.gripper import Gripper
-# tmc_suction
+# Toyota
 from tmc_suction.msg import SuctionControlAction, SuctionControlGoal
 
 
@@ -64,125 +62,3 @@ def send_gripper_goal(self, sucking, timeout=5.0):
             goal_status = self._ac_suction.get_state()
 
         return goal_status == GoalStatus.SUCCEEDED
-
-    @property
-    def occupied_by_suction(self):
-        """
-        The 'occupied_by_suction' property will return the current entity that is in the suction cup of this arm.
-        :return: robot_skills.util.entity, ED entity
-        """
-        return self._occupied_by_suction
-
-    @occupied_by_suction.setter
-    def occupied_by_suction(self, value):
-        """
-        Set the entity which occupies the suction cup.
-        :param value: robot_skills.util.entity, ED entity
-        :return: no return
-        """
-        self._occupied_by_suction = value
-
-    def send_goal_suction(self, frameStamped, timeout=30, pre_grasp=False, first_joint_pos_only=False,
-                          allowed_touch_objects=None):
-        """
-        Send a arm to a goal:
-        Using a combination of position and orientation: a kdl.Frame. A time
-        out time_out. pre_grasp means go to an offset that is normally needed
-        for things such as grasping. You can also specify the frame_id which
-        defaults to base_link
-        :param frameStamped: A FrameStamped to move the arm's end effector to
-        :param timeout: timeout in seconds; In case of 0.0, goal is executed without feedback and waiting
-        :param pre_grasp: Bool to use pre_grasp or not
-        :param first_joint_pos_only: Bool to only execute first joint position of whole trajectory
-        :param allowed_touch_objects: List of object names in the worldmodel, which are allowed to be touched
-        :return: True of False
-        """
-        if allowed_touch_objects is None:
-            allowed_touch_objects = list()
-
-        # save the arguments for debugging later
-        myargs = locals()
-
-        # If necessary, prefix frame_id
-        if frameStamped.frame_id.find(self.robot_name) < 0:
-            frameStamped.frame_id = "/" + self.robot_name + "/" + frameStamped.frame_id
-            rospy.loginfo("Grasp precompute frame id = {0}".format(frameStamped.frame_id))
-
-        # Convert to baselink, which is needed because the offset is defined in the base_link frame
-        frame_in_baselink = frameStamped.projectToFrame("/" + self.robot_name + "/base_link", self.tf_listener)
-
-        # TODO: Get rid of this custom message type
-        # Create goal:
-        grasp_precompute_goal = GraspPrecomputeGoal()
-        grasp_precompute_goal.goal.header.frame_id = frame_in_baselink.frame_id
-        grasp_precompute_goal.goal.header.stamp = rospy.Time.now()
-
-        grasp_precompute_goal.PERFORM_PRE_GRASP = pre_grasp
-        grasp_precompute_goal.FIRST_JOINT_POS_ONLY = first_joint_pos_only
-
-        grasp_precompute_goal.allowed_touch_objects = allowed_touch_objects
-
-        grasp_precompute_goal.goal.x = frame_in_baselink.frame.p.x()
-        grasp_precompute_goal.goal.y = frame_in_baselink.frame.p.y()
-        grasp_precompute_goal.goal.z = frame_in_baselink.frame.p.z()
-
-        roll, pitch, yaw = frame_in_baselink.frame.M.GetRPY()
-        grasp_precompute_goal.goal.roll = roll
-        grasp_precompute_goal.goal.pitch = pitch
-        grasp_precompute_goal.goal.yaw = yaw
-
-        self._publish_marker(grasp_precompute_goal, [1, 0, 0], "grasp_point")
-
-        # Add tunable parameters
-        # todo: generalise send_goal in base arm so it can work with multiple gripper types
-        offset_frame = frame_in_baselink.frame * self.offset_suction
-
-        grasp_precompute_goal.goal.x = offset_frame.p.x()
-        grasp_precompute_goal.goal.y = offset_frame.p.y()
-        grasp_precompute_goal.goal.z = offset_frame.p.z()
-
-        roll, pitch, yaw = offset_frame.M.GetRPY()
-        grasp_precompute_goal.goal.roll = roll
-        grasp_precompute_goal.goal.pitch = pitch
-        grasp_precompute_goal.goal.yaw = yaw
-
-        # rospy.loginfo("Arm goal: {0}".format(grasp_precompute_goal))
-
-        self._publish_marker(grasp_precompute_goal, [0, 1, 0], "grasp_point_corrected")
-
-        time.sleep(0.001)  # This is necessary: the rtt_actionlib in the hardware seems
-        # to only have a queue size of 1 and runs at 1000 hz. This
-        # means that if two goals are send approximately at the same
-        # time (e.g. an arm goal and a torso goal), one of the two
-        # goals probably won't make it. This sleep makes sure the
-        # goals will always arrive in different update hooks in the
-        # hardware TrajectoryActionLib server.
-
-        # Send goal:
-
-        if timeout == 0.0:
-            self._ac_grasp_precompute.send_goal(grasp_precompute_goal)
-            return True
-        else:
-            result = self._ac_grasp_precompute.send_goal_and_wait(
-                grasp_precompute_goal,
-                execute_timeout=rospy.Duration(timeout))
-            if result == GoalStatus.SUCCEEDED:
-
-                result_pose = self.tf_listener.lookupTransform(self.robot_name + "/base_link",
-                                                               self.robot_name + "/grippoint_{}".format(self.side),
-                                                               rospy.Time(0))
-                dx = grasp_precompute_goal.goal.x - result_pose[0][0]
-                dy = grasp_precompute_goal.goal.y - result_pose[0][1]
-                dz = grasp_precompute_goal.goal.z - result_pose[0][2]
-
-                if abs(dx) > 0.005 or abs(dy) > 0.005 or abs(dz) > 0.005:
-                    rospy.logwarn("Grasp-precompute error too large: [{}, {}, {}]".format(
-                        dx, dy, dz))
-                return True
-            else:
-                # failure
-                rospy.logerr('grasp precompute goal failed: \n%s', repr(myargs))
-                return False
-
-
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

From 58206abf50c00f95535e1a89a82078e3ee558e76 Mon Sep 17 00:00:00 2001
From: Shashank <shanki98@gmail.com>
Date: Tue, 20 Jul 2021 22:40:11 +0200
Subject: [PATCH 4/4] Changed send_goal and created suction object

---
 hero_skills/src/hero_skills/Suction_cup.py | 19 ++++++++++++-------
 hero_skills/src/hero_skills/hero.py        |  2 ++
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/hero_skills/src/hero_skills/Suction_cup.py b/hero_skills/src/hero_skills/Suction_cup.py
index dcc27a41cd..59c43b5466 100644
--- a/hero_skills/src/hero_skills/Suction_cup.py
+++ b/hero_skills/src/hero_skills/Suction_cup.py
@@ -15,14 +15,14 @@ class SuctionGripper(Gripper):
     """
     A Suction gripper that has a vacuum suction cup to grasp objects
     """
-    def __init__(self, robot_name, tf_listener, gripper_name):
+    def __init__(self, robot_name, tf_buffer, gripper_name):
         """
         constructor
         :param robot_name: robot_name
-        :param tf_listener: tf_server.TFClient()
+        :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_listener)
+        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"]))
@@ -34,7 +34,7 @@ def __init__(self, robot_name, tf_listener, gripper_name):
         # listening for goals
         self._ac_suction.wait_for_server()
 
-    def send_gripper_goal(self, sucking, timeout=5.0):
+    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
@@ -55,10 +55,15 @@ def send_gripper_goal(self, sucking, timeout=5.0):
 
         self._ac_suction.send_goal(goal)
 
-        goal_status = GoalStatus.SUCCEEDED
-
         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 == 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)