Revision history [back]

Hello,

I've created a Python script that does that, using the /clock topic.

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

class Timer():

    def __init__(self):
        self.cmd_vel_subscriber = rospy.Subscriber('/cmd_vel', Twist, self.sub_callback)
        self.cmd_vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.clock_sub = rospy.Subscriber('/clock', Clock , self.sub_clock)
        self.ctrl_c = False
        self.ref_time = -1
        self.current_time = -1
        rospy.on_shutdown(self.shutdownhook)
        self.rate = rospy.Rate(1)

    def init_timer(self):
        self.ref_time = self.current_time

    def sub_clock(self, msg):
        self.current_time = msg.clock.secs
        if self.ref_time == -1:
            self.init_timer()
        print 'Seconds without cmd_vel cmd:' + str((self.current_time - self.ref_time))
        if (self.current_time - self.ref_time) > 15:
            self.stop_robot()
            self.init_timer()

        print 'Ref Time:' + str(self.ref_time)
        print 'Current Time:' + str(self.current_time)


    def sub_callback(self, msg):
        self.init_timer()

    def publish_once_in_cmd_vel(self, cmd):
        while not self.ctrl_c:
            connections = self.cmd_vel_publisher.get_num_connections()
            if connections > 0:
                self.cmd_vel_publisher.publish(cmd)
                break
            else:
                self.rate.sleep()

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

    def stop_robot(self):
        cmd = Twist()
        cmd.linear.x = 0.0
        cmd.angular.z = 0.0
        self.publish_once_in_cmd_vel(cmd)

if __name__ == '__main__':
    rospy.init_node('timer_test', anonymous=True)
    timer_object = Timer()
    rospy.spin()

Basically, it works like this:

• In the initialization of the class, we define 2 subscribers, for /cmd_vel and for /clock, and 1 publisher for /cmd_vel.

• The init_timer function just restarts the counter

• sub_clock is the subscriber for the /clock topic. It checks if a certain period of time has passed (15 seconds in this case). If so, it restarts the counter and stops the robot.

• sub_callback is the subscriber for the /cmd_vel topic. If a message is published into the topic, the counter will be restarted.

• publish_once_in_cmd_vel it's just a helper function to publish a message into the /cmd_vel topic.

• shutdownhook it's just a function that is called when the program is terminated, and stops the robot.

• stop_robot is the function that I use to stop the robot. It basically generates a message with all zeros to stop the robot, and publishes it into the /cmd_vel topic.

You can also have a look at the following video I've created explaining a little bit the code and showing how it performs: https://www.youtube.com/watch?v=8m0QFlX38k0

Hope it helps!