Odometry-based Navigation (Move Square)

A combined publisher-subscriber node to achieve odometry-based control...

Below you will find a template Python script to show you how you can both publish to /cmd_vel and subscribe to /odom in the same node. This will help you build a closed-loop controller to make your robot follow a square motion path of size: 1m x 1m.

You can publish velocity commands to /cmd_vel to make the robot move, monitor the robot's position and orientation in real-time, determine when the desired movement has been completed, and then update the velocity commands accordingly.

Suggested Approach¶

Moving in a square can be achieved by switching between two different movement states sequentially: Moving forwards and turning on the spot. At the start of each movement step we can read the robot's current odometry, and then use this as a reference to compare to, and to tell us when the robot's position/orientation has changed by the required amount, e.g.:

With the robot stationary, read the odometry to determine its current X and Y position in the environment.
Move forwards until the robot's X and Y position indicate that it has moved linearly by 0.5m.
Stop moving forwards.
Read the robot's odometry to determine its current orientation ("yaw"/θ_z).
Turn on the spot until the robot's orientation changes by 90°.
Stop turning.
Repeat.

The Code¶

move_square.py

import rclpy
from rclpy.node import Node
from rclpy.signals import SignalHandlerOptions

from geometry_msgs.msg import Twist # (1)!
from nav_msgs.msg import Odometry # (2)!

from part2_navigation.tb3_tools import quaternion_to_euler # (3)!
from math import sqrt, pow, pi # (4)!

class Square(Node):

    def __init__(self):
        super().__init__("move_square")

        self.first_message = False
        self.turn = False 

        self.vel_msg = Twist() # (5)! 
        # (6)!
        self.x = 0.0; self.y = 0.0; self.theta_z = 0.0
        self.xref = 0.0; self.yref = 0.0; self.theta_zref = 0.0
        # (7)!
        self.yaw = 0.0 
        self.displacement = 0.0 

        self.vel_pub = self.create_publisher(
            msg_type=Twist,
            topic="cmd_vel",
            qos_profile=10,
        )

        self.odom_sub = self.create_subscription(
            msg_type=Odometry,
            topic="odom",
            callback=self.odom_callback,
            qos_profile=10,
        )

        ctrl_rate = 10 # hz
        self.timer = self.create_timer(
            timer_period_sec=1/ctrl_rate,
            callback=self.timer_callback,
        )

        self.shutdown = False

        self.get_logger().info(
            f"The '{self.get_name()}' node is initialised."
        )

    def on_shutdown(self):
        print("Stopping the robot...")
        self.vel_pub.publish(Twist())
        self.shutdown = True

    def odom_callback(self, msg_data: Odometry):
        pose = msg_data.pose.pose 

        (roll, pitch, yaw) = quaternion_to_euler(pose.orientation) # (8)!

        self.x = pose.position.x # (9)!
        self.y = pose.position.y
        self.theta_z = abs(yaw) # abs(yaw) makes life much easier!!

        if not self.first_message: # (10)!
            self.first_message = True
            self.xref = self.x
            self.yref = self.y
            self.theta_zref = self.theta_z

    def timer_callback(self):
        # here is where the code to control the motion of the robot 
        # goes. Add code here to make the robot move in a square of
        # dimensions 1 x 1m...
        if self.turn:
            # turn by 90 degrees...


        else:
            # move forwards by 1m...


        # publish whatever velocity command has been set above:
        self.vel_pub.publish(self.vel_msg)

def main(args=None):
    rclpy.init(
        args=args,
        signal_handler_options=SignalHandlerOptions.NO,
    )
    move_square = Square()
    try:
        rclpy.spin(move_square)
    except KeyboardInterrupt:
        print(
            f"{move_square.get_name()} received a shutdown request (Ctrl+C)."
        )
    finally:
        move_square.on_shutdown()
        while not move_square.shutdown:
            continue
        move_square.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

Import the Twist message for publishing velocity commands to /cmd_vel.
Import the Odometry message, for use when subscribing to the /odom topic.
Import the quaternion_to_euler function from tb3_tools.py to convert orientation from quaternions to Euler angles (about the principal axes).
Finally, import some useful mathematical operations (and pi), which may prove useful for this task:

Mathematical Operation Python Implementation

\(\sqrt{a+b}\) sqrt(a+b)

\(a^{2}+(bc)^{3}\) pow(a, 2) + pow(b*c, 3)

\(\pi r^2\) pi * pow(r, 2)
Here we establish a Twist message, which we can populate with velocities and then publish to /cmd_vel within the timer_callback() method (in order to make the robot move).
Here, we define some variables that we can use to store relevant bits of odometry data while our node is running (and read it back to implement feedback control):
- self.x, self.y and self.theta_z will be used by the odom_callback() to store the robot's current pose
- self.x0, self.y0 and self.theta_z0 can be used in the timer_callback() method to keep a record of where the robot was at a given moment in time (and determine how far it has moved since that point)
We'll also need to keep track of how far the robot has travelled (or turned) in order to determine when sufficient movement has taken place to trigger a switch to the alternative state, i.e.:
- if travelled 1 meter, then: turn
- if turned 90°, then: move forward
Here we obtain the robot's current orientation (in quaternions) and convert it to Euler angles (in radians) about the principal axes, where:
- "roll" = θ_x
- "pitch" = θ_y
- "yaw" = θ_z
We're only interested in x, y and θ_z, so we assign these to class variables self.x, self.y and self.theta_z, so that we can access them elsewhere within our Square() class.
Sometimes, it can take a few moments for the first topic message to come through, and it's useful to know when that's happened so that you know you are dealing with actual topic data! Here, we're just setting a flag to True once the callback function has executed for the first time (i.e. the first topic message has been received).

Alternative Approach: Waypoint Tracking¶

A square motion path can be fully defined by the coordinates of its four corners, and we can make the robot move to each of these corners one-by-one, using its odometry system to monitor its real-time position, and adapting linear and angular velocities accordingly.

This is slightly more complicated, and you might want to wait until you have a bit more experience with ROS before tackling it this way.

← Back to Part 2

Mathematical Operation	Python Implementation
\(\sqrt{a+b}\)	`sqrt(a+b)`
\(a^{2}+(bc)^{3}\)	`pow(a, 2) + pow(b*c, 3)`
\(\pi r^2\)	`pi * pow(r, 2)`