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()