From eaeac532dbf012bdb76ce85643326d4f573dd275 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 1 Dec 2025 14:40:14 +0800 Subject: [PATCH 01/17] sros method may be too convoluted Signed-off-by: Aaron Chong --- nexus_demos/config/zenoh/README.md | 15 + nexus_demos/config/zenoh/main.json5 | 648 +++++++++++++++++++++++ nexus_demos/config/zenoh/policy_main.xml | 93 ++++ nexus_demos/config/zenoh/zenohd.json5 | 271 ++++++++++ 4 files changed, 1027 insertions(+) create mode 100644 nexus_demos/config/zenoh/README.md create mode 100644 nexus_demos/config/zenoh/main.json5 create mode 100644 nexus_demos/config/zenoh/policy_main.xml create mode 100644 nexus_demos/config/zenoh/zenohd.json5 diff --git a/nexus_demos/config/zenoh/README.md b/nexus_demos/config/zenoh/README.md new file mode 100644 index 00000000..0474c951 --- /dev/null +++ b/nexus_demos/config/zenoh/README.md @@ -0,0 +1,15 @@ +# General setup (so far) + +```bash +ros2 launch nexus_demos launch.py headless:=false run_workcell_1:=false run_workcell_2:=false use_zenoh_bridge:=false +``` + +```bash +ros2 run zenoh_security_tools generate_configs \ + --policy policy_main.xml \ + --router-config ~/nexus_workspaces/sros_demo/rmw_zenoh/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 \ + --session-config ~/nexus_workspaces/sros_demo/rmw_zenoh/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 \ + --ros-domain-id 0 +``` + +This still generates a lot of weirdness, we will use the manual ACL method instead. diff --git a/nexus_demos/config/zenoh/main.json5 b/nexus_demos/config/zenoh/main.json5 new file mode 100644 index 00000000..d6125583 --- /dev/null +++ b/nexus_demos/config/zenoh/main.json5 @@ -0,0 +1,648 @@ +{ + "access_control": { + "default_permission": "deny", + "enabled": true, + "policies": [ + { + "id": null, + "rules": [ + "liveliness_tokens" + ], + "subjects": [ + "router" + ] + }, + { + "id": null, + "rules": [ + "incoming_queries", + "outgoing_queryables_replies", + "outgoing_queries", + "incoming_queryables_replies", + "outgoing_publications", + "incoming_subscriptions", + "outgoing_publications_transient_local_queryable", + "incoming_subscriptions_transient_local_query", + "outgoing_subscriptions", + "incoming_publications", + "outgoing_subscriptions_transient_local_query", + "incoming_subscriptions_transient_local_queryable", + "liveliness_tokens" + ], + "subjects": [ + "main" + ] + } + ], + "rules": [ + { + "flows": [ + "ingress" + ], + "id": "incoming_queries", + "key_exprs": [ + "0/get_building_map/*/*", + "0/get_work_order_state/*/*", + "0/list_transporters/*/*", + "0/list_workcells/*/*", + "0/main/available/*/*", + "0/main/change_state/*/*", + "0/main/describe_parameters/*/*", + "0/main/execute_order/_action/cancel_goal/*/*", + "0/main/execute_order/_action/get_result/*/*", + "0/main/execute_order/_action/send_goal/*/*", + "0/main/get_available_states/*/*", + "0/main/get_available_transitions/*/*", + "0/main/get_parameter_types/*/*", + "0/main/get_parameters/*/*", + "0/main/get_state/*/*", + "0/main/get_transition_graph/*/*", + "0/main/get_type_description/*/*", + "0/main/is_active/*/*", + "0/main/list_parameters/*/*", + "0/main/manage_nodes/*/*", + "0/main/set_parameters/*/*", + "0/main/set_parameters_atomically/*/*", + "0/main/signal/*/*", + "0/main/transport/_action/cancel_goal/*/*", + "0/main/transport/_action/get_result/*/*", + "0/main/transport/_action/send_goal/*/*", + "0/pause/*/*", + "0/register_transporter/*/*", + "0/register_workcell/*/*", + "0/test/set_estop/*/*" + ], + "messages": [ + "query" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_queryables_replies", + "key_exprs": [ + "0/get_building_map/*/*", + "0/get_work_order_state/*/*", + "0/list_transporters/*/*", + "0/list_workcells/*/*", + "0/main/available/*/*", + "0/main/change_state/*/*", + "0/main/describe_parameters/*/*", + "0/main/execute_order/_action/cancel_goal/*/*", + "0/main/execute_order/_action/get_result/*/*", + "0/main/execute_order/_action/send_goal/*/*", + "0/main/get_available_states/*/*", + "0/main/get_available_transitions/*/*", + "0/main/get_parameter_types/*/*", + "0/main/get_parameters/*/*", + "0/main/get_state/*/*", + "0/main/get_transition_graph/*/*", + "0/main/get_type_description/*/*", + "0/main/is_active/*/*", + "0/main/list_parameters/*/*", + "0/main/manage_nodes/*/*", + "0/main/set_parameters/*/*", + "0/main/set_parameters_atomically/*/*", + "0/main/signal/*/*", + "0/main/transport/_action/cancel_goal/*/*", + "0/main/transport/_action/get_result/*/*", + "0/main/transport/_action/send_goal/*/*", + "0/pause/*/*", + "0/register_transporter/*/*", + "0/register_workcell/*/*", + "0/test/set_estop/*/*" + ], + "messages": [ + "declare_queryable", + "reply" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_queries", + "key_exprs": [ + "0/mock_transporter_node/available/*/*", + "0/mock_transporter_node/signal/*/*", + "0/pause/*/*", + "0/register_transporter/*/*", + "0/rviz_lifecycle_manager/is_active/*/*", + "0/rviz_lifecycle_manager/manage_nodes/*/*", + "0/system_orchestrator/change_state/*/*", + "0/system_orchestrator/execute_order/_action/cancel_goal/*/*", + "0/system_orchestrator/get_state/*/*" + ], + "messages": [ + "query" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress" + ], + "id": "incoming_queryables_replies", + "key_exprs": [ + "0/mock_transporter_node/available/*/*", + "0/mock_transporter_node/signal/*/*", + "0/pause/*/*", + "0/register_transporter/*/*", + "0/rviz_lifecycle_manager/is_active/*/*", + "0/rviz_lifecycle_manager/manage_nodes/*/*", + "0/system_orchestrator/change_state/*/*", + "0/system_orchestrator/execute_order/_action/cancel_goal/*/*", + "0/system_orchestrator/get_state/*/*" + ], + "messages": [ + "declare_queryable", + "reply" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_publications", + "key_exprs": [ + "0/clicked_point/*/*", + "0/estop/*/*", + "0/goal_pose/*/*", + "0/initialpose/*/*", + "0/main/execute_order/_action/feedback/*/*", + "0/main/execute_order/_action/status/*/*", + "0/main/transition_event/*/*", + "0/main/transport/_action/feedback/*/*", + "0/main/transport/_action/status/*/*", + "0/map/*/*", + "0/parameter_events/*/*", + "0/rosout/*/*", + "0/site_map/*/*", + "0/tf/*/*", + "0/work_order_states/*/*" + ], + "messages": [ + "put", + "reply" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress" + ], + "id": "incoming_subscriptions", + "key_exprs": [ + "0/clicked_point/*/*", + "0/estop/*/*", + "0/goal_pose/*/*", + "0/initialpose/*/*", + "0/main/execute_order/_action/feedback/*/*", + "0/main/execute_order/_action/status/*/*", + "0/main/transition_event/*/*", + "0/main/transport/_action/feedback/*/*", + "0/main/transport/_action/status/*/*", + "0/map/*/*", + "0/parameter_events/*/*", + "0/rosout/*/*", + "0/site_map/*/*", + "0/tf/*/*", + "0/work_order_states/*/*" + ], + "messages": [ + "declare_subscriber" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_publications_transient_local_queryable", + "key_exprs": [ + "0/clicked_point/*/*/@adv/pub/*/*/_", + "0/estop/*/*/@adv/pub/*/*/_", + "0/goal_pose/*/*/@adv/pub/*/*/_", + "0/initialpose/*/*/@adv/pub/*/*/_", + "0/main/execute_order/_action/feedback/*/*/@adv/pub/*/*/_", + "0/main/execute_order/_action/status/*/*/@adv/pub/*/*/_", + "0/main/transition_event/*/*/@adv/pub/*/*/_", + "0/main/transport/_action/feedback/*/*/@adv/pub/*/*/_", + "0/main/transport/_action/status/*/*/@adv/pub/*/*/_", + "0/map/*/*/@adv/pub/*/*/_", + "0/parameter_events/*/*/@adv/pub/*/*/_", + "0/rosout/*/*/@adv/pub/*/*/_", + "0/site_map/*/*/@adv/pub/*/*/_", + "0/tf/*/*/@adv/pub/*/*/_", + "0/work_order_states/*/*/@adv/pub/*/*/_" + ], + "messages": [ + "declare_queryable", + "liveliness_token" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress" + ], + "id": "incoming_subscriptions_transient_local_query", + "key_exprs": [ + "0/clicked_point/*/*/@adv/**", + "0/estop/*/*/@adv/**", + "0/goal_pose/*/*/@adv/**", + "0/initialpose/*/*/@adv/**", + "0/main/execute_order/_action/feedback/*/*/@adv/**", + "0/main/execute_order/_action/status/*/*/@adv/**", + "0/main/transition_event/*/*/@adv/**", + "0/main/transport/_action/feedback/*/*/@adv/**", + "0/main/transport/_action/status/*/*/@adv/**", + "0/map/*/*/@adv/**", + "0/parameter_events/*/*/@adv/**", + "0/rosout/*/*/@adv/**", + "0/site_map/*/*/@adv/**", + "0/tf/*/*/@adv/**", + "0/work_order_states/*/*/@adv/**" + ], + "messages": [ + "query" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_subscriptions", + "key_exprs": [ + "0/clock/*/*", + "0/estop/*/*", + "0/parameter_events/*/*", + "0/tf/*/*", + "0/tf_static/*/*" + ], + "messages": [ + "declare_subscriber" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress" + ], + "id": "incoming_publications", + "key_exprs": [ + "0/clock/*/*", + "0/estop/*/*", + "0/parameter_events/*/*", + "0/tf/*/*", + "0/tf_static/*/*" + ], + "messages": [ + "put", + "reply" + ], + "permission": "allow" + }, + { + "flows": [ + "egress" + ], + "id": "outgoing_subscriptions_transient_local_query", + "key_exprs": [ + "0/clock/*/*/@adv/**", + "0/estop/*/*/@adv/**", + "0/parameter_events/*/*/@adv/**", + "0/tf/*/*/@adv/**", + "0/tf_static/*/*/@adv/**" + ], + "messages": [ + "query", + "declare_liveliness_subscriber" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress" + ], + "id": "incoming_subscriptions_transient_local_queryable", + "key_exprs": [ + "0/clock/*/*/@adv/pub/*/*/_", + "0/estop/*/*/@adv/pub/*/*/_", + "0/parameter_events/*/*/@adv/pub/*/*/_", + "0/tf/*/*/@adv/pub/*/*/_", + "0/tf_static/*/*/@adv/pub/*/*/_" + ], + "messages": [ + "declare_queryable", + "liveliness_token" + ], + "permission": "allow" + }, + { + "flows": [ + "ingress", + "egress" + ], + "id": "liveliness_tokens", + "key_exprs": [ + "@ros2_lv/0/**" + ], + "messages": [ + "liveliness_token", + "liveliness_query", + "declare_liveliness_subscriber", + "reply" + ], + "permission": "allow" + } + ], + "subjects": [ + { + "cert_common_names": null, + "id": "router", + "interfaces": null, + "link_protocols": null, + "usernames": null, + "zids": null + }, + { + "cert_common_names": null, + "id": "main", + "interfaces": null, + "link_protocols": null, + "usernames": null, + "zids": null + } + ] + }, + "adminspace": { + "enabled": true, + "permissions": { + "read": true, + "write": false + } + }, + "aggregation": { + "publishers": [], + "subscribers": [] + }, + "connect": { + "endpoints": [ + "tcp/localhost:7447" + ], + "exit_on_failure": { + "client": true, + "peer": false, + "router": false + }, + "retry": { + "period_increase_factor": 2, + "period_init_ms": 1000, + "period_max_ms": 4000 + }, + "timeout_ms": { + "client": 0, + "peer": -1, + "router": -1 + } + }, + "downsampling": [], + "id": null, + "listen": { + "endpoints": [ + "tcp/localhost:0" + ], + "exit_on_failure": true, + "retry": { + "period_increase_factor": 2, + "period_init_ms": 1000, + "period_max_ms": 4000 + }, + "timeout_ms": 0 + }, + "low_pass_filter": [], + "metadata": null, + "mode": "peer", + "namespace": null, + "open": { + "return_conditions": { + "connect_scouted": true, + "declares": true + } + }, + "plugins": {}, + "plugins_loading": { + "enabled": false, + "search_dirs": [ + { + "kind": "current_exe_parent", + "value": null + }, + ".", + "~/.zenoh/lib", + "/opt/homebrew/lib", + "/usr/local/lib", + "/usr/lib" + ] + }, + "qos": { + "network": [], + "publication": [] + }, + "queries_default_timeout": 600000, + "routing": { + "interests": { + "timeout": 10000 + }, + "peer": { + "linkstate": { + "transport_weights": [] + }, + "mode": "peer_to_peer" + }, + "router": { + "linkstate": { + "transport_weights": [] + }, + "peers_failover_brokering": true + } + }, + "scouting": { + "delay": 500, + "gossip": { + "autoconnect": { + "peer": [ + "router", + "peer" + ], + "router": [] + }, + "autoconnect_strategy": { + "peer": { + "to_peer": "greater-zid", + "to_router": "always" + } + }, + "enabled": true, + "multihop": false, + "target": { + "peer": [ + "router" + ], + "router": [ + "router", + "peer" + ] + } + }, + "multicast": { + "address": "224.0.0.224:7446", + "autoconnect": { + "client": [ + "router" + ], + "peer": [ + "router", + "peer" + ], + "router": [] + }, + "autoconnect_strategy": { + "peer": { + "to_peer": "greater-zid", + "to_router": "always" + } + }, + "enabled": false, + "interface": "auto", + "listen": true, + "ttl": 1 + }, + "timeout": 3000 + }, + "timestamping": { + "drop_future_timestamp": false, + "enabled": { + "client": true, + "peer": true, + "router": true + } + }, + "transport": { + "auth": { + "pubkey": { + "key_size": null, + "known_keys_file": null, + "private_key_file": null, + "private_key_pem": null, + "public_key_file": null, + "public_key_pem": null + }, + "usrpwd": { + "dictionary_file": null, + "password": null, + "user": null + } + }, + "link": { + "protocols": null, + "rx": { + "buffer_size": 65535, + "max_message_size": 1073741824 + }, + "tcp": { + "so_rcvbuf": null, + "so_sndbuf": null + }, + "tls": { + "close_link_on_expiration": false, + "connect_certificate": null, + "connect_private_key": null, + "enable_mtls": false, + "listen_certificate": null, + "listen_private_key": null, + "root_ca_certificate": null, + "so_rcvbuf": null, + "so_sndbuf": null, + "verify_name_on_connect": true + }, + "tx": { + "batch_size": 65535, + "keep_alive": 2, + "lease": 60000, + "queue": { + "allocation": { + "mode": "lazy" + }, + "batching": { + "enabled": true, + "time_limit": 1 + }, + "congestion_control": { + "block": { + "wait_before_close": 60000000 + }, + "drop": { + "max_wait_before_drop_fragments": 50000, + "wait_before_drop": 1000 + } + }, + "size": { + "background": 2, + "control": 2, + "data": 2, + "data_high": 2, + "data_low": 2, + "interactive_high": 2, + "interactive_low": 2, + "real_time": 2 + } + }, + "sequence_number_resolution": "32bit", + "threads": 16 + }, + "unixpipe": { + "file_access_mask": null + } + }, + "multicast": { + "compression": { + "enabled": false + }, + "join_interval": 2500, + "max_sessions": 1000, + "qos": { + "enabled": false + } + }, + "shared_memory": { + "enabled": false, + "mode": "lazy", + "transport_optimization": { + "enabled": true, + "message_size_threshold": 3072, + "pool_size": 16777216 + } + }, + "unicast": { + "accept_pending": 10000, + "accept_timeout": 60000, + "compression": { + "enabled": false + }, + "lowlatency": false, + "max_links": 1, + "max_sessions": 10000, + "open_timeout": 60000, + "qos": { + "enabled": true + } + } + } +} \ No newline at end of file diff --git a/nexus_demos/config/zenoh/policy_main.xml b/nexus_demos/config/zenoh/policy_main.xml new file mode 100644 index 00000000..8b8e48f4 --- /dev/null +++ b/nexus_demos/config/zenoh/policy_main.xml @@ -0,0 +1,93 @@ + + + + + + + + get_work_order_state + list_transporters + list_workcells + pause + register_transporter + register_workcell + ~/change_state + ~/describe_parameters + ~/execute_order/_action/cancel_goal + ~/execute_order/_action/get_result + ~/execute_order/_action/send_goal + ~/get_available_states + ~/get_available_transitions + ~/get_parameter_types + ~/get_parameters + ~/get_state + ~/get_transition_graph + ~/get_type_description + ~/list_parameters + ~/set_parameters + ~/set_parameters_atomically + + get_building_map + + /test/set_estop + + ~/available + ~/signal + ~/transport/_action/cancel_goal + ~/transport/_action/get_result + ~/transport/_action/send_goal + + ~/is_active + ~/manage_nodes + + + + /mock_transporter_node/available + /mock_transporter_node/signal + + register_transporter + + /rviz_lifecycle_manager/is_active + /rviz_lifecycle_manager/manage_nodes + /system_orchestrator/execute_order/_action/cancel_goal + pause + + /system_orchestrator/change_state + /system_orchestrator/get_state + + + + estop + clock + parameter_events + + tf + tf_static + + + + parameter_events + rosout + work_order_states + ~/execute_order/_action/feedback + ~/execute_order/_action/status + ~/transition_event + + map + site_map + + estop + + tf + ~/transport/_action/feedback + ~/transport/_action/status + + clicked_point + goal_pose + initialpose + + + + + + diff --git a/nexus_demos/config/zenoh/zenohd.json5 b/nexus_demos/config/zenoh/zenohd.json5 new file mode 100644 index 00000000..3951f019 --- /dev/null +++ b/nexus_demos/config/zenoh/zenohd.json5 @@ -0,0 +1,271 @@ +{ + "access_control": { + "default_permission": "deny", + "enabled": false, + "policies": null, + "rules": null, + "subjects": null + }, + "adminspace": { + "enabled": true, + "permissions": { + "read": true, + "write": false + } + }, + "aggregation": { + "publishers": [], + "subscribers": [] + }, + "connect": { + "endpoints": [], + "exit_on_failure": { + "client": true, + "peer": false, + "router": false + }, + "retry": { + "period_increase_factor": 2, + "period_init_ms": 1000, + "period_max_ms": 4000 + }, + "timeout_ms": { + "client": 0, + "peer": -1, + "router": -1 + } + }, + "downsampling": [], + "id": null, + "listen": { + "endpoints": [ + "tcp/[::]:7447" + ], + "exit_on_failure": true, + "retry": { + "period_increase_factor": 2, + "period_init_ms": 1000, + "period_max_ms": 4000 + }, + "timeout_ms": 0 + }, + "low_pass_filter": [], + "metadata": null, + "mode": "router", + "namespace": null, + "open": { + "return_conditions": { + "connect_scouted": true, + "declares": true + } + }, + "plugins": {}, + "plugins_loading": { + "enabled": false, + "search_dirs": [ + { + "kind": "current_exe_parent", + "value": null + }, + ".", + "~/.zenoh/lib", + "/opt/homebrew/lib", + "/usr/local/lib", + "/usr/lib" + ] + }, + "qos": { + "network": [], + "publication": [] + }, + "queries_default_timeout": 600000, + "routing": { + "interests": { + "timeout": 10000 + }, + "peer": { + "linkstate": { + "transport_weights": [] + }, + "mode": "peer_to_peer" + }, + "router": { + "linkstate": { + "transport_weights": [] + }, + "peers_failover_brokering": false + } + }, + "scouting": { + "delay": 500, + "gossip": { + "autoconnect": { + "peer": [ + "router", + "peer" + ], + "router": [] + }, + "autoconnect_strategy": { + "router": { + "to_peer": "always", + "to_router": "always" + } + }, + "enabled": true, + "multihop": false, + "target": { + "peer": [ + "router" + ], + "router": [ + "router", + "peer" + ] + } + }, + "multicast": { + "address": "224.0.0.224:7446", + "autoconnect": { + "client": [ + "router" + ], + "peer": [ + "router", + "peer" + ], + "router": [] + }, + "autoconnect_strategy": { + "router": { + "to_peer": "always", + "to_router": "always" + } + }, + "enabled": false, + "interface": "auto", + "listen": true, + "ttl": 1 + }, + "timeout": 3000 + }, + "timestamping": { + "drop_future_timestamp": false, + "enabled": { + "client": true, + "peer": true, + "router": true + } + }, + "transport": { + "auth": { + "pubkey": { + "key_size": null, + "known_keys_file": null, + "private_key_file": null, + "private_key_pem": null, + "public_key_file": null, + "public_key_pem": null + }, + "usrpwd": { + "dictionary_file": null, + "password": null, + "user": null + } + }, + "link": { + "protocols": null, + "rx": { + "buffer_size": 65535, + "max_message_size": 1073741824 + }, + "tcp": { + "so_rcvbuf": null, + "so_sndbuf": null + }, + "tls": { + "close_link_on_expiration": false, + "connect_certificate": null, + "connect_private_key": null, + "enable_mtls": false, + "listen_certificate": null, + "listen_private_key": null, + "root_ca_certificate": null, + "so_rcvbuf": null, + "so_sndbuf": null, + "verify_name_on_connect": true + }, + "tx": { + "batch_size": 65535, + "keep_alive": 2, + "lease": 60000, + "queue": { + "allocation": { + "mode": "lazy" + }, + "batching": { + "enabled": true, + "time_limit": 1 + }, + "congestion_control": { + "block": { + "wait_before_close": 5000000 + }, + "drop": { + "max_wait_before_drop_fragments": 50000, + "wait_before_drop": 1000 + } + }, + "size": { + "background": 2, + "control": 2, + "data": 2, + "data_high": 2, + "data_low": 2, + "interactive_high": 2, + "interactive_low": 2, + "real_time": 2 + } + }, + "sequence_number_resolution": "32bit", + "threads": 16 + }, + "unixpipe": { + "file_access_mask": null + } + }, + "multicast": { + "compression": { + "enabled": false + }, + "join_interval": 2500, + "max_sessions": 1000, + "qos": { + "enabled": false + } + }, + "shared_memory": { + "enabled": false, + "mode": "lazy", + "transport_optimization": { + "enabled": true, + "message_size_threshold": 3072, + "pool_size": 16777216 + } + }, + "unicast": { + "accept_pending": 10000, + "accept_timeout": 60000, + "compression": { + "enabled": false + }, + "lowlatency": false, + "max_links": 1, + "max_sessions": 10000, + "open_timeout": 60000, + "qos": { + "enabled": true + } + } + } +} \ No newline at end of file From 366f2dd5bfdc0a7f7f088e554fd1bb12dc7f4824 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 3 Dec 2025 09:58:10 +0800 Subject: [PATCH 02/17] tf issues Signed-off-by: Aaron Chong --- .../config/zenoh/inter_workcell_config.json5 | 810 ++++++++++++++++++ .../system_orchestrator_router_config.json5 | 11 + .../system_orchestrator_session_config.json5 | 25 + .../config/zenoh/workcell_1_config.json5 | 754 ++++++++++++++++ .../zenoh/workcell_1_router_config.json5 | 99 +++ .../zenoh/workcell_1_session_config.json5 | 23 + .../zenoh/workcell_2_router_config.json5 | 99 +++ .../zenoh/workcell_2_session_config.json5 | 23 + nexus_demos/launch/inter_workcell.launch.py | 40 +- nexus_demos/launch/launch.py | 39 +- nexus_demos/launch/workcell.launch.py | 73 +- nexus_demos/launch/workcell_1_tf.launch.py | 55 +- nexus_demos/launch/workcell_2_tf.launch.py | 55 +- nexus_demos/launch/zenoh_router.launch.py | 96 +++ nexus_demos/package.xml | 1 - 15 files changed, 2102 insertions(+), 101 deletions(-) create mode 100644 nexus_demos/config/zenoh/inter_workcell_config.json5 create mode 100644 nexus_demos/config/zenoh/system_orchestrator_router_config.json5 create mode 100644 nexus_demos/config/zenoh/system_orchestrator_session_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_1_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_1_router_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_1_session_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_2_router_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_2_session_config.json5 create mode 100644 nexus_demos/launch/zenoh_router.launch.py diff --git a/nexus_demos/config/zenoh/inter_workcell_config.json5 b/nexus_demos/config/zenoh/inter_workcell_config.json5 new file mode 100644 index 00000000..5df19b0d --- /dev/null +++ b/nexus_demos/config/zenoh/inter_workcell_config.json5 @@ -0,0 +1,810 @@ +/// This file attempts to list and document available configuration elements. +/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. +/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. +{ + /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) + /// that zenoh runtime will use. + /// If not set, a random unsigned 128bit integer will be used. + /// WARNING: this id must be unique in your zenoh network. + // id: "1234567890abcdef", + + /// The node's mode (router, peer or client) + mode: "peer", + + /// Which endpoints to connect to. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. + /// + /// For TCP/UDP on Linux, it is possible additionally specify the interface to be connected to: + /// E.g. tcp/192.168.0.1:7447#iface=eth0, for connect only if the IP address is reachable via the interface eth0 + /// + /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. + /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. + /// + /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: + /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 + /// For TCP, UDP, Quic and TLS links, it is possible to specify a `bind` address for the local socket: + /// E.g. tcp/192.168.0.1:7447#bind=192.168.0.1:0 + /// Note!: Currently it is unsupported to specify both `bind` and `iface`. + /// + /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: + /// E.g. tcp/192.168.0.1:7447#dscp=0x08 + connect: { + /// timeout waiting for all endpoints connected (0: no retry, -1: infinite timeout) + /// Accepts a single value (e.g. timeout_ms: 0) + /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). + timeout_ms: { router: -1, peer: -1, client: 0 }, + + /// The list of endpoints to connect to. + /// Accepts a single list (e.g. endpoints: ["tcp/10.10.10.10:7447", "tcp/11.11.11.11:7447"]) + /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/10.10.10.10:7447"], peer: ["tcp/11.11.11.11:7447"] }). + /// + /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html + /// + /// ROS setting: By default connect to the Zenoh router on localhost on port 7447. + endpoints: [ + "tcp/localhost:7447" + ], + + /// Global connect configuration, + /// Accepts a single value or different values for router, peer and client. + /// The configuration can also be specified for the separate endpoint + /// it will override the global one + /// E.g. tcp/192.168.0.1:7447#retry_period_init_ms=20000;retry_period_max_ms=10000" + + /// exit from application, if timeout exceed + exit_on_failure: { router: false, peer: false, client: true }, + /// connect establishing retry configuration + retry: { + /// initial wait timeout until next connect try + period_init_ms: 1000, + /// maximum wait timeout until next connect try + period_max_ms: 4000, + /// increase factor for the next timeout until nexti connect try + period_increase_factor: 2, + }, + }, + + /// Which endpoints to listen on. E.g. tcp/0.0.0.0:7447. + /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, + /// peers, or client can use to establish a zenoh session. + /// + /// For TCP/UDP on Linux, it is possible additionally specify the interface to be listened to: + /// E.g. tcp/0.0.0.0:7447#iface=eth0, for listen connection only on eth0 + /// + /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. + /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. + /// + /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: + /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 + /// + /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: + /// E.g. tcp/192.168.0.1:7447#dscp=0x08 + listen: { + /// timeout waiting for all listen endpoints (0: no retry, -1: infinite timeout) + /// Accepts a single value (e.g. timeout_ms: 0) + /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). + timeout_ms: 0, + + /// The list of endpoints to listen on. + /// Accepts a single list (e.g. endpoints: ["tcp/[::]:7447", "udp/[::]:7447"]) + /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/[::]:7447"], peer: ["tcp/[::]:0"] }). + /// + /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html + /// + /// ROS setting: By default accept incoming connections only from localhost (i.e. from colocalized Nodes). + /// All communications with other hosts are routed by the Zenoh router. + endpoints: [ + "tcp/localhost:0" + ], + + /// Global listen configuration, + /// Accepts a single value or different values for router, peer and client. + /// The configuration can also be specified for the separate endpoint + /// it will override the global one + /// E.g. tcp/192.168.0.1:7447#exit_on_failure=false;retry_period_max_ms=1000" + + /// exit from application, if timeout exceed + exit_on_failure: true, + /// listen retry configuration + retry: { + /// initial wait timeout until next try + period_init_ms: 1000, + /// maximum wait timeout until next try + period_max_ms: 4000, + /// increase factor for the next timeout until next try + period_increase_factor: 2, + }, + }, + + /// Configure the session open behavior. + open: { + /// Configure the conditions to be met before session open returns. + return_conditions: { + /// Session open waits to connect to scouted peers and routers before returning. + /// When set to false, first publications and queries after session open from peers may be lost. + connect_scouted: true, + /// Session open waits to receive initial declares from connected peers before returning. + /// Setting to false may cause extra traffic at startup from peers. + declares: true, + }, + }, + + /// Configure the scouting mechanisms and their behaviours + scouting: { + /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. + timeout: 3000, + /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. + delay: 500, + /// The multicast scouting configuration. + multicast: { + /// Whether multicast scouting is enabled or not + /// + /// ROS setting: disable multicast discovery by default + enabled: false, + /// The socket which should be used for multicast scouting + address: "224.0.0.224:7446", + /// The network interface which should be used for multicast scouting + interface: "auto", // If not set or set to "auto" the interface if picked automatically + /// The time-to-live on multicast scouting packets + ttl: 1, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. + /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router, peer or client mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). + /// Each value is a list of: "peer", "router" and/or "client". + autoconnect: { router: [], peer: ["router", "peer"], client: ["router"] }, + /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. + /// Possible options are: + /// - "always": always attempt to autoconnect, may result in redundant connections. + /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. + /// If both nodes use this strategy, only one will attempt the connection. + /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example + /// because of a private IP. + /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, + /// or different values for router and/or peer depending on the type of node detected + /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), + /// or different values for router or peer mode + /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). + /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip + /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. + autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, + /// Whether or not to listen for scout messages on UDP multicast and reply to them. + listen: true, + }, + /// The gossip scouting configuration. Note that instances in "client" mode do not participate in gossip. + gossip: { + /// Whether gossip scouting is enabled or not + enabled: true, + /// When true, gossip scouting information are propagated multiple hops to all nodes in the local network. + /// When false, gossip scouting information are only propagated to the next hop. + /// Activating multihop gossip implies more scouting traffic and a lower scalability. + /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have + /// direct connectivity with each other. + multihop: false, + /// Which type of Zenoh instances to send gossip messages to. + /// Accepts a single value (e.g. target: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router or peer mode (e.g. target: { router: ["router", "peer"], peer: ["router"] }). + /// Each value is a list of "peer" and/or "router". + /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip + /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. + target: { router: ["router", "peer"], peer: ["router"]}, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. + /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router or peer mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). + /// Each value is a list of: "peer" and/or "router". + autoconnect: { router: [], peer: ["router", "peer"] }, + /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. + /// Possible options are: + /// - "always": always attempt to autoconnect, may result in redundant connection which will then be closed. + /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. + /// If both nodes use this strategy, only one will attempt the connection. + /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example + /// because of a private IP. + /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, + /// or different values for router and/or peer depending on the type of node detected + /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), + /// or different values for router or peer mode + /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). + /// ROS setting: as by default all peers will interconnect to each other over the loopback interface, + /// they are all reachable to each other. Hence using "greater-zid" for peers connecting to + /// other peers is sufficient and avoids unecessary double connections between peers at startup. + autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, + }, + }, + + /// 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, + }, + + /// The default timeout to apply to queries in milliseconds. + /// ROS setting: The default value of 600000 ms (10 minutes) is already quite high, but it can be increased if needed. + /// For instance to avoid timeout with Service Server that that is unresponsive or lagging, + /// which could occur at launch time with a large number of Nodes starting all together. + /// It’s recommended to configure a value that exceeds the longest timeout specified in any + /// `spin_until_complete(service_call_future_result, timeout)` call. + /// Note that the action-related service "get_result" is hard-coded with an infinite timeout, + /// as in some use case an action could spend several hours (e.g. mission plans). + queries_default_timeout: 600000, + + /// The routing strategy to use and it's configuration. + routing: { + /// The routing strategy to use in routers and it's configuration. + router: { + /// When set to true a router will forward data between two peers + /// directly connected to it if it detects that those peers are not + /// connected to each other. + /// The failover brokering only works if gossip discovery is enabled + /// and peers are configured with gossip target "router". + peers_failover_brokering: true, + /// Linkstate mode configuration. + linkstate: { + /// Weights of the outgoing transports in linkstate mode. + /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. + /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. + /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. + // transport_weights: [ + // { dst_zid: "1", weight: "10" }, + // { dst_zid: "2", weight: "200" }, + // ] + }, + }, + /// The routing strategy to use in peers and it's configuration. + peer: { + /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). + /// This option needs to be set to the same value in all peers and routers of the subsystem. + mode: "peer_to_peer", + /// Linkstate mode configuration (only taken into account if mode == "linkstate"). + linkstate: { + /// Weights of the outgoing transports in linkstate mode. + /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. + /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. + /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. + // transport_weights: [ + // { dst_zid: "1", weight: "10" }, + // { dst_zid: "2", weight: "200" }, + // ] + }, + }, + /// The interests-based routing configuration. + /// This configuration applies regardless of the mode (router, peer or client). + interests: { + /// The timeout to wait for incoming interests declarations in milliseconds. + /// The expiration of this timeout implies that the discovery protocol might be incomplete, + /// leading to potential loss of messages, queries or liveliness tokens. + timeout: 10000, + }, + }, + + // /// Overwrite QoS options for Zenoh messages by key expression (ignores Zenoh API QoS config for overwritten values) + // qos: { + // /// Overwrite QoS options for PUT and DELETE messages + // publication: [ + // { + // /// PUT and DELETE messages on key expressions that are included by these key expressions + // /// will have their QoS options overwritten by the given config. + // key_exprs: ["demo/**", "example/key"], + // /// Configurations that will be applied on the publisher. + // /// Options that are supplied here will overwrite the configuration given in Zenoh API + // config: { + // congestion_control: "block", + // priority: "data_high", + // express: true, + // reliability: "best_effort", + // allowed_destination: "remote", + // }, + // }, + // ], + // /// Overwrite QoS options for messages sent and received from/to the network + // /// This allows more fine grained rules (per network card, etc...) but is + // /// less performant than the publication option above. + // network: [ + // { + // /// Optional Id, has to be unique. + // id: "lo0_en0_qos_overwrite", + // /// Optional list of ZIDs on which qos will be overwritten when communicating with. + // // zids: ["38a4829bce9166ee"], + // /// Optional list of interfaces, if not specified, will be applied to all interfaces. + // interfaces: [ + // "lo0", + // "en0", + // ], + // /// Optional list of link protocols. Transports with at least one of these links will have their qos overwritten. + // /// If absent, the overwrite will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// List of message types to apply to. + // messages: [ + // "put", // put publications + // "delete", // delete publications + // "query", // get queries + // "reply", // replies to queries + // ], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the rules will be applied to both flows. + // flows: ["egress", "ingress"], + // /// QoS filter to apply to the messages matching this item. + // qos: { + // congestion_control: "drop", + // priority: "data", + // express: true, + // reliability: "reliable", + // }, + // /// payload_size range for the messages matching this item. + // payload_size: "1000000..", + // key_exprs: ["test/demo"], + // overwrite: { + // /// Optional new priority value, if not specified priority of the messages will stay unchanged. + // priority: "real_time", + // /// Optional new congestion control value, if not specified congestion control of the messages will stay unchanged. + // congestion_control: "block", + // /// Optional new express value, if not specified express flag of the messages will stay unchanged. + // express: true, + // }, + // }, + // ], + // }, + + // /// The declarations aggregation strategy. + // aggregation: { + // /// A list of key-expressions for which all included subscribers will be aggregated into. + // subscribers: [ + // // key_expression + // ], + // /// A list of key-expressions for which all included publishers will be aggregated into. + // publishers: [ + // // key_expression + // ], + // }, + + // /// Namespace prefix. + // /// If specified, all outgoing key expressions will be automatically prefixed with specified string, + // /// and all incoming key expressions will be stripped of specified prefix. + // /// The namespace prefix should satisfy all key expression constraints + // /// and additionally it can not contain wild characters ('*'). + // /// Namespace is applied to the session. + // /// E. g. if session has a namespace of "1" then session.put("my/keyexpr", my_message), + // /// will put a message into 1/my/keyexpr. Same applies to all other operations within this session. + // namespace: "my/namespace", + + // /// The downsampling declaration. + // downsampling: [ + // { + // /// Optional Id, has to be unique + // id: "wlan0egress", + // /// Optional list of network interfaces messages will be processed on, the rest will be passed as is. + // /// If absent, the rules will be applied to all interfaces. An empty list is invalid. + // interfaces: [ "wlan0" ], + // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. + // /// If absent, the rules will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the rules will be applied to both flows. + // flows: ["ingress", "egress"], + // /// List of message type on which downsampling will be applied. Must not be empty. + // messages: [ + // /// Delete + // "delete", + // /// Put + // "put", + // /// Get + // "query", + // /// Queryable Reply to a Query + // "reply", + // ], + // /// A list of downsampling rules: key_expression and the maximum frequency in Hertz + // rules: [ + // { key_expr: "demo/example/zenoh-rs-pub", freq: 0.1 }, + // ], + // }, + // ], + + // /// Configure access control (ACL) rules + // access_control: { + // /// [true/false] acl will be activated only if this is set to true + // "enabled": false, + // /// [deny/allow] default permission is deny (even if this is left empty or not specified) + // "default_permission": "deny", + // /// Rule set for permissions allowing or denying access to key-expressions + // "rules": + // [ + // { + // /// Id has to be unique within the rule set + // "id": "rule1", + // "messages": [ + // "put", "delete", "declare_subscriber", + // "query", "reply", "declare_queryable", + // "liveliness_token", "liveliness_query", "declare_liveliness_subscriber", + // ], + // "flows":["egress","ingress"], + // "permission": "allow", + // "key_exprs": [ + // "test/demo" + // ], + // }, + // { + // "id": "rule2", + // "messages": [ + // "put", "delete", "declare_subscriber", + // "query", "reply", "declare_queryable", + // ], + // "flows":["ingress"], + // "permission": "allow", + // "key_exprs": [ + // "**" + // ], + // }, + // ], + // /// List of combinations of subjects. + // /// + // /// If a subject property (i.e. username, certificate common name or interface) is empty + // /// it is interpreted as a wildcard. Moreover, a subject property cannot be an empty list. + // "subjects": + // [ + // { + // /// Id has to be unique within the subjects list + // "id": "subject1", + // /// Subjects can be interfaces + // "interfaces": [ + // "lo0", + // "en0", + // ], + // /// Subjects can be cert_common_names when using TLS or Quic + // "cert_common_names": [ + // "example.zenoh.io" + // ], + // /// Subjects can be usernames when using user/password authentication + // "usernames": [ + // "zenoh-example" + // ], + // /// This instance translates internally to this filter: + // /// (interface="lo0" && cert_common_name="example.zenoh.io" && username="zenoh-example") || + // /// (interface="en0" && cert_common_name="example.zenoh.io" && username="zenoh-example") + // }, + // { + // "id": "subject2", + // "interfaces": [ + // "lo0", + // "en0", + // ], + // "cert_common_names": [ + // "example2.zenoh.io" + // ], + // /// This instance translates internally to this filter: + // /// (interface="lo0" && cert_common_name="example2.zenoh.io") || + // /// (interface="en0" && cert_common_name="example2.zenoh.io") + // }, + // { + // "id": "subject3", + // /// An empty subject combination is a wildcard + // }, + // { + // "id": "subject4", + // /// link protocols can also be used to identify transports to filter messages on. + // /// If absent, the rules will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// ZIDs can also be used to identify transports to filter messages on. + // /// NOTE: ZID is not backed by an authentication mechanism, it can only be trusted for ACL if it is + // /// dynamically added/removed by eventual dedicated Zenoh mechanisms when transports are opened/closed. + // /// If managed manually in ACL config, can be useful for prototyping but should not be used in production! + // zids: ["38a4829bce9166ee"], + // }, + // ], + // /// The policies list associates rules to subjects + // "policies": + // [ + // /// Each policy associates one or multiple rules to one or multiple subject combinations + // { + // /// Id is optional. If provided, it has to be unique within the policies list + // "id": "policy1", + // /// Rules and Subjects are identified with their unique IDs declared above + // "rules": ["rule1"], + // "subjects": ["subject1", "subject2"], + // }, + // { + // "rules": ["rule2"], + // "subjects": ["subject3", "subject4"], + // }, + // ] + // }, + + // low_pass_filter: [ + // { + // /// Optional Id, has to be unique + // "id": "filter1", + // /// Optional list of network interfaces messages will be processed on, the rest will not be filtered. + // /// If absent, the filter will be applied to all interfaces. + // interfaces: [ "wlan0" ], + // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. + // /// If absent, the rule will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the filter will be applied to both flows. + // flows: ["ingress", "egress"], + // /// List of message type on which the filter will be applied. Must not be empty. + // messages: [ + // "put", + // "delete", + // "query", + // "reply" + // ], + // /// List of key_expressions which matching messages will be filtered + // key_exprs: [ + // "demo/**", + // ], + // /// Inclusive max size of serialized payload + serialized attachment + // size_limit: 8192, + // }, + // ], + + /// Configure internal transport parameters + transport: { + unicast: { + /// Timeout in milliseconds when opening a link + /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together + open_timeout: 60000, + /// Timeout in milliseconds when accepting a link + /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together + accept_timeout: 60000, + /// Maximum number of links in pending state while performing the handshake for accepting it + /// ROS setting: increase the value to support a large number of Nodes starting all together + accept_pending: 10000, + /// Maximum number of transports that can be simultaneously alive for a single zenoh sessions + /// ROS setting: increase the value to support a large number of Nodes starting all together + max_sessions: 10000, + /// Maximum number of incoming links that are admitted per transport + max_links: 1, + /// Enables the LowLatency transport + /// This option does not make LowLatency transport mandatory, the actual implementation of transport + /// used will depend on Establish procedure and other party's settings + /// + /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. + /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to + /// enable 'lowlatency' you need to explicitly disable 'qos'. + /// NOTE: LowLatency transport does not support the fragmentation, so the message size should be + /// smaller than the tx batch_size. + lowlatency: false, + /// Enables QoS on unicast communications. + qos: { + enabled: true, + }, + /// Enables compression on unicast communications. + /// Compression capabilities are negotiated during session establishment. + /// If both Zenoh nodes support compression, then compression is activated. + compression: { + enabled: false, + }, + }, + /// WARNING: multicast communication does not perform any negotiation upon group joining. + /// Because of that, it is important that all transport parameters are the same to make + /// sure all your nodes in the system can communicate. One common parameter to configure + /// is "transport/link/tx/batch_size" since its default value depends on the actual platform + /// when operating on multicast. + /// E.g., the batch size on Linux and Windows is 65535 bytes, on Mac OS X is 9216, and anything else is 8192. + multicast: { + /// JOIN message transmission interval in milliseconds. + join_interval: 2500, + /// Maximum number of multicast sessions. + max_sessions: 1000, + /// Enables QoS on multicast communication. + /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. + qos: { + enabled: false, + }, + /// Enables compression on multicast communication. + /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. + compression: { + enabled: false, + }, + }, + link: { + /// An optional whitelist of protocols to be used for accepting and opening sessions. If not + /// configured, all the supported protocols are automatically whitelisted. The supported + /// protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream", "vsock"] For + /// example, to only enable "tls" and "quic": protocols: ["tls", "quic"], + /// + /// Configure the zenoh TX parameters of a link + tx: { + /// The resolution in bits to be used for the message sequence numbers. + /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. + /// Accepted values: 8bit, 16bit, 32bit, 64bit. + sequence_number_resolution: "32bit", + /// Link lease duration in milliseconds to announce to other zenoh nodes + /// ROS setting: increase the value to avoid lease expiration at launch time with a large number of Nodes starting all together + lease: 60000, + /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive + /// messages will be sent at the configured time interval. + /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, + /// set the actual keep_alive interval to one fourth of the lease time: i.e. send + /// 4 keep_alive messages in a lease period. Changing the lease time will have the + /// keep_alive messages sent more or less often. + /// This is in-line with the ITU-T G.8013/Y.1731 specification on continuous connectivity + /// check which considers a link as failed when no messages are received in 3.5 times the + /// target interval. + /// ROS setting: decrease the value since Nodes are communicating over the loopback + /// where keep-alive messages have less chances to be lost. + keep_alive: 2, + /// Batch size in bytes is expressed as a 16bit unsigned integer. + /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). + /// The default batch size value is the maximum batch size: 65535. + batch_size: 65535, + /// Each zenoh link has a transmission queue that can be configured + queue: { + /// The size of each priority queue indicates the number of batches a given queue can contain. + /// NOTE: the number of batches in each priority must be included between 1 and 16. Different values will result in an error. + /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. + /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, + /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. + /// If qos is false, then only the DATA priority will be allocated. + size: { + control: 2, + real_time: 2, + interactive_high: 2, + interactive_low: 2, + data_high: 2, + data: 2, + data_low: 2, + background: 2, + }, + /// Congestion occurs when the queue is empty (no available batch). + congestion_control: { + /// Behavior pushing CongestionControl::Drop messages to the queue. + drop: { + /// The maximum time in microseconds to wait for an available batch before dropping a droppable message if still no batch is available. + wait_before_drop: 1000, + /// The maximum deadline limit for multi-fragment messages. + max_wait_before_drop_fragments: 50000, + }, + /// Behavior pushing CongestionControl::Block messages to the queue. + block: { + /// The maximum time in microseconds to wait for an available batch before closing the transport session when sending a blocking message + /// if still no batch is available. + /// ROS setting: increase the value to avoid unecessary link closure at launch time where congestion is likely + /// to occur even over the loopback since all the Nodes are starting at the same time. + wait_before_close: 60000000, + }, + }, + /// Perform batching of messages if they are smaller of the batch_size + batching: { + /// Perform adaptive batching of messages if they are smaller of the batch_size. + /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be + /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput + /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. + enabled: true, + /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. + time_limit: 1, + }, + allocation: { + /// Mode for memory allocation of batches in the priority queues. + /// - "init": batches are allocated at queue initialization time. + /// - "lazy": batches are allocated when needed up to the maximum number of batches configured in the size configuration parameter. + mode: "lazy", + }, + }, + }, + /// Configure the zenoh RX parameters of a link + rx: { + /// Receiving buffer size in bytes for each link + /// The default the rx_buffer_size value is the same as the default batch size: 65535. + /// For very high throughput scenarios, the rx_buffer_size can be increased to accommodate + /// more in-flight data. This is particularly relevant when dealing with large messages. + /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. + buffer_size: 65535, + /// Maximum size of the defragmentation buffer at receiver end. + /// Fragmented messages that are larger than the configured size will be dropped. + /// The default value is 1GiB. This would work in most scenarios. + /// NOTE: reduce the value if you are operating on a memory constrained device. + max_message_size: 1073741824, + }, + /// Configure TLS specific parameters + tls: { + /// Path to the certificate of the certificate authority used to validate either the server + /// or the client's keys and certificates, depending on the node's mode. If not specified + /// on router mode then the default WebPKI certificates are used instead. + root_ca_certificate: null, + /// Path to the TLS listening side private key + listen_private_key: null, + /// Path to the TLS listening side public certificate + listen_certificate: null, + /// Enables mTLS (mutual authentication), client authentication + enable_mtls: false, + /// Path to the TLS connecting side private key + connect_private_key: null, + /// Path to the TLS connecting side certificate + connect_certificate: null, + /// Whether or not to verify the matching between hostname/dns and certificate when connecting, + /// if set to false zenoh will disregard the common names of the certificates when verifying servers. + /// This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your + /// ca to verify that the server at baz.com is actually baz.com, let this be true (default). + verify_name_on_connect: true, + /// Whether or not to close links when remote certificates expires. + /// If set to true, links that require certificates (tls/quic) will automatically disconnect when the time of expiration of the remote certificate chain is reached + /// note that mTLS (client authentication) is required for a listener to disconnect a client on expiration + close_link_on_expiration: false, + /// Optional configuration for TCP system buffers sizes for TLS links + /// + /// Configure TCP read buffer size (bytes) + // so_rcvbuf: 123456, + /// Configure TCP write buffer size (bytes) + // so_sndbuf: 123456, + }, + // // Configure optional TCP link specific parameters + // tcp: { + // /// Optional configuration for TCP system buffers sizes for TCP links + // /// + // /// Configure TCP read buffer size (bytes) + // // so_rcvbuf: 123456, + // /// Configure TCP write buffer size (bytes) + // // so_sndbuf: 123456, + // }, + }, + /// Shared memory configuration. + /// NOTE: shared memory can be used only if zenoh is compiled with "shared-memory" feature, otherwise + /// settings in this section have no effect. + shared_memory: { + /// Whether shared memory is enabled or not. + /// If set to `true`, the SHM buffer optimization support will be announced to other parties. (default `true`). + /// This option doesn't make SHM buffer optimization mandatory, the real support depends on other party setting. + /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate + /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the + /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. + /// + /// ROS setting: disabled by default until fully tested + enabled: false, + /// SHM resources initialization mode (default "lazy"). + /// - "lazy": SHM subsystem internals will be initialized lazily upon the first SHM buffer + /// allocation or reception. This setting provides better startup time and optimizes resource usage, + /// but produces extra latency at the first SHM buffer interaction. + /// - "init": SHM subsystem internals will be initialized upon Session opening. This setting sacrifices + /// startup time, but guarantees no latency impact when first SHM buffer is processed. + mode: "lazy", + transport_optimization: { + /// Enables transport optimization for large messages (default `true`). + /// Implicitly puts large messages into shared memory for transports with SHM-compatible connection. + enabled: true, + /// SHM memory size in bytes used for transport optimization (default `16 * 1024 * 1024`). + pool_size: 16777216, + /// Allow optimization for messages equal or larger than this threshold in bytes (default `3072`). + message_size_threshold: 3072, + }, + }, + auth: { + /// The configuration of authentication. + /// A password implies a username is required. + usrpwd: { + user: null, + password: null, + /// The path to a file containing the user password dictionary + dictionary_file: null, + }, + pubkey: { + public_key_pem: null, + private_key_pem: null, + public_key_file: null, + private_key_file: null, + key_size: null, + known_keys_file: null, + }, + }, + }, + + /// Configure the Admin Space + /// Unstable: this configuration part works as advertised, but may change in a future release + adminspace: { + /// Enables the admin space + enabled: true, + /// read and/or write permissions on the admin space + permissions: { + read: true, + write: false, + }, + }, + +} 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_config.json5 b/nexus_demos/config/zenoh/workcell_1_config.json5 new file mode 100644 index 00000000..a7ebed43 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_1_config.json5 @@ -0,0 +1,754 @@ +/// This file attempts to list and document available configuration elements. +/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. +/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. +{ + /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) + /// that zenoh runtime will use. + /// If not set, a random unsigned 128bit integer will be used. + /// WARNING: this id must be unique in your zenoh network. + // id: "1234567890abcdef", + + /// The node's mode (router, peer or client) + mode: "peer", + + /// Which endpoints to connect to. E.g. tcp/localhost:7447. + /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. + /// + /// For TCP/UDP on Linux, it is possible additionally specify the interface to be connected to: + /// E.g. tcp/192.168.0.1:7447#iface=eth0, for connect only if the IP address is reachable via the interface eth0 + /// + /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. + /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. + /// + /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: + /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 + /// For TCP, UDP, Quic and TLS links, it is possible to specify a `bind` address for the local socket: + /// E.g. tcp/192.168.0.1:7447#bind=192.168.0.1:0 + /// Note!: Currently it is unsupported to specify both `bind` and `iface`. + /// + /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: + /// E.g. tcp/192.168.0.1:7447#dscp=0x08 + connect: { + /// timeout waiting for all endpoints connected (0: no retry, -1: infinite timeout) + /// Accepts a single value (e.g. timeout_ms: 0) + /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). + timeout_ms: { router: -1, peer: -1, client: 0 }, + + /// The list of endpoints to connect to. + /// Accepts a single list (e.g. endpoints: ["tcp/10.10.10.10:7447", "tcp/11.11.11.11:7447"]) + /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/10.10.10.10:7447"], peer: ["tcp/11.11.11.11:7447"] }). + /// + /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html + /// + /// ROS setting: By default connect to the Zenoh router on localhost on port 7447. + endpoints: [ + "tcp/localhost:7447" + ], + + /// Global connect configuration, + /// Accepts a single value or different values for router, peer and client. + /// The configuration can also be specified for the separate endpoint + /// it will override the global one + /// E.g. tcp/192.168.0.1:7447#retry_period_init_ms=20000;retry_period_max_ms=10000" + + /// exit from application, if timeout exceed + exit_on_failure: { router: false, peer: false, client: true }, + /// connect establishing retry configuration + retry: { + /// initial wait timeout until next connect try + period_init_ms: 1000, + /// maximum wait timeout until next connect try + period_max_ms: 4000, + /// increase factor for the next timeout until nexti connect try + period_increase_factor: 2, + }, + }, + + /// Which endpoints to listen on. E.g. tcp/0.0.0.0:7447. + /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, + /// peers, or client can use to establish a zenoh session. + /// + /// For TCP/UDP on Linux, it is possible additionally specify the interface to be listened to: + /// E.g. tcp/0.0.0.0:7447#iface=eth0, for listen connection only on eth0 + /// + /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. + /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. + /// + /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: + /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 + /// + /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: + /// E.g. tcp/192.168.0.1:7447#dscp=0x08 + listen: { + /// timeout waiting for all listen endpoints (0: no retry, -1: infinite timeout) + /// Accepts a single value (e.g. timeout_ms: 0) + /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). + timeout_ms: 0, + + /// The list of endpoints to listen on. + /// Accepts a single list (e.g. endpoints: ["tcp/[::]:7447", "udp/[::]:7447"]) + /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/[::]:7447"], peer: ["tcp/[::]:0"] }). + /// + /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html + /// + /// ROS setting: By default accept incoming connections only from localhost (i.e. from colocalized Nodes). + /// All communications with other hosts are routed by the Zenoh router. + endpoints: [ + "tcp/localhost:0" + ], + + /// Global listen configuration, + /// Accepts a single value or different values for router, peer and client. + /// The configuration can also be specified for the separate endpoint + /// it will override the global one + /// E.g. tcp/192.168.0.1:7447#exit_on_failure=false;retry_period_max_ms=1000" + + /// exit from application, if timeout exceed + exit_on_failure: true, + /// listen retry configuration + retry: { + /// initial wait timeout until next try + period_init_ms: 1000, + /// maximum wait timeout until next try + period_max_ms: 4000, + /// increase factor for the next timeout until next try + period_increase_factor: 2, + }, + }, + + /// Configure the session open behavior. + open: { + /// Configure the conditions to be met before session open returns. + return_conditions: { + /// Session open waits to connect to scouted peers and routers before returning. + /// When set to false, first publications and queries after session open from peers may be lost. + connect_scouted: true, + /// Session open waits to receive initial declares from connected peers before returning. + /// Setting to false may cause extra traffic at startup from peers. + declares: true, + }, + }, + + /// Configure the scouting mechanisms and their behaviours + scouting: { + /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. + timeout: 3000, + /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. + delay: 500, + /// The multicast scouting configuration. + multicast: { + /// Whether multicast scouting is enabled or not + /// + /// ROS setting: disable multicast discovery by default + enabled: false, + /// The socket which should be used for multicast scouting + address: "224.0.0.224:7446", + /// The network interface which should be used for multicast scouting + interface: "auto", // If not set or set to "auto" the interface if picked automatically + /// The time-to-live on multicast scouting packets + ttl: 1, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. + /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router, peer or client mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). + /// Each value is a list of: "peer", "router" and/or "client". + autoconnect: { router: [], peer: ["router", "peer"], client: ["router"] }, + /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. + /// Possible options are: + /// - "always": always attempt to autoconnect, may result in redundant connections. + /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. + /// If both nodes use this strategy, only one will attempt the connection. + /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example + /// because of a private IP. + /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, + /// or different values for router and/or peer depending on the type of node detected + /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), + /// or different values for router or peer mode + /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). + /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip + /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. + autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, + /// Whether or not to listen for scout messages on UDP multicast and reply to them. + listen: true, + }, + /// The gossip scouting configuration. Note that instances in "client" mode do not participate in gossip. + gossip: { + /// Whether gossip scouting is enabled or not + enabled: true, + /// When true, gossip scouting information are propagated multiple hops to all nodes in the local network. + /// When false, gossip scouting information are only propagated to the next hop. + /// Activating multihop gossip implies more scouting traffic and a lower scalability. + /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have + /// direct connectivity with each other. + multihop: false, + /// Which type of Zenoh instances to send gossip messages to. + /// Accepts a single value (e.g. target: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router or peer mode (e.g. target: { router: ["router", "peer"], peer: ["router"] }). + /// Each value is a list of "peer" and/or "router". + /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip + /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. + target: { router: ["router", "peer"], peer: ["router"]}, + /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. + /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, + /// or different values for router or peer mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). + /// Each value is a list of: "peer" and/or "router". + autoconnect: { router: [], peer: ["router", "peer"] }, + /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. + /// Possible options are: + /// - "always": always attempt to autoconnect, may result in redundant connection which will then be closed. + /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. + /// If both nodes use this strategy, only one will attempt the connection. + /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example + /// because of a private IP. + /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, + /// or different values for router and/or peer depending on the type of node detected + /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), + /// or different values for router or peer mode + /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). + /// ROS setting: as by default all peers will interconnect to each other over the loopback interface, + /// they are all reachable to each other. Hence using "greater-zid" for peers connecting to + /// other peers is sufficient and avoids unecessary double connections between peers at startup. + autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, + }, + }, + + /// 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, + }, + + /// The default timeout to apply to queries in milliseconds. + /// ROS setting: The default value of 600000 ms (10 minutes) is already quite high, but it can be increased if needed. + /// For instance to avoid timeout with Service Server that that is unresponsive or lagging, + /// which could occur at launch time with a large number of Nodes starting all together. + /// It’s recommended to configure a value that exceeds the longest timeout specified in any + /// `spin_until_complete(service_call_future_result, timeout)` call. + /// Note that the action-related service "get_result" is hard-coded with an infinite timeout, + /// as in some use case an action could spend several hours (e.g. mission plans). + queries_default_timeout: 600000, + + /// The routing strategy to use and it's configuration. + routing: { + /// The routing strategy to use in routers and it's configuration. + router: { + /// When set to true a router will forward data between two peers + /// directly connected to it if it detects that those peers are not + /// connected to each other. + /// The failover brokering only works if gossip discovery is enabled + /// and peers are configured with gossip target "router". + peers_failover_brokering: true, + /// Linkstate mode configuration. + linkstate: { + /// Weights of the outgoing transports in linkstate mode. + /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. + /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. + /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. + // transport_weights: [ + // { dst_zid: "1", weight: "10" }, + // { dst_zid: "2", weight: "200" }, + // ] + }, + }, + /// The routing strategy to use in peers and it's configuration. + peer: { + /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). + /// This option needs to be set to the same value in all peers and routers of the subsystem. + mode: "peer_to_peer", + /// Linkstate mode configuration (only taken into account if mode == "linkstate"). + linkstate: { + /// Weights of the outgoing transports in linkstate mode. + /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. + /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. + /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. + // transport_weights: [ + // { dst_zid: "1", weight: "10" }, + // { dst_zid: "2", weight: "200" }, + // ] + }, + }, + /// The interests-based routing configuration. + /// This configuration applies regardless of the mode (router, peer or client). + interests: { + /// The timeout to wait for incoming interests declarations in milliseconds. + /// The expiration of this timeout implies that the discovery protocol might be incomplete, + /// leading to potential loss of messages, queries or liveliness tokens. + timeout: 10000, + }, + }, + + // /// Overwrite QoS options for Zenoh messages by key expression (ignores Zenoh API QoS config for overwritten values) + // qos: { + // /// Overwrite QoS options for PUT and DELETE messages + // publication: [ + // { + // /// PUT and DELETE messages on key expressions that are included by these key expressions + // /// will have their QoS options overwritten by the given config. + // key_exprs: ["demo/**", "example/key"], + // /// Configurations that will be applied on the publisher. + // /// Options that are supplied here will overwrite the configuration given in Zenoh API + // config: { + // congestion_control: "block", + // priority: "data_high", + // express: true, + // reliability: "best_effort", + // allowed_destination: "remote", + // }, + // }, + // ], + // /// Overwrite QoS options for messages sent and received from/to the network + // /// This allows more fine grained rules (per network card, etc...) but is + // /// less performant than the publication option above. + // network: [ + // { + // /// Optional Id, has to be unique. + // id: "lo0_en0_qos_overwrite", + // /// Optional list of ZIDs on which qos will be overwritten when communicating with. + // // zids: ["38a4829bce9166ee"], + // /// Optional list of interfaces, if not specified, will be applied to all interfaces. + // interfaces: [ + // "lo0", + // "en0", + // ], + // /// Optional list of link protocols. Transports with at least one of these links will have their qos overwritten. + // /// If absent, the overwrite will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// List of message types to apply to. + // messages: [ + // "put", // put publications + // "delete", // delete publications + // "query", // get queries + // "reply", // replies to queries + // ], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the rules will be applied to both flows. + // flows: ["egress", "ingress"], + // /// QoS filter to apply to the messages matching this item. + // qos: { + // congestion_control: "drop", + // priority: "data", + // express: true, + // reliability: "reliable", + // }, + // /// payload_size range for the messages matching this item. + // payload_size: "1000000..", + // key_exprs: ["test/demo"], + // overwrite: { + // /// Optional new priority value, if not specified priority of the messages will stay unchanged. + // priority: "real_time", + // /// Optional new congestion control value, if not specified congestion control of the messages will stay unchanged. + // congestion_control: "block", + // /// Optional new express value, if not specified express flag of the messages will stay unchanged. + // express: true, + // }, + // }, + // ], + // }, + + // /// The declarations aggregation strategy. + // aggregation: { + // /// A list of key-expressions for which all included subscribers will be aggregated into. + // subscribers: [ + // // key_expression + // ], + // /// A list of key-expressions for which all included publishers will be aggregated into. + // publishers: [ + // // key_expression + // ], + // }, + + // /// Namespace prefix. + // /// If specified, all outgoing key expressions will be automatically prefixed with specified string, + // /// and all incoming key expressions will be stripped of specified prefix. + // /// The namespace prefix should satisfy all key expression constraints + // /// and additionally it can not contain wild characters ('*'). + // /// Namespace is applied to the session. + // /// E. g. if session has a namespace of "1" then session.put("my/keyexpr", my_message), + // /// will put a message into 1/my/keyexpr. Same applies to all other operations within this session. + // namespace: "my/namespace", + + // /// The downsampling declaration. + // downsampling: [ + // { + // /// Optional Id, has to be unique + // id: "wlan0egress", + // /// Optional list of network interfaces messages will be processed on, the rest will be passed as is. + // /// If absent, the rules will be applied to all interfaces. An empty list is invalid. + // interfaces: [ "wlan0" ], + // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. + // /// If absent, the rules will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the rules will be applied to both flows. + // flows: ["ingress", "egress"], + // /// List of message type on which downsampling will be applied. Must not be empty. + // messages: [ + // /// Delete + // "delete", + // /// Put + // "put", + // /// Get + // "query", + // /// Queryable Reply to a Query + // "reply", + // ], + // /// A list of downsampling rules: key_expression and the maximum frequency in Hertz + // rules: [ + // { key_expr: "demo/example/zenoh-rs-pub", freq: 0.1 }, + // ], + // }, + // ], + + // /// Configure access control (ACL) rules + access_control: { + /// [true/false] acl will be activated only if this is set to true + "enabled": true, + /// [deny/allow] default permission is deny (even if this is left empty or not specified) + "default_permission": "deny", + /// Rule set for permissions allowing or denying access to key-expressions + "rules": + [ + { + "flows": [ + "ingress", + "egress" + ], + "id": "liveliness_tokens", + "key_exprs": [ + "@ros2_lv/0/**" + ], + "messages": [ + "liveliness_token", + "liveliness_query", + "declare_liveliness_subscriber", + "reply" + ], + "permission": "allow" + } + ], + /// List of combinations of subjects. + /// + /// If a subject property (i.e. username, certificate common name or interface) is empty + /// it is interpreted as a wildcard. Moreover, a subject property cannot be an empty list. + "subjects": + [ + { + "id": "workcell_1", + }, + ], + /// The policies list associates rules to subjects + "policies": + [ + /// Each policy associates one or multiple rules to one or multiple subject combinations + { + "rules": [ + "bridged_topics", + "bridged_services", + "bridged_actions", + "liveliness_tokens", + ], + "subjects": ["workcell_1"], + }, + ] + }, + + // low_pass_filter: [ + // { + // /// Optional Id, has to be unique + // "id": "filter1", + // /// Optional list of network interfaces messages will be processed on, the rest will not be filtered. + // /// If absent, the filter will be applied to all interfaces. + // interfaces: [ "wlan0" ], + // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. + // /// If absent, the rule will be applied to all transports. An empty list is invalid. + // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], + // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). + // /// If absent, the filter will be applied to both flows. + // flows: ["ingress", "egress"], + // /// List of message type on which the filter will be applied. Must not be empty. + // messages: [ + // "put", + // "delete", + // "query", + // "reply" + // ], + // /// List of key_expressions which matching messages will be filtered + // key_exprs: [ + // "demo/**", + // ], + // /// Inclusive max size of serialized payload + serialized attachment + // size_limit: 8192, + // }, + // ], + + /// Configure internal transport parameters + transport: { + unicast: { + /// Timeout in milliseconds when opening a link + /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together + open_timeout: 60000, + /// Timeout in milliseconds when accepting a link + /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together + accept_timeout: 60000, + /// Maximum number of links in pending state while performing the handshake for accepting it + /// ROS setting: increase the value to support a large number of Nodes starting all together + accept_pending: 10000, + /// Maximum number of transports that can be simultaneously alive for a single zenoh sessions + /// ROS setting: increase the value to support a large number of Nodes starting all together + max_sessions: 10000, + /// Maximum number of incoming links that are admitted per transport + max_links: 1, + /// Enables the LowLatency transport + /// This option does not make LowLatency transport mandatory, the actual implementation of transport + /// used will depend on Establish procedure and other party's settings + /// + /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. + /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to + /// enable 'lowlatency' you need to explicitly disable 'qos'. + /// NOTE: LowLatency transport does not support the fragmentation, so the message size should be + /// smaller than the tx batch_size. + lowlatency: false, + /// Enables QoS on unicast communications. + qos: { + enabled: true, + }, + /// Enables compression on unicast communications. + /// Compression capabilities are negotiated during session establishment. + /// If both Zenoh nodes support compression, then compression is activated. + compression: { + enabled: false, + }, + }, + /// WARNING: multicast communication does not perform any negotiation upon group joining. + /// Because of that, it is important that all transport parameters are the same to make + /// sure all your nodes in the system can communicate. One common parameter to configure + /// is "transport/link/tx/batch_size" since its default value depends on the actual platform + /// when operating on multicast. + /// E.g., the batch size on Linux and Windows is 65535 bytes, on Mac OS X is 9216, and anything else is 8192. + multicast: { + /// JOIN message transmission interval in milliseconds. + join_interval: 2500, + /// Maximum number of multicast sessions. + max_sessions: 1000, + /// Enables QoS on multicast communication. + /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. + qos: { + enabled: false, + }, + /// Enables compression on multicast communication. + /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. + compression: { + enabled: false, + }, + }, + link: { + /// An optional whitelist of protocols to be used for accepting and opening sessions. If not + /// configured, all the supported protocols are automatically whitelisted. The supported + /// protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream", "vsock"] For + /// example, to only enable "tls" and "quic": protocols: ["tls", "quic"], + /// + /// Configure the zenoh TX parameters of a link + tx: { + /// The resolution in bits to be used for the message sequence numbers. + /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. + /// Accepted values: 8bit, 16bit, 32bit, 64bit. + sequence_number_resolution: "32bit", + /// Link lease duration in milliseconds to announce to other zenoh nodes + /// ROS setting: increase the value to avoid lease expiration at launch time with a large number of Nodes starting all together + lease: 60000, + /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive + /// messages will be sent at the configured time interval. + /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, + /// set the actual keep_alive interval to one fourth of the lease time: i.e. send + /// 4 keep_alive messages in a lease period. Changing the lease time will have the + /// keep_alive messages sent more or less often. + /// This is in-line with the ITU-T G.8013/Y.1731 specification on continuous connectivity + /// check which considers a link as failed when no messages are received in 3.5 times the + /// target interval. + /// ROS setting: decrease the value since Nodes are communicating over the loopback + /// where keep-alive messages have less chances to be lost. + keep_alive: 2, + /// Batch size in bytes is expressed as a 16bit unsigned integer. + /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). + /// The default batch size value is the maximum batch size: 65535. + batch_size: 65535, + /// Each zenoh link has a transmission queue that can be configured + queue: { + /// The size of each priority queue indicates the number of batches a given queue can contain. + /// NOTE: the number of batches in each priority must be included between 1 and 16. Different values will result in an error. + /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. + /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, + /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. + /// If qos is false, then only the DATA priority will be allocated. + size: { + control: 2, + real_time: 2, + interactive_high: 2, + interactive_low: 2, + data_high: 2, + data: 2, + data_low: 2, + background: 2, + }, + /// Congestion occurs when the queue is empty (no available batch). + congestion_control: { + /// Behavior pushing CongestionControl::Drop messages to the queue. + drop: { + /// The maximum time in microseconds to wait for an available batch before dropping a droppable message if still no batch is available. + wait_before_drop: 1000, + /// The maximum deadline limit for multi-fragment messages. + max_wait_before_drop_fragments: 50000, + }, + /// Behavior pushing CongestionControl::Block messages to the queue. + block: { + /// The maximum time in microseconds to wait for an available batch before closing the transport session when sending a blocking message + /// if still no batch is available. + /// ROS setting: increase the value to avoid unecessary link closure at launch time where congestion is likely + /// to occur even over the loopback since all the Nodes are starting at the same time. + wait_before_close: 60000000, + }, + }, + /// Perform batching of messages if they are smaller of the batch_size + batching: { + /// Perform adaptive batching of messages if they are smaller of the batch_size. + /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be + /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput + /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. + enabled: true, + /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. + time_limit: 1, + }, + allocation: { + /// Mode for memory allocation of batches in the priority queues. + /// - "init": batches are allocated at queue initialization time. + /// - "lazy": batches are allocated when needed up to the maximum number of batches configured in the size configuration parameter. + mode: "lazy", + }, + }, + }, + /// Configure the zenoh RX parameters of a link + rx: { + /// Receiving buffer size in bytes for each link + /// The default the rx_buffer_size value is the same as the default batch size: 65535. + /// For very high throughput scenarios, the rx_buffer_size can be increased to accommodate + /// more in-flight data. This is particularly relevant when dealing with large messages. + /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. + buffer_size: 65535, + /// Maximum size of the defragmentation buffer at receiver end. + /// Fragmented messages that are larger than the configured size will be dropped. + /// The default value is 1GiB. This would work in most scenarios. + /// NOTE: reduce the value if you are operating on a memory constrained device. + max_message_size: 1073741824, + }, + /// Configure TLS specific parameters + tls: { + /// Path to the certificate of the certificate authority used to validate either the server + /// or the client's keys and certificates, depending on the node's mode. If not specified + /// on router mode then the default WebPKI certificates are used instead. + root_ca_certificate: null, + /// Path to the TLS listening side private key + listen_private_key: null, + /// Path to the TLS listening side public certificate + listen_certificate: null, + /// Enables mTLS (mutual authentication), client authentication + enable_mtls: false, + /// Path to the TLS connecting side private key + connect_private_key: null, + /// Path to the TLS connecting side certificate + connect_certificate: null, + /// Whether or not to verify the matching between hostname/dns and certificate when connecting, + /// if set to false zenoh will disregard the common names of the certificates when verifying servers. + /// This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your + /// ca to verify that the server at baz.com is actually baz.com, let this be true (default). + verify_name_on_connect: true, + /// Whether or not to close links when remote certificates expires. + /// If set to true, links that require certificates (tls/quic) will automatically disconnect when the time of expiration of the remote certificate chain is reached + /// note that mTLS (client authentication) is required for a listener to disconnect a client on expiration + close_link_on_expiration: false, + /// Optional configuration for TCP system buffers sizes for TLS links + /// + /// Configure TCP read buffer size (bytes) + // so_rcvbuf: 123456, + /// Configure TCP write buffer size (bytes) + // so_sndbuf: 123456, + }, + // // Configure optional TCP link specific parameters + // tcp: { + // /// Optional configuration for TCP system buffers sizes for TCP links + // /// + // /// Configure TCP read buffer size (bytes) + // // so_rcvbuf: 123456, + // /// Configure TCP write buffer size (bytes) + // // so_sndbuf: 123456, + // }, + }, + /// Shared memory configuration. + /// NOTE: shared memory can be used only if zenoh is compiled with "shared-memory" feature, otherwise + /// settings in this section have no effect. + shared_memory: { + /// Whether shared memory is enabled or not. + /// If set to `true`, the SHM buffer optimization support will be announced to other parties. (default `true`). + /// This option doesn't make SHM buffer optimization mandatory, the real support depends on other party setting. + /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate + /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the + /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. + /// + /// ROS setting: disabled by default until fully tested + enabled: false, + /// SHM resources initialization mode (default "lazy"). + /// - "lazy": SHM subsystem internals will be initialized lazily upon the first SHM buffer + /// allocation or reception. This setting provides better startup time and optimizes resource usage, + /// but produces extra latency at the first SHM buffer interaction. + /// - "init": SHM subsystem internals will be initialized upon Session opening. This setting sacrifices + /// startup time, but guarantees no latency impact when first SHM buffer is processed. + mode: "lazy", + transport_optimization: { + /// Enables transport optimization for large messages (default `true`). + /// Implicitly puts large messages into shared memory for transports with SHM-compatible connection. + enabled: true, + /// SHM memory size in bytes used for transport optimization (default `16 * 1024 * 1024`). + pool_size: 16777216, + /// Allow optimization for messages equal or larger than this threshold in bytes (default `3072`). + message_size_threshold: 3072, + }, + }, + auth: { + /// The configuration of authentication. + /// A password implies a username is required. + usrpwd: { + user: null, + password: null, + /// The path to a file containing the user password dictionary + dictionary_file: null, + }, + pubkey: { + public_key_pem: null, + private_key_pem: null, + public_key_file: null, + private_key_file: null, + key_size: null, + known_keys_file: null, + }, + }, + }, + + /// Configure the Admin Space + /// Unstable: this configuration part works as advertised, but may change in a future release + adminspace: { + /// Enables the admin space + enabled: true, + /// read and/or write permissions on the admin space + permissions: { + read: true, + write: 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 6d6b4190..dad51eb7 100644 --- a/nexus_demos/launch/inter_workcell.launch.py +++ b/nexus_demos/launch/inter_workcell.launch.py @@ -111,9 +111,9 @@ def launch_setup(context, *args, **kwargs): ros_domain_id = LaunchConfiguration("ros_domain_id") use_fake_hardware = LaunchConfiguration("use_fake_hardware") use_multiple_transporters = LaunchConfiguration("use_multiple_transporters") - 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") activate_system_orchestrator = LaunchConfiguration("activate_system_orchestrator") headless = LaunchConfiguration("headless") main_bt_filename = LaunchConfiguration("main_bt_filename") @@ -230,7 +230,7 @@ def launch_setup(context, *args, **kwargs): condition=UnlessCondition(headless), ) - zenoh_bridge = GroupAction( + zenoh_router = GroupAction( [ IncludeLaunchDescription( [ @@ -238,18 +238,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( @@ -261,6 +260,16 @@ def launch_setup(context, *args, **kwargs): return [ SetEnvironmentVariable("ROS_DOMAIN_ID", ros_domain_id), + zenoh_router, + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, + ] + ) + ), system_orchestrator_node, building_map_node, rmf_transporter, @@ -268,7 +277,6 @@ def launch_setup(context, *args, **kwargs): rmf_transporter_node, mock_emergency_alarm_node, nexus_panel, - zenoh_bridge, activate_system_orchestrator, activate_mock_transporter_node, activate_rmf_transporter_node, @@ -296,20 +304,20 @@ def generate_launch_description(): description="Set true to run multiple transporters including the 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", ), DeclareLaunchArgument( - name="zenoh_config_filename", - default_value="config/zenoh/system_orchestrator.json5", - description="Zenoh DDS bridge configuration filepath", + name="zenoh_router_config_filename", + default_value="config/zenoh/system_orchestrator_router_config.json5", + description="RMW Zenoh router configuration filepath", + ), + DeclareLaunchArgument( + name="zenoh_session_config_filename", + default_value="config/zenoh/system_orchestrator_session_config.json5", + description="RMW Zenoh session configuration filepath", ), DeclareLaunchArgument( "activate_system_orchestrator", diff --git a/nexus_demos/launch/launch.py b/nexus_demos/launch/launch.py index 20e46664..2c0b14b3 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_cpp is supported, the environment variable RMW_IMPLEMENTATION must be set to rmw_zenoh_cpp", file=sys.stderr, ) exit(1) headless = LaunchConfiguration("headless") use_multiple_transporters = LaunchConfiguration("use_multiple_transporters") - use_zenoh_bridge = LaunchConfiguration("use_zenoh_bridge") use_fake_hardware = LaunchConfiguration("use_fake_hardware") robot1_ip = LaunchConfiguration("robot1_ip") robot2_ip = LaunchConfiguration("robot2_ip") @@ -59,26 +58,14 @@ def launch_setup(context, *args, **kwargs): if "ROS_DOMAIN_ID" in os.environ: inter_workcell_domain_id = int(os.environ["ROS_DOMAIN_ID"]) + workcell_1_domain_id = int(os.environ["ROS_DOMAIN_ID"]) + workcell_2_domain_id = int(os.environ["ROS_DOMAIN_ID"]) if not 0 < inter_workcell_domain_id < 230: log_msg += ( "ROS_DOMAIN_ID not within the range of 0 to 230, setting it to 0. \n" ) 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 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" @@ -108,7 +95,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_multiple_transporters": use_multiple_transporters, "activate_system_orchestrator": headless, "headless": headless, @@ -143,7 +131,6 @@ def launch_setup(context, *args, **kwargs): "max_jobs": max_workcell_jobs, "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", @@ -158,7 +145,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", "io_stations_config_file_path": os.path.join(get_package_share_directory("nexus_demos"), "config", "workcell_1_io_config.yaml"), }.items(), condition=IfCondition(run_workcell_1), @@ -189,7 +177,6 @@ def launch_setup(context, *args, **kwargs): "max_jobs": max_workcell_jobs, "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", @@ -204,7 +191,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", "io_stations_config_file_path": os.path.join(get_package_share_directory("nexus_demos"), "config", "workcell_2_io_config.yaml"), }.items(), condition=IfCondition(run_workcell_2), @@ -234,13 +222,6 @@ def generate_launch_description(): description="Set true to run multiple transporters including the 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 7f75997e..ebf06bfd 100644 --- a/nexus_demos/launch/workcell.launch.py +++ b/nexus_demos/launch/workcell.launch.py @@ -92,9 +92,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") remap_task_types = LaunchConfiguration("remap_task_types") # todo(Yadunund): There is no good way to get a list of strings via CLI and parse it using # LaunchConfiguration. The best way to configure this would be via a YAML params which we @@ -183,8 +183,39 @@ def launch_setup(context, *args, **kwargs): arguments=['--ros-args', '--log-level', 'info'], ) + zenoh_router = 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(), + ) + ] + ) + return [ SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), + zenoh_router, + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, + ] + ) + ), mock_dispenser_node, mock_product_detector_node, mock_gripper_node, @@ -244,29 +275,13 @@ def launch_setup(context, *args, **kwargs): 'launch', tf_publisher_launch_file, ]) + ], + launch_arguments=[ + ] ) ] ), - 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)), ] @@ -367,20 +382,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", ), DeclareLaunchArgument( "remap_task_types", diff --git a/nexus_demos/launch/workcell_1_tf.launch.py b/nexus_demos/launch/workcell_1_tf.launch.py index 95abd1f4..1c66fbaa 100644 --- a/nexus_demos/launch/workcell_1_tf.launch.py +++ b/nexus_demos/launch/workcell_1_tf.launch.py @@ -13,16 +13,19 @@ # limitations under the License. -import rclpy -import rclpy.node from launch_ros.actions import Node from launch import LaunchDescription -from launch.actions import GroupAction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): """Publish static TFs. """ + ros_domain_id = LaunchConfiguration("ros_domain_id") + zenoh_config_package = LaunchConfiguration("zenoh_config_package") + zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") # Static TF of robot base_link to world static_tf_base_link = Node( @@ -151,16 +154,42 @@ def generate_launch_description(): ], ) - return LaunchDescription( - [ - GroupAction( - actions=[ - static_tf_base_link, - static_tf_home, - static_tf_pickup_pose, - static_tf_pallet, - static_tf_dropoff, + return [ + SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, ] ) + ), + static_tf_base_link, + static_tf_home, + static_tf_pickup_pose, + static_tf_pallet, + static_tf_dropoff, + ] + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "ros_domain_id", + default_value="0", + description="ROS_DOMAIN_ID environment variable", + ), + DeclareLaunchArgument( + name="zenoh_config_package", + default_value="nexus_demos", + description="Package containing Zenoh DDS bridge configurations", + ), + 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/workcell_2_tf.launch.py b/nexus_demos/launch/workcell_2_tf.launch.py index 361721b7..ba7796ca 100644 --- a/nexus_demos/launch/workcell_2_tf.launch.py +++ b/nexus_demos/launch/workcell_2_tf.launch.py @@ -13,16 +13,19 @@ # limitations under the License. -import rclpy -import rclpy.node from launch_ros.actions import Node from launch import LaunchDescription -from launch.actions import GroupAction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -def generate_launch_description(): +def launch_setup(context, *args, **kwargs): """Publish static TFs. """ + ros_domain_id = LaunchConfiguration("ros_domain_id") + zenoh_config_package = LaunchConfiguration("zenoh_config_package") + zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") # Static TF of robot base_link to world static_tf_base_link = Node( @@ -153,16 +156,42 @@ def generate_launch_description(): ], ) - return LaunchDescription( - [ - GroupAction( - actions=[ - static_tf_base_link, - static_tf_home, - static_tf_dropoff_goal_pose, - static_tf_sku_detection_camera, - static_tf_pallet, + return [ + SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), + SetEnvironmentVariable( + "ZENOH_SESSION_CONFIG_URI", + PathJoinSubstitution( + [ + FindPackageShare(zenoh_config_package), + zenoh_session_config_filename, ] ) + ), + static_tf_base_link, + static_tf_home, + static_tf_dropoff_goal_pose, + static_tf_sku_detection_camera, + static_tf_pallet, + ] + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument( + "ros_domain_id", + default_value="0", + description="ROS_DOMAIN_ID environment variable", + ), + DeclareLaunchArgument( + name="zenoh_config_package", + default_value="nexus_demos", + description="Package containing Zenoh DDS bridge configurations", + ), + 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 ec0cd3b0..5e71ead1 100644 --- a/nexus_demos/package.xml +++ b/nexus_demos/package.xml @@ -44,7 +44,6 @@ nexus_rviz_plugins nexus_system_orchestrator nexus_workcell_orchestrator - nexus_zenoh_bridge_dds_vendor rmf_building_map_tools From fdc643d8d44993f445eece018b81a9877f305384 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 19 Dec 2025 17:56:33 +0800 Subject: [PATCH 03/17] sim works Signed-off-by: Aaron Chong --- .../config/zenoh/inter_workcell_config.json5 | 810 ------------------ .../zenoh/inter_workcell_router_config.json5 | 27 + .../zenoh/inter_workcell_session_config.json5 | 24 + nexus_demos/config/zenoh/main.json5 | 648 -------------- nexus_demos/config/zenoh/policy_main.xml | 93 -- .../system_orchestrator_router_config.json5 | 11 - .../system_orchestrator_session_config.json5 | 25 - .../zenoh/workcell_1_bridge_config.json5 | 110 +++ .../config/zenoh/workcell_1_config.json5 | 754 ---------------- .../zenoh/workcell_1_router_config.json5 | 102 +-- .../zenoh/workcell_1_session_config.json5 | 23 +- .../zenoh/workcell_2_bridge_config.json5 | 110 +++ .../zenoh/workcell_2_router_config.json5 | 102 +-- .../zenoh/workcell_2_session_config.json5 | 23 +- nexus_demos/config/zenoh/zenohd.json5 | 271 ------ nexus_demos/launch/launch.py | 50 +- 16 files changed, 373 insertions(+), 2810 deletions(-) delete mode 100644 nexus_demos/config/zenoh/inter_workcell_config.json5 create mode 100644 nexus_demos/config/zenoh/inter_workcell_router_config.json5 create mode 100644 nexus_demos/config/zenoh/inter_workcell_session_config.json5 delete mode 100644 nexus_demos/config/zenoh/main.json5 delete mode 100644 nexus_demos/config/zenoh/policy_main.xml delete mode 100644 nexus_demos/config/zenoh/system_orchestrator_router_config.json5 delete mode 100644 nexus_demos/config/zenoh/system_orchestrator_session_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_1_bridge_config.json5 delete mode 100644 nexus_demos/config/zenoh/workcell_1_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_2_bridge_config.json5 delete mode 100644 nexus_demos/config/zenoh/zenohd.json5 diff --git a/nexus_demos/config/zenoh/inter_workcell_config.json5 b/nexus_demos/config/zenoh/inter_workcell_config.json5 deleted file mode 100644 index 5df19b0d..00000000 --- a/nexus_demos/config/zenoh/inter_workcell_config.json5 +++ /dev/null @@ -1,810 +0,0 @@ -/// This file attempts to list and document available configuration elements. -/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. -/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. -{ - /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) - /// that zenoh runtime will use. - /// If not set, a random unsigned 128bit integer will be used. - /// WARNING: this id must be unique in your zenoh network. - // id: "1234567890abcdef", - - /// The node's mode (router, peer or client) - mode: "peer", - - /// Which endpoints to connect to. E.g. tcp/localhost:7447. - /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. - /// - /// For TCP/UDP on Linux, it is possible additionally specify the interface to be connected to: - /// E.g. tcp/192.168.0.1:7447#iface=eth0, for connect only if the IP address is reachable via the interface eth0 - /// - /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. - /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. - /// - /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: - /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 - /// For TCP, UDP, Quic and TLS links, it is possible to specify a `bind` address for the local socket: - /// E.g. tcp/192.168.0.1:7447#bind=192.168.0.1:0 - /// Note!: Currently it is unsupported to specify both `bind` and `iface`. - /// - /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: - /// E.g. tcp/192.168.0.1:7447#dscp=0x08 - connect: { - /// timeout waiting for all endpoints connected (0: no retry, -1: infinite timeout) - /// Accepts a single value (e.g. timeout_ms: 0) - /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). - timeout_ms: { router: -1, peer: -1, client: 0 }, - - /// The list of endpoints to connect to. - /// Accepts a single list (e.g. endpoints: ["tcp/10.10.10.10:7447", "tcp/11.11.11.11:7447"]) - /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/10.10.10.10:7447"], peer: ["tcp/11.11.11.11:7447"] }). - /// - /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html - /// - /// ROS setting: By default connect to the Zenoh router on localhost on port 7447. - endpoints: [ - "tcp/localhost:7447" - ], - - /// Global connect configuration, - /// Accepts a single value or different values for router, peer and client. - /// The configuration can also be specified for the separate endpoint - /// it will override the global one - /// E.g. tcp/192.168.0.1:7447#retry_period_init_ms=20000;retry_period_max_ms=10000" - - /// exit from application, if timeout exceed - exit_on_failure: { router: false, peer: false, client: true }, - /// connect establishing retry configuration - retry: { - /// initial wait timeout until next connect try - period_init_ms: 1000, - /// maximum wait timeout until next connect try - period_max_ms: 4000, - /// increase factor for the next timeout until nexti connect try - period_increase_factor: 2, - }, - }, - - /// Which endpoints to listen on. E.g. tcp/0.0.0.0:7447. - /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, - /// peers, or client can use to establish a zenoh session. - /// - /// For TCP/UDP on Linux, it is possible additionally specify the interface to be listened to: - /// E.g. tcp/0.0.0.0:7447#iface=eth0, for listen connection only on eth0 - /// - /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. - /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. - /// - /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: - /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 - /// - /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: - /// E.g. tcp/192.168.0.1:7447#dscp=0x08 - listen: { - /// timeout waiting for all listen endpoints (0: no retry, -1: infinite timeout) - /// Accepts a single value (e.g. timeout_ms: 0) - /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). - timeout_ms: 0, - - /// The list of endpoints to listen on. - /// Accepts a single list (e.g. endpoints: ["tcp/[::]:7447", "udp/[::]:7447"]) - /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/[::]:7447"], peer: ["tcp/[::]:0"] }). - /// - /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html - /// - /// ROS setting: By default accept incoming connections only from localhost (i.e. from colocalized Nodes). - /// All communications with other hosts are routed by the Zenoh router. - endpoints: [ - "tcp/localhost:0" - ], - - /// Global listen configuration, - /// Accepts a single value or different values for router, peer and client. - /// The configuration can also be specified for the separate endpoint - /// it will override the global one - /// E.g. tcp/192.168.0.1:7447#exit_on_failure=false;retry_period_max_ms=1000" - - /// exit from application, if timeout exceed - exit_on_failure: true, - /// listen retry configuration - retry: { - /// initial wait timeout until next try - period_init_ms: 1000, - /// maximum wait timeout until next try - period_max_ms: 4000, - /// increase factor for the next timeout until next try - period_increase_factor: 2, - }, - }, - - /// Configure the session open behavior. - open: { - /// Configure the conditions to be met before session open returns. - return_conditions: { - /// Session open waits to connect to scouted peers and routers before returning. - /// When set to false, first publications and queries after session open from peers may be lost. - connect_scouted: true, - /// Session open waits to receive initial declares from connected peers before returning. - /// Setting to false may cause extra traffic at startup from peers. - declares: true, - }, - }, - - /// Configure the scouting mechanisms and their behaviours - scouting: { - /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. - timeout: 3000, - /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. - delay: 500, - /// The multicast scouting configuration. - multicast: { - /// Whether multicast scouting is enabled or not - /// - /// ROS setting: disable multicast discovery by default - enabled: false, - /// The socket which should be used for multicast scouting - address: "224.0.0.224:7446", - /// The network interface which should be used for multicast scouting - interface: "auto", // If not set or set to "auto" the interface if picked automatically - /// The time-to-live on multicast scouting packets - ttl: 1, - /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. - /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router, peer or client mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). - /// Each value is a list of: "peer", "router" and/or "client". - autoconnect: { router: [], peer: ["router", "peer"], client: ["router"] }, - /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. - /// Possible options are: - /// - "always": always attempt to autoconnect, may result in redundant connections. - /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. - /// If both nodes use this strategy, only one will attempt the connection. - /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example - /// because of a private IP. - /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, - /// or different values for router and/or peer depending on the type of node detected - /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), - /// or different values for router or peer mode - /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). - /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip - /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. - autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, - /// Whether or not to listen for scout messages on UDP multicast and reply to them. - listen: true, - }, - /// The gossip scouting configuration. Note that instances in "client" mode do not participate in gossip. - gossip: { - /// Whether gossip scouting is enabled or not - enabled: true, - /// When true, gossip scouting information are propagated multiple hops to all nodes in the local network. - /// When false, gossip scouting information are only propagated to the next hop. - /// Activating multihop gossip implies more scouting traffic and a lower scalability. - /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have - /// direct connectivity with each other. - multihop: false, - /// Which type of Zenoh instances to send gossip messages to. - /// Accepts a single value (e.g. target: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router or peer mode (e.g. target: { router: ["router", "peer"], peer: ["router"] }). - /// Each value is a list of "peer" and/or "router". - /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip - /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. - target: { router: ["router", "peer"], peer: ["router"]}, - /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. - /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router or peer mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). - /// Each value is a list of: "peer" and/or "router". - autoconnect: { router: [], peer: ["router", "peer"] }, - /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. - /// Possible options are: - /// - "always": always attempt to autoconnect, may result in redundant connection which will then be closed. - /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. - /// If both nodes use this strategy, only one will attempt the connection. - /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example - /// because of a private IP. - /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, - /// or different values for router and/or peer depending on the type of node detected - /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), - /// or different values for router or peer mode - /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). - /// ROS setting: as by default all peers will interconnect to each other over the loopback interface, - /// they are all reachable to each other. Hence using "greater-zid" for peers connecting to - /// other peers is sufficient and avoids unecessary double connections between peers at startup. - autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, - }, - }, - - /// 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, - }, - - /// The default timeout to apply to queries in milliseconds. - /// ROS setting: The default value of 600000 ms (10 minutes) is already quite high, but it can be increased if needed. - /// For instance to avoid timeout with Service Server that that is unresponsive or lagging, - /// which could occur at launch time with a large number of Nodes starting all together. - /// It’s recommended to configure a value that exceeds the longest timeout specified in any - /// `spin_until_complete(service_call_future_result, timeout)` call. - /// Note that the action-related service "get_result" is hard-coded with an infinite timeout, - /// as in some use case an action could spend several hours (e.g. mission plans). - queries_default_timeout: 600000, - - /// The routing strategy to use and it's configuration. - routing: { - /// The routing strategy to use in routers and it's configuration. - router: { - /// When set to true a router will forward data between two peers - /// directly connected to it if it detects that those peers are not - /// connected to each other. - /// The failover brokering only works if gossip discovery is enabled - /// and peers are configured with gossip target "router". - peers_failover_brokering: true, - /// Linkstate mode configuration. - linkstate: { - /// Weights of the outgoing transports in linkstate mode. - /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. - /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. - /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. - // transport_weights: [ - // { dst_zid: "1", weight: "10" }, - // { dst_zid: "2", weight: "200" }, - // ] - }, - }, - /// The routing strategy to use in peers and it's configuration. - peer: { - /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). - /// This option needs to be set to the same value in all peers and routers of the subsystem. - mode: "peer_to_peer", - /// Linkstate mode configuration (only taken into account if mode == "linkstate"). - linkstate: { - /// Weights of the outgoing transports in linkstate mode. - /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. - /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. - /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. - // transport_weights: [ - // { dst_zid: "1", weight: "10" }, - // { dst_zid: "2", weight: "200" }, - // ] - }, - }, - /// The interests-based routing configuration. - /// This configuration applies regardless of the mode (router, peer or client). - interests: { - /// The timeout to wait for incoming interests declarations in milliseconds. - /// The expiration of this timeout implies that the discovery protocol might be incomplete, - /// leading to potential loss of messages, queries or liveliness tokens. - timeout: 10000, - }, - }, - - // /// Overwrite QoS options for Zenoh messages by key expression (ignores Zenoh API QoS config for overwritten values) - // qos: { - // /// Overwrite QoS options for PUT and DELETE messages - // publication: [ - // { - // /// PUT and DELETE messages on key expressions that are included by these key expressions - // /// will have their QoS options overwritten by the given config. - // key_exprs: ["demo/**", "example/key"], - // /// Configurations that will be applied on the publisher. - // /// Options that are supplied here will overwrite the configuration given in Zenoh API - // config: { - // congestion_control: "block", - // priority: "data_high", - // express: true, - // reliability: "best_effort", - // allowed_destination: "remote", - // }, - // }, - // ], - // /// Overwrite QoS options for messages sent and received from/to the network - // /// This allows more fine grained rules (per network card, etc...) but is - // /// less performant than the publication option above. - // network: [ - // { - // /// Optional Id, has to be unique. - // id: "lo0_en0_qos_overwrite", - // /// Optional list of ZIDs on which qos will be overwritten when communicating with. - // // zids: ["38a4829bce9166ee"], - // /// Optional list of interfaces, if not specified, will be applied to all interfaces. - // interfaces: [ - // "lo0", - // "en0", - // ], - // /// Optional list of link protocols. Transports with at least one of these links will have their qos overwritten. - // /// If absent, the overwrite will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// List of message types to apply to. - // messages: [ - // "put", // put publications - // "delete", // delete publications - // "query", // get queries - // "reply", // replies to queries - // ], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the rules will be applied to both flows. - // flows: ["egress", "ingress"], - // /// QoS filter to apply to the messages matching this item. - // qos: { - // congestion_control: "drop", - // priority: "data", - // express: true, - // reliability: "reliable", - // }, - // /// payload_size range for the messages matching this item. - // payload_size: "1000000..", - // key_exprs: ["test/demo"], - // overwrite: { - // /// Optional new priority value, if not specified priority of the messages will stay unchanged. - // priority: "real_time", - // /// Optional new congestion control value, if not specified congestion control of the messages will stay unchanged. - // congestion_control: "block", - // /// Optional new express value, if not specified express flag of the messages will stay unchanged. - // express: true, - // }, - // }, - // ], - // }, - - // /// The declarations aggregation strategy. - // aggregation: { - // /// A list of key-expressions for which all included subscribers will be aggregated into. - // subscribers: [ - // // key_expression - // ], - // /// A list of key-expressions for which all included publishers will be aggregated into. - // publishers: [ - // // key_expression - // ], - // }, - - // /// Namespace prefix. - // /// If specified, all outgoing key expressions will be automatically prefixed with specified string, - // /// and all incoming key expressions will be stripped of specified prefix. - // /// The namespace prefix should satisfy all key expression constraints - // /// and additionally it can not contain wild characters ('*'). - // /// Namespace is applied to the session. - // /// E. g. if session has a namespace of "1" then session.put("my/keyexpr", my_message), - // /// will put a message into 1/my/keyexpr. Same applies to all other operations within this session. - // namespace: "my/namespace", - - // /// The downsampling declaration. - // downsampling: [ - // { - // /// Optional Id, has to be unique - // id: "wlan0egress", - // /// Optional list of network interfaces messages will be processed on, the rest will be passed as is. - // /// If absent, the rules will be applied to all interfaces. An empty list is invalid. - // interfaces: [ "wlan0" ], - // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. - // /// If absent, the rules will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the rules will be applied to both flows. - // flows: ["ingress", "egress"], - // /// List of message type on which downsampling will be applied. Must not be empty. - // messages: [ - // /// Delete - // "delete", - // /// Put - // "put", - // /// Get - // "query", - // /// Queryable Reply to a Query - // "reply", - // ], - // /// A list of downsampling rules: key_expression and the maximum frequency in Hertz - // rules: [ - // { key_expr: "demo/example/zenoh-rs-pub", freq: 0.1 }, - // ], - // }, - // ], - - // /// Configure access control (ACL) rules - // access_control: { - // /// [true/false] acl will be activated only if this is set to true - // "enabled": false, - // /// [deny/allow] default permission is deny (even if this is left empty or not specified) - // "default_permission": "deny", - // /// Rule set for permissions allowing or denying access to key-expressions - // "rules": - // [ - // { - // /// Id has to be unique within the rule set - // "id": "rule1", - // "messages": [ - // "put", "delete", "declare_subscriber", - // "query", "reply", "declare_queryable", - // "liveliness_token", "liveliness_query", "declare_liveliness_subscriber", - // ], - // "flows":["egress","ingress"], - // "permission": "allow", - // "key_exprs": [ - // "test/demo" - // ], - // }, - // { - // "id": "rule2", - // "messages": [ - // "put", "delete", "declare_subscriber", - // "query", "reply", "declare_queryable", - // ], - // "flows":["ingress"], - // "permission": "allow", - // "key_exprs": [ - // "**" - // ], - // }, - // ], - // /// List of combinations of subjects. - // /// - // /// If a subject property (i.e. username, certificate common name or interface) is empty - // /// it is interpreted as a wildcard. Moreover, a subject property cannot be an empty list. - // "subjects": - // [ - // { - // /// Id has to be unique within the subjects list - // "id": "subject1", - // /// Subjects can be interfaces - // "interfaces": [ - // "lo0", - // "en0", - // ], - // /// Subjects can be cert_common_names when using TLS or Quic - // "cert_common_names": [ - // "example.zenoh.io" - // ], - // /// Subjects can be usernames when using user/password authentication - // "usernames": [ - // "zenoh-example" - // ], - // /// This instance translates internally to this filter: - // /// (interface="lo0" && cert_common_name="example.zenoh.io" && username="zenoh-example") || - // /// (interface="en0" && cert_common_name="example.zenoh.io" && username="zenoh-example") - // }, - // { - // "id": "subject2", - // "interfaces": [ - // "lo0", - // "en0", - // ], - // "cert_common_names": [ - // "example2.zenoh.io" - // ], - // /// This instance translates internally to this filter: - // /// (interface="lo0" && cert_common_name="example2.zenoh.io") || - // /// (interface="en0" && cert_common_name="example2.zenoh.io") - // }, - // { - // "id": "subject3", - // /// An empty subject combination is a wildcard - // }, - // { - // "id": "subject4", - // /// link protocols can also be used to identify transports to filter messages on. - // /// If absent, the rules will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// ZIDs can also be used to identify transports to filter messages on. - // /// NOTE: ZID is not backed by an authentication mechanism, it can only be trusted for ACL if it is - // /// dynamically added/removed by eventual dedicated Zenoh mechanisms when transports are opened/closed. - // /// If managed manually in ACL config, can be useful for prototyping but should not be used in production! - // zids: ["38a4829bce9166ee"], - // }, - // ], - // /// The policies list associates rules to subjects - // "policies": - // [ - // /// Each policy associates one or multiple rules to one or multiple subject combinations - // { - // /// Id is optional. If provided, it has to be unique within the policies list - // "id": "policy1", - // /// Rules and Subjects are identified with their unique IDs declared above - // "rules": ["rule1"], - // "subjects": ["subject1", "subject2"], - // }, - // { - // "rules": ["rule2"], - // "subjects": ["subject3", "subject4"], - // }, - // ] - // }, - - // low_pass_filter: [ - // { - // /// Optional Id, has to be unique - // "id": "filter1", - // /// Optional list of network interfaces messages will be processed on, the rest will not be filtered. - // /// If absent, the filter will be applied to all interfaces. - // interfaces: [ "wlan0" ], - // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. - // /// If absent, the rule will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the filter will be applied to both flows. - // flows: ["ingress", "egress"], - // /// List of message type on which the filter will be applied. Must not be empty. - // messages: [ - // "put", - // "delete", - // "query", - // "reply" - // ], - // /// List of key_expressions which matching messages will be filtered - // key_exprs: [ - // "demo/**", - // ], - // /// Inclusive max size of serialized payload + serialized attachment - // size_limit: 8192, - // }, - // ], - - /// Configure internal transport parameters - transport: { - unicast: { - /// Timeout in milliseconds when opening a link - /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together - open_timeout: 60000, - /// Timeout in milliseconds when accepting a link - /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together - accept_timeout: 60000, - /// Maximum number of links in pending state while performing the handshake for accepting it - /// ROS setting: increase the value to support a large number of Nodes starting all together - accept_pending: 10000, - /// Maximum number of transports that can be simultaneously alive for a single zenoh sessions - /// ROS setting: increase the value to support a large number of Nodes starting all together - max_sessions: 10000, - /// Maximum number of incoming links that are admitted per transport - max_links: 1, - /// Enables the LowLatency transport - /// This option does not make LowLatency transport mandatory, the actual implementation of transport - /// used will depend on Establish procedure and other party's settings - /// - /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. - /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to - /// enable 'lowlatency' you need to explicitly disable 'qos'. - /// NOTE: LowLatency transport does not support the fragmentation, so the message size should be - /// smaller than the tx batch_size. - lowlatency: false, - /// Enables QoS on unicast communications. - qos: { - enabled: true, - }, - /// Enables compression on unicast communications. - /// Compression capabilities are negotiated during session establishment. - /// If both Zenoh nodes support compression, then compression is activated. - compression: { - enabled: false, - }, - }, - /// WARNING: multicast communication does not perform any negotiation upon group joining. - /// Because of that, it is important that all transport parameters are the same to make - /// sure all your nodes in the system can communicate. One common parameter to configure - /// is "transport/link/tx/batch_size" since its default value depends on the actual platform - /// when operating on multicast. - /// E.g., the batch size on Linux and Windows is 65535 bytes, on Mac OS X is 9216, and anything else is 8192. - multicast: { - /// JOIN message transmission interval in milliseconds. - join_interval: 2500, - /// Maximum number of multicast sessions. - max_sessions: 1000, - /// Enables QoS on multicast communication. - /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. - qos: { - enabled: false, - }, - /// Enables compression on multicast communication. - /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. - compression: { - enabled: false, - }, - }, - link: { - /// An optional whitelist of protocols to be used for accepting and opening sessions. If not - /// configured, all the supported protocols are automatically whitelisted. The supported - /// protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream", "vsock"] For - /// example, to only enable "tls" and "quic": protocols: ["tls", "quic"], - /// - /// Configure the zenoh TX parameters of a link - tx: { - /// The resolution in bits to be used for the message sequence numbers. - /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. - /// Accepted values: 8bit, 16bit, 32bit, 64bit. - sequence_number_resolution: "32bit", - /// Link lease duration in milliseconds to announce to other zenoh nodes - /// ROS setting: increase the value to avoid lease expiration at launch time with a large number of Nodes starting all together - lease: 60000, - /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive - /// messages will be sent at the configured time interval. - /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, - /// set the actual keep_alive interval to one fourth of the lease time: i.e. send - /// 4 keep_alive messages in a lease period. Changing the lease time will have the - /// keep_alive messages sent more or less often. - /// This is in-line with the ITU-T G.8013/Y.1731 specification on continuous connectivity - /// check which considers a link as failed when no messages are received in 3.5 times the - /// target interval. - /// ROS setting: decrease the value since Nodes are communicating over the loopback - /// where keep-alive messages have less chances to be lost. - keep_alive: 2, - /// Batch size in bytes is expressed as a 16bit unsigned integer. - /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). - /// The default batch size value is the maximum batch size: 65535. - batch_size: 65535, - /// Each zenoh link has a transmission queue that can be configured - queue: { - /// The size of each priority queue indicates the number of batches a given queue can contain. - /// NOTE: the number of batches in each priority must be included between 1 and 16. Different values will result in an error. - /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. - /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, - /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. - /// If qos is false, then only the DATA priority will be allocated. - size: { - control: 2, - real_time: 2, - interactive_high: 2, - interactive_low: 2, - data_high: 2, - data: 2, - data_low: 2, - background: 2, - }, - /// Congestion occurs when the queue is empty (no available batch). - congestion_control: { - /// Behavior pushing CongestionControl::Drop messages to the queue. - drop: { - /// The maximum time in microseconds to wait for an available batch before dropping a droppable message if still no batch is available. - wait_before_drop: 1000, - /// The maximum deadline limit for multi-fragment messages. - max_wait_before_drop_fragments: 50000, - }, - /// Behavior pushing CongestionControl::Block messages to the queue. - block: { - /// The maximum time in microseconds to wait for an available batch before closing the transport session when sending a blocking message - /// if still no batch is available. - /// ROS setting: increase the value to avoid unecessary link closure at launch time where congestion is likely - /// to occur even over the loopback since all the Nodes are starting at the same time. - wait_before_close: 60000000, - }, - }, - /// Perform batching of messages if they are smaller of the batch_size - batching: { - /// Perform adaptive batching of messages if they are smaller of the batch_size. - /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be - /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput - /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. - enabled: true, - /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. - time_limit: 1, - }, - allocation: { - /// Mode for memory allocation of batches in the priority queues. - /// - "init": batches are allocated at queue initialization time. - /// - "lazy": batches are allocated when needed up to the maximum number of batches configured in the size configuration parameter. - mode: "lazy", - }, - }, - }, - /// Configure the zenoh RX parameters of a link - rx: { - /// Receiving buffer size in bytes for each link - /// The default the rx_buffer_size value is the same as the default batch size: 65535. - /// For very high throughput scenarios, the rx_buffer_size can be increased to accommodate - /// more in-flight data. This is particularly relevant when dealing with large messages. - /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. - buffer_size: 65535, - /// Maximum size of the defragmentation buffer at receiver end. - /// Fragmented messages that are larger than the configured size will be dropped. - /// The default value is 1GiB. This would work in most scenarios. - /// NOTE: reduce the value if you are operating on a memory constrained device. - max_message_size: 1073741824, - }, - /// Configure TLS specific parameters - tls: { - /// Path to the certificate of the certificate authority used to validate either the server - /// or the client's keys and certificates, depending on the node's mode. If not specified - /// on router mode then the default WebPKI certificates are used instead. - root_ca_certificate: null, - /// Path to the TLS listening side private key - listen_private_key: null, - /// Path to the TLS listening side public certificate - listen_certificate: null, - /// Enables mTLS (mutual authentication), client authentication - enable_mtls: false, - /// Path to the TLS connecting side private key - connect_private_key: null, - /// Path to the TLS connecting side certificate - connect_certificate: null, - /// Whether or not to verify the matching between hostname/dns and certificate when connecting, - /// if set to false zenoh will disregard the common names of the certificates when verifying servers. - /// This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your - /// ca to verify that the server at baz.com is actually baz.com, let this be true (default). - verify_name_on_connect: true, - /// Whether or not to close links when remote certificates expires. - /// If set to true, links that require certificates (tls/quic) will automatically disconnect when the time of expiration of the remote certificate chain is reached - /// note that mTLS (client authentication) is required for a listener to disconnect a client on expiration - close_link_on_expiration: false, - /// Optional configuration for TCP system buffers sizes for TLS links - /// - /// Configure TCP read buffer size (bytes) - // so_rcvbuf: 123456, - /// Configure TCP write buffer size (bytes) - // so_sndbuf: 123456, - }, - // // Configure optional TCP link specific parameters - // tcp: { - // /// Optional configuration for TCP system buffers sizes for TCP links - // /// - // /// Configure TCP read buffer size (bytes) - // // so_rcvbuf: 123456, - // /// Configure TCP write buffer size (bytes) - // // so_sndbuf: 123456, - // }, - }, - /// Shared memory configuration. - /// NOTE: shared memory can be used only if zenoh is compiled with "shared-memory" feature, otherwise - /// settings in this section have no effect. - shared_memory: { - /// Whether shared memory is enabled or not. - /// If set to `true`, the SHM buffer optimization support will be announced to other parties. (default `true`). - /// This option doesn't make SHM buffer optimization mandatory, the real support depends on other party setting. - /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate - /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the - /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. - /// - /// ROS setting: disabled by default until fully tested - enabled: false, - /// SHM resources initialization mode (default "lazy"). - /// - "lazy": SHM subsystem internals will be initialized lazily upon the first SHM buffer - /// allocation or reception. This setting provides better startup time and optimizes resource usage, - /// but produces extra latency at the first SHM buffer interaction. - /// - "init": SHM subsystem internals will be initialized upon Session opening. This setting sacrifices - /// startup time, but guarantees no latency impact when first SHM buffer is processed. - mode: "lazy", - transport_optimization: { - /// Enables transport optimization for large messages (default `true`). - /// Implicitly puts large messages into shared memory for transports with SHM-compatible connection. - enabled: true, - /// SHM memory size in bytes used for transport optimization (default `16 * 1024 * 1024`). - pool_size: 16777216, - /// Allow optimization for messages equal or larger than this threshold in bytes (default `3072`). - message_size_threshold: 3072, - }, - }, - auth: { - /// The configuration of authentication. - /// A password implies a username is required. - usrpwd: { - user: null, - password: null, - /// The path to a file containing the user password dictionary - dictionary_file: null, - }, - pubkey: { - public_key_pem: null, - private_key_pem: null, - public_key_file: null, - private_key_file: null, - key_size: null, - known_keys_file: null, - }, - }, - }, - - /// Configure the Admin Space - /// Unstable: this configuration part works as advertised, but may change in a future release - adminspace: { - /// Enables the admin space - enabled: true, - /// read and/or write permissions on the admin space - permissions: { - read: true, - write: false, - }, - }, - -} diff --git a/nexus_demos/config/zenoh/inter_workcell_router_config.json5 b/nexus_demos/config/zenoh/inter_workcell_router_config.json5 new file mode 100644 index 00000000..7522eb5d --- /dev/null +++ b/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, + }, +} diff --git a/nexus_demos/config/zenoh/inter_workcell_session_config.json5 b/nexus_demos/config/zenoh/inter_workcell_session_config.json5 new file mode 100644 index 00000000..e07e47a8 --- /dev/null +++ b/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, + }, +} diff --git a/nexus_demos/config/zenoh/main.json5 b/nexus_demos/config/zenoh/main.json5 deleted file mode 100644 index d6125583..00000000 --- a/nexus_demos/config/zenoh/main.json5 +++ /dev/null @@ -1,648 +0,0 @@ -{ - "access_control": { - "default_permission": "deny", - "enabled": true, - "policies": [ - { - "id": null, - "rules": [ - "liveliness_tokens" - ], - "subjects": [ - "router" - ] - }, - { - "id": null, - "rules": [ - "incoming_queries", - "outgoing_queryables_replies", - "outgoing_queries", - "incoming_queryables_replies", - "outgoing_publications", - "incoming_subscriptions", - "outgoing_publications_transient_local_queryable", - "incoming_subscriptions_transient_local_query", - "outgoing_subscriptions", - "incoming_publications", - "outgoing_subscriptions_transient_local_query", - "incoming_subscriptions_transient_local_queryable", - "liveliness_tokens" - ], - "subjects": [ - "main" - ] - } - ], - "rules": [ - { - "flows": [ - "ingress" - ], - "id": "incoming_queries", - "key_exprs": [ - "0/get_building_map/*/*", - "0/get_work_order_state/*/*", - "0/list_transporters/*/*", - "0/list_workcells/*/*", - "0/main/available/*/*", - "0/main/change_state/*/*", - "0/main/describe_parameters/*/*", - "0/main/execute_order/_action/cancel_goal/*/*", - "0/main/execute_order/_action/get_result/*/*", - "0/main/execute_order/_action/send_goal/*/*", - "0/main/get_available_states/*/*", - "0/main/get_available_transitions/*/*", - "0/main/get_parameter_types/*/*", - "0/main/get_parameters/*/*", - "0/main/get_state/*/*", - "0/main/get_transition_graph/*/*", - "0/main/get_type_description/*/*", - "0/main/is_active/*/*", - "0/main/list_parameters/*/*", - "0/main/manage_nodes/*/*", - "0/main/set_parameters/*/*", - "0/main/set_parameters_atomically/*/*", - "0/main/signal/*/*", - "0/main/transport/_action/cancel_goal/*/*", - "0/main/transport/_action/get_result/*/*", - "0/main/transport/_action/send_goal/*/*", - "0/pause/*/*", - "0/register_transporter/*/*", - "0/register_workcell/*/*", - "0/test/set_estop/*/*" - ], - "messages": [ - "query" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_queryables_replies", - "key_exprs": [ - "0/get_building_map/*/*", - "0/get_work_order_state/*/*", - "0/list_transporters/*/*", - "0/list_workcells/*/*", - "0/main/available/*/*", - "0/main/change_state/*/*", - "0/main/describe_parameters/*/*", - "0/main/execute_order/_action/cancel_goal/*/*", - "0/main/execute_order/_action/get_result/*/*", - "0/main/execute_order/_action/send_goal/*/*", - "0/main/get_available_states/*/*", - "0/main/get_available_transitions/*/*", - "0/main/get_parameter_types/*/*", - "0/main/get_parameters/*/*", - "0/main/get_state/*/*", - "0/main/get_transition_graph/*/*", - "0/main/get_type_description/*/*", - "0/main/is_active/*/*", - "0/main/list_parameters/*/*", - "0/main/manage_nodes/*/*", - "0/main/set_parameters/*/*", - "0/main/set_parameters_atomically/*/*", - "0/main/signal/*/*", - "0/main/transport/_action/cancel_goal/*/*", - "0/main/transport/_action/get_result/*/*", - "0/main/transport/_action/send_goal/*/*", - "0/pause/*/*", - "0/register_transporter/*/*", - "0/register_workcell/*/*", - "0/test/set_estop/*/*" - ], - "messages": [ - "declare_queryable", - "reply" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_queries", - "key_exprs": [ - "0/mock_transporter_node/available/*/*", - "0/mock_transporter_node/signal/*/*", - "0/pause/*/*", - "0/register_transporter/*/*", - "0/rviz_lifecycle_manager/is_active/*/*", - "0/rviz_lifecycle_manager/manage_nodes/*/*", - "0/system_orchestrator/change_state/*/*", - "0/system_orchestrator/execute_order/_action/cancel_goal/*/*", - "0/system_orchestrator/get_state/*/*" - ], - "messages": [ - "query" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress" - ], - "id": "incoming_queryables_replies", - "key_exprs": [ - "0/mock_transporter_node/available/*/*", - "0/mock_transporter_node/signal/*/*", - "0/pause/*/*", - "0/register_transporter/*/*", - "0/rviz_lifecycle_manager/is_active/*/*", - "0/rviz_lifecycle_manager/manage_nodes/*/*", - "0/system_orchestrator/change_state/*/*", - "0/system_orchestrator/execute_order/_action/cancel_goal/*/*", - "0/system_orchestrator/get_state/*/*" - ], - "messages": [ - "declare_queryable", - "reply" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_publications", - "key_exprs": [ - "0/clicked_point/*/*", - "0/estop/*/*", - "0/goal_pose/*/*", - "0/initialpose/*/*", - "0/main/execute_order/_action/feedback/*/*", - "0/main/execute_order/_action/status/*/*", - "0/main/transition_event/*/*", - "0/main/transport/_action/feedback/*/*", - "0/main/transport/_action/status/*/*", - "0/map/*/*", - "0/parameter_events/*/*", - "0/rosout/*/*", - "0/site_map/*/*", - "0/tf/*/*", - "0/work_order_states/*/*" - ], - "messages": [ - "put", - "reply" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress" - ], - "id": "incoming_subscriptions", - "key_exprs": [ - "0/clicked_point/*/*", - "0/estop/*/*", - "0/goal_pose/*/*", - "0/initialpose/*/*", - "0/main/execute_order/_action/feedback/*/*", - "0/main/execute_order/_action/status/*/*", - "0/main/transition_event/*/*", - "0/main/transport/_action/feedback/*/*", - "0/main/transport/_action/status/*/*", - "0/map/*/*", - "0/parameter_events/*/*", - "0/rosout/*/*", - "0/site_map/*/*", - "0/tf/*/*", - "0/work_order_states/*/*" - ], - "messages": [ - "declare_subscriber" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_publications_transient_local_queryable", - "key_exprs": [ - "0/clicked_point/*/*/@adv/pub/*/*/_", - "0/estop/*/*/@adv/pub/*/*/_", - "0/goal_pose/*/*/@adv/pub/*/*/_", - "0/initialpose/*/*/@adv/pub/*/*/_", - "0/main/execute_order/_action/feedback/*/*/@adv/pub/*/*/_", - "0/main/execute_order/_action/status/*/*/@adv/pub/*/*/_", - "0/main/transition_event/*/*/@adv/pub/*/*/_", - "0/main/transport/_action/feedback/*/*/@adv/pub/*/*/_", - "0/main/transport/_action/status/*/*/@adv/pub/*/*/_", - "0/map/*/*/@adv/pub/*/*/_", - "0/parameter_events/*/*/@adv/pub/*/*/_", - "0/rosout/*/*/@adv/pub/*/*/_", - "0/site_map/*/*/@adv/pub/*/*/_", - "0/tf/*/*/@adv/pub/*/*/_", - "0/work_order_states/*/*/@adv/pub/*/*/_" - ], - "messages": [ - "declare_queryable", - "liveliness_token" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress" - ], - "id": "incoming_subscriptions_transient_local_query", - "key_exprs": [ - "0/clicked_point/*/*/@adv/**", - "0/estop/*/*/@adv/**", - "0/goal_pose/*/*/@adv/**", - "0/initialpose/*/*/@adv/**", - "0/main/execute_order/_action/feedback/*/*/@adv/**", - "0/main/execute_order/_action/status/*/*/@adv/**", - "0/main/transition_event/*/*/@adv/**", - "0/main/transport/_action/feedback/*/*/@adv/**", - "0/main/transport/_action/status/*/*/@adv/**", - "0/map/*/*/@adv/**", - "0/parameter_events/*/*/@adv/**", - "0/rosout/*/*/@adv/**", - "0/site_map/*/*/@adv/**", - "0/tf/*/*/@adv/**", - "0/work_order_states/*/*/@adv/**" - ], - "messages": [ - "query" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_subscriptions", - "key_exprs": [ - "0/clock/*/*", - "0/estop/*/*", - "0/parameter_events/*/*", - "0/tf/*/*", - "0/tf_static/*/*" - ], - "messages": [ - "declare_subscriber" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress" - ], - "id": "incoming_publications", - "key_exprs": [ - "0/clock/*/*", - "0/estop/*/*", - "0/parameter_events/*/*", - "0/tf/*/*", - "0/tf_static/*/*" - ], - "messages": [ - "put", - "reply" - ], - "permission": "allow" - }, - { - "flows": [ - "egress" - ], - "id": "outgoing_subscriptions_transient_local_query", - "key_exprs": [ - "0/clock/*/*/@adv/**", - "0/estop/*/*/@adv/**", - "0/parameter_events/*/*/@adv/**", - "0/tf/*/*/@adv/**", - "0/tf_static/*/*/@adv/**" - ], - "messages": [ - "query", - "declare_liveliness_subscriber" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress" - ], - "id": "incoming_subscriptions_transient_local_queryable", - "key_exprs": [ - "0/clock/*/*/@adv/pub/*/*/_", - "0/estop/*/*/@adv/pub/*/*/_", - "0/parameter_events/*/*/@adv/pub/*/*/_", - "0/tf/*/*/@adv/pub/*/*/_", - "0/tf_static/*/*/@adv/pub/*/*/_" - ], - "messages": [ - "declare_queryable", - "liveliness_token" - ], - "permission": "allow" - }, - { - "flows": [ - "ingress", - "egress" - ], - "id": "liveliness_tokens", - "key_exprs": [ - "@ros2_lv/0/**" - ], - "messages": [ - "liveliness_token", - "liveliness_query", - "declare_liveliness_subscriber", - "reply" - ], - "permission": "allow" - } - ], - "subjects": [ - { - "cert_common_names": null, - "id": "router", - "interfaces": null, - "link_protocols": null, - "usernames": null, - "zids": null - }, - { - "cert_common_names": null, - "id": "main", - "interfaces": null, - "link_protocols": null, - "usernames": null, - "zids": null - } - ] - }, - "adminspace": { - "enabled": true, - "permissions": { - "read": true, - "write": false - } - }, - "aggregation": { - "publishers": [], - "subscribers": [] - }, - "connect": { - "endpoints": [ - "tcp/localhost:7447" - ], - "exit_on_failure": { - "client": true, - "peer": false, - "router": false - }, - "retry": { - "period_increase_factor": 2, - "period_init_ms": 1000, - "period_max_ms": 4000 - }, - "timeout_ms": { - "client": 0, - "peer": -1, - "router": -1 - } - }, - "downsampling": [], - "id": null, - "listen": { - "endpoints": [ - "tcp/localhost:0" - ], - "exit_on_failure": true, - "retry": { - "period_increase_factor": 2, - "period_init_ms": 1000, - "period_max_ms": 4000 - }, - "timeout_ms": 0 - }, - "low_pass_filter": [], - "metadata": null, - "mode": "peer", - "namespace": null, - "open": { - "return_conditions": { - "connect_scouted": true, - "declares": true - } - }, - "plugins": {}, - "plugins_loading": { - "enabled": false, - "search_dirs": [ - { - "kind": "current_exe_parent", - "value": null - }, - ".", - "~/.zenoh/lib", - "/opt/homebrew/lib", - "/usr/local/lib", - "/usr/lib" - ] - }, - "qos": { - "network": [], - "publication": [] - }, - "queries_default_timeout": 600000, - "routing": { - "interests": { - "timeout": 10000 - }, - "peer": { - "linkstate": { - "transport_weights": [] - }, - "mode": "peer_to_peer" - }, - "router": { - "linkstate": { - "transport_weights": [] - }, - "peers_failover_brokering": true - } - }, - "scouting": { - "delay": 500, - "gossip": { - "autoconnect": { - "peer": [ - "router", - "peer" - ], - "router": [] - }, - "autoconnect_strategy": { - "peer": { - "to_peer": "greater-zid", - "to_router": "always" - } - }, - "enabled": true, - "multihop": false, - "target": { - "peer": [ - "router" - ], - "router": [ - "router", - "peer" - ] - } - }, - "multicast": { - "address": "224.0.0.224:7446", - "autoconnect": { - "client": [ - "router" - ], - "peer": [ - "router", - "peer" - ], - "router": [] - }, - "autoconnect_strategy": { - "peer": { - "to_peer": "greater-zid", - "to_router": "always" - } - }, - "enabled": false, - "interface": "auto", - "listen": true, - "ttl": 1 - }, - "timeout": 3000 - }, - "timestamping": { - "drop_future_timestamp": false, - "enabled": { - "client": true, - "peer": true, - "router": true - } - }, - "transport": { - "auth": { - "pubkey": { - "key_size": null, - "known_keys_file": null, - "private_key_file": null, - "private_key_pem": null, - "public_key_file": null, - "public_key_pem": null - }, - "usrpwd": { - "dictionary_file": null, - "password": null, - "user": null - } - }, - "link": { - "protocols": null, - "rx": { - "buffer_size": 65535, - "max_message_size": 1073741824 - }, - "tcp": { - "so_rcvbuf": null, - "so_sndbuf": null - }, - "tls": { - "close_link_on_expiration": false, - "connect_certificate": null, - "connect_private_key": null, - "enable_mtls": false, - "listen_certificate": null, - "listen_private_key": null, - "root_ca_certificate": null, - "so_rcvbuf": null, - "so_sndbuf": null, - "verify_name_on_connect": true - }, - "tx": { - "batch_size": 65535, - "keep_alive": 2, - "lease": 60000, - "queue": { - "allocation": { - "mode": "lazy" - }, - "batching": { - "enabled": true, - "time_limit": 1 - }, - "congestion_control": { - "block": { - "wait_before_close": 60000000 - }, - "drop": { - "max_wait_before_drop_fragments": 50000, - "wait_before_drop": 1000 - } - }, - "size": { - "background": 2, - "control": 2, - "data": 2, - "data_high": 2, - "data_low": 2, - "interactive_high": 2, - "interactive_low": 2, - "real_time": 2 - } - }, - "sequence_number_resolution": "32bit", - "threads": 16 - }, - "unixpipe": { - "file_access_mask": null - } - }, - "multicast": { - "compression": { - "enabled": false - }, - "join_interval": 2500, - "max_sessions": 1000, - "qos": { - "enabled": false - } - }, - "shared_memory": { - "enabled": false, - "mode": "lazy", - "transport_optimization": { - "enabled": true, - "message_size_threshold": 3072, - "pool_size": 16777216 - } - }, - "unicast": { - "accept_pending": 10000, - "accept_timeout": 60000, - "compression": { - "enabled": false - }, - "lowlatency": false, - "max_links": 1, - "max_sessions": 10000, - "open_timeout": 60000, - "qos": { - "enabled": true - } - } - } -} \ No newline at end of file diff --git a/nexus_demos/config/zenoh/policy_main.xml b/nexus_demos/config/zenoh/policy_main.xml deleted file mode 100644 index 8b8e48f4..00000000 --- a/nexus_demos/config/zenoh/policy_main.xml +++ /dev/null @@ -1,93 +0,0 @@ - - - - - - - - get_work_order_state - list_transporters - list_workcells - pause - register_transporter - register_workcell - ~/change_state - ~/describe_parameters - ~/execute_order/_action/cancel_goal - ~/execute_order/_action/get_result - ~/execute_order/_action/send_goal - ~/get_available_states - ~/get_available_transitions - ~/get_parameter_types - ~/get_parameters - ~/get_state - ~/get_transition_graph - ~/get_type_description - ~/list_parameters - ~/set_parameters - ~/set_parameters_atomically - - get_building_map - - /test/set_estop - - ~/available - ~/signal - ~/transport/_action/cancel_goal - ~/transport/_action/get_result - ~/transport/_action/send_goal - - ~/is_active - ~/manage_nodes - - - - /mock_transporter_node/available - /mock_transporter_node/signal - - register_transporter - - /rviz_lifecycle_manager/is_active - /rviz_lifecycle_manager/manage_nodes - /system_orchestrator/execute_order/_action/cancel_goal - pause - - /system_orchestrator/change_state - /system_orchestrator/get_state - - - - estop - clock - parameter_events - - tf - tf_static - - - - parameter_events - rosout - work_order_states - ~/execute_order/_action/feedback - ~/execute_order/_action/status - ~/transition_event - - map - site_map - - estop - - tf - ~/transport/_action/feedback - ~/transport/_action/status - - clicked_point - goal_pose - initialpose - - - - - - diff --git a/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 b/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 deleted file mode 100644 index 779ea0ca..00000000 --- a/nexus_demos/config/zenoh/system_orchestrator_router_config.json5 +++ /dev/null @@ -1,11 +0,0 @@ -{ - 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 deleted file mode 100644 index e66c01e0..00000000 --- a/nexus_demos/config/zenoh/system_orchestrator_session_config.json5 +++ /dev/null @@ -1,25 +0,0 @@ -{ - 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_bridge_config.json5 b/nexus_demos/config/zenoh/workcell_1_bridge_config.json5 new file mode 100644 index 00000000..d8ab4c42 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_1_bridge_config.json5 @@ -0,0 +1,110 @@ +{ + mode: "router", + connect: { + endpoints: [ + "tcp/localhost:7447", + "tcp/localhost:7448", + ], + }, + listen: { + endpoints: [], + }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + 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"], + }, + ] + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/config/zenoh/workcell_1_config.json5 b/nexus_demos/config/zenoh/workcell_1_config.json5 deleted file mode 100644 index a7ebed43..00000000 --- a/nexus_demos/config/zenoh/workcell_1_config.json5 +++ /dev/null @@ -1,754 +0,0 @@ -/// This file attempts to list and document available configuration elements. -/// For a more complete view of the configuration's structure, check out `zenoh/src/config.rs`'s `Config` structure. -/// Note that the values here are correctly typed, but may not be sensible, so copying this file to change only the parts that matter to you is not good practice. -{ - /// The identifier (as unsigned 128bit integer in hexadecimal lowercase - leading zeros are not accepted) - /// that zenoh runtime will use. - /// If not set, a random unsigned 128bit integer will be used. - /// WARNING: this id must be unique in your zenoh network. - // id: "1234567890abcdef", - - /// The node's mode (router, peer or client) - mode: "peer", - - /// Which endpoints to connect to. E.g. tcp/localhost:7447. - /// By configuring the endpoints, it is possible to tell zenoh which router/peer to connect to at startup. - /// - /// For TCP/UDP on Linux, it is possible additionally specify the interface to be connected to: - /// E.g. tcp/192.168.0.1:7447#iface=eth0, for connect only if the IP address is reachable via the interface eth0 - /// - /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. - /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. - /// - /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: - /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 - /// For TCP, UDP, Quic and TLS links, it is possible to specify a `bind` address for the local socket: - /// E.g. tcp/192.168.0.1:7447#bind=192.168.0.1:0 - /// Note!: Currently it is unsupported to specify both `bind` and `iface`. - /// - /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: - /// E.g. tcp/192.168.0.1:7447#dscp=0x08 - connect: { - /// timeout waiting for all endpoints connected (0: no retry, -1: infinite timeout) - /// Accepts a single value (e.g. timeout_ms: 0) - /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). - timeout_ms: { router: -1, peer: -1, client: 0 }, - - /// The list of endpoints to connect to. - /// Accepts a single list (e.g. endpoints: ["tcp/10.10.10.10:7447", "tcp/11.11.11.11:7447"]) - /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/10.10.10.10:7447"], peer: ["tcp/11.11.11.11:7447"] }). - /// - /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html - /// - /// ROS setting: By default connect to the Zenoh router on localhost on port 7447. - endpoints: [ - "tcp/localhost:7447" - ], - - /// Global connect configuration, - /// Accepts a single value or different values for router, peer and client. - /// The configuration can also be specified for the separate endpoint - /// it will override the global one - /// E.g. tcp/192.168.0.1:7447#retry_period_init_ms=20000;retry_period_max_ms=10000" - - /// exit from application, if timeout exceed - exit_on_failure: { router: false, peer: false, client: true }, - /// connect establishing retry configuration - retry: { - /// initial wait timeout until next connect try - period_init_ms: 1000, - /// maximum wait timeout until next connect try - period_max_ms: 4000, - /// increase factor for the next timeout until nexti connect try - period_increase_factor: 2, - }, - }, - - /// Which endpoints to listen on. E.g. tcp/0.0.0.0:7447. - /// By configuring the endpoints, it is possible to tell zenoh which are the endpoints that other routers, - /// peers, or client can use to establish a zenoh session. - /// - /// For TCP/UDP on Linux, it is possible additionally specify the interface to be listened to: - /// E.g. tcp/0.0.0.0:7447#iface=eth0, for listen connection only on eth0 - /// - /// It is also possible to specify a priority range and/or a reliability setting to be used on the link. - /// For example `tcp/localhost?prio=6-7;rel=0` assigns priorities "data_low" and "background" to the established link. - /// - /// For TCP and TLS links, it is possible to specify the TCP buffer sizes: - /// E.g. tcp/192.168.0.1:7447#so_sndbuf=65000;so_rcvbuf=65000 - /// - /// For TCP/UDP links, it's possible to specify the DSCP field of the IP header: - /// E.g. tcp/192.168.0.1:7447#dscp=0x08 - listen: { - /// timeout waiting for all listen endpoints (0: no retry, -1: infinite timeout) - /// Accepts a single value (e.g. timeout_ms: 0) - /// or different values for router, peer and client (e.g. timeout_ms: { router: -1, peer: -1, client: 0 }). - timeout_ms: 0, - - /// The list of endpoints to listen on. - /// Accepts a single list (e.g. endpoints: ["tcp/[::]:7447", "udp/[::]:7447"]) - /// or different lists for router, peer and client (e.g. endpoints: { router: ["tcp/[::]:7447"], peer: ["tcp/[::]:0"] }). - /// - /// See https://docs.rs/zenoh/latest/zenoh/config/struct.EndPoint.html - /// - /// ROS setting: By default accept incoming connections only from localhost (i.e. from colocalized Nodes). - /// All communications with other hosts are routed by the Zenoh router. - endpoints: [ - "tcp/localhost:0" - ], - - /// Global listen configuration, - /// Accepts a single value or different values for router, peer and client. - /// The configuration can also be specified for the separate endpoint - /// it will override the global one - /// E.g. tcp/192.168.0.1:7447#exit_on_failure=false;retry_period_max_ms=1000" - - /// exit from application, if timeout exceed - exit_on_failure: true, - /// listen retry configuration - retry: { - /// initial wait timeout until next try - period_init_ms: 1000, - /// maximum wait timeout until next try - period_max_ms: 4000, - /// increase factor for the next timeout until next try - period_increase_factor: 2, - }, - }, - - /// Configure the session open behavior. - open: { - /// Configure the conditions to be met before session open returns. - return_conditions: { - /// Session open waits to connect to scouted peers and routers before returning. - /// When set to false, first publications and queries after session open from peers may be lost. - connect_scouted: true, - /// Session open waits to receive initial declares from connected peers before returning. - /// Setting to false may cause extra traffic at startup from peers. - declares: true, - }, - }, - - /// Configure the scouting mechanisms and their behaviours - scouting: { - /// In client mode, the period in milliseconds dedicated to scouting for a router before failing. - timeout: 3000, - /// In peer mode, the maximum period in milliseconds dedicated to scouting remote peers before attempting other operations. - delay: 500, - /// The multicast scouting configuration. - multicast: { - /// Whether multicast scouting is enabled or not - /// - /// ROS setting: disable multicast discovery by default - enabled: false, - /// The socket which should be used for multicast scouting - address: "224.0.0.224:7446", - /// The network interface which should be used for multicast scouting - interface: "auto", // If not set or set to "auto" the interface if picked automatically - /// The time-to-live on multicast scouting packets - ttl: 1, - /// Which type of Zenoh instances to automatically establish sessions with upon discovery on UDP multicast. - /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router, peer or client mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). - /// Each value is a list of: "peer", "router" and/or "client". - autoconnect: { router: [], peer: ["router", "peer"], client: ["router"] }, - /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. - /// Possible options are: - /// - "always": always attempt to autoconnect, may result in redundant connections. - /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. - /// If both nodes use this strategy, only one will attempt the connection. - /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example - /// because of a private IP. - /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, - /// or different values for router and/or peer depending on the type of node detected - /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), - /// or different values for router or peer mode - /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). - /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip - /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. - autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, - /// Whether or not to listen for scout messages on UDP multicast and reply to them. - listen: true, - }, - /// The gossip scouting configuration. Note that instances in "client" mode do not participate in gossip. - gossip: { - /// Whether gossip scouting is enabled or not - enabled: true, - /// When true, gossip scouting information are propagated multiple hops to all nodes in the local network. - /// When false, gossip scouting information are only propagated to the next hop. - /// Activating multihop gossip implies more scouting traffic and a lower scalability. - /// It mostly makes sense when using "linkstate" routing mode where all nodes in the subsystem don't have - /// direct connectivity with each other. - multihop: false, - /// Which type of Zenoh instances to send gossip messages to. - /// Accepts a single value (e.g. target: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router or peer mode (e.g. target: { router: ["router", "peer"], peer: ["router"] }). - /// Each value is a list of "peer" and/or "router". - /// ROS setting: by default all peers rely on the router to discover each other. Thus configuring the peer to send gossip - /// messages only to the router is sufficient and avoids unecessary traffic between Nodes at launch time. - target: { router: ["router", "peer"], peer: ["router"]}, - /// Which type of Zenoh instances to automatically establish sessions with upon discovery on gossip. - /// Accepts a single value (e.g. autoconnect: ["router", "peer"]) which applies whatever the configured "mode" is, - /// or different values for router or peer mode (e.g. autoconnect: { router: [], peer: ["router", "peer"] }). - /// Each value is a list of: "peer" and/or "router". - autoconnect: { router: [], peer: ["router", "peer"] }, - /// Strategy for autoconnection, mainly to avoid nodes connecting to each other redundantly. - /// Possible options are: - /// - "always": always attempt to autoconnect, may result in redundant connection which will then be closed. - /// - "greater-zid": attempt to connect to another node only if its own zid is greater than the other's. - /// If both nodes use this strategy, only one will attempt the connection. - /// This strategy may not be suited if one of the nodes is not reachable by the other one, for example - /// because of a private IP. - /// Accepts a single value (e.g. autoconnect: "always") which applies whatever node would be auto-connected to, - /// or different values for router and/or peer depending on the type of node detected - /// (e.g. autoconnect_strategy : { to_router: "always", to_peer: "greater-zid" }), - /// or different values for router or peer mode - /// (e.g. autoconnect_strategy : { peer: { to_router: "always", to_peer: "greater-zid" } }). - /// ROS setting: as by default all peers will interconnect to each other over the loopback interface, - /// they are all reachable to each other. Hence using "greater-zid" for peers connecting to - /// other peers is sufficient and avoids unecessary double connections between peers at startup. - autoconnect_strategy: { peer: { to_router: "always", to_peer: "greater-zid" } }, - }, - }, - - /// 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, - }, - - /// The default timeout to apply to queries in milliseconds. - /// ROS setting: The default value of 600000 ms (10 minutes) is already quite high, but it can be increased if needed. - /// For instance to avoid timeout with Service Server that that is unresponsive or lagging, - /// which could occur at launch time with a large number of Nodes starting all together. - /// It’s recommended to configure a value that exceeds the longest timeout specified in any - /// `spin_until_complete(service_call_future_result, timeout)` call. - /// Note that the action-related service "get_result" is hard-coded with an infinite timeout, - /// as in some use case an action could spend several hours (e.g. mission plans). - queries_default_timeout: 600000, - - /// The routing strategy to use and it's configuration. - routing: { - /// The routing strategy to use in routers and it's configuration. - router: { - /// When set to true a router will forward data between two peers - /// directly connected to it if it detects that those peers are not - /// connected to each other. - /// The failover brokering only works if gossip discovery is enabled - /// and peers are configured with gossip target "router". - peers_failover_brokering: true, - /// Linkstate mode configuration. - linkstate: { - /// Weights of the outgoing transports in linkstate mode. - /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. - /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. - /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. - // transport_weights: [ - // { dst_zid: "1", weight: "10" }, - // { dst_zid: "2", weight: "200" }, - // ] - }, - }, - /// The routing strategy to use in peers and it's configuration. - peer: { - /// The routing strategy to use in peers. ("peer_to_peer" or "linkstate"). - /// This option needs to be set to the same value in all peers and routers of the subsystem. - mode: "peer_to_peer", - /// Linkstate mode configuration (only taken into account if mode == "linkstate"). - linkstate: { - /// Weights of the outgoing transports in linkstate mode. - /// If none of the two endpoint nodes of a transport specifies its weight, a weight of 100 is applied. - /// If only one of the two endpoint nodes of a transport specifies its weight, the specified weight is applied. - /// If both endpoint nodes of a transport specify its weight, the greater weight is applied. - // transport_weights: [ - // { dst_zid: "1", weight: "10" }, - // { dst_zid: "2", weight: "200" }, - // ] - }, - }, - /// The interests-based routing configuration. - /// This configuration applies regardless of the mode (router, peer or client). - interests: { - /// The timeout to wait for incoming interests declarations in milliseconds. - /// The expiration of this timeout implies that the discovery protocol might be incomplete, - /// leading to potential loss of messages, queries or liveliness tokens. - timeout: 10000, - }, - }, - - // /// Overwrite QoS options for Zenoh messages by key expression (ignores Zenoh API QoS config for overwritten values) - // qos: { - // /// Overwrite QoS options for PUT and DELETE messages - // publication: [ - // { - // /// PUT and DELETE messages on key expressions that are included by these key expressions - // /// will have their QoS options overwritten by the given config. - // key_exprs: ["demo/**", "example/key"], - // /// Configurations that will be applied on the publisher. - // /// Options that are supplied here will overwrite the configuration given in Zenoh API - // config: { - // congestion_control: "block", - // priority: "data_high", - // express: true, - // reliability: "best_effort", - // allowed_destination: "remote", - // }, - // }, - // ], - // /// Overwrite QoS options for messages sent and received from/to the network - // /// This allows more fine grained rules (per network card, etc...) but is - // /// less performant than the publication option above. - // network: [ - // { - // /// Optional Id, has to be unique. - // id: "lo0_en0_qos_overwrite", - // /// Optional list of ZIDs on which qos will be overwritten when communicating with. - // // zids: ["38a4829bce9166ee"], - // /// Optional list of interfaces, if not specified, will be applied to all interfaces. - // interfaces: [ - // "lo0", - // "en0", - // ], - // /// Optional list of link protocols. Transports with at least one of these links will have their qos overwritten. - // /// If absent, the overwrite will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// List of message types to apply to. - // messages: [ - // "put", // put publications - // "delete", // delete publications - // "query", // get queries - // "reply", // replies to queries - // ], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the rules will be applied to both flows. - // flows: ["egress", "ingress"], - // /// QoS filter to apply to the messages matching this item. - // qos: { - // congestion_control: "drop", - // priority: "data", - // express: true, - // reliability: "reliable", - // }, - // /// payload_size range for the messages matching this item. - // payload_size: "1000000..", - // key_exprs: ["test/demo"], - // overwrite: { - // /// Optional new priority value, if not specified priority of the messages will stay unchanged. - // priority: "real_time", - // /// Optional new congestion control value, if not specified congestion control of the messages will stay unchanged. - // congestion_control: "block", - // /// Optional new express value, if not specified express flag of the messages will stay unchanged. - // express: true, - // }, - // }, - // ], - // }, - - // /// The declarations aggregation strategy. - // aggregation: { - // /// A list of key-expressions for which all included subscribers will be aggregated into. - // subscribers: [ - // // key_expression - // ], - // /// A list of key-expressions for which all included publishers will be aggregated into. - // publishers: [ - // // key_expression - // ], - // }, - - // /// Namespace prefix. - // /// If specified, all outgoing key expressions will be automatically prefixed with specified string, - // /// and all incoming key expressions will be stripped of specified prefix. - // /// The namespace prefix should satisfy all key expression constraints - // /// and additionally it can not contain wild characters ('*'). - // /// Namespace is applied to the session. - // /// E. g. if session has a namespace of "1" then session.put("my/keyexpr", my_message), - // /// will put a message into 1/my/keyexpr. Same applies to all other operations within this session. - // namespace: "my/namespace", - - // /// The downsampling declaration. - // downsampling: [ - // { - // /// Optional Id, has to be unique - // id: "wlan0egress", - // /// Optional list of network interfaces messages will be processed on, the rest will be passed as is. - // /// If absent, the rules will be applied to all interfaces. An empty list is invalid. - // interfaces: [ "wlan0" ], - // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. - // /// If absent, the rules will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the rules will be applied to both flows. - // flows: ["ingress", "egress"], - // /// List of message type on which downsampling will be applied. Must not be empty. - // messages: [ - // /// Delete - // "delete", - // /// Put - // "put", - // /// Get - // "query", - // /// Queryable Reply to a Query - // "reply", - // ], - // /// A list of downsampling rules: key_expression and the maximum frequency in Hertz - // rules: [ - // { key_expr: "demo/example/zenoh-rs-pub", freq: 0.1 }, - // ], - // }, - // ], - - // /// Configure access control (ACL) rules - access_control: { - /// [true/false] acl will be activated only if this is set to true - "enabled": true, - /// [deny/allow] default permission is deny (even if this is left empty or not specified) - "default_permission": "deny", - /// Rule set for permissions allowing or denying access to key-expressions - "rules": - [ - { - "flows": [ - "ingress", - "egress" - ], - "id": "liveliness_tokens", - "key_exprs": [ - "@ros2_lv/0/**" - ], - "messages": [ - "liveliness_token", - "liveliness_query", - "declare_liveliness_subscriber", - "reply" - ], - "permission": "allow" - } - ], - /// List of combinations of subjects. - /// - /// If a subject property (i.e. username, certificate common name or interface) is empty - /// it is interpreted as a wildcard. Moreover, a subject property cannot be an empty list. - "subjects": - [ - { - "id": "workcell_1", - }, - ], - /// The policies list associates rules to subjects - "policies": - [ - /// Each policy associates one or multiple rules to one or multiple subject combinations - { - "rules": [ - "bridged_topics", - "bridged_services", - "bridged_actions", - "liveliness_tokens", - ], - "subjects": ["workcell_1"], - }, - ] - }, - - // low_pass_filter: [ - // { - // /// Optional Id, has to be unique - // "id": "filter1", - // /// Optional list of network interfaces messages will be processed on, the rest will not be filtered. - // /// If absent, the filter will be applied to all interfaces. - // interfaces: [ "wlan0" ], - // /// Optional list of link protocols. Transports with at least one of these links will have their messages filtered. - // /// If absent, the rule will be applied to all transports. An empty list is invalid. - // link_protocols: [ "tcp", "udp", "tls", "quic", "ws", "serial", "unixsock-stream", "unixpipe", "vsock"], - // /// Optional list of data flows messages will be processed on ("egress" and/or "ingress"). - // /// If absent, the filter will be applied to both flows. - // flows: ["ingress", "egress"], - // /// List of message type on which the filter will be applied. Must not be empty. - // messages: [ - // "put", - // "delete", - // "query", - // "reply" - // ], - // /// List of key_expressions which matching messages will be filtered - // key_exprs: [ - // "demo/**", - // ], - // /// Inclusive max size of serialized payload + serialized attachment - // size_limit: 8192, - // }, - // ], - - /// Configure internal transport parameters - transport: { - unicast: { - /// Timeout in milliseconds when opening a link - /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together - open_timeout: 60000, - /// Timeout in milliseconds when accepting a link - /// ROS setting: increase the value to avoid timeout at launch time with a large number of Nodes starting all together - accept_timeout: 60000, - /// Maximum number of links in pending state while performing the handshake for accepting it - /// ROS setting: increase the value to support a large number of Nodes starting all together - accept_pending: 10000, - /// Maximum number of transports that can be simultaneously alive for a single zenoh sessions - /// ROS setting: increase the value to support a large number of Nodes starting all together - max_sessions: 10000, - /// Maximum number of incoming links that are admitted per transport - max_links: 1, - /// Enables the LowLatency transport - /// This option does not make LowLatency transport mandatory, the actual implementation of transport - /// used will depend on Establish procedure and other party's settings - /// - /// NOTE: Currently, the LowLatency transport doesn't preserve QoS prioritization. - /// NOTE: Due to the note above, 'lowlatency' is incompatible with 'qos' option, so in order to - /// enable 'lowlatency' you need to explicitly disable 'qos'. - /// NOTE: LowLatency transport does not support the fragmentation, so the message size should be - /// smaller than the tx batch_size. - lowlatency: false, - /// Enables QoS on unicast communications. - qos: { - enabled: true, - }, - /// Enables compression on unicast communications. - /// Compression capabilities are negotiated during session establishment. - /// If both Zenoh nodes support compression, then compression is activated. - compression: { - enabled: false, - }, - }, - /// WARNING: multicast communication does not perform any negotiation upon group joining. - /// Because of that, it is important that all transport parameters are the same to make - /// sure all your nodes in the system can communicate. One common parameter to configure - /// is "transport/link/tx/batch_size" since its default value depends on the actual platform - /// when operating on multicast. - /// E.g., the batch size on Linux and Windows is 65535 bytes, on Mac OS X is 9216, and anything else is 8192. - multicast: { - /// JOIN message transmission interval in milliseconds. - join_interval: 2500, - /// Maximum number of multicast sessions. - max_sessions: 1000, - /// Enables QoS on multicast communication. - /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. - qos: { - enabled: false, - }, - /// Enables compression on multicast communication. - /// Default to false for Zenoh-to-Zenoh-Pico out-of-the-box compatibility. - compression: { - enabled: false, - }, - }, - link: { - /// An optional whitelist of protocols to be used for accepting and opening sessions. If not - /// configured, all the supported protocols are automatically whitelisted. The supported - /// protocols are: ["tcp" , "udp", "tls", "quic", "ws", "unixsock-stream", "vsock"] For - /// example, to only enable "tls" and "quic": protocols: ["tls", "quic"], - /// - /// Configure the zenoh TX parameters of a link - tx: { - /// The resolution in bits to be used for the message sequence numbers. - /// When establishing a session with another Zenoh instance, the lowest value of the two instances will be used. - /// Accepted values: 8bit, 16bit, 32bit, 64bit. - sequence_number_resolution: "32bit", - /// Link lease duration in milliseconds to announce to other zenoh nodes - /// ROS setting: increase the value to avoid lease expiration at launch time with a large number of Nodes starting all together - lease: 60000, - /// Number of keep-alive messages in a link lease duration. If no data is sent, keep alive - /// messages will be sent at the configured time interval. - /// NOTE: In order to consider eventual packet loss and transmission latency and jitter, - /// set the actual keep_alive interval to one fourth of the lease time: i.e. send - /// 4 keep_alive messages in a lease period. Changing the lease time will have the - /// keep_alive messages sent more or less often. - /// This is in-line with the ITU-T G.8013/Y.1731 specification on continuous connectivity - /// check which considers a link as failed when no messages are received in 3.5 times the - /// target interval. - /// ROS setting: decrease the value since Nodes are communicating over the loopback - /// where keep-alive messages have less chances to be lost. - keep_alive: 2, - /// Batch size in bytes is expressed as a 16bit unsigned integer. - /// Therefore, the maximum batch size is 2^16-1 (i.e. 65535). - /// The default batch size value is the maximum batch size: 65535. - batch_size: 65535, - /// Each zenoh link has a transmission queue that can be configured - queue: { - /// The size of each priority queue indicates the number of batches a given queue can contain. - /// NOTE: the number of batches in each priority must be included between 1 and 16. Different values will result in an error. - /// The amount of memory being allocated for each queue is then SIZE_XXX * BATCH_SIZE. - /// In the case of the transport link MTU being smaller than the ZN_BATCH_SIZE, - /// then amount of memory being allocated for each queue is SIZE_XXX * LINK_MTU. - /// If qos is false, then only the DATA priority will be allocated. - size: { - control: 2, - real_time: 2, - interactive_high: 2, - interactive_low: 2, - data_high: 2, - data: 2, - data_low: 2, - background: 2, - }, - /// Congestion occurs when the queue is empty (no available batch). - congestion_control: { - /// Behavior pushing CongestionControl::Drop messages to the queue. - drop: { - /// The maximum time in microseconds to wait for an available batch before dropping a droppable message if still no batch is available. - wait_before_drop: 1000, - /// The maximum deadline limit for multi-fragment messages. - max_wait_before_drop_fragments: 50000, - }, - /// Behavior pushing CongestionControl::Block messages to the queue. - block: { - /// The maximum time in microseconds to wait for an available batch before closing the transport session when sending a blocking message - /// if still no batch is available. - /// ROS setting: increase the value to avoid unecessary link closure at launch time where congestion is likely - /// to occur even over the loopback since all the Nodes are starting at the same time. - wait_before_close: 60000000, - }, - }, - /// Perform batching of messages if they are smaller of the batch_size - batching: { - /// Perform adaptive batching of messages if they are smaller of the batch_size. - /// When the network is detected to not be fast enough to transmit every message individually, many small messages may be - /// batched together and sent all at once on the wire reducing the overall network overhead. This is typically of a high-throughput - /// scenario mainly composed of small messages. In other words, batching is activated by the network back-pressure. - enabled: true, - /// The maximum time limit (in ms) a message should be retained for batching when back-pressure happens. - time_limit: 1, - }, - allocation: { - /// Mode for memory allocation of batches in the priority queues. - /// - "init": batches are allocated at queue initialization time. - /// - "lazy": batches are allocated when needed up to the maximum number of batches configured in the size configuration parameter. - mode: "lazy", - }, - }, - }, - /// Configure the zenoh RX parameters of a link - rx: { - /// Receiving buffer size in bytes for each link - /// The default the rx_buffer_size value is the same as the default batch size: 65535. - /// For very high throughput scenarios, the rx_buffer_size can be increased to accommodate - /// more in-flight data. This is particularly relevant when dealing with large messages. - /// E.g. for 16MiB rx_buffer_size set the value to: 16777216. - buffer_size: 65535, - /// Maximum size of the defragmentation buffer at receiver end. - /// Fragmented messages that are larger than the configured size will be dropped. - /// The default value is 1GiB. This would work in most scenarios. - /// NOTE: reduce the value if you are operating on a memory constrained device. - max_message_size: 1073741824, - }, - /// Configure TLS specific parameters - tls: { - /// Path to the certificate of the certificate authority used to validate either the server - /// or the client's keys and certificates, depending on the node's mode. If not specified - /// on router mode then the default WebPKI certificates are used instead. - root_ca_certificate: null, - /// Path to the TLS listening side private key - listen_private_key: null, - /// Path to the TLS listening side public certificate - listen_certificate: null, - /// Enables mTLS (mutual authentication), client authentication - enable_mtls: false, - /// Path to the TLS connecting side private key - connect_private_key: null, - /// Path to the TLS connecting side certificate - connect_certificate: null, - /// Whether or not to verify the matching between hostname/dns and certificate when connecting, - /// if set to false zenoh will disregard the common names of the certificates when verifying servers. - /// This could be dangerous because your CA can have signed a server cert for foo.com, that's later being used to host a server at baz.com. If you wan't your - /// ca to verify that the server at baz.com is actually baz.com, let this be true (default). - verify_name_on_connect: true, - /// Whether or not to close links when remote certificates expires. - /// If set to true, links that require certificates (tls/quic) will automatically disconnect when the time of expiration of the remote certificate chain is reached - /// note that mTLS (client authentication) is required for a listener to disconnect a client on expiration - close_link_on_expiration: false, - /// Optional configuration for TCP system buffers sizes for TLS links - /// - /// Configure TCP read buffer size (bytes) - // so_rcvbuf: 123456, - /// Configure TCP write buffer size (bytes) - // so_sndbuf: 123456, - }, - // // Configure optional TCP link specific parameters - // tcp: { - // /// Optional configuration for TCP system buffers sizes for TCP links - // /// - // /// Configure TCP read buffer size (bytes) - // // so_rcvbuf: 123456, - // /// Configure TCP write buffer size (bytes) - // // so_sndbuf: 123456, - // }, - }, - /// Shared memory configuration. - /// NOTE: shared memory can be used only if zenoh is compiled with "shared-memory" feature, otherwise - /// settings in this section have no effect. - shared_memory: { - /// Whether shared memory is enabled or not. - /// If set to `true`, the SHM buffer optimization support will be announced to other parties. (default `true`). - /// This option doesn't make SHM buffer optimization mandatory, the real support depends on other party setting. - /// A probing procedure for shared memory is performed upon session opening. To enable zenoh to operate - /// over shared memory (and to not fallback on network mode), shared memory needs to be enabled also on the - /// subscriber side. By doing so, the probing procedure will succeed and shared memory will operate as expected. - /// - /// ROS setting: disabled by default until fully tested - enabled: false, - /// SHM resources initialization mode (default "lazy"). - /// - "lazy": SHM subsystem internals will be initialized lazily upon the first SHM buffer - /// allocation or reception. This setting provides better startup time and optimizes resource usage, - /// but produces extra latency at the first SHM buffer interaction. - /// - "init": SHM subsystem internals will be initialized upon Session opening. This setting sacrifices - /// startup time, but guarantees no latency impact when first SHM buffer is processed. - mode: "lazy", - transport_optimization: { - /// Enables transport optimization for large messages (default `true`). - /// Implicitly puts large messages into shared memory for transports with SHM-compatible connection. - enabled: true, - /// SHM memory size in bytes used for transport optimization (default `16 * 1024 * 1024`). - pool_size: 16777216, - /// Allow optimization for messages equal or larger than this threshold in bytes (default `3072`). - message_size_threshold: 3072, - }, - }, - auth: { - /// The configuration of authentication. - /// A password implies a username is required. - usrpwd: { - user: null, - password: null, - /// The path to a file containing the user password dictionary - dictionary_file: null, - }, - pubkey: { - public_key_pem: null, - private_key_pem: null, - public_key_file: null, - private_key_file: null, - key_size: null, - known_keys_file: null, - }, - }, - }, - - /// Configure the Admin Space - /// Unstable: this configuration part works as advertised, but may change in a future release - adminspace: { - /// Enables the admin space - enabled: true, - /// read and/or write permissions on the admin space - permissions: { - read: true, - write: false, - }, - }, - -} diff --git a/nexus_demos/config/zenoh/workcell_1_router_config.json5 b/nexus_demos/config/zenoh/workcell_1_router_config.json5 index 5aadbe20..be25179e 100644 --- a/nexus_demos/config/zenoh/workcell_1_router_config.json5 +++ b/nexus_demos/config/zenoh/workcell_1_router_config.json5 @@ -1,99 +1,27 @@ { mode: "router", connect: { - endpoints: [ - // system orchestrator - "tcp/localhost:7447", - ], + endpoints: [], }, listen: { endpoints: [ "tcp/[::]:7448" ], }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, 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"], - }, - ] + "enabled": false, + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + drop_future_timestamp: false, }, } diff --git a/nexus_demos/config/zenoh/workcell_1_session_config.json5 b/nexus_demos/config/zenoh/workcell_1_session_config.json5 index efbd6490..6d40df63 100644 --- a/nexus_demos/config/zenoh/workcell_1_session_config.json5 +++ b/nexus_demos/config/zenoh/workcell_1_session_config.json5 @@ -2,22 +2,23 @@ mode: "peer", connect: { endpoints: [ - // workcell orchestrator "tcp/localhost:7448", ], }, - - /// Configuration of data messages timestamps management. + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + access_control: { + "enabled": false, + }, 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_bridge_config.json5 b/nexus_demos/config/zenoh/workcell_2_bridge_config.json5 new file mode 100644 index 00000000..92579282 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_2_bridge_config.json5 @@ -0,0 +1,110 @@ +{ + mode: "router", + connect: { + endpoints: [ + "tcp/localhost:7447", + "tcp/localhost:7449", + ], + }, + listen: { + endpoints: [], + }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + 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"], + }, + ] + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + 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 index 43cbbdd7..4760cbc5 100644 --- a/nexus_demos/config/zenoh/workcell_2_router_config.json5 +++ b/nexus_demos/config/zenoh/workcell_2_router_config.json5 @@ -1,99 +1,27 @@ { mode: "router", connect: { - endpoints: [ - // system orchestrator - "tcp/localhost:7447", - ], + endpoints: [], }, listen: { endpoints: [ "tcp/[::]:7449" ], }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, 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"], - }, - ] + "enabled": false, + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + drop_future_timestamp: false, }, } diff --git a/nexus_demos/config/zenoh/workcell_2_session_config.json5 b/nexus_demos/config/zenoh/workcell_2_session_config.json5 index 16a441f7..2b1be2d1 100644 --- a/nexus_demos/config/zenoh/workcell_2_session_config.json5 +++ b/nexus_demos/config/zenoh/workcell_2_session_config.json5 @@ -2,22 +2,23 @@ mode: "peer", connect: { endpoints: [ - // workcell orchestrator "tcp/localhost:7449", ], }, - - /// Configuration of data messages timestamps management. + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + access_control: { + "enabled": false, + }, 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/zenohd.json5 b/nexus_demos/config/zenoh/zenohd.json5 deleted file mode 100644 index 3951f019..00000000 --- a/nexus_demos/config/zenoh/zenohd.json5 +++ /dev/null @@ -1,271 +0,0 @@ -{ - "access_control": { - "default_permission": "deny", - "enabled": false, - "policies": null, - "rules": null, - "subjects": null - }, - "adminspace": { - "enabled": true, - "permissions": { - "read": true, - "write": false - } - }, - "aggregation": { - "publishers": [], - "subscribers": [] - }, - "connect": { - "endpoints": [], - "exit_on_failure": { - "client": true, - "peer": false, - "router": false - }, - "retry": { - "period_increase_factor": 2, - "period_init_ms": 1000, - "period_max_ms": 4000 - }, - "timeout_ms": { - "client": 0, - "peer": -1, - "router": -1 - } - }, - "downsampling": [], - "id": null, - "listen": { - "endpoints": [ - "tcp/[::]:7447" - ], - "exit_on_failure": true, - "retry": { - "period_increase_factor": 2, - "period_init_ms": 1000, - "period_max_ms": 4000 - }, - "timeout_ms": 0 - }, - "low_pass_filter": [], - "metadata": null, - "mode": "router", - "namespace": null, - "open": { - "return_conditions": { - "connect_scouted": true, - "declares": true - } - }, - "plugins": {}, - "plugins_loading": { - "enabled": false, - "search_dirs": [ - { - "kind": "current_exe_parent", - "value": null - }, - ".", - "~/.zenoh/lib", - "/opt/homebrew/lib", - "/usr/local/lib", - "/usr/lib" - ] - }, - "qos": { - "network": [], - "publication": [] - }, - "queries_default_timeout": 600000, - "routing": { - "interests": { - "timeout": 10000 - }, - "peer": { - "linkstate": { - "transport_weights": [] - }, - "mode": "peer_to_peer" - }, - "router": { - "linkstate": { - "transport_weights": [] - }, - "peers_failover_brokering": false - } - }, - "scouting": { - "delay": 500, - "gossip": { - "autoconnect": { - "peer": [ - "router", - "peer" - ], - "router": [] - }, - "autoconnect_strategy": { - "router": { - "to_peer": "always", - "to_router": "always" - } - }, - "enabled": true, - "multihop": false, - "target": { - "peer": [ - "router" - ], - "router": [ - "router", - "peer" - ] - } - }, - "multicast": { - "address": "224.0.0.224:7446", - "autoconnect": { - "client": [ - "router" - ], - "peer": [ - "router", - "peer" - ], - "router": [] - }, - "autoconnect_strategy": { - "router": { - "to_peer": "always", - "to_router": "always" - } - }, - "enabled": false, - "interface": "auto", - "listen": true, - "ttl": 1 - }, - "timeout": 3000 - }, - "timestamping": { - "drop_future_timestamp": false, - "enabled": { - "client": true, - "peer": true, - "router": true - } - }, - "transport": { - "auth": { - "pubkey": { - "key_size": null, - "known_keys_file": null, - "private_key_file": null, - "private_key_pem": null, - "public_key_file": null, - "public_key_pem": null - }, - "usrpwd": { - "dictionary_file": null, - "password": null, - "user": null - } - }, - "link": { - "protocols": null, - "rx": { - "buffer_size": 65535, - "max_message_size": 1073741824 - }, - "tcp": { - "so_rcvbuf": null, - "so_sndbuf": null - }, - "tls": { - "close_link_on_expiration": false, - "connect_certificate": null, - "connect_private_key": null, - "enable_mtls": false, - "listen_certificate": null, - "listen_private_key": null, - "root_ca_certificate": null, - "so_rcvbuf": null, - "so_sndbuf": null, - "verify_name_on_connect": true - }, - "tx": { - "batch_size": 65535, - "keep_alive": 2, - "lease": 60000, - "queue": { - "allocation": { - "mode": "lazy" - }, - "batching": { - "enabled": true, - "time_limit": 1 - }, - "congestion_control": { - "block": { - "wait_before_close": 5000000 - }, - "drop": { - "max_wait_before_drop_fragments": 50000, - "wait_before_drop": 1000 - } - }, - "size": { - "background": 2, - "control": 2, - "data": 2, - "data_high": 2, - "data_low": 2, - "interactive_high": 2, - "interactive_low": 2, - "real_time": 2 - } - }, - "sequence_number_resolution": "32bit", - "threads": 16 - }, - "unixpipe": { - "file_access_mask": null - } - }, - "multicast": { - "compression": { - "enabled": false - }, - "join_interval": 2500, - "max_sessions": 1000, - "qos": { - "enabled": false - } - }, - "shared_memory": { - "enabled": false, - "mode": "lazy", - "transport_optimization": { - "enabled": true, - "message_size_threshold": 3072, - "pool_size": 16777216 - } - }, - "unicast": { - "accept_pending": 10000, - "accept_timeout": 60000, - "compression": { - "enabled": false - }, - "lowlatency": false, - "max_links": 1, - "max_sessions": 10000, - "open_timeout": 60000, - "qos": { - "enabled": true - } - } - } -} \ No newline at end of file diff --git a/nexus_demos/launch/launch.py b/nexus_demos/launch/launch.py index 2c0b14b3..fe752a91 100644 --- a/nexus_demos/launch/launch.py +++ b/nexus_demos/launch/launch.py @@ -95,8 +95,8 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "ros_domain_id": str(inter_workcell_domain_id), "zenoh_config_package": "nexus_demos", - "zenoh_router_config_filename": "config/zenoh/system_orchestrator_router_config.json5", - "zenoh_session_config_filename": "config/zenoh/system_orchestrator_session_config.json5", + "zenoh_router_config_filename": "config/zenoh/inter_workcell_router_config.json5", + "zenoh_session_config_filename": "config/zenoh/inter_workcell_session_config.json5", "use_multiple_transporters": use_multiple_transporters, "activate_system_orchestrator": headless, "headless": headless, @@ -154,6 +154,28 @@ def launch_setup(context, *args, **kwargs): ], ) + launch_workcell_1_bridge_router = GroupAction( + [ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_demos"), + "launch", + "zenoh_router.launch.py", + ] + ) + ], + launch_arguments={ + "zenoh_config_package": "nexus_demos", + "zenoh_router_config_filename": "config/zenoh/workcell_1_bridge_config.json5", + "ros_domain_id": str(workcell_1_domain_id), + }.items(), + condition=IfCondition(run_workcell_1), + ) + ] + ) + launch_workcell_2 = GroupAction( actions=[ IncludeLaunchDescription( @@ -200,11 +222,35 @@ def launch_setup(context, *args, **kwargs): ], ) + launch_workcell_2_bridge_router = GroupAction( + [ + IncludeLaunchDescription( + [ + PathJoinSubstitution( + [ + FindPackageShare("nexus_demos"), + "launch", + "zenoh_router.launch.py", + ] + ) + ], + launch_arguments={ + "zenoh_config_package": "nexus_demos", + "zenoh_router_config_filename": "config/zenoh/workcell_2_bridge_config.json5", + "ros_domain_id": str(workcell_2_domain_id), + }.items(), + condition=IfCondition(run_workcell_2), + ) + ] + ) + return [ LogInfo(msg=log_msg), launch_inter_workcell, launch_workcell_1, + launch_workcell_1_bridge_router, launch_workcell_2, + launch_workcell_2_bridge_router, ] From 7130d177db719fb2ddf589d954d573024119b953 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 19 Dec 2025 18:01:31 +0800 Subject: [PATCH 04/17] update dependencies, remove unused bridge vendor package Signed-off-by: Aaron Chong --- README.md | 2 +- nexus_demos/CMakeLists.txt | 13 -- nexus_demos/config/zenoh/README.md | 15 --- .../config/zenoh/nexus_network_config.yaml | 69 ----------- nexus_demos/launch/zenoh_bridge.launch.py | 112 ------------------ nexus_demos/package.xml | 1 - nexus_zenoh_bridge_dds_vendor/CHANGELOG.rst | 10 -- nexus_zenoh_bridge_dds_vendor/CMakeLists.txt | 26 ---- nexus_zenoh_bridge_dds_vendor/package.xml | 19 --- 9 files changed, 1 insertion(+), 266 deletions(-) delete mode 100644 nexus_demos/config/zenoh/README.md delete mode 100644 nexus_demos/config/zenoh/nexus_network_config.yaml delete mode 100644 nexus_demos/launch/zenoh_bridge.launch.py delete mode 100644 nexus_zenoh_bridge_dds_vendor/CHANGELOG.rst delete mode 100644 nexus_zenoh_bridge_dds_vendor/CMakeLists.txt delete mode 100644 nexus_zenoh_bridge_dds_vendor/package.xml diff --git a/README.md b/README.md index 3e71c8d7..c63764eb 100644 --- a/README.md +++ b/README.md @@ -65,7 +65,7 @@ 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 +### Generating Zenoh bridge configurations [deprecated] 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 -r -o ` to generate the Zenoh bridges. diff --git a/nexus_demos/CMakeLists.txt b/nexus_demos/CMakeLists.txt index d81cb963..d2fca3b1 100644 --- a/nexus_demos/CMakeLists.txt +++ b/nexus_demos/CMakeLists.txt @@ -125,19 +125,6 @@ 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) diff --git a/nexus_demos/config/zenoh/README.md b/nexus_demos/config/zenoh/README.md deleted file mode 100644 index 0474c951..00000000 --- a/nexus_demos/config/zenoh/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# General setup (so far) - -```bash -ros2 launch nexus_demos launch.py headless:=false run_workcell_1:=false run_workcell_2:=false use_zenoh_bridge:=false -``` - -```bash -ros2 run zenoh_security_tools generate_configs \ - --policy policy_main.xml \ - --router-config ~/nexus_workspaces/sros_demo/rmw_zenoh/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5 \ - --session-config ~/nexus_workspaces/sros_demo/rmw_zenoh/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_SESSION_CONFIG.json5 \ - --ros-domain-id 0 -``` - -This still generates a lot of weirdness, we will use the manual ACL method instead. diff --git a/nexus_demos/config/zenoh/nexus_network_config.yaml b/nexus_demos/config/zenoh/nexus_network_config.yaml deleted file mode 100644 index 2dc2d466..00000000 --- a/nexus_demos/config/zenoh/nexus_network_config.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# Mode of Zenoh bridge, specified as either 'client' or 'peer'. -# Use 'client' when there is an available Zenoh router, otherwise -# use 'peer' for a distributed system. -mode: peer -# When true, activates a REST API used to administer Zenoh Bridge configurations -enable_rest_api: true - -system_orchestrators: - - namespace: system_orchestrator # ROS Namespace of the endpoints - # ROS Domain ID - domain_id: 0 - # Listening TCP Address of Zenoh bridge - tcp_listen: ["0.0.0.0:7447"] - # HTTP Port for the REST API - rest_api_http_port: 8000 - allow: - publishers: [] - # TODO(luca) check if we need estop - subscribers: ["/estop", "/.*/workcell_state"] - service_servers: ["/list_workcells", "/register_workcell", "/register_transporter"] - # TODO(luca) check if transporter needs available endpoint - service_clients: ["/.*/queue_task", "/.*/remove_pending_task", "/.*/pause", "/.*/signal", "/.*/get_state", "/.*/change_state", "/.*/is_task_doable"] - action_servers: [] - # TODO(luca) check if we really need transporter client - action_clients: ["/.*/request", "/.*/transport"] - queries_timeout: - default: 600.0 - - -workcell_orchestrators: - - namespace: workcell_1 - domain_id: 1 - tcp_connect: ["0.0.0.0:7447"] - rest_api_http_port: 8001 - allow: - publishers: ["/workcell_1/workcell_state"] - subscribers: [] - service_servers: ["/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"] - service_clients: ["/register_workcell"] - action_servers: ["/workcell_1/request"] - action_clients: [] - queries_timeout: - default: 600.0 - - namespace: workcell_2 - domain_id: 2 - tcp_connect: ["0.0.0.0:7447"] - rest_api_http_port: 8002 - allow: - publishers: ["/workcell_2/workcell_state"] - subscribers: [] - service_servers: ["/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"] - service_clients: ["/register_workcell"] - action_servers: ["/workcell_2/request"] - action_clients: [] - queries_timeout: - default: 600.0 - - namespace: workcell_3 - domain_id: 3 - tcp_connect: ["0.0.0.0:7447"] - rest_api_http_port: 8003 - allow: - publishers: ["workcell_3/workcell_state"] - subscribers: [] - service_servers: ["workcell_3/is_task_doable", "workcell_3/queue_task", "workcell_3/remove_pending_task", "workcell_3/get_state", "workcell_3/change_state"] - service_clients: ["/register_workcell"] - action_servers: ["workcell_3/request"] - action_clients: [] - queries_timeout: - default: 600.0 diff --git a/nexus_demos/launch/zenoh_bridge.launch.py b/nexus_demos/launch/zenoh_bridge.launch.py deleted file mode 100644 index 8dad1272..00000000 --- a/nexus_demos/launch/zenoh_bridge.launch.py +++ /dev/null @@ -1,112 +0,0 @@ -# Copyright 2022 Johnson & Johnson -# -# 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.conditions import IfCondition -from launch.substitutions import ( - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) -from launch_ros.substitutions import ( - ExecutableInPackage, - FindPackageShare, -) - - -def launch_setup(context, *args, **kwargs): - - zenoh_config_package = LaunchConfiguration("zenoh_config_package") - zenoh_config_filename = LaunchConfiguration("zenoh_config_filename") - dds_localhost_only = LaunchConfiguration("dds_localhost_only") - log_level = LaunchConfiguration("log_level") - ros_domain_id = LaunchConfiguration("ros_domain_id") - - cmd = [ - ExecutableInPackage( - executable="zenoh_bridge_ros2dds", - package="nexus_zenoh_bridge_dds_vendor", - ), - "--config", - PathJoinSubstitution([ - FindPackageShare(zenoh_config_package), - zenoh_config_filename - ]), - "--domain", - ros_domain_id, - ] - - if IfCondition(dds_localhost_only).evaluate(context): - cmd.append("--dds-localhost-only") - - zenoh_bridge_exec = ExecuteProcess( - cmd=cmd, - shell=True, - additional_env={"RUST_LOG": log_level.perform(context)}, - ) - - return [zenoh_bridge_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="error", - description="Log level of zenoh DDS Bridge", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - name="dds_localhost_only", - default_value="false", - description="If true, the DDS discovery and traffic " - "will occur only on the localhost interface (127.0.0.1)", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - name="zenoh_config_package", - default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", - ) - ) - - declared_arguments.append( - DeclareLaunchArgument( - name="zenoh_config_filename", - default_value="config/zenoh/system_orchestrator.json5", - description="Zenoh DDS bridge configuration filepath", - ) - ) - - return LaunchDescription( - declared_arguments + [OpaqueFunction(function=launch_setup)] - ) diff --git a/nexus_demos/package.xml b/nexus_demos/package.xml index 5e71ead1..cb23e086 100644 --- a/nexus_demos/package.xml +++ b/nexus_demos/package.xml @@ -8,7 +8,6 @@ Apache License 2.0 nexus_endpoints - nexus_network_configuration ament_cmake diff --git a/nexus_zenoh_bridge_dds_vendor/CHANGELOG.rst b/nexus_zenoh_bridge_dds_vendor/CHANGELOG.rst deleted file mode 100644 index 0808a004..00000000 --- a/nexus_zenoh_bridge_dds_vendor/CHANGELOG.rst +++ /dev/null @@ -1,10 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package nexus_zenoh_bridge_dds_vendor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2023-11-20) ------------------- - -0.1.0 (2023-11-06) ------------------- -* A package to vendor the Zenoh DDS bridge. diff --git a/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt b/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt deleted file mode 100644 index 13e508cc..00000000 --- a/nexus_zenoh_bridge_dds_vendor/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(nexus_zenoh_bridge_dds_vendor) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) -endif() - -find_package(ament_cmake REQUIRED) -find_package(ament_cmake_vendor_package REQUIRED) - -ament_vendor(zeno_bridge_dds_vendor - VCS_URL https://github.com/eclipse-zenoh/zenoh-plugin-ros2dds.git - VCS_VERSION 1.0.3 -) - -# TODO(sloretz) make a nice way to get this path from ament_vendor -set(INSTALL_DIR "${CMAKE_CURRENT_BINARY_DIR}/zeno_bridge_dds_vendor-prefix/install") -install( - DIRECTORY "${INSTALL_DIR}/lib/zenoh_bridge_ros2dds/" - DESTINATION "lib/${PROJECT_NAME}" - USE_SOURCE_PERMISSIONS -) - -ament_package() diff --git a/nexus_zenoh_bridge_dds_vendor/package.xml b/nexus_zenoh_bridge_dds_vendor/package.xml deleted file mode 100644 index 79bb1bcc..00000000 --- a/nexus_zenoh_bridge_dds_vendor/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - nexus_zenoh_bridge_dds_vendor - 0.1.1 - Newer version of zenoh_bridge_dds for NEXUS - Teo Koon Peng - Apache License 2.0 - - ament_cmake - ament_cmake_vendor_package - - cargo - clang - - - ament_cmake - - From 6198fabf603ced9a76031f8240ab8591424b2a5c Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Fri, 19 Dec 2025 23:40:19 +0800 Subject: [PATCH 05/17] update docs, default values, and tests Signed-off-by: Aaron Chong --- .../workflows/nexus_integration_tests.yaml | 4 +- nexus_demos/README.md | 49 +++++++++++-------- nexus_demos/launch/inter_workcell.launch.py | 8 +-- nexus_demos/launch/workcell.launch.py | 2 +- nexus_demos/launch/workcell_1_tf.launch.py | 2 +- nexus_demos/launch/workcell_2_tf.launch.py | 2 +- nexus_demos/launch/zenoh_router.launch.py | 2 +- nexus_demos/package.xml | 2 +- nexus_network_configuration/README.md | 2 + 9 files changed, 42 insertions(+), 31 deletions(-) diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index 72a8da2d..b4eeaa5e 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -37,6 +37,6 @@ jobs: - name: build run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_gazebo nexus_demos nexus_motion_planner --mixin release - 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 32e6c343..39b54033 100644 --- a/nexus_demos/README.md +++ b/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=` and `robot2_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:= +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 ### 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_bridge_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_bridge_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 diff --git a/nexus_demos/launch/inter_workcell.launch.py b/nexus_demos/launch/inter_workcell.launch.py index dad51eb7..6bb303e6 100644 --- a/nexus_demos/launch/inter_workcell.launch.py +++ b/nexus_demos/launch/inter_workcell.launch.py @@ -307,16 +307,16 @@ def generate_launch_description(): DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", + description="Package containing Zenoh configurations", ), DeclareLaunchArgument( name="zenoh_router_config_filename", - default_value="config/zenoh/system_orchestrator_router_config.json5", + default_value="config/zenoh/inter_workcell_router_config.json5", description="RMW Zenoh router configuration filepath", ), DeclareLaunchArgument( name="zenoh_session_config_filename", - default_value="config/zenoh/system_orchestrator_session_config.json5", + default_value="config/zenoh/inter_workcell_session_config.json5", description="RMW Zenoh session configuration filepath", ), DeclareLaunchArgument( @@ -336,7 +336,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "remap_task_types", - default_value="\"pick_and_place: [place_on_conveyor, pick_from_conveyor]\"", + default_value="\"pick_and_place: [place_on_amr, place_on_conveyor, pick_from_amr, pick_from_conveyor]\"", description="A yaml containing a dictionary of task types and an array of remaps", ), DeclareLaunchArgument( diff --git a/nexus_demos/launch/workcell.launch.py b/nexus_demos/launch/workcell.launch.py index ebf06bfd..1183e7a4 100644 --- a/nexus_demos/launch/workcell.launch.py +++ b/nexus_demos/launch/workcell.launch.py @@ -385,7 +385,7 @@ def generate_launch_description(): DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", + description="Package containing Zenoh configurations", ), DeclareLaunchArgument( name="zenoh_router_config_filename", diff --git a/nexus_demos/launch/workcell_1_tf.launch.py b/nexus_demos/launch/workcell_1_tf.launch.py index 1c66fbaa..034c206b 100644 --- a/nexus_demos/launch/workcell_1_tf.launch.py +++ b/nexus_demos/launch/workcell_1_tf.launch.py @@ -183,7 +183,7 @@ def generate_launch_description(): DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", + description="Package containing Zenoh configurations", ), DeclareLaunchArgument( name="zenoh_session_config_filename", diff --git a/nexus_demos/launch/workcell_2_tf.launch.py b/nexus_demos/launch/workcell_2_tf.launch.py index ba7796ca..7a82944e 100644 --- a/nexus_demos/launch/workcell_2_tf.launch.py +++ b/nexus_demos/launch/workcell_2_tf.launch.py @@ -185,7 +185,7 @@ def generate_launch_description(): DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", - description="Package containing Zenoh DDS bridge configurations", + description="Package containing Zenoh configurations", ), DeclareLaunchArgument( name="zenoh_session_config_filename", diff --git a/nexus_demos/launch/zenoh_router.launch.py b/nexus_demos/launch/zenoh_router.launch.py index e6e58f11..3a371e27 100644 --- a/nexus_demos/launch/zenoh_router.launch.py +++ b/nexus_demos/launch/zenoh_router.launch.py @@ -71,7 +71,7 @@ def generate_launch_description(): DeclareLaunchArgument( name="log_level", default_value="zenoh=debug", - description="Log level of zenoh DDS Bridge", + description="Log level of zenoh router", ) ) diff --git a/nexus_demos/package.xml b/nexus_demos/package.xml index cb23e086..9893648b 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 diff --git a/nexus_network_configuration/README.md b/nexus_network_configuration/README.md index bcddfd30..a4b1dfc0 100644 --- a/nexus_network_configuration/README.md +++ b/nexus_network_configuration/README.md @@ -1,5 +1,7 @@ # Network Config Generator +> Note: As of December 2025, Nexus has migrated to using `rmw_zenoh_cpp` directly, hence `main` no longer contains any examples of usage. Please reference past commits. + This package simplifies the Zenoh DDS Bridge setup for multiple NEXUS orchestrators through the generation of Zenoh bridge configurations from the following 2 files: 1. NEXUS Network Configuration: This YAML file describes the properties of different machines such as ROS Domain ID, Zenoh bridge TCP connection endpoints etc. Refer to [Quick Start](#quick-start) for an example 2. REDF Configuration: This YAML file describes the ROS Endpoints of the NEXUS orchestrators From 19e64548503a700865102444865af2b85f932639 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Sat, 20 Dec 2025 00:14:00 +0800 Subject: [PATCH 06/17] fix cmakelists file Signed-off-by: Aaron Chong --- nexus_demos/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nexus_demos/CMakeLists.txt b/nexus_demos/CMakeLists.txt index d2fca3b1..7277bfc4 100644 --- a/nexus_demos/CMakeLists.txt +++ b/nexus_demos/CMakeLists.txt @@ -126,6 +126,8 @@ add_test(NAME all_tests COMMAND python3 -m unittest WORKING_DIRECTORY ${CMAKE_CU ############################################################################### +install(DIRECTORY launch config rviz scripts DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(rmf_utils REQUIRED) find_package(ament_cmake_uncrustify REQUIRED) From c4482d77b49c587d74ead3a9cfd445cdc13ed6fd Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Sat, 20 Dec 2025 00:18:25 +0800 Subject: [PATCH 07/17] remove pkill workaround Signed-off-by: Aaron Chong --- nexus_demos/test_invalid_place_on_conveyor.py | 9 --------- nexus_demos/test_parallel_duplicated_wo.py | 9 --------- nexus_demos/test_parallel_pick_and_place_rmf.py.disabled | 9 --------- nexus_demos/test_parallel_wo.py | 9 --------- nexus_demos/test_pick_and_place.py | 9 --------- nexus_demos/test_pick_and_place_rmf.py | 9 --------- 6 files changed, 54 deletions(-) diff --git a/nexus_demos/test_invalid_place_on_conveyor.py b/nexus_demos/test_invalid_place_on_conveyor.py index 4a42a34a..c7a6e503 100644 --- a/nexus_demos/test_invalid_place_on_conveyor.py +++ b/nexus_demos/test_invalid_place_on_conveyor.py @@ -30,15 +30,6 @@ class InvalidPlaceOnConveyorTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ("ros2", "launch", "nexus_demos", "launch.py", "workcell_1_remap_task_types:='invalid_place_on_conveyor: [place_on_conveyor]'"), ) diff --git a/nexus_demos/test_parallel_duplicated_wo.py b/nexus_demos/test_parallel_duplicated_wo.py index 1943b5c3..c26e50c6 100644 --- a/nexus_demos/test_parallel_duplicated_wo.py +++ b/nexus_demos/test_parallel_duplicated_wo.py @@ -28,15 +28,6 @@ class ParallelWoTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ("ros2", "launch", "nexus_demos", "launch.py"), ) diff --git a/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled b/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled index b98a54a1..b2c0cb4a 100644 --- a/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled +++ b/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled @@ -30,15 +30,6 @@ import subprocess class ParallelPickAndPlaceRmfTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ( "ros2", diff --git a/nexus_demos/test_parallel_wo.py b/nexus_demos/test_parallel_wo.py index dd8de44f..9cafb6e8 100644 --- a/nexus_demos/test_parallel_wo.py +++ b/nexus_demos/test_parallel_wo.py @@ -28,15 +28,6 @@ class ParallelWoTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ("ros2", "launch", "nexus_demos", "launch.py"), ) diff --git a/nexus_demos/test_pick_and_place.py b/nexus_demos/test_pick_and_place.py index 11d135e1..5a0fe084 100644 --- a/nexus_demos/test_pick_and_place.py +++ b/nexus_demos/test_pick_and_place.py @@ -30,15 +30,6 @@ class PickAndPlaceTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ("ros2", "launch", "nexus_demos", "launch.py"), ) diff --git a/nexus_demos/test_pick_and_place_rmf.py b/nexus_demos/test_pick_and_place_rmf.py index dbc2e100..4ddbdc0a 100644 --- a/nexus_demos/test_pick_and_place_rmf.py +++ b/nexus_demos/test_pick_and_place_rmf.py @@ -30,15 +30,6 @@ class PickAndPlaceRMFTest(NexusTestCase): @RosTestCase.timeout(60) async def asyncSetUp(self): - # todo(YV): Find a better fix to the problem below. - # zenoh-bridge was bumped to 0.72 as part of the upgrade to - # ROS 2 Iron. However with this upgrade, the bridge does not clearly - # terminate when a SIGINT is received leaving behind zombie bridge - # processes from previous test cases. As a result, workcell registration - # fails for this testcase due to multiple bridges remaining active. - # Hence we explicitly kill any zenoh processes before launching the test. - subprocess.Popen('pkill -9 -f zenoh', shell=True) - self.proc = managed_process( ( "ros2", From b925f26392dc49c7a45254ad02bd080516b68f97 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 09:34:52 +0800 Subject: [PATCH 08/17] test without rmf Signed-off-by: Aaron Chong --- ..._pick_and_place_rmf.py => test_pick_and_place_rmf.py.disabled} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename nexus_demos/{test_pick_and_place_rmf.py => test_pick_and_place_rmf.py.disabled} (100%) diff --git a/nexus_demos/test_pick_and_place_rmf.py b/nexus_demos/test_pick_and_place_rmf.py.disabled similarity index 100% rename from nexus_demos/test_pick_and_place_rmf.py rename to nexus_demos/test_pick_and_place_rmf.py.disabled From 4f9a43499ce757a2a0217fe67d7766a332b8f340 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 10:13:56 +0800 Subject: [PATCH 09/17] revert RMF test Signed-off-by: Aaron Chong --- ..._pick_and_place_rmf.py.disabled => test_pick_and_place_rmf.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename nexus_demos/{test_pick_and_place_rmf.py.disabled => test_pick_and_place_rmf.py} (100%) diff --git a/nexus_demos/test_pick_and_place_rmf.py.disabled b/nexus_demos/test_pick_and_place_rmf.py similarity index 100% rename from nexus_demos/test_pick_and_place_rmf.py.disabled rename to nexus_demos/test_pick_and_place_rmf.py From f4b9114a52d20fcc95bf498f3406f604ebe66a9d Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 10:14:36 +0800 Subject: [PATCH 10/17] change all executors to events executors Signed-off-by: Aaron Chong --- nexus_calibration/CMakeLists.txt | 2 +- nexus_demos/CMakeLists.txt | 8 ++++---- nexus_system_orchestrator/CMakeLists.txt | 1 + nexus_transporter/CMakeLists.txt | 2 +- nexus_workcell_orchestrator/CMakeLists.txt | 1 + 5 files changed, 8 insertions(+), 6 deletions(-) diff --git a/nexus_calibration/CMakeLists.txt b/nexus_calibration/CMakeLists.txt index 760e9277..910fbb19 100644 --- a/nexus_calibration/CMakeLists.txt +++ b/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) #=============================================================================== diff --git a/nexus_demos/CMakeLists.txt b/nexus_demos/CMakeLists.txt index 7277bfc4..e381b4f4 100644 --- a/nexus_demos/CMakeLists.txt +++ b/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 diff --git a/nexus_system_orchestrator/CMakeLists.txt b/nexus_system_orchestrator/CMakeLists.txt index fb152bf5..c7d2f993 100644 --- a/nexus_system_orchestrator/CMakeLists.txt +++ b/nexus_system_orchestrator/CMakeLists.txt @@ -49,6 +49,7 @@ target_compile_features(${PROJECT_NAME}_plugin PRIVATE cxx_std_17) rclcpp_components_register_node(${PROJECT_NAME}_plugin PLUGIN "nexus::system_orchestrator::SystemOrchestrator" EXECUTABLE nexus_system_orchestrator + EXECUTOR EventsExecutor ) install(TARGETS ${PROJECT_NAME}_plugin DESTINATION lib) diff --git a/nexus_transporter/CMakeLists.txt b/nexus_transporter/CMakeLists.txt index 2750ce92..22f5a3c7 100644 --- a/nexus_transporter/CMakeLists.txt +++ b/nexus_transporter/CMakeLists.txt @@ -106,7 +106,7 @@ target_compile_features(nexus_transporter_component INTERFACE cxx_std_17) rclcpp_components_register_node(nexus_transporter_component PLUGIN "nexus_transporter::TransporterNode" EXECUTABLE nexus_transporter_node - EXECUTOR SingleThreadedExecutor) + EXECUTOR EventsExecutor) #=============================================================================== # Builtin transporter plugins diff --git a/nexus_workcell_orchestrator/CMakeLists.txt b/nexus_workcell_orchestrator/CMakeLists.txt index 0f687425..8e030c22 100644 --- a/nexus_workcell_orchestrator/CMakeLists.txt +++ b/nexus_workcell_orchestrator/CMakeLists.txt @@ -70,6 +70,7 @@ target_compile_definitions(${PROJECT_NAME}_plugin PUBLIC "COMPOSITION_BUILDING_D rclcpp_components_register_node(${PROJECT_NAME}_plugin PLUGIN "nexus::workcell_orchestrator::WorkcellOrchestrator" EXECUTABLE nexus_workcell_orchestrator + EXECUTOR EventsExecutor ) install(TARGETS ${PROJECT_NAME}_plugin DESTINATION lib) From 75d2270e0a3279c1e3ff6721e6786f381a895fe0 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 11:06:26 +0800 Subject: [PATCH 11/17] rename bridge to connection router Signed-off-by: Aaron Chong --- nexus_demos/README.md | 4 +- .../workcell_1_connection_router_config.json5 | 110 ++++++++++++++++++ .../workcell_2_connection_router_config.json5 | 110 ++++++++++++++++++ nexus_demos/launch/launch.py | 12 +- 4 files changed, 228 insertions(+), 8 deletions(-) create mode 100644 nexus_demos/config/zenoh/workcell_1_connection_router_config.json5 create mode 100644 nexus_demos/config/zenoh/workcell_2_connection_router_config.json5 diff --git a/nexus_demos/README.md b/nexus_demos/README.md index 39b54033..cfc315dc 100644 --- a/nexus_demos/README.md +++ b/nexus_demos/README.md @@ -44,7 +44,7 @@ ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_1 support_packa Start the bridge router, ```bash -ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_1_bridge_config.json5 +ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_1_connection_router_config.json5 ``` ### IRB1300 Workcell @@ -55,7 +55,7 @@ ros2 launch nexus_demos workcell.launch.py workcell_id:=workcell_2 support_packa Start the bridge router, ```bash -ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_2_bridge_config.json5 +ros2 launch nexus_demos zenoh_router.launch.py zenoh_router_config_filename:=config/zenoh/workcell_2_connection_router_config.json5 ``` ## Submit a job diff --git a/nexus_demos/config/zenoh/workcell_1_connection_router_config.json5 b/nexus_demos/config/zenoh/workcell_1_connection_router_config.json5 new file mode 100644 index 00000000..d8ab4c42 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_1_connection_router_config.json5 @@ -0,0 +1,110 @@ +{ + mode: "router", + connect: { + endpoints: [ + "tcp/localhost:7447", + "tcp/localhost:7448", + ], + }, + listen: { + endpoints: [], + }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + 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"], + }, + ] + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/config/zenoh/workcell_2_connection_router_config.json5 b/nexus_demos/config/zenoh/workcell_2_connection_router_config.json5 new file mode 100644 index 00000000..92579282 --- /dev/null +++ b/nexus_demos/config/zenoh/workcell_2_connection_router_config.json5 @@ -0,0 +1,110 @@ +{ + mode: "router", + connect: { + endpoints: [ + "tcp/localhost:7447", + "tcp/localhost:7449", + ], + }, + listen: { + endpoints: [], + }, + scouting: { + multicast: { + enabled: false, + }, + gossip: { + enabled: true, + multihop: false, + }, + }, + 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"], + }, + ] + }, + timestamping: { + enabled: { router: true, peer: true, client: true }, + drop_future_timestamp: false, + }, +} diff --git a/nexus_demos/launch/launch.py b/nexus_demos/launch/launch.py index fe752a91..565d298e 100644 --- a/nexus_demos/launch/launch.py +++ b/nexus_demos/launch/launch.py @@ -154,7 +154,7 @@ def launch_setup(context, *args, **kwargs): ], ) - launch_workcell_1_bridge_router = GroupAction( + launch_workcell_1_connection_router_router = GroupAction( [ IncludeLaunchDescription( [ @@ -168,7 +168,7 @@ def launch_setup(context, *args, **kwargs): ], launch_arguments={ "zenoh_config_package": "nexus_demos", - "zenoh_router_config_filename": "config/zenoh/workcell_1_bridge_config.json5", + "zenoh_router_config_filename": "config/zenoh/workcell_1_connection_router_config.json5", "ros_domain_id": str(workcell_1_domain_id), }.items(), condition=IfCondition(run_workcell_1), @@ -222,7 +222,7 @@ def launch_setup(context, *args, **kwargs): ], ) - launch_workcell_2_bridge_router = GroupAction( + launch_workcell_2_connection_router_router = GroupAction( [ IncludeLaunchDescription( [ @@ -236,7 +236,7 @@ def launch_setup(context, *args, **kwargs): ], launch_arguments={ "zenoh_config_package": "nexus_demos", - "zenoh_router_config_filename": "config/zenoh/workcell_2_bridge_config.json5", + "zenoh_router_config_filename": "config/zenoh/workcell_2_connection_router_config.json5", "ros_domain_id": str(workcell_2_domain_id), }.items(), condition=IfCondition(run_workcell_2), @@ -248,9 +248,9 @@ def launch_setup(context, *args, **kwargs): LogInfo(msg=log_msg), launch_inter_workcell, launch_workcell_1, - launch_workcell_1_bridge_router, + launch_workcell_1_connection_router_router, launch_workcell_2, - launch_workcell_2_bridge_router, + launch_workcell_2_connection_router_router, ] From 1e8dc3b95843e70c4edd3e8ed998559893c3d045 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 11:15:08 +0800 Subject: [PATCH 12/17] remove ros domain id manipulation Signed-off-by: Aaron Chong --- nexus_demos/launch/inter_workcell.launch.py | 8 ------ nexus_demos/launch/launch.py | 24 ----------------- nexus_demos/launch/workcell.launch.py | 15 +++-------- nexus_demos/launch/workcell_1_tf.launch.py | 7 ----- nexus_demos/launch/workcell_2_tf.launch.py | 7 ----- nexus_demos/launch/zenoh_router.launch.py | 29 ++++----------------- 6 files changed, 8 insertions(+), 82 deletions(-) diff --git a/nexus_demos/launch/inter_workcell.launch.py b/nexus_demos/launch/inter_workcell.launch.py index 6bb303e6..d29d1c36 100644 --- a/nexus_demos/launch/inter_workcell.launch.py +++ b/nexus_demos/launch/inter_workcell.launch.py @@ -108,7 +108,6 @@ def activate_node(target_node: LifecycleNode, depend_node: LifecycleNode = None) def launch_setup(context, *args, **kwargs): - ros_domain_id = LaunchConfiguration("ros_domain_id") use_fake_hardware = LaunchConfiguration("use_fake_hardware") use_multiple_transporters = LaunchConfiguration("use_multiple_transporters") zenoh_config_package = LaunchConfiguration("zenoh_config_package") @@ -245,7 +244,6 @@ def launch_setup(context, *args, **kwargs): 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(), ) ] @@ -259,7 +257,6 @@ def launch_setup(context, *args, **kwargs): ) return [ - SetEnvironmentVariable("ROS_DOMAIN_ID", ros_domain_id), zenoh_router, SetEnvironmentVariable( "ZENOH_SESSION_CONFIG_URI", @@ -288,11 +285,6 @@ def generate_launch_description(): return launch.LaunchDescription( [ - DeclareLaunchArgument( - "ros_domain_id", - default_value="0", - description="ROS_DOMAIN_ID environment variable", - ), DeclareLaunchArgument( "use_fake_hardware", default_value="true", diff --git a/nexus_demos/launch/launch.py b/nexus_demos/launch/launch.py index 565d298e..d959833d 100644 --- a/nexus_demos/launch/launch.py +++ b/nexus_demos/launch/launch.py @@ -51,27 +51,8 @@ def launch_setup(context, *args, **kwargs): run_workcell_2 = LaunchConfiguration("run_workcell_2") workcell_2_remap_task_types = LaunchConfiguration("workcell_2_remap_task_types") - inter_workcell_domain_id = 0 - workcell_1_domain_id = 0 - workcell_2_domain_id = 0 log_msg = "" - if "ROS_DOMAIN_ID" in os.environ: - inter_workcell_domain_id = int(os.environ["ROS_DOMAIN_ID"]) - workcell_1_domain_id = int(os.environ["ROS_DOMAIN_ID"]) - workcell_2_domain_id = int(os.environ["ROS_DOMAIN_ID"]) - if not 0 < inter_workcell_domain_id < 230: - log_msg += ( - "ROS_DOMAIN_ID not within the range of 0 to 230, setting it to 0. \n" - ) - inter_workcell_domain_id = 0 - - 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" - if run_workcell_2.perform(context).lower() == "true": - log_msg += f"Workcell 2 has ROS_DOMAIN_ID {workcell_2_domain_id}\n" - main_bt_filename = "main.xml" remap_task_types = """{ pick_and_place: [place_on_amr, place_on_conveyor, pick_from_amr, pick_from_conveyor], @@ -93,7 +74,6 @@ def launch_setup(context, *args, **kwargs): ) ], launch_arguments={ - "ros_domain_id": str(inter_workcell_domain_id), "zenoh_config_package": "nexus_demos", "zenoh_router_config_filename": "config/zenoh/inter_workcell_router_config.json5", "zenoh_session_config_filename": "config/zenoh/inter_workcell_session_config.json5", @@ -129,7 +109,6 @@ def launch_setup(context, *args, **kwargs): "remap_task_types": workcell_1_remap_task_types, "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "max_jobs": max_workcell_jobs, - "ros_domain_id": str(workcell_1_domain_id), "headless": headless, "controller_config_package": "nexus_demos", "planner_config_package": "nexus_demos", @@ -169,7 +148,6 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "zenoh_config_package": "nexus_demos", "zenoh_router_config_filename": "config/zenoh/workcell_1_connection_router_config.json5", - "ros_domain_id": str(workcell_1_domain_id), }.items(), condition=IfCondition(run_workcell_1), ) @@ -197,7 +175,6 @@ def launch_setup(context, *args, **kwargs): "remap_task_types": workcell_2_remap_task_types, "task_checker_plugin": "nexus::task_checkers::FilepathChecker", "max_jobs": max_workcell_jobs, - "ros_domain_id": str(workcell_2_domain_id), "headless": headless, "controller_config_package": "nexus_demos", "planner_config_package": "nexus_demos", @@ -237,7 +214,6 @@ def launch_setup(context, *args, **kwargs): launch_arguments={ "zenoh_config_package": "nexus_demos", "zenoh_router_config_filename": "config/zenoh/workcell_2_connection_router_config.json5", - "ros_domain_id": str(workcell_2_domain_id), }.items(), condition=IfCondition(run_workcell_2), ) diff --git a/nexus_demos/launch/workcell.launch.py b/nexus_demos/launch/workcell.launch.py index 1183e7a4..62ada2e5 100644 --- a/nexus_demos/launch/workcell.launch.py +++ b/nexus_demos/launch/workcell.launch.py @@ -37,14 +37,13 @@ from typing import List -def activate_node_service(node_name, ros_domain_id): +def activate_node_service(node_name): activate_node_proc = ExecuteProcess( cmd=[ 'python3', [FindPackageShare('nexus_demos'), "/scripts/activate_node.py"], node_name, - ], - additional_env={'ROS_DOMAIN_ID': ros_domain_id}, + ] ) def check_activate_return_code(event, _): @@ -77,7 +76,6 @@ def launch_setup(context, *args, **kwargs): bt_path = LaunchConfiguration("bt_path") task_checker_plugin = LaunchConfiguration("task_checker_plugin") max_jobs = LaunchConfiguration("max_jobs") - ros_domain_id = LaunchConfiguration("ros_domain_id") headless = LaunchConfiguration("headless") controller_config_package = LaunchConfiguration("controller_config_package") planner_config_package = LaunchConfiguration("planner_config_package") @@ -198,14 +196,12 @@ def launch_setup(context, *args, **kwargs): 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(), ) ] ) return [ - SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), zenoh_router, SetEnvironmentVariable( "ZENOH_SESSION_CONFIG_URI", @@ -282,7 +278,7 @@ def launch_setup(context, *args, **kwargs): ) ] ), - activate_node_service("motion_planner_server", ros_domain_id.perform(context)), + activate_node_service("motion_planner_server"), ] @@ -307,11 +303,6 @@ def generate_launch_description(): default_value="1", description="Maximum number of parallel jobs that this workcell is allowed to handle.", ), - DeclareLaunchArgument( - "ros_domain_id", - default_value="0", - description="ROS_DOMAIN_ID environment variable", - ), DeclareLaunchArgument( "headless", default_value="true", diff --git a/nexus_demos/launch/workcell_1_tf.launch.py b/nexus_demos/launch/workcell_1_tf.launch.py index 034c206b..1e163a47 100644 --- a/nexus_demos/launch/workcell_1_tf.launch.py +++ b/nexus_demos/launch/workcell_1_tf.launch.py @@ -23,7 +23,6 @@ def launch_setup(context, *args, **kwargs): """Publish static TFs. """ - ros_domain_id = LaunchConfiguration("ros_domain_id") zenoh_config_package = LaunchConfiguration("zenoh_config_package") zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") @@ -155,7 +154,6 @@ def launch_setup(context, *args, **kwargs): ) return [ - SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), SetEnvironmentVariable( "ZENOH_SESSION_CONFIG_URI", PathJoinSubstitution( @@ -175,11 +173,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( [ - DeclareLaunchArgument( - "ros_domain_id", - default_value="0", - description="ROS_DOMAIN_ID environment variable", - ), DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", diff --git a/nexus_demos/launch/workcell_2_tf.launch.py b/nexus_demos/launch/workcell_2_tf.launch.py index 7a82944e..ebe88ad2 100644 --- a/nexus_demos/launch/workcell_2_tf.launch.py +++ b/nexus_demos/launch/workcell_2_tf.launch.py @@ -23,7 +23,6 @@ def launch_setup(context, *args, **kwargs): """Publish static TFs. """ - ros_domain_id = LaunchConfiguration("ros_domain_id") zenoh_config_package = LaunchConfiguration("zenoh_config_package") zenoh_session_config_filename = LaunchConfiguration("zenoh_session_config_filename") @@ -157,7 +156,6 @@ def launch_setup(context, *args, **kwargs): ) return [ - SetEnvironmentVariable('ROS_DOMAIN_ID', ros_domain_id), SetEnvironmentVariable( "ZENOH_SESSION_CONFIG_URI", PathJoinSubstitution( @@ -177,11 +175,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): return LaunchDescription( [ - DeclareLaunchArgument( - "ros_domain_id", - default_value="0", - description="ROS_DOMAIN_ID environment variable", - ), DeclareLaunchArgument( name="zenoh_config_package", default_value="nexus_demos", diff --git a/nexus_demos/launch/zenoh_router.launch.py b/nexus_demos/launch/zenoh_router.launch.py index 3a371e27..c44ed3bb 100644 --- a/nexus_demos/launch/zenoh_router.launch.py +++ b/nexus_demos/launch/zenoh_router.launch.py @@ -29,7 +29,6 @@ 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") @@ -45,7 +44,6 @@ def launch_setup(context, *args, **kwargs): 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 @@ -57,40 +55,23 @@ def launch_setup(context, *args, **kwargs): 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( + declared_arguments = [ DeclareLaunchArgument( name="log_level", default_value="zenoh=debug", description="Log level of zenoh router", - ) - ) - - 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)] ) From 1717d5115f1bb5912a2860d374f173283ce59a38 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 12:57:39 +0800 Subject: [PATCH 13/17] disable rmf test Signed-off-by: Aaron Chong --- ..._pick_and_place_rmf.py => test_pick_and_place_rmf.py.disabled} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename nexus_demos/{test_pick_and_place_rmf.py => test_pick_and_place_rmf.py.disabled} (100%) diff --git a/nexus_demos/test_pick_and_place_rmf.py b/nexus_demos/test_pick_and_place_rmf.py.disabled similarity index 100% rename from nexus_demos/test_pick_and_place_rmf.py rename to nexus_demos/test_pick_and_place_rmf.py.disabled From 9b6a647ea6478ef210df86b6019e81d113939e97 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 13:29:00 +0800 Subject: [PATCH 14/17] isolate rmf test, set unit test to use cyclone Signed-off-by: Aaron Chong --- .../workflows/nexus_integration_tests.yaml | 2 +- nexus_demos/test_invalid_place_on_conveyor.py | 78 ----------- nexus_demos/test_parallel_duplicated_wo.py | 81 ----------- ...st_parallel_pick_and_place_rmf.py.disabled | 126 ------------------ nexus_demos/test_parallel_wo.py | 81 ----------- nexus_demos/test_pick_and_place.py | 91 ------------- ...py.disabled => test_pick_and_place_rmf.py} | 0 7 files changed, 1 insertion(+), 458 deletions(-) delete mode 100644 nexus_demos/test_invalid_place_on_conveyor.py delete mode 100644 nexus_demos/test_parallel_duplicated_wo.py delete mode 100644 nexus_demos/test_parallel_pick_and_place_rmf.py.disabled delete mode 100644 nexus_demos/test_parallel_wo.py delete mode 100644 nexus_demos/test_pick_and_place.py rename nexus_demos/{test_pick_and_place_rmf.py.disabled => test_pick_and_place_rmf.py} (100%) diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index b4eeaa5e..7ea98796 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -37,6 +37,6 @@ jobs: - name: build run: /ros_entrypoint.sh colcon build --packages-up-to nexus_calibration nexus_gazebo nexus_demos nexus_motion_planner --mixin release - name: Test - Unit Tests - run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_zenoh_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ + 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_zenoh_cpp /ros_entrypoint.sh python3 -m unittest diff --git a/nexus_demos/test_invalid_place_on_conveyor.py b/nexus_demos/test_invalid_place_on_conveyor.py deleted file mode 100644 index c7a6e503..00000000 --- a/nexus_demos/test_invalid_place_on_conveyor.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright (C) 2025 Open Source Robotics Foundation -# -# 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. - -import os -import sys -from typing import cast - -from action_msgs.msg import GoalStatus -from managed_process import managed_process -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_orchestrator_msgs.msg import TaskState -from nexus_test_case import NexusTestCase -from rclpy import Future -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle, GoalStatus -from ros_testcase import RosTestCase -import subprocess - -class InvalidPlaceOnConveyorTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - self.proc = managed_process( - ("ros2", "launch", "nexus_demos", "launch.py", "workcell_1_remap_task_types:='invalid_place_on_conveyor: [place_on_conveyor]'"), - ) - self.proc.__enter__() - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2") - print("all workcells are ready") - await self.wait_for_transporters("mock_transporter_node") - print("all transporters are ready") - - # give some time for discovery to happen - await self.ros_sleep(5) - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(600) # 10min - async def test_abort_invalid_place_on_conveyor_wo(self): - self.action_client.wait_for_server() - goal_msg = ExecuteWorkOrder.Goal() - goal_msg.order.work_order_id = "1" - with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: - goal_msg.order.work_order = f.read() - feedbacks: list[ExecuteWorkOrder.Feedback] = [] - fb_fut = Future() - - def on_fb(msg): - feedbacks.append(msg.feedback) - if len(feedbacks) >= 5: - fb_fut.set_result(None) - - goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) - ) - self.assertTrue(goal_handle.accepted) - - results = await goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_ABORTED) diff --git a/nexus_demos/test_parallel_duplicated_wo.py b/nexus_demos/test_parallel_duplicated_wo.py deleted file mode 100644 index c26e50c6..00000000 --- a/nexus_demos/test_parallel_duplicated_wo.py +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright (C) 2025 Open Source Robotics Foundation -# -# 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. - -import os -import sys -from typing import cast - -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_test_case import NexusTestCase -from managed_process import managed_process -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle -from ros_testcase import RosTestCase -import subprocess - - -class ParallelWoTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - self.proc = managed_process( - ("ros2", "launch", "nexus_demos", "launch.py"), - ) - self.proc.__enter__() - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2") - print("all workcells are ready") - await self.wait_for_transporters("mock_transporter_node") - print("all transporters are ready") - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - self.action_client.wait_for_server() - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(180) # 3min - async def test_reject_jobs_over_max(self): - """ - New jobs should be rejected when the max number of jobs is already executing. - """ - goal_msg = ExecuteWorkOrder.Goal() - goal_msg.order.work_order_id = "1" - with open(f"{os.path.dirname(__file__)}/config/pick_and_place_conveyor.json") as f: - goal_msg.order.work_order = f.read() - goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg) - ) - self.assertTrue(goal_handle.accepted) - - goal_msg_2 = ExecuteWorkOrder.Goal() - goal_msg_2.order.work_order_id = "2" - with open(f"{os.path.dirname(__file__)}/config/pick_and_place_conveyor.json") as f: - goal_msg_2.order.work_order = f.read() - goal_handle_2 = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_2) - ) - self.assertTrue(goal_handle_2.accepted) - - goal_msg_3 = goal_msg - goal_msg_3.order.work_order_id = "3" - goal_handle_3 = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3) - ) - self.assertFalse(goal_handle_3.accepted) diff --git a/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled b/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled deleted file mode 100644 index b2c0cb4a..00000000 --- a/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled +++ /dev/null @@ -1,126 +0,0 @@ -# Copyright (C) 2025 Open Source Robotics Foundation -# -# 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. - -import os -import sys -from typing import cast - -from action_msgs.msg import GoalStatus -from managed_process import managed_process -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_orchestrator_msgs.msg import TaskState -from nexus_test_case import NexusTestCase -from rclpy import Future -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle, GoalStatus -from ros_testcase import RosTestCase -import subprocess - -class ParallelPickAndPlaceRmfTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - self.proc = managed_process( - ( - "ros2", - "launch", - "nexus_demos", - "launch.py", - "sim_update_rate:=10000", - "use_multiple_transporters:=true" - ), - ) - self.proc.__enter__() - - # give some time for discovery to happen - await self.ros_sleep(5) - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(600) # 5min - async def test_parallel_pick_and_place_wo(self): - # The waiting for nodes and workcells have been shifted to this test - # function to avoid a race condition that somehow occurs in this test - # suite. - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2", "rmf_nexus_transporter") - print("all workcells are ready") - await self.wait_for_robot_state() - print("AMRs are ready") - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - self.action_client.wait_for_server() - - goal_msg = ExecuteWorkOrder.Goal() - with open(f"{os.path.dirname(__file__)}/config/pick_and_place_amr.json") as f: - goal_msg.order.work_order = f.read() - - # First goal - goal_msg.order.work_order_id = "1" - first_feedbacks: list[ExecuteWorkOrder.Feedback] = [] - first_fb_fut = Future() - - def on_first_fb(msg): - first_feedbacks.append(msg.feedback) - if len(first_feedbacks) >= 5: - first_fb_fut.set_result(None) - - first_goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_first_fb) - ) - self.assertTrue(first_goal_handle.accepted) - - # Second goal - goal_msg.order.work_order_id = "2" - second_feedbacks: list[ExecuteWorkOrder.Feedback] = [] - second_fb_fut = Future() - - def on_second_fb(msg): - second_feedbacks.append(msg.feedback) - if len(second_feedbacks) >= 5: - second_fb_fut.set_result(None) - - second_goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_second_fb) - ) - self.assertTrue(second_goal_handle.accepted) - - # Third goal - goal_msg.order.work_order_id = "3" - third_feedbacks: list[ExecuteWorkOrder.Feedback] = [] - third_fb_fut = Future() - - def on_third_fb(msg): - third_feedbacks.append(msg.feedback) - if len(third_feedbacks) >= 5: - third_fb_fut.set_result(None) - - third_goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_third_fb) - ) - self.assertTrue(third_goal_handle.accepted) - - # Results - results = await first_goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) - results = await second_goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) - results = await third_goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) diff --git a/nexus_demos/test_parallel_wo.py b/nexus_demos/test_parallel_wo.py deleted file mode 100644 index 9cafb6e8..00000000 --- a/nexus_demos/test_parallel_wo.py +++ /dev/null @@ -1,81 +0,0 @@ -# Copyright (C) 2022 Johnson & Johnson -# -# 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. - -import os -import sys -from typing import cast - -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_test_case import NexusTestCase -from managed_process import managed_process -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle -from ros_testcase import RosTestCase -import subprocess - - -class ParallelWoTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - self.proc = managed_process( - ("ros2", "launch", "nexus_demos", "launch.py"), - ) - self.proc.__enter__() - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2") - print("all workcells are ready") - await self.wait_for_transporters("mock_transporter_node") - print("all transporters are ready") - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - self.action_client.wait_for_server() - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(180) # 3min - async def test_reject_jobs_over_max(self): - """ - New jobs should be rejected when the max number of jobs is already executing. - """ - goal_msg = ExecuteWorkOrder.Goal() - goal_msg.order.work_order_id = "1" - with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: - goal_msg.order.work_order = f.read() - goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg) - ) - self.assertTrue(goal_handle.accepted) - - goal_msg_2 = ExecuteWorkOrder.Goal() - goal_msg_2.order.work_order_id = "2" - with open(f"{os.path.dirname(__file__)}/config/pick_from_conveyor.json") as f: - goal_msg_2.order.work_order = f.read() - goal_handle_2 = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_2) - ) - self.assertTrue(goal_handle_2.accepted) - - goal_msg_3 = goal_msg - goal_msg_3.order.work_order_id = "3" - goal_handle_3 = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3) - ) - self.assertFalse(goal_handle_3.accepted) diff --git a/nexus_demos/test_pick_and_place.py b/nexus_demos/test_pick_and_place.py deleted file mode 100644 index 5a0fe084..00000000 --- a/nexus_demos/test_pick_and_place.py +++ /dev/null @@ -1,91 +0,0 @@ -# Copyright (C) 2022 Johnson & Johnson -# -# 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. - -import os -import sys -from typing import cast - -from action_msgs.msg import GoalStatus -from managed_process import managed_process -from nexus_orchestrator_msgs.action import ExecuteWorkOrder -from nexus_orchestrator_msgs.msg import TaskState -from nexus_test_case import NexusTestCase -from rclpy import Future -from rclpy.action import ActionClient -from rclpy.action.client import ClientGoalHandle, GoalStatus -from ros_testcase import RosTestCase -import subprocess - -class PickAndPlaceTest(NexusTestCase): - @RosTestCase.timeout(60) - async def asyncSetUp(self): - self.proc = managed_process( - ("ros2", "launch", "nexus_demos", "launch.py"), - ) - self.proc.__enter__() - print("waiting for nodes to be ready...", file=sys.stderr) - self.wait_for_nodes("system_orchestrator") - await self.wait_for_lifecycle_active("system_orchestrator") - - await self.wait_for_workcells("workcell_1", "workcell_2") - print("all workcells are ready") - await self.wait_for_transporters("mock_transporter_node") - print("all transporters are ready") - - # give some time for discovery to happen - await self.ros_sleep(5) - - # create action client to send work order - self.action_client = ActionClient( - self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" - ) - - def tearDown(self): - self.proc.__exit__(None, None, None) - - @RosTestCase.timeout(600) # 10min - async def test_pick_and_place_wo(self): - self.action_client.wait_for_server() - goal_msg = ExecuteWorkOrder.Goal() - goal_msg.order.work_order_id = "1" - with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: - goal_msg.order.work_order = f.read() - feedbacks: list[ExecuteWorkOrder.Feedback] = [] - fb_fut = Future() - - def on_fb(msg): - feedbacks.append(msg.feedback) - if len(feedbacks) >= 5: - fb_fut.set_result(None) - - goal_handle = cast( - ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) - ) - self.assertTrue(goal_handle.accepted) - - results = await goal_handle.get_result_async() - self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) - - # check that we receive the correct feedbacks - # FIXME(koonpeng): First few feedbacks are sometimes missed when the system in under - # high load so we only check the last feedback as a workaround. - self.assertGreater(len(feedbacks), 0) - for msg in feedbacks: - self.assertEqual(len(msg.task_states), 1) - state: TaskState = msg.task_states[0] # type: ignore - self.assertEqual(state.workcell_id, "workcell_1") - self.assertEqual(state.task_id,"1/place_on_conveyor/0") - - state: TaskState = feedbacks[-1].task_states[0] # type: ignore - self.assertEqual(state.status, TaskState.STATUS_FINISHED) diff --git a/nexus_demos/test_pick_and_place_rmf.py.disabled b/nexus_demos/test_pick_and_place_rmf.py similarity index 100% rename from nexus_demos/test_pick_and_place_rmf.py.disabled rename to nexus_demos/test_pick_and_place_rmf.py From f8d6578f8073283dcc26f05f0aedd3ef1f61cc2c Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Mon, 22 Dec 2025 13:49:59 +0800 Subject: [PATCH 15/17] revert tests Signed-off-by: Aaron Chong --- nexus_demos/test_invalid_place_on_conveyor.py | 78 +++++++++++ nexus_demos/test_parallel_duplicated_wo.py | 81 +++++++++++ ...st_parallel_pick_and_place_rmf.py.disabled | 126 ++++++++++++++++++ nexus_demos/test_parallel_wo.py | 81 +++++++++++ nexus_demos/test_pick_and_place.py | 91 +++++++++++++ 5 files changed, 457 insertions(+) create mode 100644 nexus_demos/test_invalid_place_on_conveyor.py create mode 100644 nexus_demos/test_parallel_duplicated_wo.py create mode 100644 nexus_demos/test_parallel_pick_and_place_rmf.py.disabled create mode 100644 nexus_demos/test_parallel_wo.py create mode 100644 nexus_demos/test_pick_and_place.py diff --git a/nexus_demos/test_invalid_place_on_conveyor.py b/nexus_demos/test_invalid_place_on_conveyor.py new file mode 100644 index 00000000..c7a6e503 --- /dev/null +++ b/nexus_demos/test_invalid_place_on_conveyor.py @@ -0,0 +1,78 @@ +# Copyright (C) 2025 Open Source Robotics Foundation +# +# 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. + +import os +import sys +from typing import cast + +from action_msgs.msg import GoalStatus +from managed_process import managed_process +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_orchestrator_msgs.msg import TaskState +from nexus_test_case import NexusTestCase +from rclpy import Future +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle, GoalStatus +from ros_testcase import RosTestCase +import subprocess + +class InvalidPlaceOnConveyorTest(NexusTestCase): + @RosTestCase.timeout(60) + async def asyncSetUp(self): + self.proc = managed_process( + ("ros2", "launch", "nexus_demos", "launch.py", "workcell_1_remap_task_types:='invalid_place_on_conveyor: [place_on_conveyor]'"), + ) + self.proc.__enter__() + print("waiting for nodes to be ready...", file=sys.stderr) + self.wait_for_nodes("system_orchestrator") + await self.wait_for_lifecycle_active("system_orchestrator") + + await self.wait_for_workcells("workcell_1", "workcell_2") + print("all workcells are ready") + await self.wait_for_transporters("mock_transporter_node") + print("all transporters are ready") + + # give some time for discovery to happen + await self.ros_sleep(5) + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(600) # 10min + async def test_abort_invalid_place_on_conveyor_wo(self): + self.action_client.wait_for_server() + goal_msg = ExecuteWorkOrder.Goal() + goal_msg.order.work_order_id = "1" + with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: + goal_msg.order.work_order = f.read() + feedbacks: list[ExecuteWorkOrder.Feedback] = [] + fb_fut = Future() + + def on_fb(msg): + feedbacks.append(msg.feedback) + if len(feedbacks) >= 5: + fb_fut.set_result(None) + + goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) + ) + self.assertTrue(goal_handle.accepted) + + results = await goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_ABORTED) diff --git a/nexus_demos/test_parallel_duplicated_wo.py b/nexus_demos/test_parallel_duplicated_wo.py new file mode 100644 index 00000000..c26e50c6 --- /dev/null +++ b/nexus_demos/test_parallel_duplicated_wo.py @@ -0,0 +1,81 @@ +# Copyright (C) 2025 Open Source Robotics Foundation +# +# 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. + +import os +import sys +from typing import cast + +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_test_case import NexusTestCase +from managed_process import managed_process +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle +from ros_testcase import RosTestCase +import subprocess + + +class ParallelWoTest(NexusTestCase): + @RosTestCase.timeout(60) + async def asyncSetUp(self): + self.proc = managed_process( + ("ros2", "launch", "nexus_demos", "launch.py"), + ) + self.proc.__enter__() + print("waiting for nodes to be ready...", file=sys.stderr) + self.wait_for_nodes("system_orchestrator") + await self.wait_for_lifecycle_active("system_orchestrator") + + await self.wait_for_workcells("workcell_1", "workcell_2") + print("all workcells are ready") + await self.wait_for_transporters("mock_transporter_node") + print("all transporters are ready") + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + self.action_client.wait_for_server() + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(180) # 3min + async def test_reject_jobs_over_max(self): + """ + New jobs should be rejected when the max number of jobs is already executing. + """ + goal_msg = ExecuteWorkOrder.Goal() + goal_msg.order.work_order_id = "1" + with open(f"{os.path.dirname(__file__)}/config/pick_and_place_conveyor.json") as f: + goal_msg.order.work_order = f.read() + goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg) + ) + self.assertTrue(goal_handle.accepted) + + goal_msg_2 = ExecuteWorkOrder.Goal() + goal_msg_2.order.work_order_id = "2" + with open(f"{os.path.dirname(__file__)}/config/pick_and_place_conveyor.json") as f: + goal_msg_2.order.work_order = f.read() + goal_handle_2 = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_2) + ) + self.assertTrue(goal_handle_2.accepted) + + goal_msg_3 = goal_msg + goal_msg_3.order.work_order_id = "3" + goal_handle_3 = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3) + ) + self.assertFalse(goal_handle_3.accepted) diff --git a/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled b/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled new file mode 100644 index 00000000..b2c0cb4a --- /dev/null +++ b/nexus_demos/test_parallel_pick_and_place_rmf.py.disabled @@ -0,0 +1,126 @@ +# Copyright (C) 2025 Open Source Robotics Foundation +# +# 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. + +import os +import sys +from typing import cast + +from action_msgs.msg import GoalStatus +from managed_process import managed_process +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_orchestrator_msgs.msg import TaskState +from nexus_test_case import NexusTestCase +from rclpy import Future +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle, GoalStatus +from ros_testcase import RosTestCase +import subprocess + +class ParallelPickAndPlaceRmfTest(NexusTestCase): + @RosTestCase.timeout(60) + async def asyncSetUp(self): + self.proc = managed_process( + ( + "ros2", + "launch", + "nexus_demos", + "launch.py", + "sim_update_rate:=10000", + "use_multiple_transporters:=true" + ), + ) + self.proc.__enter__() + + # give some time for discovery to happen + await self.ros_sleep(5) + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(600) # 5min + async def test_parallel_pick_and_place_wo(self): + # The waiting for nodes and workcells have been shifted to this test + # function to avoid a race condition that somehow occurs in this test + # suite. + print("waiting for nodes to be ready...", file=sys.stderr) + self.wait_for_nodes("system_orchestrator") + await self.wait_for_lifecycle_active("system_orchestrator") + + await self.wait_for_workcells("workcell_1", "workcell_2", "rmf_nexus_transporter") + print("all workcells are ready") + await self.wait_for_robot_state() + print("AMRs are ready") + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + self.action_client.wait_for_server() + + goal_msg = ExecuteWorkOrder.Goal() + with open(f"{os.path.dirname(__file__)}/config/pick_and_place_amr.json") as f: + goal_msg.order.work_order = f.read() + + # First goal + goal_msg.order.work_order_id = "1" + first_feedbacks: list[ExecuteWorkOrder.Feedback] = [] + first_fb_fut = Future() + + def on_first_fb(msg): + first_feedbacks.append(msg.feedback) + if len(first_feedbacks) >= 5: + first_fb_fut.set_result(None) + + first_goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_first_fb) + ) + self.assertTrue(first_goal_handle.accepted) + + # Second goal + goal_msg.order.work_order_id = "2" + second_feedbacks: list[ExecuteWorkOrder.Feedback] = [] + second_fb_fut = Future() + + def on_second_fb(msg): + second_feedbacks.append(msg.feedback) + if len(second_feedbacks) >= 5: + second_fb_fut.set_result(None) + + second_goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_second_fb) + ) + self.assertTrue(second_goal_handle.accepted) + + # Third goal + goal_msg.order.work_order_id = "3" + third_feedbacks: list[ExecuteWorkOrder.Feedback] = [] + third_fb_fut = Future() + + def on_third_fb(msg): + third_feedbacks.append(msg.feedback) + if len(third_feedbacks) >= 5: + third_fb_fut.set_result(None) + + third_goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_third_fb) + ) + self.assertTrue(third_goal_handle.accepted) + + # Results + results = await first_goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) + results = await second_goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) + results = await third_goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) diff --git a/nexus_demos/test_parallel_wo.py b/nexus_demos/test_parallel_wo.py new file mode 100644 index 00000000..9cafb6e8 --- /dev/null +++ b/nexus_demos/test_parallel_wo.py @@ -0,0 +1,81 @@ +# Copyright (C) 2022 Johnson & Johnson +# +# 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. + +import os +import sys +from typing import cast + +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_test_case import NexusTestCase +from managed_process import managed_process +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle +from ros_testcase import RosTestCase +import subprocess + + +class ParallelWoTest(NexusTestCase): + @RosTestCase.timeout(60) + async def asyncSetUp(self): + self.proc = managed_process( + ("ros2", "launch", "nexus_demos", "launch.py"), + ) + self.proc.__enter__() + print("waiting for nodes to be ready...", file=sys.stderr) + self.wait_for_nodes("system_orchestrator") + await self.wait_for_lifecycle_active("system_orchestrator") + + await self.wait_for_workcells("workcell_1", "workcell_2") + print("all workcells are ready") + await self.wait_for_transporters("mock_transporter_node") + print("all transporters are ready") + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + self.action_client.wait_for_server() + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(180) # 3min + async def test_reject_jobs_over_max(self): + """ + New jobs should be rejected when the max number of jobs is already executing. + """ + goal_msg = ExecuteWorkOrder.Goal() + goal_msg.order.work_order_id = "1" + with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: + goal_msg.order.work_order = f.read() + goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg) + ) + self.assertTrue(goal_handle.accepted) + + goal_msg_2 = ExecuteWorkOrder.Goal() + goal_msg_2.order.work_order_id = "2" + with open(f"{os.path.dirname(__file__)}/config/pick_from_conveyor.json") as f: + goal_msg_2.order.work_order = f.read() + goal_handle_2 = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_2) + ) + self.assertTrue(goal_handle_2.accepted) + + goal_msg_3 = goal_msg + goal_msg_3.order.work_order_id = "3" + goal_handle_3 = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg_3) + ) + self.assertFalse(goal_handle_3.accepted) diff --git a/nexus_demos/test_pick_and_place.py b/nexus_demos/test_pick_and_place.py new file mode 100644 index 00000000..5a0fe084 --- /dev/null +++ b/nexus_demos/test_pick_and_place.py @@ -0,0 +1,91 @@ +# Copyright (C) 2022 Johnson & Johnson +# +# 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. + +import os +import sys +from typing import cast + +from action_msgs.msg import GoalStatus +from managed_process import managed_process +from nexus_orchestrator_msgs.action import ExecuteWorkOrder +from nexus_orchestrator_msgs.msg import TaskState +from nexus_test_case import NexusTestCase +from rclpy import Future +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle, GoalStatus +from ros_testcase import RosTestCase +import subprocess + +class PickAndPlaceTest(NexusTestCase): + @RosTestCase.timeout(60) + async def asyncSetUp(self): + self.proc = managed_process( + ("ros2", "launch", "nexus_demos", "launch.py"), + ) + self.proc.__enter__() + print("waiting for nodes to be ready...", file=sys.stderr) + self.wait_for_nodes("system_orchestrator") + await self.wait_for_lifecycle_active("system_orchestrator") + + await self.wait_for_workcells("workcell_1", "workcell_2") + print("all workcells are ready") + await self.wait_for_transporters("mock_transporter_node") + print("all transporters are ready") + + # give some time for discovery to happen + await self.ros_sleep(5) + + # create action client to send work order + self.action_client = ActionClient( + self.node, ExecuteWorkOrder, "/system_orchestrator/execute_order" + ) + + def tearDown(self): + self.proc.__exit__(None, None, None) + + @RosTestCase.timeout(600) # 10min + async def test_pick_and_place_wo(self): + self.action_client.wait_for_server() + goal_msg = ExecuteWorkOrder.Goal() + goal_msg.order.work_order_id = "1" + with open(f"{os.path.dirname(__file__)}/config/place_on_conveyor.json") as f: + goal_msg.order.work_order = f.read() + feedbacks: list[ExecuteWorkOrder.Feedback] = [] + fb_fut = Future() + + def on_fb(msg): + feedbacks.append(msg.feedback) + if len(feedbacks) >= 5: + fb_fut.set_result(None) + + goal_handle = cast( + ClientGoalHandle, await self.action_client.send_goal_async(goal_msg, on_fb) + ) + self.assertTrue(goal_handle.accepted) + + results = await goal_handle.get_result_async() + self.assertEqual(results.status, GoalStatus.STATUS_SUCCEEDED) + + # check that we receive the correct feedbacks + # FIXME(koonpeng): First few feedbacks are sometimes missed when the system in under + # high load so we only check the last feedback as a workaround. + self.assertGreater(len(feedbacks), 0) + for msg in feedbacks: + self.assertEqual(len(msg.task_states), 1) + state: TaskState = msg.task_states[0] # type: ignore + self.assertEqual(state.workcell_id, "workcell_1") + self.assertEqual(state.task_id,"1/place_on_conveyor/0") + + state: TaskState = feedbacks[-1].task_states[0] # type: ignore + self.assertEqual(state.status, TaskState.STATUS_FINISHED) From d3781ece729baa2504d79e0e5243d08e4296bfdc Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Tue, 23 Dec 2025 11:11:05 +0800 Subject: [PATCH 16/17] explicitly select tests, and run RMF on its own Signed-off-by: Aaron Chong --- .github/workflows/nexus_integration_tests.yaml | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index 7ea98796..f0089dcb 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.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_zenoh_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 From c653788f02fdbd01f51ae953bf9bca11a73661f7 Mon Sep 17 00:00:00 2001 From: Aaron Chong Date: Wed, 7 Jan 2026 11:00:07 +0800 Subject: [PATCH 17/17] remove nexus_network_configuration entirely Signed-off-by: Aaron Chong --- .github/workflows/style.yaml | 5 +- README.md | 6 - nexus_network_configuration/CHANGELOG.rst | 10 - nexus_network_configuration/README.md | 58 --- .../nexus_network_configuration/__init__.py | 0 .../nexus_network_configuration.py | 339 ------------------ nexus_network_configuration/package.xml | 16 - .../resource/nexus_network_configuration | 0 .../schemas/nexus_network_schema.json | 140 -------- .../scripts/set_up_network.sh | 65 ---- nexus_network_configuration/setup.cfg | 4 - nexus_network_configuration/setup.py | 32 -- 12 files changed, 2 insertions(+), 673 deletions(-) delete mode 100644 nexus_network_configuration/CHANGELOG.rst delete mode 100644 nexus_network_configuration/README.md delete mode 100644 nexus_network_configuration/nexus_network_configuration/__init__.py delete mode 100644 nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py delete mode 100644 nexus_network_configuration/package.xml delete mode 100644 nexus_network_configuration/resource/nexus_network_configuration delete mode 100644 nexus_network_configuration/schemas/nexus_network_schema.json delete mode 100755 nexus_network_configuration/scripts/set_up_network.sh delete mode 100644 nexus_network_configuration/setup.cfg delete mode 100644 nexus_network_configuration/setup.py diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml index 07b3bdb3..1e2baba3 100644 --- a/.github/workflows/style.yaml +++ b/.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: diff --git a/README.md b/README.md index c63764eb..892e58a0 100644 --- a/README.md +++ b/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 [deprecated] - -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 -r -o ` 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) diff --git a/nexus_network_configuration/CHANGELOG.rst b/nexus_network_configuration/CHANGELOG.rst deleted file mode 100644 index 67f2ff14..00000000 --- a/nexus_network_configuration/CHANGELOG.rst +++ /dev/null @@ -1,10 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package nexus_network_configuration -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.1 (2023-11-20) ------------------- - -0.1.0 (2023-11-06) ------------------- -* Provides scripts to generate Zenoh DDS Bridge configurations. diff --git a/nexus_network_configuration/README.md b/nexus_network_configuration/README.md deleted file mode 100644 index a4b1dfc0..00000000 --- a/nexus_network_configuration/README.md +++ /dev/null @@ -1,58 +0,0 @@ -# Network Config Generator - -> Note: As of December 2025, Nexus has migrated to using `rmw_zenoh_cpp` directly, hence `main` no longer contains any examples of usage. Please reference past commits. - -This package simplifies the Zenoh DDS Bridge setup for multiple NEXUS orchestrators through the generation of Zenoh bridge configurations from the following 2 files: -1. NEXUS Network Configuration: This YAML file describes the properties of different machines such as ROS Domain ID, Zenoh bridge TCP connection endpoints etc. Refer to [Quick Start](#quick-start) for an example -2. REDF Configuration: This YAML file describes the ROS Endpoints of the NEXUS orchestrators - -# 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. -``` -./scripts/set_up_network.sh -``` - -2. Create a NEXUS Network configuration. An example is provided in the `nexus_poc` package at [nexus_network_config.yaml](../nexus_poc/config/zenoh/nexus_network_config.yaml) Here we have 1 system orchestrator connected to 3 workcell orchestrators, all on different domain IDs from 14 to 17. -You can refer to [nexus_network_schema.json](schemas/nexus_network_schema.json) for the config schema. - -```yaml -# Mode of Zenoh bridge, specified as either 'client' or 'peer'. -# Use 'client' when there is an available Zenoh router, otherwise -# use 'peer' for a distributed system. -mode: peer -# When true, discovery info is forwarded to the remote plugins/bridges -forward_discovery: false -# When true, activates a REST API used to administer Zenoh Bridge configurations -enable_rest_api: true - -system_orchestrators: - - ros_namespace: system_orchestrator # ROS Namespace of the endpoints - # ROS Domain ID - domain_id: 14 - # Listening TCP Address of Zenoh bridge - tcp_listen: "localhost:7447" - # HTTP Port for the REST API - rest_api_http_port: 8000 - -workcell_orchestrators: - - ros_namespace: workcell_1 - domain_id: 15 - tcp_connect: "localhost:7447" - rest_api_http_port: 8001 - - - ros_namespace: workcell_2 - domain_id: 16 - tcp_connect: "localhost:7447" - rest_api_http_port: 8002 - - - ros_namespace: workcell_3 - domain_id: 17 - tcp_connect: "localhost:7447" - rest_api_http_port: 8003 -``` - -3. Generate zenoh bridge configurations from NEXUS Network config and REDF config. The `ZENOH_CONFIGS_OUTPUT_DIRECTORY` will be where your zenoh bridge configurations will be saved. -```bash -ros2 run nexus_network_configuration nexus_network_configuration -n -r -o -``` diff --git a/nexus_network_configuration/nexus_network_configuration/__init__.py b/nexus_network_configuration/nexus_network_configuration/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py b/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py deleted file mode 100644 index 7eb10649..00000000 --- a/nexus_network_configuration/nexus_network_configuration/nexus_network_configuration.py +++ /dev/null @@ -1,339 +0,0 @@ -# Copyright 2022 Johnson & Johnson -# -# 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. - -import argparse -import json -import re -import os -from jsonschema import validate, ValidationError -import sys -import yaml -from ament_index_python.packages import get_package_share_directory - - -class NEXUSConfigGenerator: - """NEXUSConfigGenerator generates Zenoh DDS Bridge configurations.""" - - def __init__( - self, nexus_net_cfg_filepath, - redf_cfg_filepath, schema_filepath): - """ - Construct NEXUSConfigGenerator. - - Parameters - ---------- - nexus_net_cfg_filepath : str - NEXUS network configuration filepath - redf_cfg_filepath : str - REDF configuration filepath - schema_filepath : str - NEXUS Network configuration schema filepath - - """ - self.nexus_net_cfg = self.parse_file(nexus_net_cfg_filepath) - self.redf_cfg = self.parse_file(redf_cfg_filepath) - - self.nexus_net_cfg_schema = self.parse_file(schema_filepath) - - # Set global variables - self.enable_rest_api = self.nexus_net_cfg["enable_rest_api"] - self.bridge_mode = self.nexus_net_cfg["mode"] - - self.zenoh_cfg_file_extension = "json5" - - def parse_file(self, filepath): - """ - Parse a configuration file (YAML/JSON) into a python dictionary. - - Parameters - ---------- - filepath : str - filepath to configuration to parse - - Returns - ------- - dict - Dictionary parsed from given file - - """ - if filepath is None: - return {} - - try: - file_name, file_extension = os.path.splitext(filepath) - dict_obj = {} - if file_extension == ".json": - with open(filepath, "r", encoding="utf-8") as reader: - dict_obj = json.load(reader) - elif file_extension == ".yaml": - with open(filepath, "rt", encoding="utf-8") as reader: - dict_obj = yaml.safe_load(reader.read()) - else: - print( - f"ERROR: File {file_name} is neither of extension '.json' \ - 'nor '.yaml'") - return None - return dict_obj - except Exception as e: - print(f"Caught exception when parsing file: {filepath}. What: {e}") - sys.exit(1) - - def validate_nexus_net_cfg_schema(self): - """ - Check if NEXUS Network configuration follows the pre-defined schema. - - Returns - ------- - bool - If true, NEXUS Network configuration follows the schema. - - """ - try: - validate(self.nexus_net_cfg, schema=self.nexus_net_cfg_schema) - return True - except ValidationError as validation_err: - print("NEXUS Network configuration schema validation failed") - print(f"Validation error: {validation_err}") - - return False - - def zenoh_cfg( - self, - orchestrator, - allowed_endpoints, - tcp_connect, - tcp_listen - ): - """ - Configure and return a Zenoh bridge from given parameters. - - Parameters - ---------- - orchestrator : dict - Orchestrator dictionary with configuration specific to it - allowed_endpoints : str - ROS endpoints that will be allowed through the Zenoh bridge - tcp_connect : str - TCP Addresses of Zenoh bridges to connect to - tcp_listen : str - TCP Addresses of Zenoh bridges to listen to - - Returns - ------- - dict - Python dictionary of Zenoh bridge configuration - - """ - # Prepend "tcp/" to connect and listen endpoints to denote TCP Protocol - tcp_connect_endpoints = \ - [("tcp/" + addr) for addr in tcp_connect] - tcp_listen_endpoints = \ - [("tcp/" + addr) for addr in tcp_listen] - - zenoh_dict = { - "plugins": { - "ros2dds": { - "domain": orchestrator["domain_id"], - "namespace": orchestrator["namespace"], - "allow": orchestrator["allow"], - "queries_timeout": orchestrator["queries_timeout"], - }, - }, - "mode": self.bridge_mode, - "connect": {"endpoints": tcp_connect_endpoints}, - "listen": {"endpoints": tcp_listen_endpoints}, - } - - if self.enable_rest_api: - zenoh_dict["plugins"]["rest"] = { - "http_port": orchestrator["rest_api_http_port"] - } - - return zenoh_dict - - def generate_zenoh_config(self, output_dir): - """ - Generate Zenoh bridge configs and output to directory 'output_dir'. - - Parameters - ---------- - output_dir : str - Output directory for Zenoh configurations - - """ - nexus_net_cfg = self.nexus_net_cfg - - orchestrators_zenoh_cfg = [] - - allowed_endpoints = self.generate_allowed_endpoints( - self.redf_cfg) - - # Generate system orchestrator zenoh dictionary - for orchestrator in nexus_net_cfg["system_orchestrators"]: - - zenoh_cfg = self.zenoh_cfg( - orchestrator=orchestrator, - allowed_endpoints=allowed_endpoints, - tcp_connect=[], - tcp_listen=orchestrator["tcp_listen"] - ) - - orchestrators_zenoh_cfg.append(zenoh_cfg) - - # Generate workcell orchestrators zenoh dictionary - for orchestrator in nexus_net_cfg["workcell_orchestrators"]: - - zenoh_cfg = self.zenoh_cfg( - orchestrator=orchestrator, - allowed_endpoints=allowed_endpoints, - tcp_connect=orchestrator["tcp_connect"], - tcp_listen=[] - ) - - orchestrators_zenoh_cfg.append(zenoh_cfg) - - # Write system orchestrator config to file - if not os.path.exists(output_dir): - os.makedirs(output_dir) - for zenoh_cfg in orchestrators_zenoh_cfg: - write_filepath = os.path.join( - output_dir, - zenoh_cfg["plugins"]["ros2dds"]["namespace"] - + "." - + self.zenoh_cfg_file_extension, - ) - del zenoh_cfg["plugins"]["ros2dds"]["namespace"] - self.write_to_json(write_filepath, zenoh_cfg) - print(f"Generated Zenoh configuration at {write_filepath}") - - def generate_allowed_endpoints(self, - redf_cfg - ): - """ - Generate allowed endpoints string from REDF configuration. - - This string is used to only allow selected ROS endpoints through the - Zenoh bridges. - - Parameters - ---------- - redf_cfg : dict - REDF Configuration dictionary - - Returns - ------- - str - Allowed endpoints string - - """ - allowed_endpoints = [] - - for endpoint in redf_cfg.get('endpoints', []): - if endpoint.get("topic"): - allowed_endpoints.append(endpoint["topic"]) - elif endpoint.get("action_name"): - allowed_endpoints.append(endpoint["action_name"]) - elif endpoint.get("service_name"): - allowed_endpoints.append(endpoint["service_name"]) - - allowed_endpoints_str = "" - for endpoint_str in allowed_endpoints: - # Searches for namespace defined in curly braces in REDF endpoints. - namespace_search = re.compile(r'\{\w*\}') - namespace_found = namespace_search.search(endpoint_str) - if namespace_found: - endpoint_str = namespace_search.sub(".*", endpoint_str) - allowed_endpoints_str += endpoint_str + "|" - - # Remove the last "|" character - allowed_endpoints_str = allowed_endpoints_str[:-1] - - return allowed_endpoints_str - - def write_to_json(self, filepath, cfg_dict): - """ - Write python dictionary to to a JSON File. - - Parameters - ---------- - filepath : str - Filepath to write to - cfg_dict : dict - Zenoh configuration - - """ - with open(filepath, "wt", encoding="utf-8") as writer: - json.dump(cfg_dict, writer, indent=2, default=str) - - -def main(argv=sys.argv): - """Entrypoint.""" - - parser = argparse.ArgumentParser( - description="Generate Zenoh configurations from a NEXUS Network \ - and REDF Configuration", - formatter_class=argparse.ArgumentDefaultsHelpFormatter, - ) - - parser.add_argument( - '-n', - '--nexus_net_cfg', - required=True, - type=str, - help="The NEXUS Network configuration filepath") - - parser.add_argument( - '-r', - '--redf_cfg', - required=False, - type=str, - help="The REDF configuration filepath") - - parser.add_argument( - "-o", - "--output", - required=True, - type=str, - help="Output directory for Zenoh bridge configurations", - ) - - args = parser.parse_args(argv[1:]) - - # Get REDF yaml filepath - redf_cfg_filepath = args.redf_cfg - # Get Network configuration yaml filepath - nexus_net_cfg_filepath = args.nexus_net_cfg - # Get schema filepath - schema_filepath = os.path.join( - get_package_share_directory("nexus_network_configuration"), - "schemas", - "nexus_network_schema.json" - ) - - nexus_config_gen = NEXUSConfigGenerator( - nexus_net_cfg_filepath, redf_cfg_filepath, schema_filepath - ) - - if not nexus_config_gen.validate_nexus_net_cfg_schema(): - print( - "NEXUS Network configuration does not follow the schema. Please \ - check again before generating the Zenoh configs" - ) - return - - nexus_config_gen.generate_zenoh_config(args.output) - - -if __name__ == "__main__": - main(sys.argv) diff --git a/nexus_network_configuration/package.xml b/nexus_network_configuration/package.xml deleted file mode 100644 index a7414ac1..00000000 --- a/nexus_network_configuration/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - - nexus_network_configuration - 0.1.1 - This package generates zenoh bridge configurations for the NEXUS Network - john - Apache License 2.0 - - python3-jsonschema - python3-yaml - - - ament_python - - diff --git a/nexus_network_configuration/resource/nexus_network_configuration b/nexus_network_configuration/resource/nexus_network_configuration deleted file mode 100644 index e69de29b..00000000 diff --git a/nexus_network_configuration/schemas/nexus_network_schema.json b/nexus_network_configuration/schemas/nexus_network_schema.json deleted file mode 100644 index c6cfc8d5..00000000 --- a/nexus_network_configuration/schemas/nexus_network_schema.json +++ /dev/null @@ -1,140 +0,0 @@ -{ - "$schema": "https://json-schema.org/draft/2020-12/schema", - "$id": "https://raw.githubusercontent.com/osrf/nexus/main/nexus_network_configuration/schemas/nexus_network_schema.json", - "title": "NEXUS Network Configuration", - "description": "A network configuration of the NEXUS Orchestrators", - "type": "object", - "required": [ - "mode", - "enable_rest_api", - "system_orchestrators", - "workcell_orchestrators" - ], - "properties": { - "mode":{ - "description": "Mode of Zenoh bridge, specified as either 'client' or 'peer'. Use 'client' when there is an available Zenoh router, otherwise use 'peer' for a distributed system.", - "type": "string", - "enum": ["peer", "client"] - }, - "enable_rest_api":{ - "description": "When true, activates a REST API used to administer Zenoh Bridge configurations", - "type": "boolean" - }, - "system_orchestrators": { - "type": "array", - "items": { "$ref": "#/$defs/system_orchestrator_unit"} - }, - "workcell_orchestrators": { - "type": "array", - "items": { "$ref": "#/$defs/workcell_orchestrator_unit"} - } - }, - - "$defs": { - "queries_timeout": { - "description": "Timeout parameters for Zenoh queries", - "type": "object", - "properties": { - "default": { - "type": "number" - } - } - }, - "allow": { - "description": "Additional endpoints to allow, these could be endpoints not defined in REDF but are necessary for lifecycle transitions", - "type": "object", - "properties": { - "subscribers": { - "type": "array" - }, - "publishers": { - "type": "array" - }, - "service_servers": { - "type": "array" - }, - "service_clients": { - "type": "array" - }, - "action_servers": { - "type": "array" - }, - "action_clients": { - "type": "array" - } - } - }, - "system_orchestrator_unit": { - "type": "object", - "required": ["namespace", "domain_id", "tcp_listen"], - "properties": { - "namespace":{ - "$ref": "#/$defs/namespace" - }, - "domain_id":{ - "$ref": "#/$defs/domain_id" - }, - "tcp_listen":{ - "$ref": "#/$defs/tcp_listen" - }, - "rest_api_http_port":{ - "$ref": "#/$defs/rest_api_http_port" - }, - "allow":{ - "$ref": "#/$defs/allow" - }, - "queries_timeout":{ - "$ref": "#/$defs/queries_timeout" - } - } - }, - "workcell_orchestrator_unit": { - "type": "object", - "required": ["namespace", "domain_id", "tcp_connect"], - "properties": { - "namespace":{ - "$ref": "#/$defs/namespace" - }, - "domain_id":{ - "$ref": "#/$defs/domain_id" - }, - "tcp_connect":{ - "$ref": "#/$defs/tcp_connect" - }, - "rest_api_http_port":{ - "$ref": "#/$defs/rest_api_http_port" - }, - "allow":{ - "$ref": "#/$defs/allow" - }, - "queries_timeout":{ - "$ref": "#/$defs/queries_timeout" - } - } - }, - "namespace":{ - "description": "ROS Namespace of the endpoints", - "type": "string" - }, - "domain_id":{ - "description": "ROS Domain ID", - "type": "integer", - "minimum": 0, - "maximum": 232 - }, - "tcp_connect":{ - "description": "TCP Address of system orchestrator Zenoh bridge", - "type": "array" - }, - "tcp_listen":{ - "description": "TCP Address of system orchestrator Zenoh bridge", - "type": "array" - }, - "rest_api_http_port":{ - "description": "HTTP Port for the REST API", - "type": "integer", - "minimum": 0, - "maximum": 65535 - } - } -} diff --git a/nexus_network_configuration/scripts/set_up_network.sh b/nexus_network_configuration/scripts/set_up_network.sh deleted file mode 100755 index 76e64a6d..00000000 --- a/nexus_network_configuration/scripts/set_up_network.sh +++ /dev/null @@ -1,65 +0,0 @@ -#!/bin/bash - -# This script does the following: -# 1. Install CycloneDDS (Iron dist.) if not already installed -# 2. Change RMW_IMPLEMENTATION to CycloneDDS -# 3. Enable multicast on loopback interface - -RMW_PACKAGE="ros-jazzy-rmw-cyclonedds-cpp" - -PKG_OK=$(dpkg-query -W --showformat='${Status}\n' $RMW_PACKAGE|grep "install ok installed") - -echo "Checking if $REQUIRED_PKG is installed: $PKG_OK" - -if [ "$PKG_OK" = "" ]; then - while true; do - read -p "$RMW_PACKAGE not installed. Would you like to install it? (y/n)" yn - case $yn in - [Yy]* ) sudo apt-get --yes install $RMW_PACKAGE; break;; - [Nn]* ) exit;; - * ) echo "Please answer yes or no.";; - esac - done -fi - -while true; do - read -p "Restart ROS Daemon and set RMW_IMPLEMENTATION to 'rmw_cyclonedds_cpp'? (y/n)" yn - case $yn in - [Yy]* ) - ros2 daemon stop; - echo "Stopped ROS2 Daemon" - export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp; - ros2 daemon start; - echo "Started ROS2 Daemon" - break;; - [Nn]* ) break;; - * ) echo "Please answer yes or no.";; - esac -done - -while true; do - read -p "Add route to enable multicast on loopback interface? (y/n)" yn - case $yn in - [Yy]* ) - # Enable multicast on loopback interface - sudo ifconfig lo multicast; - sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo; - echo "Enabled multicast on loopback interface" - break;; - [Nn]* ) break;; - * ) echo "Please answer yes or no.";; - esac -done - -while true; do - read -p "Set ROS_LOCALHOST_ONLY to 1? (y/n)" yn - case $yn in - [Yy]* ) - # Export environment variables for configuring LOCALHOST only communication - export ROS_LOCALHOST_ONLY=1; - echo "ROS_LOCALHOST_ONLY set to 1" - break;; - [Nn]* ) exit;; - * ) echo "Please answer yes or no.";; - esac -done diff --git a/nexus_network_configuration/setup.cfg b/nexus_network_configuration/setup.cfg deleted file mode 100644 index c96afa11..00000000 --- a/nexus_network_configuration/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/nexus_network_configuration -[install] -install_scripts=$base/lib/nexus_network_configuration diff --git a/nexus_network_configuration/setup.py b/nexus_network_configuration/setup.py deleted file mode 100644 index c8dd9efa..00000000 --- a/nexus_network_configuration/setup.py +++ /dev/null @@ -1,32 +0,0 @@ -import os -from glob import glob -from setuptools import setup - -package_name = 'nexus_network_configuration' - -setup( - name=package_name, - version='0.1.1', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - [os.path.join('resource', package_name)]), - (os.path.join('share', package_name), ['package.xml']), - (os.path.join('share', package_name, 'schemas'), - glob('schemas/*.json')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='john', - maintainer_email='johntan@openrobotics.org', - description='This package generates zenoh bridge configurations for the \ - NEXUS Network', - license='Apache License 2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'nexus_network_configuration = \ - nexus_network_configuration.nexus_network_configuration:main' - ], - }, -)