Stable version of GQCNN Grasp Planning Service

CMakeLists.txt

@@ -53,7 +53,7 @@ catkin_python_setup()
 ## Generate services in the 'srv' folder
  add_service_files(
   FILES
-  GraspPlanner.srv
+  GQCNNGraspPlanner.srv
  )
 
 ## Generate actions in the 'action' folder

cfg/ros_nodes/grasp_planner_node.yaml

@@ -1,24 +1,16 @@
-# ros_topics to subscribe to
-ros_topics:
-  camera_intrinsics: /primesense_overhead/rgb/camera_info
-  object_detector:
-    bounding_boxes: /yumi_primesense_object_detector/bounding_boxes
-    rgb_image: /yumi_primesense_object_detector/image/rgb
-    depth_image: /yumi_primesense_object_detector/image/depth
-
-# visualize cropped images
-vis_cropped_images: 0
+# visualization
+vis:
+  vis_cropped_rgbd_image: 1
+  vis_uncropped_color_image: 0
+  vis_uncropped_depth_image: 0
 
 # padding for the cropped images so when they are rotated to be centered on the grasp point there will not be any gaps where there is no part of the image
-width_pad: 60
-height_pad: 60
+width_pad: 0
+height_pad: 0
 
 # inpaint
 inpaint_rescale_factor: 0.5
 
-# sensor calibration dir
-calib_dir: /home/autolab/Public/alan/calib
-
 # policy params
 policy:
   # optimization params
@@ -30,7 +22,7 @@ policy:
   gmm_reg_covar: 0.01
 
   # gqcnn params
-  gqcnn_model: /home/autolab/Public/data/dex-net/data/models/gqcnn_test_network
+  gqcnn_model: /mnt/hdd/dex-net/data/models/grasp_quality/gqcnn_dexnet_large
 
   # general params
   deterministic: 1
@@ -62,13 +54,14 @@ policy:
     depth_sample_win_height: 1
     depth_sample_win_width: 1
     min_depth_offset: 0.015
+    max_depth_offset: .03
     max_depth: 0.69
 
   # visualization
   vis:
     grasp_sampling : 0
     tf_images: 0
-    grasp_candidates: 0
+    grasp_candidates: 1
     elite_grasps: 0
     grasp_ranking: 0
     grasp_plan: 0

launch/gqcnn.launch

@@ -2,7 +2,10 @@
   <!-- Namespace for the node and services -->
   <arg name="ns"  default="gqcnn" />
 
-  <group ns="$(arg ns)">
-    <node name="grasp_sampler_node"  pkg="gqcnn" type="grasp_sampler_node.py" />
-  </group>
+  <!-- Configuration file for Grasp Planner Node -->
+  <arg name="config"           default="$(find gqcnn)/cfg/ros_nodes/grasp_planner_node.yaml" />
+
+  <node name="GQCNN_grasp_planner"  pkg="gqcnn" type="grasp_planner_node.py" output="screen" >
+    	<param name="config"           value="$(arg config)" />
+  </node>
 </launch>

ros_nodes/grasp_planner_node.py

@@ -12,96 +12,157 @@
 from autolab_core import YamlConfig
 from gqcnn import CrossEntropyAntipodalGraspingPolicy, RgbdImageState
 from gqcnn import NoValidGraspsException, NoAntipodalPairsFoundException
+from gqcnn import Visualizer as vis
 
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Header
-from gqcnn.srv import GraspPlanner
+from gqcnn.srv import GQCNNGraspPlanner
+from gqcnn.msg import GQCNNGrasp
 
 class GraspPlanner(object):
     def __init__(self, cfg, cv_bridge, grasping_policy, grasp_pose_publisher):
+        """
+        Parameters
+        ----------
+        cfg : dict
+            dictionary of configuration parameters
+        cv_bridge: :obj:`CvBridge`
+            ROS CvBridge
+        grasping_policy: :obj:`GraspingPolicy`
+            grasping policy to use
+        grasp_pose_publisher: :obj:`Publisher`
+            ROS Publisher to publish planned grasp's ROS Pose only for visualization
+        """
         self.cfg = cfg
         self.cv_bridge = cv_bridge
         self.grasping_policy = grasping_policy
         self.grasp_pose_publisher = grasp_pose_publisher
 
     def plan_grasp(self, req):
