def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose( "/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat( tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3, 0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3, 0:3] self.cheek_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish( PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'
def pub_head_registration(ud): cheek_pose_base_link = self.tf_list.transformPose("/base_link", ud.cheek_pose) # find the center of the ellipse given a cheek click cheek_transformation = np.mat(tf_trans.euler_matrix(2.6 * np.pi/6, 0, 0, 'szyx')) cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link) #b_B_c[0:3,0:3] = np.eye(3) norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2]) head_rot = np.arctan2(norm_xy[1], norm_xy[0]) cheek_pose[0:3,0:3] = tf_trans.euler_matrix(0, 0, head_rot, 'sxyz')[0:3,0:3] self.cheek_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose)) ell_center = cheek_pose * cheek_transformation self.ell_center_pub.publish(PoseConverter.to_pose_stamped_msg("/base_link", ell_center)) # create an ellipsoid msg and command it ep = EllipsoidParams() ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center) ep.height = 0.924 ep.E = 0.086 self.ep_pub.publish(ep) return 'succeeded'