Revision history [back]

Depends on what topics are being published and in what language you are developing C++/python. Look for topics, such as /odom or /amcl/ From thos you can extract the position information. Beware, distances are in m and rotations are in quaternions or radians.

def odom_callback(odom_message):

    if not settings.odomData_valid:
        settings.odomData_valid = True
        rospy.loginfo("odomData validity!")
    else:
        # < code >
        # rospy.loginfo("odom_callback!")
        # settings.nop = 0

        # position x/y
        settings.pose_odom_x = odom_message.pose.pose.position.x
        settings.pose_odom_y = odom_message.pose.pose.position.y

        # rotation quaternion
        settings.pose_odom_rot_quat[0] = odom_message.pose.pose.orientation.x
        settings.pose_odom_rot_quat[1] = odom_message.pose.pose.orientation.y
        settings.pose_odom_rot_quat[2] = odom_message.pose.pose.orientation.z
        settings.pose_odom_rot_quat[3] = odom_message.pose.pose.orientation.w

        # rotation euler
        settings.pose_odom_rot_euler = calc.euler_from_quaternion(settings.pose_odom_rot_quat)

        # rotation degree
        settings.odom_rot_deg = calc.euler_to_degrees(settings.pose_odom_rot_euler)

def euler_from_quaternion(quat):
    """
    Convert a quaternion into euler angles (roll, pitch, yaw)
    roll is rotation around x in radians (counterclockwise)
    pitch is rotation around y in radians (counterclockwise)
    yaw is rotation around z in radians (counterclockwise)
    """
    # print("<euler_from_quaternion>")
    # print("----------------------------")
    x, y, z, w = quat
    # t0 = +2.0 * (w * x + y * z)
    # t1 = +1.0 - 2.0 * (x * x + y * y)
    # roll_x = math.atan2(t0, t1)

    # t2 = +2.0 * (w * y - z * x)
    # t2 = +1.0 if t2 > +1.0 else t2
    # t2 = -1.0 if t2 < -1.0 else t2
    # pitch_y = math.asin(t2)

    t3 = +2.0 * (w * z + x * y) 
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)

    #return roll_x, pitch_y, yaw_z # in radians
    return yaw_z 

# -----------------------------------------------------------------------------
def euler_to_degrees(angle_euler):
    # print("<euler_to_degrees>")
    # print("----------------------------")
    angle_deg = angle_euler * 180 / 3.141592654
    return angle_deg

You will have to remove the "settings.", "calc." etc leading statements, as I have my code highly organized into different files Best to try it out is throwing everything into a single file.