Migrate to rmw_zenoh_cpp

.github/workflows/nexus_integration_tests.yaml

@@ -39,4 +39,17 @@ jobs:
     - 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+
     - 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 \
+          test_invalid_place_on_conveyor.py \
+          test_parallel_duplicated_wo.py \
+          test_parallel_wo.py \
+          test_pick_and_place.py
+    - name: Test - Integration test with RMF
+      run: |
+        . ./install/setup.bash
+        cd nexus_demos
+        RMW_IMPLEMENTATION=rmw_zenoh_cpp /ros_entrypoint.sh python3 -m unittest \
+          test_pick_and_place_rmf.py

.github/workflows/style.yaml

@@ -22,10 +22,9 @@ jobs:
       #  run: |
       #    sudo apt update && sudo apt install -y ros-jazzy-rmf-utils
       #    /ros_entrypoint.sh ament_uncrustify -c /opt/ros/jazzy/share/rmf_utils/rmf_code_style.cfg . --language C++ --exclude nexus_endpoints/nexus_endpoints.hpp
-      - name: pycodestyle
+      - name: deps
         run: |
-          sudo apt update && sudo apt install -y pycodestyle curl
-          pycodestyle nexus_network_configuration/
+          sudo apt update && sudo apt install -y curl
       - name: Setup Rust
         uses: dtolnay/rust-toolchain@1.75
         with:

README.md

@@ -65,12 +65,6 @@ The framework to register capabilities and map them to processes that can be per
 At present, behavior trees can be viewed and modified using [Groot](https://github.com/BehaviorTree/Groot). Once `Groot` is launched, click "Load palette from file" and select [nexus_tree_nodes.xml](./nexus_tree_nodes.xml). Then any of the configured BTs can be loaded via the "Load Tree" button.
 A current limitation of this approach is the need to manually update the palette file when the plugins loaded by a task capability changes. In the future, the goals is to more closely couple the generation of this file and the skill plugins the orchestrators are capable of loading.
 
-### Generating Zenoh bridge configurations
-
-The script in `nexus_network_configuration` helps to simplify configuration of Zenoh bridges for multiple machines. The Zenoh bridge files are generated from [NEXUS Network Configuration](nexus_demos/config/zenoh/nexus_network_config.yaml) and [nexus_endpoints.redf.yaml](./nexus_endpoints.redf.yaml). After configuring the [NEXUS Network Configuration](nexus_demos/config/zenoh/nexus_network_config.yaml), you can run `ros2 run nexus_network_configuration nexus_network_configuration -n <PATH_TO_NEXUS_NETWORK_CONFIG> -r <PATH_TO_REDF_CONFIGS> -o <ZENOH_CONFIGS_OUTPUT_DIRECTORY>` to generate the Zenoh bridges.
-
-Further detailed instructions on running the Zenoh bridges with said configurations are in the [package README](nexus_network_configuration/README.md)
-
 ## Demos
 
 ![](./docs/media/nexus_demo.png)

nexus_calibration/CMakeLists.txt

@@ -39,7 +39,7 @@ target_compile_features(nexus_calibration_component INTERFACE cxx_std_17)
 rclcpp_components_register_node(nexus_calibration_component
   PLUGIN "nexus::CalibrationNode"
   EXECUTABLE nexus_calibration_node
-  EXECUTOR SingleThreadedExecutor)
+  EXECUTOR EventsExecutor)
 
 
 #===============================================================================

nexus_demos/CMakeLists.txt