-        """ Grasp planner request handler """
-
+        """ Grasp planner request handler 
+        
+        Parameters
+        ---------
+        req: :obj:`ROS ServiceRequest`
+            ROS ServiceRequest for grasp planner service
+        """
+        rospy.loginfo('Planning Grasp')
+
         # get the raw depth and color images as ROS Image objects
         raw_color = req.color_image
         raw_depth = req.depth_image
 
         # get the raw camera info as ROS CameraInfo object
         raw_camera_info = req.camera_info
-
-        # get the bounding box as a Custom ROS BoundingBox msg 
+        
+        # get the bounding box as a custom ROS BoundingBox msg 
         bounding_box = req.bounding_box
 
         # wrap the camera info in a perception CameraIntrinsics object
         camera_intrinsics = perception.CameraIntrinsics(raw_camera_info.header.frame_id, raw_camera_info.K[0], raw_camera_info.K[4], raw_camera_info.K[2], raw_camera_info.K[5], raw_camera_info.K[1], raw_camera_info.height, raw_camera_info.width)
 
-        ### create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge and wrapping them ###
+        ### Create wrapped Perception RGB and Depth Images by unpacking the ROS Images using CVBridge ###
         try:
-            color_image = perception.ColorImage(self.cv_bridge.imgmsg_to_cv2(data, "rgb8"), frame=camera_intrinsics.frame)
-            depth_image = perception.DepthImage(self.cv_bridge.imgmsg_to_cv2(data, desired_encoding = "passthrough"), frame=camera_intrinsics.frame)
+            color_image = perception.ColorImage(self.cv_bridge.imgmsg_to_cv2(raw_color, "rgb8"), frame=camera_intrinsics.frame)
+            depth_image = perception.DepthImage(self.cv_bridge.imgmsg_to_cv2(raw_depth, desired_encoding = "passthrough"), frame=camera_intrinsics.frame)
         except CvBridgeError as cv_bridge_exception:
             rospy.logerr(cv_bridge_exception)
 
-        # inpaint to remove holes
-        inpainted_color_image = color_image.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
-        inpainted_depth_image = depth_image.inpaint(rescale_factor=cfg['inpaint_rescale_factor'])
+        # visualize
+        if self.cfg['vis']['vis_uncropped_color_image']:
+            vis.imshow(color_image)
+            vis.show()
+        if self.cfg['vis']['vis_uncropped_depth_image']:
+            vis.imshow(depth_image)
+            vis.show()
 
         # aggregate color and depth images into a single perception rgbdimage
-        rgbd_image = perception.RgbdImage.from_color_and_depth(inpainted_color_image, inpainted_depth_image)
+        rgbd_image = perception.RgbdImage.from_color_and_depth(color_image, depth_image)
 
         # calc crop parameters
         minX = bounding_box.minX
         minY = bounding_box.minY
         maxX = bounding_box.maxX
         maxY = bounding_box.maxY
+
+        # contain box to image->don't let it exceed image height/width bounds
+        no_pad = False
+        if minX < 0:
+            minX = 0
+            no_pad = True
+        if minY < 0:
+            minY = 0
+            no_pad = True
+        if maxX > rgbd_image.width:
+            maxX = rgbd_image.width
+            no_pad = True
+        if maxY > rgbd_image.height:
+            maxY = rgbd_image.height
+            no_pad = True
+
         centroidX = (maxX + minX) / 2
         centroidY = (maxY + minY) / 2
 
         # add some padding to bounding box to prevent empty pixel regions when the image is rotated during grasp planning
-        width = (maxX - minX) + self.cfg['width_pad']
-        height = (maxY - minY) + self.cfg['height_pad']
-
+        if not no_pad:
+            width = (maxX - minX) + self.cfg['width_pad']
+            height = (maxY - minY) + self.cfg['height_pad']
+        else:
+            width = (maxX - minX)
+            height = (maxY - minY)
+
         # crop camera intrinsics and rgbd image
-        cropped_camera_intrinsics = camera_intrinsics.crop(height, width, centroidX, centroidY)
-        cropped_rgbd_image = rgbd_image.crop(height, width, centroidX, centroidY)
+        cropped_camera_intrinsics = camera_intrinsics.crop(height, width, centroidY, centroidX)
+        cropped_rgbd_image = rgbd_image.crop(height, width, centroidY, centroidX)
 
         # visualize  
