example.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 import numpy as np
00003 import roslib
00004 roslib.load_manifest("hrl_geom")
00005 import rospy
00006 from hrl_geom.pose_converter import PoseConv
00007 
00008 if __name__ == "__main__":
00009     rospy.init_node("test_poseconv")
00010     homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
00011     pose_msg = PoseConv.to_pose_msg(homo_mat)
00012     pos, quat = PoseConv.to_pos_quat(pose_msg)
00013     pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
00014     pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
00015     tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
00016     p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
00017     for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg", 
00018                  "pose_stamped_msg2", "tf_stamped", "p", "R"]:
00019         print "%s: \n" % name, locals()[name]


hrl_geom
Author(s): Kelsey Hawkins / kphawkins@gatech.edu, Advisor: Prof. Charlie Kemp (Healthcare Robotics Lab at Georgia Tech)
autogenerated on Wed Nov 27 2013 11:37:25