Stopping robot on shutdown event

Course Support ROS2 In 5 Days Python
,
1

Hi,

I created the first rosject for the wall follower robot.
When I press CTRL+C the node exits but the robot does not stop. It continuous its working according to the last message in the topic /cmd_vel.
I decided to take an “emergency action”, so when my node exits, I would like to publish an all-zero Twist command.

Here is my launch file:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import (ExecuteProcess,
                            LogInfo, RegisterEventHandler)
from launch.event_handlers import (OnShutdown)
from launch.substitutions import (FindExecutable)


def generate_launch_description():
    stop_robot = ExecuteProcess(
        cmd=[[
            FindExecutable(name='ros2'),
            ' topic pub --once ',
            ' /cmd_vel ',
            ' geometry_msgs/msg/Twist ',
            '"{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"'
        ]],
        shell=True
    )

    wall_follower_node = Node(
            package='wall_follower',
            executable='wall_following',
            output='screen'
    )

    return LaunchDescription([
        wall_follower_node,
        RegisterEventHandler(
            OnShutdown(
                on_shutdown=[
                    LogInfo(msg='Wall follower exiting'),
                    LogInfo(msg='Stopping robot'),
                    stop_robot,
                    LogInfo(msg='Stopping done'),
                ]
            )
        )
    ])

When I press CTRL+C, the logging appears (“Stopping robot” and “Stopping done”) but the robot does not stop. I checked the /cmd_vel topic with ros2 topic echo and there is no all-zero Twist message in that, so the stop_robot command is not executed.
Unfortunately, I did not find any error messages.
What I suspect, is that the ros2 command was not found, so maybe I have to issue the well known “source …” command. But it is not needed in rosject, but maybe it is needed when the shell is not interactive.

Can someone help me?
Thank you in advance
Peter

You can see here that the shutdown hook is executed: the lines starting with “[INFO] [launch.user]”.
The console output:

[wall_following-1] [INFO] [1700113944.311203648] [wall_follower]: Forward distance: "0.81788569688797"
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[INFO] [launch.user]: Wall follower exiting
[INFO] [launch.user]: Stopping robot
[INFO] [launch.user]: Stopping done
[wall_following-1] Traceback (most recent call last):
[wall_following-1]   File "/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_following", line 33, in <module>
[wall_following-1]     sys.exit(load_entry_point('wall-follower==0.0.0', 'console_scripts', 'wall_following')())
[wall_following-1]   File "/home/user/ros2_ws/install/wall_follower/lib/python3.8/site-packages/wall_follower/wall_following.py", line 67, in main
[wall_following-1]     rclpy.spin(wallFollower)
[wall_following-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py", line 191, in spin
[wall_following-1]     executor.spin_once()
[wall_following-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 703, in spin_once
[wall_following-1]     handler, entity, node = self.wait_for_ready_callbacks(timeout_sec=timeout_sec)
[wall_following-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 689, in wait_for_ready_callbacks
[wall_following-1]     return next(self._cb_iter)
[wall_following-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 586, in _wait_for_ready_callbacks
[wall_following-1]     _rclpy.rclpy_wait(wait_set, timeout_nsec)
[wall_following-1] KeyboardInterrupt
[ERROR] [wall_following-1]: process has died [pid 3263, exit code -2, cmd '/home/user/ros2_ws/install/wall_follower/lib/wall_follower/wall_following --ros-args']
2

Hi @peterborkuti ,

I believe this is due to data traffic in the topic of cmd_vel.
I am assuming that your program that is actually publishing Twist values to cmd_vel has more queue_size and the rate of publishing is high.
Since you are using a --once tag to your stop_robot command, that specific single Twist message sent to cmd_vel is being overridden by the other Twist messages pushed by the program onto the cmd_vel topic.
So, at some point, the cmd_vel would have received your “all-zeros” stop command, but it might have been immediately replaced with some non-zeros command that overtook the stop command.

I hope this makes sense.

But the best way to stop your robot is through your main program.

Regards,
Girish

3

Thanks for your help.
I checked the /cmd_vel topic with “ros2 topic /cmd_vel echo” and I did not notice any all-zero Twist message, so maybe not this is the root cause.
But based on your idea, I will try to send lets say, 10 stop messages, not only one.

4

I tried with sending 10 messages, but unfortunately, it did not help. I was pretty sure about it, because I checked the topic and absolutely there was no zero Twist messages in that.
I also logged the PATH enironment variable and there was /opt/ros/foxy/bin so ros2 is on the path.

I also changed “ros2” to “ros3” to let the command 100% fail, but nothing happened. So maybe that stop_robot is not ran or its output/error is hiding somewhere.

When I run the stop_robot in an on_process_start event handler, it works. So there is a possibility, that I can not execute commands on shutdown.

Can anyone guide me to the right direction?

So any proposal is more than welcome which helps me stopping the robot when the node is exiting.

I re-phrase the requirements:
I’d like to publish a message into a topic when a node is stopped by CTRL+C.
It does not matter if you do it with a launch file or a python node or whatever.

Thank you in advance
Péter

5

I started to diff the on_process_start.py and on_process_exit.py, but I was not smart enough to find out how to use on_exit.
But I found this Execute another process on process exit - ROS Answers: Open Source Q&A Forum

So the answer is:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import (OpaqueFunction,
                            LogInfo, RegisterEventHandler)
from launch.event_handlers import (OnProcessExit)

import subprocess


def exit_process_function(_launch_context):
    subprocess.run('ros2 topic pub -t 1 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"', shell=True)

def generate_launch_description():
    wall_follower_node = Node(
            package='wall_follower',
            executable='wall_following',
            output='screen'
    )

    return LaunchDescription([
        wall_follower_node,
        RegisterEventHandler(
            OnProcessExit(
                target_action=wall_follower_node,
                on_exit=[
                    LogInfo(msg='Wall follower exiting'),
                    LogInfo(msg='Stopping robot'),
                    OpaqueFunction(
                        function=exit_process_function
                    ),
                    LogInfo(msg='Stopping done'),
                ]
            )
        )
    ])
7

Hi @peterborkuti ,

Sorry for the late reply.

You can do this inside the node program itself using exception handling block.
try...catch if you are using C++ or try...except if you are using Python.
You can catch KeyboardInterruptException and print the messages you need to print, then stop the robot and then shutdown the node properly.

Doing it outside using launch file is not a proper way or a recommended way to do it.
Of course, you can implement it as a “hack” or some form of “quick fix”, but in the long term, it is not the best solution - since the robot is not stopped by the program and it is done externally.

You can try using a shutdown hook in your program, if you are really concerned about implementing this logic.

Regards,
Girish

8

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.