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.
I think you're not googling right, because you're question is also very vague. do you mean the robot base pose relative to /world, or the x,y of the tcp (the end of the robot)? I assume you use moveit? getCurrentPose() might help you.