grasp data sending

Author: Yuxiang Gao

ropi_msgs/msg/GraspData.msg

@@ -1,4 +1,5 @@
-geometry_msgs/Point center
-float32 radius
+geometry_msgs/Point position
+geometry_msgs/Point target_position
+float32 diameter
 float32 angle
 float32 height

ropi_msgs/msg/GraspDataArray.msg

@@ -0,0 +1 @@
+ropi_msgs/GraspData[] data

ropi_msgs/srv/MoveObjects.srv

@@ -2,6 +2,5 @@ ropi_msgs/RegionSelectionMsg source_selection
 ropi_msgs/RegionSelectionMsg target_selection
 ---
 bool success
-ropi_msgs/GraspData[] pick_data
-ropi_msgs/GraspData[] place_data
+ropi_msgs/GraspData[] object_data
 string message

ropi_tangible_surface/scripts/pick_and_place.py

@@ -193,7 +193,7 @@ def move_joints(self, joints, duration=3.0):
                 self.joints_pos_start = joints
                 rospy.loginfo('[move_joints] Result: {}'.format(self.client.wait_for_result()))
                 finish_state = self.client.get_state()
-                if (not finish_state == UR_STATES['ACTIVE'] or not finish_state == UR_STATES['SUCCEEDED']):
+                if (not finish_state == UR_STATES['ACTIVE'] and not finish_state == UR_STATES['SUCCEEDED']):
                     raise UR5MissionError(finish_state, inspect.stack()[0][3])
             except KeyboardInterrupt:
                 self.client.cancel_goal()
@@ -223,7 +223,7 @@ def move_traj(self, traj, duration=3.0):
                 # self.client.wait_for_result()
                 rospy.loginfo('[move_traj] Result: {}'.format(self.client.wait_for_result()))
                 finish_state = self.client.get_state()
-                if (not finish_state == UR_STATES['ACTIVE'] or finish_state == UR_STATES['SUCCEEDED']):
+                if (not finish_state == UR_STATES['ACTIVE'] and not finish_state == UR_STATES['SUCCEEDED']):
                     raise UR5MissionError(finish_state, inspect.stack()[0][3])
             except KeyboardInterrupt:
                 self.client.cancel_goal()
@@ -259,7 +259,7 @@ def find_ik(self, traj):
     # pt: [[x,y], angle]
     def pick_and_place(self, pt1, pt2, object_height, object_diameter=0.14):
         try:
-            self.gripper_sc.move_to(np.clip(object_diameter + 0.02, 0, 0.14))
+            self.gripper_sc.move_to(np.clip(object_diameter + 0.05, 0, 0.14))
             traj = self.generate_trajectory(pt1, pt2, object_height)
             # print('traj: {}'.format(traj))
             self.move_traj(traj[:2])

ropi_tangible_surface/scripts/ropi_tangible_surface/object_detection.py

@@ -5,6 +5,9 @@
 import cv2
 import tf
 
+from ropi_msgs.msg import GraspData
+from geometry_msgs.msg import Point
+
 class GraspDataClass:
     def __init__(self, position, diameter, angle, height):
         self.position = np.asarray(position)
@@ -26,6 +29,15 @@ def draw(self, img, color):
         cv2.line(debug_img, pt1, pt2, color, 5)
         return debug_img
 
+    def make_msg(self):
+        msg = GraspData()
+        msg.position = Point(self.position[0], self.position[1], self.height)
+        if self.target_position is not None:
+            msg.target_position = Point(self.target_position[0], self.target_position[1], self.height)
+        msg.diameter = self.diameter
+        msg.height = self.height
+        msg.angle = self.angle
+
     def __repr__(self):
         return '[GraspDataClass]: {}-{}-|Height: {}|Angle: {} |Diameter: {}'.format(self.position, self.target_position, self.height,self.angle, self.diameter)
         # return repr(self.position)

ropi_tangible_surface/scripts/tangible_surface.py

@@ -81,6 +81,7 @@ def subscribe(self, img_topic, depth_topic):
             'delete_selection', DeleteSelection, self.delete_selection_callback)
         self.move_server = rospy.Service(
             'move_objects', MoveObjects, self.move_objects_callback)
+        self.grasp_pub = rospy.Publisher('grasp_data', GraspData, queue_size=1)
 
     def detect_objects_in_region(self, msg):
         self.selection_manager.update([msg])
@@ -99,6 +100,8 @@ def move_objects_callback(self, req):
             response.success = True
             response.message = '{} source objects detected.'.format(len(mission))
             rospy.loginfo('mission generated, executing mission.')
+            # response.object_data = [x.make_msg() for x in mission]
+            # self.grasp_pub.publish([x.make_msg() for x in mission])
             try:
                 self.robot_interface.pick_and_place_mission(mission)
             except:
@@ -161,9 +164,9 @@ def region_selection_callback(self, req):
             grasp_data = []
             for gp in grasp_points:
                 grasp_datum = GraspData()
-                grasp_datum.center.x = gp.center[0]
-                grasp_datum.center.y = gp.center[1]
-                grasp_datum.radius = gp.radius
+                grasp_datum.position.x = gp.position[0]
+                grasp_datum.position.y = gp.position[1]
+                grasp_datum.diameter = gp.diameter 
                 grasp_datum.angle = gp.angle
                 grasp_datum.height = gp.height
                 grasp_data.append(grasp_datum)
@@ -294,7 +297,7 @@ def detect_fingertip(self, img):
 
     def detect_object(self, img):
         objects = []
-        threshed = my_threshold(img, 15, 150)
+        threshed = my_threshold(img, 5, 150)
         mask = np.zeros(img.shape, dtype=np.uint8)
         mask[threshed > 0] = 255
         dst = img.copy()
@@ -304,7 +307,8 @@ def detect_object(self, img):
         merge_list = lambda l1, l2: l2 if not l1 else (l1 if not l2 else np.concatenate((l1, l2)))
         object_contours = []
         if len(contours) != 0:
-            contours = filter(lambda c: cv2.contourArea(c) > 400 and cv2.contourArea(c) < 1500, contours)
+            # print([cv2.contourArea(c) for c in contours])
+            contours = filter(lambda c: cv2.contourArea(c) > 400 and cv2.contourArea(c) < 3000, contours)
             debug_img = dst.copy()
             objects = self.object_detector.update(contours, dst, debug_img)
             debug_img = self.object_detector.debug_img