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)
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)
#linear:
# x: 0.1
# y: 0.2
# z: 0.3
#angular:
# x: 0.3
# y: 0.0
# z: 0.0
homo_mat = PoseConv.to_homo_mat(twist_msg)
print homo_mat
#[[ 1. 0. 0. 0.1 ]
# [ 0. 0.95533649 -0.29552021 0.2 ]
# [-0. 0.29552021 0.95533649 0.3 ]
# [ 0. 0. 0. 1. ]]
[pos, rot] = homo_mat[:3, 3], homo_mat[:3, :3]
tf_stamped_msg = PoseConv.to_tf_stamped_msg("base_link", pos, rot)
print tf_stamped_msg
#header:
# seq: 0
# stamp:
# secs: 1341611677
# nsecs: 579762935
# frame_id: base_link
#child_frame_id: ''
#transform:
# translation:
# x: 0.1
# y: 0.2
# z: 0.3
# rotation:
# x: 0.149438132474
#!/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]
