00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 import roslib; roslib.load_manifest('pal_blort')
00038 import re
00039 import subprocess
00040 from multiprocessing import Process
00041 from geometry_msgs.msg import Pose
00042 import sys
00043
00044 def readPose(filename):
00045 f = open(filename, 'r')
00046 content = f.read()
00047 f.close()
00048 vects = re.findall(r'\[.*\]', content)
00049 pose = Pose()
00050 position = re.findall(r'[-0-9.]+', vects[0])
00051 orientation = re.findall(r'[-0-9.]+', vects[1])
00052
00053 pose.position.x = float(position[0])
00054 pose.position.y = float(position[1])
00055 pose.position.z = float(position[2])
00056 pose.orientation.x = float(orientation[0])
00057 pose.orientation.y = float(orientation[1])
00058 pose.orientation.z = float(orientation[2])
00059 pose.orientation.w = float(orientation[3])
00060 return pose
00061
00062 def readPoses(filename):
00063 poses = []
00064 f = open(filename, 'r')
00065 content = f.read()
00066 f.close()
00067 numbers = re.findall(r'[-0-9.]+', content)
00068 for i in range(0,len(numbers), 7):
00069 pose = Pose()
00070 pose.position.x = float(numbers[i])
00071 pose.position.y = float(numbers[i+1])
00072 pose.position.z = float(numbers[i+2])
00073 pose.orientation.x = float(numbers[i+3])
00074 pose.orientation.y = float(numbers[i+4])
00075 pose.orientation.z = float(numbers[i+5])
00076 pose.orientation.w = float(numbers[i+6])
00077 poses.append(pose)
00078 return poses
00079
00080 def poseDiff(pose1, pose2):
00081 result = Pose()
00082 result.position.x = abs(pose1.position.x - pose2.position.x)
00083 result.position.y = abs(pose1.position.y-pose2.position.y)
00084 result.position.z = abs(pose1.position.z-pose2.position.z)
00085 result.orientation.x = abs(pose1.orientation.x-pose2.orientation.x)
00086 result.orientation.y = abs(pose1.orientation.y-pose2.orientation.y)
00087 result.orientation.z = abs(pose1.orientation.z-pose2.orientation.z)
00088 result.orientation.w = abs(pose1.orientation.w-pose2.orientation.w)
00089 return result
00090
00091 def poseListMean(poseList):
00092 result = Pose()
00093 count = float(len(poseList))
00094 result.position.x = sum([act_pose.position.x for act_pose in poseList]) / count
00095 result.position.y = sum([act_pose.position.y for act_pose in poseList]) / count
00096 result.position.z = sum([act_pose.position.z for act_pose in poseList]) / count
00097 result.orientation.x = sum([act_pose.orientation.x for act_pose in poseList]) / count
00098 result.orientation.y = sum([act_pose.orientation.y for act_pose in poseList]) / count
00099 result.orientation.z = sum([act_pose.orientation.z for act_pose in poseList]) / count
00100 result.orientation.w = sum([act_pose.orientation.w for act_pose in poseList]) / count
00101 return result
00102
00103 def poseListAvgDiff(pose, poselist):
00104 diffs = [poseDiff(pose, act_pose) for act_pose in poselist]
00105 return poseListMean(diffs)
00106
00107 def poseListVariance(poseList):
00108 meanPose = poseListMean(poseList)
00109 return poseDiffList(meanPose, poseList)
00110
00111 def core():
00112 subprocess.call("roscore", executable="bash", shell=False)
00113
00114 def disparity():
00115 subprocess.call("roslaunch pal_disparity_segmentation disparity_check.launch", executable="bash", shell=False)
00116
00117 def blort():
00118 subprocess.call("roslaunch pal_blort blort_singleshot_disparity.launch", executable="bash", shell=False)
00119
00120 def disparity():
00121 subprocess.call("roslaunch pal_disparity_segmentation disparity_check.launch", executable="bash", shell=False)
00122
00123 def launch_procs():
00124 processes = [Process(target=core), Process(target=disparity), Process(target=blort)]
00125 for p in processes:
00126 p.start()
00127 return processes
00128
00129 def poseValidate(pose, pose_estimate, max_error):
00130 diff = poseDiff(pose, pose_estimate)
00131 if (diff.position.x - max_error.position.x < 0) and \
00132 (diff.position.y - max_error.position.y < 0) and \
00133 (diff.position.z - max_error.position.z < 0) and \
00134 (diff.orientation.x - max_error.orientation.x < 0) and \
00135 (diff.orientation.y - max_error.orientation.y < 0) and \
00136 (diff.orientation.z - max_error.orientation.z < 0) and \
00137 (diff.orientation.w - max_error.orientation.w < 0):
00138 return True
00139 else:
00140 return False
00141
00142 def validatePosesFromFile(pose, max_error, filename):
00143 poses = readPoses(filename)
00144 return [poseValidate(pose, act, max_error) for act in poses]
00145
00146 def geomQuatTo3x3Mat(quaternion):
00147 x2 = quaternion.x * quaternion.x;
00148 y2 = quaternion.y * quaternion.y;
00149 z2 = quaternion.z * quaternion.z;
00150 xy = quaternion.x * quaternion.y;
00151 xz = quaternion.x * quaternion.z;
00152 yz = quaternion.y * quaternion.z;
00153 wx = quaternion.w * quaternion.x;
00154 wy = quaternion.w * quaternion.y;
00155 wz = quaternion.w * quaternion.z;
00156
00157 result = [[1.0-(2.0*(y2 + z2)), 2.0*(xy - wz), 2.0*(xz + wy)], \
00158 [2.0*(xy + wz), 1.0-2.0*(x2 + z2), 2.0*(yz - wx)], \
00159 [2.0*(xz - wy), 2.0*(yz + wx), 1.0-(2.0*(x2 + y2))]]
00160 return result
00161
