In this post, we will see how to write a ROS program (in Python) to make a robot rotate according to user input. We are going to fix an error in the code that prevents this program from working as we go on.

PS: This ROS project is part of our ROS Mini Challenge series, which gives you an opportunity to win an amazing ROS Developers T-shirt! This challenge is already solved. For updates on future challenges, please stay tuned to our Twitter channel.

Step 1: Grab a copy of the ROS Project that contains the almost-working Python code

Click here to get your own copy of the project. If you don’t have an account on the ROS Development Studio, you will be asked to create one. Once you create an account or log in, the project will be copied to your workspace. That done, open your ROSject using the Open button. This might take a few moments, please be patient.

You should now see a notebook with detailed instructions about the challenge. Please ignore the section that says “Claim your Prize!” because, well, the challenge is closed already 🙂

Step 2: Start the Simulation and run the program

1. Click on the Simulations menu and then Choose launch file… . In the dialog that appears, select rotw1.launch, then click the Launch button. You should see a Gazebo window popup showing the simulation: a robot standing in front of a very beautiful house!
2. Keeping the Gazebo window in view, pick a Shell from Tools > Shell and run the following commands (input the same numbers shown):

user:~$ rosrun rotw1_code rotate_robot.py
Enter desired angular speed (degrees): 60
Enter desired angle (degrees): 90
Do you want to rotate clockwise? (y/n): y
[INFO] [1580744469.184313, 1044.133000]: shutdown time! Stop the robot

Did the robot rotate? I would be surprised if it did!

Step 3: Find out what the problem is (was!)

Fire up the IDE and locate the file named rotate_robot.py. You will find in the catkin_ws/src/rotw1_code/src directory.

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
import time


class RobotControl():

    def __init__(self):
        rospy.init_node('robot_control_node', anonymous=True)
        self.vel_publisher = rospy.Publisher('/velocity', Twist, queue_size=1)
        self.cmd = Twist()
        self.ctrl_c = False
        self.rate = rospy.Rate(10)
        rospy.on_shutdown(self.shutdownhook)

    def publish_once_in_cmd_vel(self):
        """
        This is because publishing in topics sometimes fails the first time you publish.
        In continuous publishing systems, this is no big deal, but in systems that publish only
        once, it IS very important.
        """
        while not self.ctrl_c:
            connections = self.vel_publisher.get_num_connections()
            if connections > 0:
                self.vel_publisher.publish(self.cmd)
                break
            else:
                self.rate.sleep()

    def shutdownhook(self):
        self.stop_robot()
        self.ctrl_c = True

    def stop_robot(self):
        rospy.loginfo("shutdown time! Stop the robot")
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        self.publish_once_in_cmd_vel()

    def get_inputs_rotate(self):
        self.angular_speed_d = int(
            raw_input('Enter desired angular speed (degrees): '))
        self.angle_d = int(raw_input('Enter desired angle (degrees): '))
        clockwise_yn = raw_input('Do you want to rotate clockwise? (y/n): ')
        if clockwise_yn == "y":
            self.clockwise = True
        if clockwise_yn == "n":
            self.clockwise = False

        return [self.angular_speed_d, self.angle_d]

    def convert_degree_to_rad(self, speed_deg, angle_deg):

        self.angular_speed_r = speed_deg * 3.14 / 180
        self.angle_r = angle_deg * 3.14 / 180
        return [self.angular_speed_r, self.angle_r]

    def rotate(self):

        # Initilize velocities
        self.cmd.linear.x = 0
        self.cmd.linear.y = 0
        self.cmd.linear.z = 0
        self.cmd.angular.x = 0
        self.cmd.angular.y = 0

        # Convert speed and angle to radians
        speed_d, angle_d = self.get_inputs_rotate()
        self.convert_degree_to_rad(speed_d, angle_d)

        # Check the direction of the rotation
        if self.clockwise:
            self.cmd.angular.z = -abs(self.angular_speed_r)
        else:
            self.cmd.angular.z = abs(self.angular_speed_r)

        # t0 is the current time
        t0 = rospy.Time.now().secs

        current_angle = 0

        # loop to publish the velocity estimate, current_distance = velocity * (t1 - t0)
        while (current_angle < self.angle_r):

            # Publish the velocity
            self.vel_publisher.publish(self.cmd)
            # t1 is the current time
            t1 = rospy.Time.now().secs
            # Calculate current angle
            current_angle = self.angular_speed_r * (t1 - t0)
            self.rate.sleep()

        # set velocity to zero to stop the robot
        #self.stop_robot()


if __name__ == '__main__':
    robotcontrol_object = RobotControl()
    try:
        res = robotcontrol_object.rotate()
    except rospy.ROSInterruptException:
        pass

The problem was in the def __init__(self) function – we are publishing to the wrong topic – /velocity – instead of /cmd_vel.

Next, let’s see how we found the problem.

Step 4: Learn how the problem was solved

If you’re familiar with ROS, you’ll know that /cmd_vel is the most common name for the topic responsible for moving the robot. It’s not always that, but at least the /velocity topic instantly became suspect! Therefore, we went ahead to examine it to see if it was a proper topic:

user:~$ rostopic info /velocity
Type: geometry_msgs/Twist

Publishers:
 * /robot_control_node_5744_1580744450546 (http://rosdscomputer:45129/)

Subscribers: None

And our suspicions were confirmed: the topic had no subscribers. This was strange, as usually /gazebo would be one of the subscribers if it was connected to the simulation.

On the other hand, /cmd_vel was connected to /gazebo. So, maybe it’s the right topic after all, but let’s confirm that next.

Step 5: Replace /velocity with /cmd_vel and rotate the robot!

Replace /velocity with /cmd_vel in the def __init__(self) function and save.

Now run the program again:

user:~$ rosrun rotw1_code rotate_robot.py
Enter desired angular speed (degrees): 60
Enter desired angle (degrees): 90
Do you want to rotate clockwise? (y/n): y
[INFO] [1580744469.184313, 1044.133000]: shutdown time! Stop the robot

Now, the robot should rotate according to user input – try different numbers!

And that was it. Hope you found this useful.

Extra: Video of this post

We made a video showing how this challenge was solved. If you prefer “sights and sounds” to “black and white”, here you go:

Related Resources

• ROS Development Studio (ROSDS)
• Robot Ignite Academy
• ROS for Beginners Course (Python)

Feedback

Did you like this post? Do you have questions about what was explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS or ROS2 topics, please let us know in the comments area and we will do a video or post about it.