Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

support invalid handle exception from rclpy in the action LoadComposableNodes #391

Open
wants to merge 1 commit into
base: rolling
Choose a base branch
from

Conversation

firemark
Copy link

@firemark firemark commented Feb 2, 2024

When the shutdown event will be invoked while loading nodes into a composable node then is a high change to raise rclpy.InvalidHandle exception. My pull request fixes that.

But ain't sure is a good way - maybe is not a problem here but in rclpy?

In my opinion in this code should be return self.context.ok() and self.service_is_ready() similar to the above line while ….

Maybe is a problem with priority - the ros node in the launcher shouldn't be turned off before running coroutines. But I don't know is easy to fix that (or is this a current case).

Steps to reproduce the bug

  1. Create a file "test.launch.py"
Code
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import ComposableNodeContainer, Node
from launch.actions import Shutdown
from launch import LaunchDescription


def generate_launch_description():
    return LaunchDescription(
        [
            ComposableNodeContainer(
                name="demo",
                namespace="",
                package="rclcpp_components",
                executable="component_container",
                output="both",
                composable_node_descriptions=create_nodes(),
          ),
          Shutdown(reason="¯\_(ツ)_/¯"),
      ]
  )

def create_nodes():
  return [
      ComposableNode(
          package="demo_nodes_cpp",
          plugin="demo_nodes_cpp::Listener",
          name="listener",
          namespace="/",
      ),
      ComposableNode(
          package="demo_nodes_cpp",
          plugin="demo_nodes_cpp::Talker",
          name="talker",
          namespace="/",
      ),
  ]
  1. Run ros2 test.launch.py

Before fix

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [58172]
[component_container-1] [INFO] [1706798578.381711999] [demo]: Load Library: /opt/ros/humble/lib/libtopics_library.so
[component_container-1] [INFO] [1706798578.397282248] [demo]: Found class: rclcpp_components::NodeFactoryTemplate<demo_nodes_cpp::ContentFilteringPublisher>
[component_container-1] [INFO] [1706798578.397331365] [demo]: Found class: rclcpp_components::NodeFactoryTemplate<demo_nodes_cpp::ContentFilteringSubscriber>
[component_container-1] [INFO] [1706798578.397347057] [demo]: Found class: rclcpp_components::NodeFactoryTemplate<demo_nodes_cpp::Listener>
[component_container-1] [INFO] [1706798578.397358232] [demo]: Instantiate class: rclcpp_components::NodeFactoryTemplate<demo_nodes_cpp::Listener>
[INFO] [component_container-1]: sending signal 'SIGINT' to process[component_container-1]
[component_container-1] [INFO] [1706798578.618317891] [rclcpp]: signal_handler(signum=2)
[INFO] [component_container-1]: process has finished cleanly [pid 58172]
[WARNING] [launch_ros.actions.load_composable_nodes]: Abandoning wait for the '/demo/_container/load_node' service response, due to shutdown.
Future exception was never retrieved
future: <Future finished exception=InvalidHandle('cannot use Destroyable because destruction was requested')>
Traceback (most recent call last):
  File "/usr/lib/python3.10/concurrent/futures/thread.py", line 58, in run
    result = self.fn(*self.args, **self.kwargs)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/actions/load_composable_nodes.py", line 197, in _load_in_sequence
    self._load_node(next_load_node_request, context)
  File "/opt/ros/humble/lib/python3.10/site-packages/launch_ros/actions/load_composable_nodes.py", line 119, in _load_node
    while not self.__rclpy_load_node_client.wait_for_service(timeout_sec=1.0):
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/client.py", line 180, in wait_for_service
    return self.service_is_ready()
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/client.py", line 159, in service_is_ready
    with self.handle:
rclpy._rclpy_pybind11.InvalidHandle: cannot use Destroyable because destruction was requested

After fix

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [7864]
[component_container-1] [ERROR] [1706807191.272317427] [demo]: Could not find requested resource in ament index
[INFO] [component_container-1]: sending signal 'SIGINT' to process[component_container-1]
[component_container-1] [INFO] [1706807191.276060981] [rclcpp]: signal_handler(signum=2)
[INFO] [component_container-1]: process has finished cleanly [pid 7864]
[WARNING] [launch_ros.actions.load_composable_nodes]: Abandoning wait for the '/demo/_container/load_node' service response, due to shutdown.
[WARNING] [launch_ros.actions.load_composable_nodes]: Abandoning wait for the '/demo/_container/load_node' service, due to invalid rclpy handle.

…bleNodes

Signed-off-by: Marek Piechula <mpiechula@autonomous-systems.pl>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

2 participants