A Move Square Python Template for Part 2

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.

move_square.py

import rospy
from geometry_msgs.msg import Twist # (1)!
from nav_msgs.msg import Odometry # (2)!
from tf.transformations import euler_from_quaternion # (3)!
from math import sqrt, pow, pi # (4)!

class Square():
    def callback(self, topic_data: Odometry):
        pose = topic_data.pose.pose # (5)!
        position = pose.position
        orientation = pose.orientation

        pos_x = position.x # (6)!
        pos_y = position.y

        (roll, pitch, yaw) = euler_from_quaternion(
            [orientation.x, orientation.y, orientation.z, orientation.w], "sxyz"
        ) # (7)!

        self.x = pos_x # (8)!
        self.y = pos_y
        self.theta_z = yaw

        if not self.first_message: # (9)!
            self.first_message = True

    def __init__(self):
        node_name = "move_square"
        self.first_message = False
        self.turn = False # (10)!

        # (11)!
        self.pub = rospy.Publisher("cmd_vel", Twist, queue_size=10)
        self.sub = rospy.Subscriber("odom", Odometry, self.callback)

        rospy.init_node(node_name, anonymous=True)
        self.rate = rospy.Rate(10)  # hz

        # (12)!
        self.x = 0.0
        self.y = 0.0
        self.theta_z = 0.0
        self.x0 = 0.0
        self.y0 = 0.0
        self.theta_z0 = 0.0

        self.vel = Twist() # (13)!

        self.ctrl_c = False
        rospy.on_shutdown(self.shutdownhook)

        rospy.loginfo(f"the {node_name} node has been initialised...")

    def shutdownhook(self):
        self.pub.publish(Twist()) # (14)!
        self.ctrl_c = True

    def main(self):
        while not self.ctrl_c:
            # here is where your code would go to control the motion of your
            # robot. Add code here to make your robot move in a square of
            # dimensions 1 x 1m...

            # publish whatever velocity command has been set in your code above:
            self.pub.publish(self.vel)
            self.rate.sleep() # maintain the loop rate @ 10 hz

if __name__ == "__main__":
    node = Square()
    node.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 euler_from_quaternion function to convert orientation from quaternions (as provided in the Odometry message) to Euler angles (about the principal axes).
Finally, import some useful mathematical operations (and pi), which you may find useful:

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)
Obtain the relevant data from the /odom topic. For this, we need to know the robot's position and orientation in its environment, all of which is contained within the "pose" part of the Odometry message:
```
pose = topic_data.pose.pose
```
From there, we can separate out the position and orientation parts:
```
position = pose.position
```
```
orientation = pose.orientation
```
Here we obtain the robot's current position coordinates.
And 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).
This might be useful in the main() class method (below), to switch between turning and moving forwards...
This node needs to read message data from one topic (/odom) and write messages on another (/cmd_vel), so we need to set up a subscriber and a publisher here accordingly.
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 callback function to store the robot's current pose
- self.x0, self.y0 and self.theta_z0 can be used in the main() 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)
Here we establish a Twist message, which we can populate with velocities and then publish to /cmd_vel within the main() method in order to make the robot move.
Publish an empty Twist message to stop the robot (by default all velocities will be zero).

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 (we'll also cover this in the COM2009 lecture course).

← Back to Part 2 - Exercise 5

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