-        if self.cfg['vis_cropped_image']:
-            vis.imshow(cropped_rgbd_image, 'Cropped RGB Image')
+        if self.cfg['vis']['vis_cropped_rgbd_image']:
+            vis.imshow(cropped_rgbd_image)
             vis.show()
 
         # create an RGBDImageState with the cropped RGBDImage and CameraIntrinsics
         image_state = RgbdImageState(cropped_rgbd_image, cropped_camera_intrinsics)
+
         # execute policy
         try:
-            return self.execute_policy(image_state, self.grasping_policy, self.grasp_pose_publisher)
+            return self.execute_policy(image_state, self.grasping_policy, self.grasp_pose_publisher, cropped_camera_intrinsics.frame)
         except NoValidGraspsException:
             rospy.logerr('While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!')
+            raise rospy.ServiceException('While executing policy found no valid grasps from sampled antipodal point pairs. Aborting Policy!')
         except NoAntipodalPairsFoundException:
             rospy.logerr('While executing policy could not sample any antipodal point pairs from input image. Aborting Policy! Please check if there is an object in the workspace or if the output of the object detector is reasonable.')
+            raise rospy.ServiceException('While executing policy could not sample any antipodal point pairs from input image. Aborting Policy! Please check if there is an object in the workspace or if the output of the object detector is reasonable.')
 
-    def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher):
+    def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher, pose_frame):
+        """ Executes a grasping policy on an RgbdImageState
+        
+        Parameters
+        ----------
+        rgbd_image_state: :obj:`RgbdImageState`
+            RgbdImageState from perception module to encapsulate depth and color image along with camera intrinsics
+        grasping_policy: :obj:`GraspingPolicy`
+            grasping policy to use
+        grasp_pose_publisher: :obj:`Publisher`
+            ROS Publisher to publish planned grasp's ROS Pose only for visualization
+        pose_frame: :obj:`str`
+            frame of reference to publish pose alone in
+        """
         # execute the policy's action
         rospy.loginfo('Planning Grasp')
         grasp_planning_start_time = time.time()
         grasp = grasping_policy(rgbd_image_state)
-
+  
         # create GQCNNGrasp return msg and populate it
         gqcnn_grasp = GQCNNGrasp()
-        gqcnn_grasp.grasp_success_prob = grasp.p_success[0]
+        gqcnn_grasp.grasp_success_prob = grasp.q_value
         gqcnn_grasp.pose = grasp.grasp.pose().pose_msg
 
         # create and publish the pose alone for visualization ease of grasp pose in rviz
         pose_stamped = PoseStamped()
         pose_stamped.pose = grasp.grasp.pose().pose_msg
         header = Header()
         header.stamp = rospy.Time.now()
-        header.frame_id = camera_intrinsics.frame
+        header.frame_id = pose_frame
         pose_stamped.header = header
         grasp_pose_publisher.publish(pose_stamped)
 
@@ -119,7 +180,7 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher
     cv_bridge = CvBridge()
 
     # get configs
-    cfg = YamlConfig('cfg/ros_nodes/grasp_planner_node.yaml')
+    cfg = YamlConfig(rospy.get_param('~config'))
     policy_cfg = cfg['policy']
 
     # create publisher to publish pose only of final grasp
@@ -133,7 +194,7 @@ def execute_policy(self, rgbd_image_state, grasping_policy, grasp_pose_publisher
     grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher)
 
     # initialize the service        
-    service = rospy.Service('plan_gqcnn_grasp', PlanGQCNNGrasp, grasp_planner.plan_grasp)
+    service = rospy.Service('plan_gqcnn_grasp', GQCNNGraspPlanner, grasp_planner.plan_grasp)
     rospy.loginfo('Grasp Sampler Server Initialized')
 
     # spin

srv/GraspPlanner.srv → srv/GQCNNGraspPlanner.srv

@@ -5,4 +5,4 @@ sensor_msgs/CameraInfo camera_info
 BoundingBox bounding_box
 ---
 # response params
-GQGNNGrasp grasp
+GQCNNGrasp grasp