本文整理汇总了Python中hrl_geom.pose_converter.PoseConv.to_tf_stamped_msg方法的典型用法代码示例。如果您正苦于以下问题：Python PoseConv.to_tf_stamped_msg方法的具体用法？Python PoseConv.to_tf_stamped_msg怎么用？Python PoseConv.to_tf_stamped_msg使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类hrl_geom.pose_converter.PoseConv的用法示例。
在下文中一共展示了PoseConv.to_tf_stamped_msg方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: register_ellipse
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_tf_stamped_msg [as 别名]
def register_ellipse(self, mode, side):
reg_e_params = EllipsoidParams()
try:
now = rospy.Time.now()
self.tfl.waitForTransform("/base_link", "/head_frame", now, rospy.Duration(10.))
pos, quat = self.tfl.lookupTransform("/head_frame", "/base_frame", now)
self.head_pose = PoseStamped()
self.head_pose.header.stamp = now
self.head_pose.header.frame_id = "/base_link"
self.head_pose.pose.position = Point(*pos)
self.head_pose.pose.orientation = Quaternion(*quat)
except:
rospy.logwarn("[%s] Head registration not loaded yet." %rospy.get_name())
return (False, reg_e_params)
reg_prefix = rospy.get_param("~registration_prefix", "")
registration_files = rospy.get_param("~registration_files", None)
if mode not in registration_files:
rospy.logerr("[%s] Mode not in registration_files parameters" % (rospy.get_name()))
return (False, reg_e_params)
try:
bag_str = reg_prefix + registration_files[mode][side]
rospy.loginfo("[%s] Loading %s" %(rospy.get_name(), bag_str))
bag = rosbag.Bag(bag_str, 'r')
e_params = None
for topic, msg, ts in bag.read_messages():
e_params = msg
assert e_params is not None
bag.close()
except:
rospy.logerr("[%s] Cannot load registration parameters from %s" %(rospy.get_name(), bag_str))
return (False, reg_e_params)
head_reg_mat = PoseConv.to_homo_mat(self.head_pose)
ell_reg = PoseConv.to_homo_mat(Transform(e_params.e_frame.transform.translation,
e_params.e_frame.transform.rotation))
reg_e_params.e_frame = PoseConv.to_tf_stamped_msg(head_reg_mat**-1 * ell_reg)
reg_e_params.e_frame.header.frame_id = self.head_reg_tf.header.frame_id
reg_e_params.e_frame.child_frame_id = '/ellipse_frame'
reg_e_params.height = e_params.height
reg_e_params.E = e_params.E
self.ell_params_pub.publish(reg_e_params)
self.ell_frame = reg_e_params.e_frame
return (True, reg_e_params)示例2: locals
# 需要导入模块: from hrl_geom.pose_converter import PoseConv [as 别名]
# 或者: from hrl_geom.pose_converter.PoseConv import to_tf_stamped_msg [as 别名]
#!/usr/bin/python
import numpy as np
import roslib
roslib.load_manifest("hrl_geom")
import rospy
from hrl_geom.pose_converter import PoseConv
if __name__ == "__main__":
rospy.init_node("test_poseconv")
homo_mat = PoseConv.to_homo_mat([0., 1., 2.], [0., 0., np.pi/2])
pose_msg = PoseConv.to_pose_msg(homo_mat)
pos, quat = PoseConv.to_pos_quat(pose_msg)
pose_stamped_msg = PoseConv.to_pose_stamped_msg("/base_link", pos, quat)
pose_stamped_msg2 = PoseConv.to_pose_stamped_msg("/base_link", [pos, quat])
tf_stamped = PoseConv.to_tf_stamped_msg("/new_link", pose_stamped_msg)
p, R = PoseConv.to_pos_rot("/new_link", tf_stamped)
for name in ["homo_mat", "pose_msg", "pos", "quat", "pose_stamped_msg",
"pose_stamped_msg2", "tf_stamped", "p", "R"]:
print "%s: \n" % name, locals()[name]