Ask Your Question
0

stop my robot if /cmd_vel doesn't receive a message within a certain time period

asked 2018-05-28 18:01:22 -0500

abdelkrim gravatar image

how can make my robot stop moving when /cmd_vel doesn't receive a message within a certain time period

edit retag flag offensive close merge delete

Comments

@abdelkrim: take a look at how (fi) diff_drive_controller does this: here.

gvdhoorn gravatar image gvdhoorn  ( 2018-05-29 02:06:55 -0500 )edit

@gvdhoorn yeah exactly what i am looking for thank you ... but i still have question how i can implement that in my node i am coding with python and in this example it's not very clear how he used timeout

abdelkrim gravatar image abdelkrim  ( 2018-05-29 06:06:44 -0500 )edit

3 Answers

Sort by ยป oldest newest most voted
-2

answered 2018-05-31 05:03:11 -0500

Alberto E. gravatar image

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=8m0QF...

Hope it helps!

edit flag offensive delete link more

Comments

1

rospy.Timer was too convenient (docs)?

gvdhoorn gravatar image gvdhoorn  ( 2018-05-31 05:27:02 -0500 )edit
2

I've actually decided to downvote this answer. If for one reason only: it's overly complicated for what the OP asks and doesn't help make the basic principle clear. We have to remember that ROS Answers primarily targets beginners (with the occasional hard question by experienced members). If ..

gvdhoorn gravatar image gvdhoorn  ( 2018-05-31 05:29:08 -0500 )edit
2

.. we start showing them these kind of convoluted setups for something which should realistically be something that rospy.Timer could do, then I believe we're not doing a very good job at helping those beginners.

gvdhoorn gravatar image gvdhoorn  ( 2018-05-31 05:29:51 -0500 )edit

@Alberto E. thank you very much and thank you to robotigniteacademy :)

abdelkrim gravatar image abdelkrim  ( 2018-06-01 05:09:10 -0500 )edit

@abdelkrim: if you feel Alberto answered your question, you may want to click the checkmark to the left of the answer to mark your question as solved.

gvdhoorn gravatar image gvdhoorn  ( 2018-06-01 05:10:10 -0500 )edit
0

answered 2018-05-28 19:26:41 -0500

mntan gravatar image

I don't think this is possible if the robot velocity is set directly by whatever is published to /cmd_vel

But if you had a node subscribed to the original velocity commands, it could figure out based on the rostime if the last message is older than a certain time period and if so, publish a 0 to /cmd_vel

edit flag offensive delete link more

Comments

What the OP asks is actually pretty standard: in the driver node, configure a ROS timer to fire at a certain period. In the callback, reset all motion commands (ie: zero velocity). Now reset that timer each time a new Twist is received.

gvdhoorn gravatar image gvdhoorn  ( 2018-05-29 02:05:35 -0500 )edit
0

answered 2018-06-21 04:22:51 -0500

ipa-jba gravatar image

A quite general version of this problem I came up with is attached:

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from ackermann_msgs.msg import AckermannDriveStamped

class SendEmptyWatchdog(object):
    def __init__(self, datatype, topic, timeout=0.15, send_value=None):
        self.timeout = timeout
        self.datatype = datatype

        if send_value is None:
            self.send_value = datatype()
        else:
            self.send_value = send_value

        self.twist_subscriber = rospy.Subscriber(topic, self.datatype, self.message_callback, queue_size=1)
        self.twist_publisher = rospy.Publisher(topic, self.datatype, queue_size=1)
        self.timer = rospy.Timer(rospy.Duration(self.timeout), self.timer_callback, oneshot=True)
        self.timeout_triggered = False

    def message_callback(self, msg):
        rospy.logdebug("SendEmptyWatchdog{}: Message Callback".format(self.datatype))
        if self.timeout_triggered:
            self.timeout_triggered = False
        else:
            self.timer.shutdown()
            self.timer = rospy.Timer(rospy.Duration(self.timeout), self.timer_callback, oneshot=True)

    def timer_callback(self, event):
        rospy.logwarn("SendEmptyWatchdog{}: Timeout Triggered. Received no message for {} seconds".format(self.datatype, self.timeout))

        self.timeout_triggered = True
        self.twist_publisher.publish(self.send_value)

if __name__ == '__main__':
    rospy.init_node("command_watchdog")
    twist_watchdog = SendEmptyWatchdog(Twist, "cmd_vel")
    ackermann_watchdog = SendEmptyWatchdog(AckermannDriveStamped, "cmd_ackermann")
    rospy.spin()

What it does is to subscribe to the topic "cmd_vel" and reset a oneshot timer to send a "zero twist" message once the time is expired. The timer.shutdown() and reinitializing is to stop the previously generated timer from firing and spamming "zero twist" messages self.timeout seconds after each received message on "cmd_vel".

To avoid the reset of the timer with a published "zero twist" message, no timer is started for the first message after a timeout wast triggered.

The snippet works with Twists and basically any other datatype (Example with "AckermannDriveStamped" in the code).

Since by default all numeric fields of a newly created message are set to 0 stopping the robot in case of a timeout doesn't need a "send_value" specified. However you can do so, if e.g. a frame has to be specified.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-05-28 18:01:22 -0500

Seen: 928 times

Last updated: May 31 '18