@@ -32,7 +32,7 @@ target_compile_features(mock_gripper_component INTERFACE cxx_std_17)
 rclcpp_components_register_node(mock_gripper_component
   PLUGIN "nexus_demos::MockGripper"
   EXECUTABLE mock_gripper
-  EXECUTOR SingleThreadedExecutor)
+  EXECUTOR EventsExecutor)
 install(
   TARGETS mock_gripper_component
   EXPORT export_${PROJECT_NAME}
@@ -59,7 +59,7 @@ target_compile_features(mock_detector_component INTERFACE cxx_std_17)
 rclcpp_components_register_node(mock_detector_component
   PLUGIN "nexus_demos::MockDetector"
   EXECUTABLE mock_detector
-  EXECUTOR SingleThreadedExecutor)
+  EXECUTOR EventsExecutor)
 install(
   TARGETS mock_detector_component
   EXPORT export_${PROJECT_NAME}
@@ -80,7 +80,7 @@ target_link_libraries(mock_emergency_alarm_component
 rclcpp_components_register_node(mock_emergency_alarm_component
   PLUGIN "MockEmergencyAlarm"
   EXECUTABLE mock_emergency_alarm
-  EXECUTOR SingleThreadedExecutor)
+  EXECUTOR EventsExecutor)
 install(
   TARGETS mock_emergency_alarm_component
   EXPORT export_${PROJECT_NAME}
@@ -110,7 +110,7 @@ target_compile_features(mock_printer INTERFACE cxx_std_17)
 rclcpp_components_register_node(mock_printer
   PLUGIN "MockPrinter"
   EXECUTABLE mock_printer_node
-  EXECUTOR SingleThreadedExecutor)
+  EXECUTOR EventsExecutor)
 
 install(
   TARGETS
@@ -125,19 +125,8 @@ install(
 add_test(NAME all_tests COMMAND python3 -m unittest WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR})
 
 ###############################################################################
-# Generate zenoh bridge configuration files from a NEXUS network configuration file
-set(nexus_network_cfg_path ${CMAKE_CURRENT_SOURCE_DIR}/config/zenoh/nexus_network_config.yaml)
-set(zenoh_cfg_output_dir ${CMAKE_CURRENT_BINARY_DIR}/config/zenoh)
-
-# Invoke the nexus_network_configuration script and generate config json within the build directory.
-add_custom_target(generate_zenoh_bridge_configs ALL
-  COMMAND ros2 run nexus_network_configuration nexus_network_configuration -n ${nexus_network_cfg_path} -o ${zenoh_cfg_output_dir}
-)
 
 install(DIRECTORY launch config rviz scripts DESTINATION share/${PROJECT_NAME})
-# Install the zenoh config directory containing generated configs.
-message("zenoh_cfg_output_dir: " ${zenoh_cfg_output_dir})
-install(DIRECTORY ${zenoh_cfg_output_dir} DESTINATION share/${PROJECT_NAME}/config/)
 
 if(BUILD_TESTING)
   find_package(rmf_utils REQUIRED)

nexus_demos/README.md

@@ -3,63 +3,72 @@
 ![](../docs/media/nexus_demo.png)
 
 ## Launch system and workcell orchestrators and all mock nodes
-The [launch.py script](launch/launch.py) will launch the system orchestrator and 2 workcells orchestrators (each with a IRB910SC and IRB1300 robot), along with a Zenoh bridge to link selected ROS endpoints between them. These 3 orchestrators and their accompany components will be in different ROS_DOMAIN_IDs. To launch these components individually, use the following examples given.
+The [launch.py script](launch/launch.py) will launch the inter-workcell nodes (including the system orchestrator) and 2 workcells orchestrators (each with a IRB910SC and IRB1300 robot). The inter-workcell system, as well as each workcell's system will also be launched with a Zenoh router, while each workcell will require a Zenoh router that acts as a bridge for specific topics, services and actions. To launch these components individually, use the following examples given.
 
->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`
+### Method 1: Launch system orchestrator, IRB1300 workcell and IRB910SC Workcell together with designated routers and bridges
+> NOTE: Before running any of these commands, you must set the rmw implmentation to rmw_zenoh_cpp with
+`export RMW_IMPLEMENTATION=rmw_zenoh_cpp`
 (If testing with real hardware, specify the arguments `use_fake_hardware=False`, `robot1_ip=<IP>` and `robot2_ip=<IP>`)
 ```bash
 ros2 launch nexus_demos launch.py headless:=False
 ```
 
-### Method 2: Launch System Orchestrator and 1 Workcell without Zenoh bridge (Same ROS_DOMAIN_ID)
+### Method 2: Launch System Orchestrator and 1 Workcell
 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:=<IP_ADDR>
+ros2 launch nexus_demos launch.py headless:=False run_workcell_1:=True run_workcell_2:=False use_fake_hardware:=False robot1_ip:=<IP_ADDR>
 ```
 
 ## Launch Orchestrators individually
 
 ### System Orchestrator
 ```bash
-ros2 launch nexus_demos control_center.launch.py ros_domain_id:=0 headless:=False
+ros2 launch nexus_demos inter_workcell.launch.py headless:=False
 ```
 
 ### IRB910SC Workcell
 ```bash
-ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_1 ros_domain_id:=1 support_package:=abb_irb910sc_support robot_xacro_file:=irb910sc_3_45.xacro moveit_config_package:=abb_irb910sc_3_45_moveit_config controllers_file:=abb_irb910sc_controllers.yaml moveit_config_file:=abb_irb910sc_3_45.srdf.xacro tf_publisher_launch_file:=irb910sc_tf.launch.py planner_config_package:=nexus_demos planner_config_file:=irb910sc_planner_params.yaml sku_detection_params_file:=irb910sc_detection.yaml zenoh_config_file:=workcell_1.json5 headless:=False
+ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_1 support_package:=abb_irb910sc_support robot_xacro_file:=irb910sc_3_45.xacro moveit_config_package:=abb_irb910sc_3_45_moveit_config controllers_file:=abb_irb910sc_controllers.yaml moveit_config_file:=abb_irb910sc_3_45.srdf.xacro tf_publisher_launch_file:=irb910sc_tf.launch.py planner_config_package:=nexus_demos planner_config_file:=irb910sc_planner_params.yaml sku_detection_params_file:=irb910sc_detection.yaml zenoh_router_config_filename:=config/zenoh/workcell_1_router_config.json5 zenoh_session_config_filename:=config/zenoh/workcell_1_session_config.json5 io_stations_config_file_path:=src/nexus/nexus_demos/config/workcell_1_io_config.yaml headless:=False
 ```
 
-### IRB1300 Workcell
+Start the bridge router,
+
 ```bash
-ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_2 ros_domain_id:=2 support_package:=abb_irb1300_support robot_xacro_file:=irb1300_10_115.xacro moveit_config_package:=abb_irb1300_10_115_moveit_config controllers_file:=abb_irb1300_controllers.yaml moveit_config_file:=abb_irb1300_10_115.srdf.xacro tf_publisher_launch_file:=irb1300_tf.launch.py sku_detection_params_file:=irb1300_detection.yaml zenoh_config_file:=workcell_2.json5 headless:=False
+ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_1_connection_router_config.json5
 ```
 
-## Submit a job
+### IRB1300 Workcell
+```bash
+ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_2 support_package:=abb_irb1300_support robot_xacro_file:=irb1300_10_115.xacro moveit_config_package:=abb_irb1300_10_115_moveit_config controllers_file:=abb_irb1300_controllers.yaml moveit_config_file:=abb_irb1300_10_115.srdf.xacro tf_publisher_launch_file:=irb1300_tf.launch.py sku_detection_params_file:=irb1300_detection.yaml zenoh_router_config_filename:=config/zenoh/workcell_2_router_config.json5 zenoh_session_config_filename:=config/zenoh/workcell_2_session_config.json5 io_stations_config_file_path:=src/nexus/nexus_demos/config/workcell_2_io_config.yaml headless:=False
+```
 
-> Note: Set your ROS_DOMAIN_ID environment variable to that of the system orchestrator before executing the work order
+Start the bridge router,
 
-`place_on_conveyor` work order:
 ```bash
-ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {work_order_id: '23', work_order: '$(cat config/place_on_conveyor.json)'}}"
+ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_2_connection_router_config.json5
 ```
 
-`pick_from_conveyor` work order:
+## Submit a job
+
+Modify the values of `work_order_id` and the name of the work orders to these provided values in our examples,
+- `place_on_conveyor`
+- `pick_from_conveyor`, only works if `workcell_2` is launched
+- `pick_and_place_conveyor`, only works if both `workcell_1` and `workcell_2` are launched
+- `pick_and_place_amr`, only works if both `workcell_1` and `workcell_2` are launched, and the demo is started with `use_multiple_transporters:=True`
+
+Using `place_on_conveyor` as an example work order, with ID 23:
 ```bash
-ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {work_order_id: '24', work_order: '$(cat config/pick_from_conveyor.json)'}}"
+ros2 action send_goal /system_orchestrator/execute_order nexus_orchestrator_msgs/action/ExecuteWorkOrder "{order: {work_order_id: '23', work_order: '$(cat config/place_on_conveyor.json)'}}"
 ```
 
 ## Debugging

nexus_demos/config/zenoh/inter_workcell_router_config.json5

@@ -0,0 +1,27 @@
+{
+  mode: "router",
+  connect: {
+    endpoints: [],
+  },
+  listen: {
+    endpoints: [
+      "tcp/[::]:7447"
+    ],
+  },
+  scouting: {
+    multicast: {
+      enabled: false,
+    },
+    gossip: {
+      enabled: true,
+      multihop: false,
+    },
+  },
+  access_control: {
+    "enabled": false,
+  },
+  timestamping: {
+    enabled: { router: true, peer: true, client: true },
+    drop_future_timestamp: false,
+  },
+}

nexus_demos/config/zenoh/inter_workcell_session_config.json5

@@ -0,0 +1,24 @@
+{
+  mode: "peer",
+  connect: {
+    endpoints: [
+      "tcp/localhost:7447"
+    ],
+  },
+  scouting: {
+    multicast: {
+      enabled: false,
+    },
+    gossip: {
+      enabled: true,
+      multihop: false,
+    },
+  },
+  access_control: {
+    "enabled": false,
+  },
+  timestamping: {
+    enabled: { router: true, peer: true, client: true },
+    drop_future_timestamp: false,
+  },
+}