diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index a4ad124b..963e9c31 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -27,6 +27,9 @@ jobs: - name: vcs run: | vcs import . < abb.repos + - name: rmw_zenoh_cpp source + run: | + git clone https://github.com/ros2/rmw_zenoh -b jazzy - name: rosdep run: | apt update @@ -38,6 +41,6 @@ jobs: - name: build run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_gazebo nexus_demos nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - name: Test - Unit Tests - run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ + run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_zenoh_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ - name: Test - Integration Test - run: . ./install/setup.bash && cd nexus_demos && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh python3 -m unittest + run: . ./install/setup.bash && cd nexus_demos && RMW_IMPLEMENTATION=rmw_zenoh_cpp /ros_entrypoint.sh python3 -m unittest diff --git a/nexus_demos/README.md b/nexus_demos/README.md index 5dbbb22c..3303c85a 100644 --- a/nexus_demos/README.md +++ b/nexus_demos/README.md @@ -8,8 +8,8 @@ The [launch.py script](launch/launch.py) will launch the system orchestrator and >NOTE: The ROS_DOMAIN_ID occupied by the Zenoh bridges during launch time may be different from the `domain` values in the Zenoh bridge configurations. This is because the launch file overrides the domain ID of the zenoh bridges to ensure that it is same as that of the orchestrator. ### Method 1: Launch system orchestrator, IRB1300 workcell and IRB910SC Workcell together with Zenoh bridge -> NOTE: Before running any of these commands, you must set the rmw implmentation to cyclonedds with -`export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp` +> NOTE: Before running any of these commands, you must set the rmw implmentation to Zenoh with +`export RMW_IMPLEMENTATION=rmw_zenoh_cpp` (If testing with real hardware, specify the arguments `use_fake_hardware=False`, `robot1_ip=` and `robot2_ip=`) ```bash ros2 launch nexus_demos launch.py headless:=False @@ -18,17 +18,17 @@ ros2 launch nexus_demos launch.py headless:=False ### Method 2: Launch System Orchestrator and 1 Workcell without Zenoh bridge (Same ROS_DOMAIN_ID) Launch with Workcell 1 ```bash -ros2 launch nexus_demos launch.py headless:=False use_zenoh_bridge:=False run_workcell_1:=true run_workcell_2:=false +ros2 launch nexus_demos launch.py headless:=False run_workcell_1:=true run_workcell_2:=false ``` Launch with Workcell 2 ```bash -ros2 launch nexus_demos launch.py headless:=False use_zenoh_bridge:=False run_workcell_1:=false run_workcell_2:=true +ros2 launch nexus_demos launch.py headless:=False run_workcell_1:=false run_workcell_2:=true ``` Testing with real hardware ```bash -ros2 launch nexus_demos launch.py headless:=False use_zenoh_bridge:=False run_workcell_1:=True run_workcell_2:=False use_fake_hardware:=False robot1_ip:= +ros2 launch nexus_demos launch.py headless:=False run_workcell_1:=True run_workcell_2:=False use_fake_hardware:=False robot1_ip:= ``` ## Launch Orchestrators individually diff --git a/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 b/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 new file mode 100644 index 00000000..779ea0ca --- /dev/null +++ b/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 @@ -0,0 +1,11 @@ +{ + mode: "router", + connect: { + endpoints: [], + }, + listen: { + endpoints: [ + "tcp/[::]:7447" + ], + }, +} diff --git a/nexus_demos/config/zenoh/system_orchestrator_session_config.json5 b/nexus_demos/config/zenoh/system_orchestrator_session_config.json5 new file mode 100644 index 00000000..e66c01e0 --- /dev/null +++ b/nexus_demos/config/zenoh/system_orchestrator_session_config.json5 @@ -0,0 +1,25 @@ +{ + mode: "peer", + connect: { + endpoints: { + router: [ + // system orchestrator + "tcp/localhost:7447" + ] + }, + }, + + /// Configuration of data messages timestamps management. + timestamping: { + /// Whether data messages should be timestamped if not already. + /// Accepts a single boolean value or different values for router, peer and client. + /// + /// ROS setting: PublicationCache which is required for transient_local durability + /// only works when time-stamping is enabled. + enabled: { router: true, peer: true, client: true }, + /// Whether data messages with timestamps in the future should be dropped or not. + /// If set to false (default), messages with timestamps in the future are retimestamped. + /// Timestamps are ignored if timestamping is disabled. + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/config/zenoh/workcell_1_router_config.json5 b/nexus_demos/config/zenoh/workcell_1_router_config.json5 new file mode 100644 index 00000000..5aadbe20 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_1_router_config.json5 @@ -0,0 +1,99 @@ +{ + mode: "router", + connect: { + endpoints: [ + // system orchestrator + "tcp/localhost:7447", + ], + }, + listen: { + endpoints: [ + "tcp/[::]:7448" + ], + }, + access_control: { + "enabled": true, + "default_permission": "deny", + "rules": + [ + { + "id": "liveliness_tokens", + "messages": [ + "liveliness_token", + "liveliness_query", + "declare_liveliness_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "@ros2_lv/**" + ], + }, + { + "id": "bridged_topics", + "messages": [ + "put", + "declare_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/workcell_1/workcell_state/**", + ], + }, + { + "id": "bridged_services", + "messages": [ + "query", + "declare_queryable", + "reply", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/register_workcell/**", + "*/workcell_1/is_task_doable/**", + "*/workcell_1/queue_task/**", + "*/workcell_1/remove_pending_task/**", + "*/workcell_1/get_state/**", + "*/workcell_1/change_state/**", + "*/workcell_1/signal/**", + "*/workcell_1/pause/**", + ], + }, + { + "id": "bridged_actions", + "messages": [ + "query", + "declare_queryable", + "reply", + "put", + "declare_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/workcell_1/request/**", + ], + }, + ], + "subjects": + [ + { + "id": "all_subjects", + }, + ], + "policies": + [ + { + "rules": [ + "bridged_topics", + "bridged_services", + "bridged_actions", + "liveliness_tokens", + ], + "subjects": ["all_subjects"], + }, + ] + }, +} diff --git a/nexus_demos/config/zenoh/workcell_1_session_config.json5 b/nexus_demos/config/zenoh/workcell_1_session_config.json5 new file mode 100644 index 00000000..efbd6490 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_1_session_config.json5 @@ -0,0 +1,23 @@ +{ + mode: "peer", + connect: { + endpoints: [ + // workcell orchestrator + "tcp/localhost:7448", + ], + }, + + /// Configuration of data messages timestamps management. + timestamping: { + /// Whether data messages should be timestamped if not already. + /// Accepts a single boolean value or different values for router, peer and client. + /// + /// ROS setting: PublicationCache which is required for transient_local durability + /// only works when time-stamping is enabled. + enabled: { router: true, peer: true, client: true }, + /// Whether data messages with timestamps in the future should be dropped or not. + /// If set to false (default), messages with timestamps in the future are retimestamped. + /// Timestamps are ignored if timestamping is disabled. + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/config/zenoh/workcell_2_router_config.json5 b/nexus_demos/config/zenoh/workcell_2_router_config.json5 new file mode 100644 index 00000000..43cbbdd7 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_2_router_config.json5 @@ -0,0 +1,99 @@ +{ + mode: "router", + connect: { + endpoints: [ + // system orchestrator + "tcp/localhost:7447", + ], + }, + listen: { + endpoints: [ + "tcp/[::]:7449" + ], + }, + access_control: { + "enabled": true, + "default_permission": "deny", + "rules": + [ + { + "id": "liveliness_tokens", + "messages": [ + "liveliness_token", + "liveliness_query", + "declare_liveliness_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "@ros2_lv/**" + ], + }, + { + "id": "bridged_topics", + "messages": [ + "put", + "declare_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/workcell_2/workcell_state/**", + ], + }, + { + "id": "bridged_services", + "messages": [ + "query", + "declare_queryable", + "reply", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/register_workcell/**", + "*/workcell_2/is_task_doable/**", + "*/workcell_2/queue_task/**", + "*/workcell_2/remove_pending_task/**", + "*/workcell_2/get_state/**", + "*/workcell_2/change_state/**", + "*/workcell_2/signal/**", + "*/workcell_2/pause/**", + ], + }, + { + "id": "bridged_actions", + "messages": [ + "query", + "declare_queryable", + "reply", + "put", + "declare_subscriber", + ], + "flows": ["ingress", "egress"], + "permission": "allow", + "key_exprs": [ + "*/workcell_2/request/**", + ], + }, + ], + "subjects": + [ + { + "id": "all_subjects", + }, + ], + "policies": + [ + { + "rules": [ + "bridged_topics", + "bridged_services", + "bridged_actions", + "liveliness_tokens", + ], + "subjects": ["all_subjects"], + }, + ] + }, +} diff --git a/nexus_demos/config/zenoh/workcell_2_session_config.json5 b/nexus_demos/config/zenoh/workcell_2_session_config.json5 new file mode 100644 index 00000000..16a441f7 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_2_session_config.json5 @@ -0,0 +1,23 @@ +{ + mode: "peer", + connect: { + endpoints: [ + // workcell orchestrator + "tcp/localhost:7449", + ], + }, + + /// Configuration of data messages timestamps management. + timestamping: { + /// Whether data messages should be timestamped if not already. + /// Accepts a single boolean value or different values for router, peer and client. + /// + /// ROS setting: PublicationCache which is required for transient_local durability + /// only works when time-stamping is enabled. + enabled: { router: true, peer: true, client: true }, + /// Whether data messages with timestamps in the future should be dropped or not. + /// If set to false (default), messages with timestamps in the future are retimestamped. + /// Timestamps are ignored if timestamping is disabled. + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/launch/inter_workcell.launch.py b/nexus_demos/launch/inter_workcell.launch.py index c1965f9b..fb5eedd5 100644 --- a/nexus_demos/launch/inter_workcell.launch.py +++ b/nexus_demos/launch/inter_workcell.launch.py @@ -109,9 +109,9 @@ def launch_setup(context, *args, **kwargs): ros_domain_id = LaunchConfiguration("ros_domain_id") use_fake_hardware = LaunchConfiguration("use_fake_hardware") use_rmf_transporter = LaunchConfiguration("use_rmf_transporter") - use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") zenoh_config_package = LaunchConfiguration("zenoh_config_package") - zenoh_config_filename = LaunchConfiguration("zenoh_config_filename") + zenoh_router_config_filename = LaunchConfiguration("zenoh_router_config_filename") + zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") transporter_plugin = LaunchConfiguration("transporter_plugin") activate_system_orchestrator = LaunchConfiguration("activate_system_orchestrator") headless = LaunchConfiguration("headless") @@ -194,7 +194,7 @@ def launch_setup(context, *args, **kwargs): condition=UnlessCondition(headless), ) - zenoh_bridge = GroupAction( + zenoh_router = GroupAction( [ IncludeLaunchDescription( [ @@ -202,18 +202,17 @@ def launch_setup(context, *args, **kwargs): [ FindPackageShare("nexus_demos"), "launch", - "zenoh_bridge.launch.py", + "zenoh_router.launch.py", ] ) ], launch_arguments={ "zenoh_config_package": zenoh_config_package, - "zenoh_config_filename": zenoh_config_filename, + "zenoh_router_config_filename": zenoh_router_config_filename, "ros_domain_id": ros_domain_id.perform(context), }.items(), ) - ], - condition=IfCondition(use_zenoh_bridge), + ] ) activate_system_orchestrator = GroupAction( @@ -224,13 +223,22 @@ def launch_setup(context, *args, **kwargs): ) return [ + zenoh_router, SetEnvironmentVariable("ROS_DOMAIN_ID", ros_domain_id), + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, + ] + ) + ), system_orchestrator_node, rmf_transporter, transporter_node, mock_emergency_alarm_node, nexus_panel, - zenoh_bridge, activate_system_orchestrator, activate_transporter_node, activate_node(mock_emergency_alarm_node), @@ -257,20 +265,20 @@ def generate_launch_description(): description="Set true to rely on an Open-RMF managed fleet to transport material\ between workcells.", ), - DeclareLaunchArgument( - "use_zenoh_bridge", - default_value="true", - description="Set true to launch the Zenoh DDS Bridge", - ), DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", + description="Package containing RMW Zenoh router and session configurations", + ), + DeclareLaunchArgument( + name="zenoh_router_config_filename", + default_value="config/zenoh/system_orchestrator_router_config.json5", + description="RMW Zenoh router configuration filepath", ), DeclareLaunchArgument( - name="zenoh_config_filename", - default_value="config/zenoh/system_orchestrator.json5", - description="Zenoh DDS bridge configuration filepath", + name="zenoh_session_config_filename", + default_value="config/zenoh/system_orchestrator_session_config.json5", + description="RMW Zenoh session configuration filepath", ), DeclareLaunchArgument( "transporter_plugin", diff --git a/nexus_demos/launch/launch.py b/nexus_demos/launch/launch.py index fefc8fe0..a5f1798c 100644 --- a/nexus_demos/launch/launch.py +++ b/nexus_demos/launch/launch.py @@ -33,17 +33,16 @@ def launch_setup(context, *args, **kwargs): if ( "RMW_IMPLEMENTATION" not in os.environ - or os.environ["RMW_IMPLEMENTATION"] != "rmw_cyclonedds_cpp" + or os.environ["RMW_IMPLEMENTATION"] != "rmw_zenoh_cpp" ): print( - "Only cycloneDDS is supported, the environment variable RMW_IMPLEMENTATION must be set to rmw_cyclonedds_cpp", + "Only RMW Zenoh is supported, the environment variable RMW_IMPLEMENTATION must be set to rmw_zenoh_cpp", file=sys.stderr, ) exit(1) headless = LaunchConfiguration("headless") use_rmf_transporter = LaunchConfiguration("use_rmf_transporter") - use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") use_fake_hardware = LaunchConfiguration("use_fake_hardware") robot1_ip = LaunchConfiguration("robot1_ip") robot2_ip = LaunchConfiguration("robot2_ip") @@ -63,20 +62,9 @@ def launch_setup(context, *args, **kwargs): ) inter_workcell_domain_id = 0 - if use_zenoh_bridge.perform(context).lower() == "true": - log_msg += "Using the zenoh bridge\n" - workcell_1_domain_id = inter_workcell_domain_id + 1 - workcell_2_domain_id = inter_workcell_domain_id + 2 - else: - log_msg += "Not using zenoh bridge\n" - if ( - run_workcell_1.perform(context).lower() == "true" - and run_workcell_2.perform(context).lower() == "true" - ): - print("To run both workcells, enable the Zenoh Bridge") - sys.exit(1) - workcell_1_domain_id = inter_workcell_domain_id - workcell_2_domain_id = inter_workcell_domain_id + workcell_1_domain_id = inter_workcell_domain_id + 1 + workcell_2_domain_id = inter_workcell_domain_id + 2 + log_msg += f"Inter-workcell has ROS_DOMAIN_ID {inter_workcell_domain_id}\n" if run_workcell_1.perform(context).lower() == "true": log_msg += f"Workcell 1 has ROS_DOMAIN_ID {workcell_1_domain_id}\n" @@ -114,7 +102,8 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "ros_domain_id": str(inter_workcell_domain_id), "zenoh_config_package": "nexus_demos", - "zenoh_config_filename": "config/zenoh/system_orchestrator.json5", + "zenoh_router_config_filename": "config/zenoh/system_orchestrator_router_config.json5", + "zenoh_session_config_filename": "config/zenoh/system_orchestrator_session_config.json5", "use_rmf_transporter": use_rmf_transporter, "transporter_plugin": "nexus_transporter::MockTransporter", "activate_system_orchestrator": headless, @@ -148,7 +137,6 @@ def launch_setup(context, *args, **kwargs): "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "ros_domain_id": str(workcell_1_domain_id), "headless": headless, - "use_zenoh_bridge": use_zenoh_bridge, "controller_config_package": "nexus_demos", "planner_config_package": "nexus_demos", "planner_config_file": "abb_irb910sc_planner_params.yaml", @@ -163,7 +151,8 @@ def launch_setup(context, *args, **kwargs): "use_fake_hardware": use_fake_hardware, "robot_ip": robot1_ip, "zenoh_config_package": "nexus_demos", - "zenoh_config_filename": "config/zenoh/workcell_1.json5", + "zenoh_router_config_filename": "config/zenoh/workcell_1_router_config.json5", + "zenoh_session_config_filename": "config/zenoh/workcell_1_session_config.json5", }.items(), condition=IfCondition(run_workcell_1), ) @@ -191,7 +180,6 @@ def launch_setup(context, *args, **kwargs): "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "ros_domain_id": str(workcell_2_domain_id), "headless": headless, - "use_zenoh_bridge": use_zenoh_bridge, "controller_config_package": "nexus_demos", "planner_config_package": "nexus_demos", "planner_config_file": "abb_irb1300_planner_params.yaml", @@ -206,7 +194,8 @@ def launch_setup(context, *args, **kwargs): "use_fake_hardware": use_fake_hardware, "robot_ip": robot2_ip, "zenoh_config_package": "nexus_demos", - "zenoh_config_filename": "config/zenoh/workcell_2.json5", + "zenoh_router_config_filename": "config/zenoh/workcell_2_router_config.json5", + "zenoh_session_config_filename": "config/zenoh/workcell_2_session_config.json5", }.items(), condition=IfCondition(run_workcell_2), ) @@ -235,13 +224,6 @@ def generate_launch_description(): description="Set true to rely on an Open-RMF managed fleet to transport material\ between workcells.", ), - DeclareLaunchArgument( - "use_zenoh_bridge", - default_value="true", - description="Set true to launch the Zenoh DDS Bridge with each orchestrator\ - in different domains. Else, all orchestrators are launched under the \ - same Domain ID without a Zenoh bridge.", - ), DeclareLaunchArgument( "use_fake_hardware", default_value="true", diff --git a/nexus_demos/launch/workcell.launch.py b/nexus_demos/launch/workcell.launch.py index f3c14196..e35f2fa4 100644 --- a/nexus_demos/launch/workcell.launch.py +++ b/nexus_demos/launch/workcell.launch.py @@ -12,9 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - -from ament_index_python.packages import get_package_share_directory from launch_ros.actions import LifecycleNode from launch_ros.parameter_descriptions import Parameter @@ -32,10 +29,9 @@ RegisterEventHandler, SetEnvironmentVariable, ) -from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.events import Shutdown -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, FindExecutable +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution def activate_node_service(node_name, ros_domain_id): activate_node_proc = ExecuteProcess( @@ -70,8 +66,6 @@ def check_activate_return_code(event, _): def launch_setup(context, *args, **kwargs): - config_path = get_package_share_directory("nexus_demos") - # Initialize launch configuration workcell_id = LaunchConfiguration("workcell_id") bt_path = LaunchConfiguration("bt_path") @@ -91,9 +85,9 @@ def launch_setup(context, *args, **kwargs): dispenser_properties = LaunchConfiguration("dispenser_properties") use_fake_hardware = LaunchConfiguration("use_fake_hardware") robot_ip = LaunchConfiguration("robot_ip") - use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") zenoh_config_package = LaunchConfiguration("zenoh_config_package") - zenoh_config_filename = LaunchConfiguration("zenoh_config_filename") + zenoh_router_config_filename = LaunchConfiguration("zenoh_router_config_filename") + zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") workcell_id_str = workcell_id.perform(context) @@ -189,6 +183,35 @@ def launch_setup(context, *args, **kwargs): return [ SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), + GroupAction( + [ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_demos"), + "launch", + "zenoh_router.launch.py", + ] + ) + ], + launch_arguments={ + "zenoh_config_package": zenoh_config_package, + "zenoh_router_config_filename": zenoh_router_config_filename, + 'ros_domain_id': ros_domain_id.perform(context), + }.items(), + ) + ] + ), + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, + ] + ) + ), mock_dispenser_node, mock_product_detector_node, mock_gripper_node, @@ -252,25 +275,6 @@ def launch_setup(context, *args, **kwargs): ) ] ), - GroupAction( - [ - IncludeLaunchDescription( - [ - PathJoinSubstitution([ - FindPackageShare('nexus_demos'), - 'launch', - 'zenoh_bridge.launch.py' - ]) - ], - launch_arguments={ - 'zenoh_config_package': zenoh_config_package, - 'zenoh_config_filename': zenoh_config_filename, - 'ros_domain_id': ros_domain_id.perform(context), - }.items() - ) - ], - condition=IfCondition(use_zenoh_bridge), - ), activate_node_service("motion_planner_server", ros_domain_id.perform(context)), ] @@ -366,20 +370,20 @@ def generate_launch_description(): default_value="", description="The IP address of the real robot when use_fake_hardware is False.", ), - DeclareLaunchArgument( - "use_zenoh_bridge", - default_value="true", - description="Set true to launch the Zenoh DDS Bridge", - ), DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", description="Package containing Zenoh DDS bridge configurations", ), DeclareLaunchArgument( - name="zenoh_config_filename", - default_value="config/zenoh/workcell_1.json5", - description="Zenoh DDS bridge configuration filepath", + name="zenoh_router_config_filename", + default_value="config/zenoh/workcell_1_router_config.json5", + description="RMW Zenoh router configuration filepath", + ), + DeclareLaunchArgument( + name="zenoh_session_config_filename", + default_value="config/zenoh/workcell_1_session_config.json5", + description="RMW Zenoh session configuration filepath", ), OpaqueFunction(function = launch_setup) ]) diff --git a/nexus_demos/launch/zenoh_router.launch.py b/nexus_demos/launch/zenoh_router.launch.py new file mode 100644 index 00000000..e6e58f11 --- /dev/null +++ b/nexus_demos/launch/zenoh_router.launch.py @@ -0,0 +1,96 @@ +# Copyright 2025 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + OpaqueFunction, +) +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import ( + ExecutableInPackage, + FindPackageShare, +) + + +def launch_setup(context, *args, **kwargs): + ros_domain_id = LaunchConfiguration("ros_domain_id") + log_level = LaunchConfiguration("log_level") + zenoh_config_package = LaunchConfiguration("zenoh_config_package") + zenoh_router_config_filename = LaunchConfiguration("zenoh_router_config_filename") + + cmd = [ + ExecutableInPackage( + executable="rmw_zenohd", + package="rmw_zenoh_cpp", + ) + ] + + zenoh_router_exec = ExecuteProcess( + cmd=cmd, + shell=True, + additional_env={ + "ROS_DOMAIN_ID": ros_domain_id.perform(context), + "ZENOH_ROUTER_CONFIG_URI": PathJoinSubstitution([ + FindPackageShare(zenoh_config_package), + zenoh_router_config_filename + ]), + "RUST_LOG": log_level.perform(context) + }, + ) + + return [zenoh_router_exec] + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + name="ros_domain_id", + default_value="0", + description="ROS_DOMAIN_ID environment variable", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + name="log_level", + default_value="zenoh=debug", + description="Log level of zenoh DDS Bridge", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + name="zenoh_config_package", + default_value="nexus_demos", + description="Package containing RWM Zenoh configurations", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + name="zenoh_router_config_filename", + default_value="config/zenoh/system_orchestrator_router_config.json5", + description="RMW Zenoh configuration filepath", + ) + ) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) diff --git a/nexus_demos/package.xml b/nexus_demos/package.xml index 16ae9b6b..e38a57d8 100644 --- a/nexus_demos/package.xml +++ b/nexus_demos/package.xml @@ -26,7 +26,7 @@ tf2_ros yaml-cpp yaml_cpp_vendor - rmw_cyclonedds_cpp + rmw_zenoh_cpp abb_bringup abb_irb1300_support @@ -43,7 +43,6 @@ nexus_rviz_plugins nexus_system_orchestrator nexus_workcell_orchestrator - nexus_zenoh_bridge_dds_vendor rmf_building_map_tools diff --git a/nexus_network_configuration/README.md b/nexus_network_configuration/README.md index bcddfd30..1a28605d 100644 --- a/nexus_network_configuration/README.md +++ b/nexus_network_configuration/README.md @@ -6,7 +6,7 @@ This package simplifies the Zenoh DDS Bridge setup for multiple NEXUS orchestrat # First-time setup for deploying a LOCALHOST only environment -1. Make sure that CycloneDDS is installed and made the default middleware, and that multicast on loopback interface is enabled for localhost-only communication. +1. Make sure that `rmw_zenoh_cpp` is installed and made the default middleware, and that multicast on loopback interface is enabled for localhost-only communication. ``` ./scripts/set_up_network.sh ``` diff --git a/nexus_network_configuration/scripts/set_up_network.sh b/nexus_network_configuration/scripts/set_up_network.sh index 76e64a6d..4ddc5a8b 100755 --- a/nexus_network_configuration/scripts/set_up_network.sh +++ b/nexus_network_configuration/scripts/set_up_network.sh @@ -1,11 +1,11 @@ #!/bin/bash # This script does the following: -# 1. Install CycloneDDS (Iron dist.) if not already installed -# 2. Change RMW_IMPLEMENTATION to CycloneDDS +# 1. Install RMW Zenoh (Jazzy dist.) if not already installed +# 2. Change RMW_IMPLEMENTATION to rmw_zenoh_cpp # 3. Enable multicast on loopback interface -RMW_PACKAGE="ros-jazzy-rmw-cyclonedds-cpp" +RMW_PACKAGE="ros-jazzy-rmw-zenoh-cpp" PKG_OK=$(dpkg-query -W --showformat='${Status}\n' $RMW_PACKAGE|grep "install ok installed") @@ -23,12 +23,12 @@ if [ "$PKG_OK" = "" ]; then fi while true; do - read -p "Restart ROS Daemon and set RMW_IMPLEMENTATION to 'rmw_cyclonedds_cpp'? (y/n)" yn + read -p "Restart ROS Daemon and set RMW_IMPLEMENTATION to 'rmw_zenoh_cpp'? (y/n)" yn case $yn in [Yy]* ) ros2 daemon stop; echo "Stopped ROS2 Daemon" - export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp; + export RMW_IMPLEMENTATION=rmw_zenoh_cpp; ros2 daemon start; echo "Started ROS2 Daemon" break;;