os.path.join(config['calib_dir'], sensor_frame, tf_filename)) # setup sensor sensor = RgbdSensorFactory.sensor(sensor_type, config['sensor']) sensor.start() camera_intr = sensor.ir_intrinsics # read images color_im, depth_im, _ = sensor.frames() color_im = color_im.inpaint(rescale_factor=inpaint_rescale_factor) depth_im = depth_im.inpaint(rescale_factor=inpaint_rescale_factor) rgbd_im = RgbdImage.from_color_and_depth(color_im, depth_im) state = RgbdImageState(rgbd_im, camera_intr) # init policy policy = CrossEntropyAntipodalGraspingPolicy(policy_config) policy_start = time.time() action = policy(state) logging.info('Planning took %.3f sec' % (time.time() - policy_start)) # vis final grasp if policy_config['vis']['final_grasp']: vis.figure(size=(10, 10)) vis.subplot(1, 2, 1) vis.imshow(rgbd_im.color) vis.grasp(action.grasp, scale=1.5, show_center=False, show_axis=True) vis.title('Planned grasp on color (Q=%.3f)' % (action.q_value)) vis.subplot(1, 2, 2) vis.imshow(rgbd_im.depth) vis.grasp(action.grasp, scale=1.5, show_center=False, show_axis=True) vis.title('Planned grasp on depth (Q=%.3f)' % (action.q_value))
return gqcnn_grasp if __name__ == '__main__': # initialize the ROS node rospy.init_node('Grasp_Sampler_Server') # initialize cv_bridge cv_bridge = CvBridge() # get configs cfg = YamlConfig(rospy.get_param('~config')) policy_cfg = cfg['policy'] # create publisher to publish pose only of final grasp grasp_pose_publisher = rospy.Publisher('/gqcnn_grasp/pose', PoseStamped, queue_size=10) # create a policy rospy.loginfo('Creating Grasp Policy') grasping_policy = CrossEntropyAntipodalGraspingPolicy(policy_cfg) # create a grasp planner object grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher) # initialize the service service = rospy.Service('plan_gqcnn_grasp', GQCNNGraspPlanner, grasp_planner.plan_grasp) rospy.loginfo('Grasp Sampler Server Initialized') # spin rospy.spin()