Go to the documentation of this file.00001
00002 import roslib; roslib.load_manifest('hai_sandbox')
00003 import hrl_camera.ros_camera as rc
00004 import hrl_lib.rutils as ru
00005 import hrl_lib.util as ut
00006 import rospy
00007 import math
00008 import cv
00009
00010
00011 import tf
00012 import hrl_lib.tf_utils as tfu
00013 import tf.transformations as tr
00014
00015 if __name__ == '__main__':
00016 ls = ru.LaserScanner('point_cloud_srv')
00017 prosilica = rc.Prosilica('prosilica', 'streaming')
00018
00019
00020 if __name__ == '__main__':
00021 import sys
00022
00023 base_name = sys.argv[1]
00024 test = 'laser'
00025 ls = ru.LaserScanner('point_cloud_srv')
00026 prosilica = rc.Prosilica('prosilica', 'streaming')
00027 tf_listener = tf.TransformListener()
00028
00029 rospy.loginfo( 'Getting laser scan.')
00030 points = ls.scan(math.radians(180.), math.radians(-180.), 20.)
00031 rospy.loginfo('Size of point cloud: %d' % len(points.points))
00032 rospy.loginfo( 'Grabbing image.')
00033 image = prosilica.get_frame()
00034 rospy.loginfo( 'Grabbing transforms.')
00035
00036
00037
00038 pro_T_bf = tfu.transform('/high_def_optical_frame', '/base_footprint', tf_listener)
00039
00040
00041 map_T_bf = tfu.transform('/map', '/base_footprint', tf_listener)
00042
00043
00044 rospy.loginfo('Waiting for camera_info.')
00045 calibration = rc.ROSCameraCalibration('/prosilica/camera_info')
00046 r = rospy.Rate(10)
00047 while not rospy.is_shutdown() and calibration.has_msg == False:
00048 r.sleep()
00049
00050 rospy.loginfo('Saving.')
00051 pkl_name = '%s.pkl' % base_name
00052 img_name = '%s.png' % base_name
00053 ut.save_pickle({'points': points,
00054 'pro_T_bf': pro_T_bf,
00055 'map_T_bf': map_T_bf,
00056 'camera_info': calibration}, pkl_name)
00057 cv.SaveImage(img_name, image)
00058 rospy.loginfo( 'Saved to %s and %s.' % (pkl_name, img_name))
00059
00060