Go to the documentation of this file.00001
00002
00003 import roslib; roslib.load_manifest('hardcoded_facts')
00004 import rospy
00005
00006 from geometry_msgs import msg as geometry
00007
00008 def read_pose_stamped_file(file_name):
00009 data_file = open(file_name, 'r')
00010 poses_data = data_file.readlines()
00011 data_file.close()
00012 named_poses = dict()
00013 for object in poses_data:
00014 object = object.strip()
00015 if len(object) == 0 or object.startswith('#'):
00016 continue
00017 print object
00018 pose = geometry.PoseStamped()
00019 [name, secs, nsecs, frame_id, x, y, z, qx, qy, qz, qw] = object.split()
00020 pose.header.frame_id = frame_id
00021 pose.pose.position.x = float(x)
00022 pose.pose.position.y = float(y)
00023 pose.pose.position.z = float(z)
00024 pose.pose.orientation.x = float(qx)
00025 pose.pose.orientation.y = float(qy)
00026 pose.pose.orientation.z = float(qz)
00027 pose.pose.orientation.w = float(qw)
00028 named_poses[name]=pose
00029 return named_poses
00030
00031 def read_table_file(file_name):
00032 data_file = open(file_name, 'r')
00033 poses_data = data_file.readlines()
00034 data_file.close()
00035 tables = list()
00036 for object in poses_data:
00037 object = object.strip()
00038 if len(object) == 0 or object.startswith('#'):
00039 continue
00040 print object
00041 pose = geometry.PoseStamped()
00042 [name, secs, nsecs, frame_id, x, y, z, qx, qy, qz, qw, size_x, size_y, size_z] = object.split()
00043 pose.header.frame_id = frame_id
00044 pose.pose.position.x = float(x)
00045 pose.pose.position.y = float(y)
00046 pose.pose.position.z = float(z)
00047 pose.pose.orientation.x = float(qx)
00048 pose.pose.orientation.y = float(qy)
00049 pose.pose.orientation.z = float(qz)
00050 pose.pose.orientation.w = float(qw)
00051 tables.append([pose, [float(size_x), float(size_y), float(size_z)], name])
00052 return tables
00053
00054
00055
