00001
00002 import yaml
00003 from camera_pose_calibration.msg import CameraPose
00004
00005 from tf_conversions import posemath
00006
00007 def to_urdf(cameras):
00008 parent_link = "world_link"
00009 urdf = ""
00010 urdf += '<?xml version="1.0">\n'
00011 urdf += '<robot>\n'
00012 urdf += ' <link name="%s"/>\n' % parent_link
00013 for cur_cam in cameras:
00014 urdf += ' <link name="%s">' % cur_cam.camera_id
00015 urdf += ' <joint name="%s_joint" type="fixed">\n' % cur_cam.camera_id
00016 urdf += ' <parent link="%s"/>\n' % parent_link
00017 urdf += ' <child link="%s"/>\n' % cur_cam.camera_id
00018
00019 f = posemath.fromMsg(cur_cam.pose)
00020 urdf += ' <origin xyz="%f %f %f" rpy="%f %f %f" />\n' % ( (f.p.x(), f.p.y(), f.p.z()) + f.M.GetRPY() )
00021 urdf += ' </joint>\n'
00022 urdf += '</robot>\n'
00023 return urdf
00024
00025 def to_dict_list(cameras):
00026 d = [ {'camera_id': cam.camera_id,
00027 'position':
00028 {'x':cam.pose.position.x, 'y':cam.pose.position.y, 'z':cam.pose.position.z},
00029 'orientation':
00030 {'x':float(cam.pose.orientation.x), 'y':float(cam.pose.orientation.y), 'z':float(cam.pose.orientation.z), 'w':float(cam.pose.orientation.w)}
00031 } for cam in cameras]
00032 return d
00033
