ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
-1

How does one get the x and y coordinates of a robot?

asked 2022-01-06 08:03:54 -0500

PumpkinIcedTea gravatar image

As mentioned above, how does one get current position of the robot in x and y coordinates? It can't be that hard right? Yet when I try to google I can't find any simple code...

Thanks

edit retag flag offensive close merge delete

Comments

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.

crnewton gravatar image crnewton  ( 2022-01-15 05:48:29 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2022-01-08 13:16:16 -0500

sniegs gravatar image

This is a very open, non-specific question, so I can only answer about what I find most useful. If you use MoveIt! you should check out the get commands for MoveGroupInterface like getCurrentPose etc.

edit flag offensive delete link more
0

answered 2022-01-08 16:08:07 -0500

USAotearoa gravatar image

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.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-01-06 08:03:54 -0500

Seen: 143 times

Last updated: Jan 08 '22