From b17a4898913b36b3337ee2e69613913479f7e157 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Tue, 30 Jul 2024 03:35:36 +0300 Subject: [PATCH 1/4] Made runnable zenoh router in launch file --- ...io_perception_pipeline_benchmark.launch.py | 46 ++++++++++++++++++- 1 file changed, 45 insertions(+), 1 deletion(-) diff --git a/launch/scenario_perception_pipeline_benchmark.launch.py b/launch/scenario_perception_pipeline_benchmark.launch.py index 2c620dd..b6b2663 100644 --- a/launch/scenario_perception_pipeline_benchmark.launch.py +++ b/launch/scenario_perception_pipeline_benchmark.launch.py @@ -15,6 +15,50 @@ from launch.event_handlers import OnProcessExit from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder +import subprocess + + +class ZenohRouterStarter: + def __init__(self): + self.tty_file_descriptor = None + self.zenoh_router_sub_process = None + + def start_router(self): + try: + self.tty_file_descriptor = open("/dev/tty") + self.zenoh_router_sub_process = subprocess.Popen( + ["./install/rmw_zenoh_cpp/lib/rmw_zenoh_cpp/rmw_zenohd"], + stdin=self.tty_file_descriptor, + ) + except Exception as err: + print(f"Unexpected {err=}, {type(err)=}") + raise + + def terminate_router(self): + self.zenoh_router_sub_process.terminate() + self.zenoh_router_sub_process.wait() + + def __del__(self): + if self.tty_file_descriptor is not None: + self.tty_file_descriptor.close() + + +# for rmw_zenoh_router +zenoh_router_starter = ZenohRouterStarter() +if ( + "RMW_IMPLEMENTATION" in os.environ.keys() + and os.environ.get("RMW_IMPLEMENTATION") == "rmw_zenoh_cpp" +): + zenoh_router_starter.start_router() + + +def shutdown_launch(): + if ( + "RMW_IMPLEMENTATION" in os.environ.keys() + and os.environ.get("RMW_IMPLEMENTATION") == "rmw_zenoh_cpp" + ): + zenoh_router_starter.terminate_router() + Shutdown()() def launch_setup(context, *args, **kwargs): @@ -132,7 +176,7 @@ def launch_setup(context, *args, **kwargs): {"use_sim_time": True}, {"selected_test_case_index": selected_test_case_index}, ], - on_exit=Shutdown(), + on_exit=lambda *args: shutdown_launch(), ) return [ From 6cd56736d7756d31023df7ba2152893207d2258a Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Tue, 30 Jul 2024 04:05:29 +0300 Subject: [PATCH 2/4] Made getting rmw_zenohd path flexible using ament libraries --- launch/scenario_perception_pipeline_benchmark.launch.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/launch/scenario_perception_pipeline_benchmark.launch.py b/launch/scenario_perception_pipeline_benchmark.launch.py index b6b2663..ba3a37f 100644 --- a/launch/scenario_perception_pipeline_benchmark.launch.py +++ b/launch/scenario_perception_pipeline_benchmark.launch.py @@ -3,7 +3,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch.conditions import IfCondition from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, @@ -25,9 +25,13 @@ def __init__(self): def start_router(self): try: + rmw_zenoh_cpp_router_executable_path = os.path.join( + get_package_prefix("rmw_zenoh_cpp"), "lib/rmw_zenoh_cpp/rmw_zenohd" + ) + self.tty_file_descriptor = open("/dev/tty") self.zenoh_router_sub_process = subprocess.Popen( - ["./install/rmw_zenoh_cpp/lib/rmw_zenoh_cpp/rmw_zenohd"], + [rmw_zenoh_cpp_router_executable_path], stdin=self.tty_file_descriptor, ) except Exception as err: From f0755660abc036c861a6bf2ef07389f6ef610482 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 19 Aug 2024 01:59:46 +0300 Subject: [PATCH 3/4] Documented why zenoh needs to run in separate subprocess --- .../scenario_perception_pipeline_benchmark.launch.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/launch/scenario_perception_pipeline_benchmark.launch.py b/launch/scenario_perception_pipeline_benchmark.launch.py index ba3a37f..9a7cdf6 100644 --- a/launch/scenario_perception_pipeline_benchmark.launch.py +++ b/launch/scenario_perception_pipeline_benchmark.launch.py @@ -17,6 +17,18 @@ from moveit_configs_utils import MoveItConfigsBuilder import subprocess +""" +Zenoh router needs to use /dev/tty as stdin. We cannot set stdin in ExecuteProcess. +The functionality of launch tool doesn't allow to set stdin, Even if we use +shell=True parameter and set stdin using ros2 run rmw_zenoh_cpp rmw_zenoh < /dev/tty, +launch closes process unsoundly, it somehow trigger neither the SIGTERM nor SIGINT. +I could solve this using separate subprocess mechanism, which allows us to modify stdin of process directly. +See my comment in this issue. ( https://github.com/ros2/rmw_zenoh/issues/206#issuecomment-2257292941 ) + +TODO: (CihatAltiparmak) Remove this class when rmw_zenoh becomes runnable without starting router +separately or when tty option is removed from router executable +""" + class ZenohRouterStarter: def __init__(self): From 228b27e90dbbb3fb91922c8782961e53f445a334 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 19 Aug 2024 02:06:09 +0300 Subject: [PATCH 4/4] Added zenoh router start for basic_service_client_benchmark --- ...o_basic_service_client_benchmark.launch.py | 64 ++++++++++++++++++- 1 file changed, 62 insertions(+), 2 deletions(-) diff --git a/launch/scenario_basic_service_client_benchmark.launch.py b/launch/scenario_basic_service_client_benchmark.launch.py index 9ab7b1d..6af9c64 100644 --- a/launch/scenario_basic_service_client_benchmark.launch.py +++ b/launch/scenario_basic_service_client_benchmark.launch.py @@ -1,7 +1,7 @@ import os from launch import LaunchDescription from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_directory, get_package_prefix from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, @@ -10,6 +10,66 @@ ) from launch_ros.substitutions import FindPackageShare from launch_ros.actions import Node +import subprocess + +""" +Zenoh router needs to use /dev/tty as stdin. We cannot set stdin in ExecuteProcess. +The functionality of launch tool doesn't allow to set stdin, Even if we use +shell=True parameter and set stdin using ros2 run rmw_zenoh_cpp rmw_zenoh < /dev/tty, +launch closes process unsoundly, it somehow trigger neither the SIGTERM nor SIGINT. +I could solve this using separate subprocess mechanism, which allows us to modify stdin of process directly. +See my comment in this issue. ( https://github.com/ros2/rmw_zenoh/issues/206#issuecomment-2257292941 ) + +TODO: (CihatAltiparmak) Remove this class when rmw_zenoh becomes runnable without starting router +separately or when tty option is removed from router executable +""" + + +class ZenohRouterStarter: + def __init__(self): + self.tty_file_descriptor = None + self.zenoh_router_sub_process = None + + def start_router(self): + try: + rmw_zenoh_cpp_router_executable_path = os.path.join( + get_package_prefix("rmw_zenoh_cpp"), "lib/rmw_zenoh_cpp/rmw_zenohd" + ) + + self.tty_file_descriptor = open("/dev/tty") + self.zenoh_router_sub_process = subprocess.Popen( + [rmw_zenoh_cpp_router_executable_path], + stdin=self.tty_file_descriptor, + ) + except Exception as err: + print(f"Unexpected {err=}, {type(err)=}") + raise + + def terminate_router(self): + self.zenoh_router_sub_process.terminate() + self.zenoh_router_sub_process.wait() + + def __del__(self): + if self.tty_file_descriptor is not None: + self.tty_file_descriptor.close() + + +# for rmw_zenoh_router +zenoh_router_starter = ZenohRouterStarter() +if ( + "RMW_IMPLEMENTATION" in os.environ.keys() + and os.environ.get("RMW_IMPLEMENTATION") == "rmw_zenoh_cpp" +): + zenoh_router_starter.start_router() + + +def shutdown_launch(): + if ( + "RMW_IMPLEMENTATION" in os.environ.keys() + and os.environ.get("RMW_IMPLEMENTATION") == "rmw_zenoh_cpp" + ): + zenoh_router_starter.terminate_router() + Shutdown()() def launch_setup(context, *args, **kwargs): @@ -36,7 +96,7 @@ def launch_setup(context, *args, **kwargs): parameters=[ {"sending_request_number": sending_request_number}, ], - on_exit=Shutdown(), + on_exit=lambda *args: shutdown_launch(), ) return [add_two_ints_server_node, benchmark_main_node]