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]