def odom_to_pose(odom): pose = Pose() pose.header = odom.header pose.x = odom.pose.pose.position.x pose.y = odom.pose.pose.position.y qz = odom.pose.pose.orientation.z qw = odom.pose.pose.orientation.w pose.theta = atan2(2*qw*qz, 1-2*qz*qz) return pose
def odom_to_pose(odom): """Utility function to convert an Odometry message into a Pose message.""" pose = Pose() pose.header = odom.header pose.x = odom.pose.pose.position.x pose.y = odom.pose.pose.position.y qz = odom.pose.pose.orientation.z qw = odom.pose.pose.orientation.w pose.theta = atan2(2 * qw * qz, 1 - 2 * qz * qz) pose.cov = reduce_covariance(odom.pose.covariance) return pose