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.:

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

move_square.py

import rospy
from geometry_msgs.msg import Twist 
from nav_msgs.msg import Odometry 
from tf.transformations import euler_from_quaternion 
from math import sqrt, pow, pi 

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

        pos_x = position.x 
        pos_y = position.y

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

        self.x = pos_x 
        self.y = pos_y
        self.theta_z = yaw

        if not self.first_message: 
            self.first_message = True

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

        
        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

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

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

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