dump_estimate.py
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08