How to stop your robot when ROS is shutting down

You might already know that ROS topics operate in “obey the last command” mode when working with robots in Gazebo simulations. In other words, the robot continues to execute the last message published to a given topic, even after you stop publishing the message.

What this means, for example, is that if you publish a Twist message to move your robot and then shut down the ROS system, the robot continues moving. But this is not always desirable, so how do you ensure the robot stops when the ROS system is stopped?

Enter the ROS on_shutdown() hook. Let’s see an example here, using Python. You may test the code with any simulation of your choice.

#! /usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import time

class MoveRobotStopOnShutdown(object):
    '''Class responsible for moving robot and stopping it on shutdown '''

    def __init__(self):
        ''' Initialize! '''

        # create publisher and message as instance variables
        self.publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.msg = Twist()

        # do some cleanup on shutdown
        rospy.on_shutdown(self.clean_shutdown)

        # start by moving robot
        rospy.init_node('move_and_stop_robot')
        self.move_robot()
        rospy.spin()

    def publish(self, msg_type="move"):
        ''' Publish the Twist message '''

        while self.publisher.get_num_connections() < 1:
            # wait for a connection to publisher
            rospy.loginfo("Waiting for connection to publisher...")
            time.sleep(1)

        rospy.loginfo("Connected to publisher.")

        rospy.loginfo("Publishing %s message..." % msg_type)
        self.publisher.publish(self.msg)

    def move_robot(self):
        ''' Set linear velocity to move robot'''

        self.msg.linear.x = 0.1
        self.publish()

        time.sleep(10) # sleep 10 secs and stop

        rospy.signal_shutdown("We are done here!")

    def clean_shutdown(self):
        ''' Stop robot when shutting down '''

        rospy.loginfo("System is shutting down. Stopping robot...")
        self.msg.linear.x = 0
        self.publish("stop")

if __name__ == '__main__':
    MoveRobotStopOnShutdown()

This code simply moves a robot and then shuts down the ROS system after 10 seconds. You may also interrupt the program at any time before it automatically shuts down. Either way, the shutdown hook stops the robot.

If you want to see the other side of the story, try commenting out the line doing the magic of stopping the robot in shutdown.

# rospy.on_shutdown(self.clean_shutdown)

I hope you find this useful.

Cheers.

2 Likes

A post was split to a new topic: Robot not stopping at shutdown as specified on rospy.on_shutdown