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
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
