From f0d0c6a55f43428eda0005b61768b4c7a0fc52d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 25 Oct 2026 20:49:08 +0100 Subject: [PATCH 1/3] Playgrounds and Zenoh, per-robot TF with a fleet RViz, READMEs and fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Simulations from easynav_playgrounds (simulation_launch.yaml in each package); rmw_zenoh_cpp by default, with its router. - Each robot keeps its TF under its namespace; navigation_manager relays odom, base_footprint and base_link to /tf for a central RViz (no RobotModel). - New launch arguments to choose each robot's EasyNav, navigation and navigation manager parameters, its RViz and the mission. - Python fake capabilities for the simple_api_py deployment; the fastdds (DDS Router) and farm (Summit, GPS, NavMap) deployments. - Navigation capability: its executor only spun from on_execute(), and a cancel waits for EasyNav's answer, so it no longer disturbs the next goal. - The collaboration mission reports which robot missed which waypoint instead of always announcing success; robot_node takes config_package. - A README per deployment; run instructions point to simulation_launch.yaml. Signed-off-by: Francisco Martín Rico --- README.md | 1 + .../README.md | 92 +++ .../config/navigation_manager.params.yaml | 5 + .../config/navigation_params.yaml | 2 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- .../launch/easynav_collaboration_launch.yaml | 33 +- .../launch/robot_1_launch.yaml | 29 +- .../launch/robot_2_launch.yaml | 29 +- .../launch/simulation_launch.yaml | 43 ++ .../package.xml | 18 +- .../src/main_easynav_collaboration.cpp | 117 +++- .../src/robot_node.cpp | 5 +- .../CMakeLists.txt | 127 ++++ .../README.md | 85 +++ .../config/navigation.json | 78 +++ .../config/navigation_manager.params.yaml | 41 ++ .../config/navigation_params.yaml | 81 +++ .../config/robot_1/dds_router.yaml | 152 +++++ .../easynav_system.costmap_rpp.params.yaml | 160 +++++ .../config/robot_1/perception.json | 70 ++ .../config/robot_1/perception_params.yaml | 27 + .../config/robot_1/robot_params.yaml | 25 + .../config/robot_2/dds_router.yaml | 85 +++ .../easynav_system.costmap_rpp.params.yaml | 156 +++++ .../config/robot_2/manipulation.json | 69 ++ .../config/robot_2/manipulation_params.yaml | 33 + .../config/robot_2/robot_params.yaml | 25 + ...llaboration_fastdds_deployment_plugins.xml | 28 + .../manipulation_fake_capability.hpp | 91 +++ .../perception_fake_capability.hpp | 103 +++ .../launch/easynav_collaboration_launch.yaml | 118 ++++ .../launch/robot_1_launch.yaml | 179 +++++ .../launch/robot_2_launch.yaml | 144 ++++ .../launch/simulation_launch.yaml | 52 ++ .../package.xml | 65 ++ .../rviz/robot_1.rviz | 637 ++++++++++++++++++ .../rviz/robot_2.rviz | 637 ++++++++++++++++++ .../scripts/build_dds_router.sh | 85 +++ .../scripts/set_central.sh | 38 ++ .../scripts/set_robot_1.sh | 46 ++ .../scripts/set_robot_2.sh | 38 ++ .../manipulation_fake_capability.cpp | 129 ++++ .../perception_fake_capability.cpp | 184 +++++ .../src/main_easynav_collaboration.cpp | 261 +++++++ .../src/robot_node.cpp | 44 ++ .../test/CMakeLists.txt | 47 ++ .../test_manipulation_fake_action_server.cpp | 279 ++++++++ .../test_manipulation_fake_capability.cpp | 353 ++++++++++ .../test_perception_fake_action_server.cpp | 278 ++++++++ .../test/test_perception_fake_capability.cpp | 314 +++++++++ .../test/test_utils.hpp | 72 ++ .../README.md | 80 +++ .../config/navigation_manager.params.yaml | 5 + .../config/navigation_params.yaml | 2 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- ...boration_simple_api_deployment_plugins.xml | 2 +- .../launch/easynav_collaboration_launch.yaml | 11 +- .../launch/robot_1_launch.yaml | 17 +- .../launch/robot_2_launch.yaml | 17 +- .../launch/simulation_launch.yaml | 43 ++ .../package.xml | 9 + .../README.md | 65 ++ .../manipulation_fake_capability.py | 109 +++ .../perception_fake_capability.py | 151 +++++ .../launch/easynav_collaboration_launch.yaml | 28 +- .../launch/robot_1_launch.yaml | 91 +++ .../launch/robot_2_launch.yaml | 89 +++ .../launch/simulation_launch.yaml | 43 ++ .../package.xml | 35 +- .../setup.py | 17 +- .../README.md | 84 +++ .../config/navigation_manager.params.yaml | 5 + .../config/navigation_params.yaml | 2 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- .../config/robot_1/zenoh_router.json5 | 38 +- .../easynav_system.costmap_rpp.params.yaml | 2 +- .../config/robot_2/zenoh_router.json5 | 20 +- ...collaboration_zenoh_deployment_plugins.xml | 2 +- .../launch/easynav_collaboration_launch.yaml | 9 +- .../launch/robot_1_launch.yaml | 16 +- .../launch/robot_2_launch.yaml | 16 +- .../launch/simulation_launch.yaml | 156 ++--- .../package.xml | 9 + easyfleet_easynav_deployment/README.md | 105 +++ .../easynav_navigation_capability.hpp | 8 +- .../launch/easynav_gazebo_launch.yaml | 25 +- .../launch/easynav_robot_gazebo_launch.yaml | 39 +- .../launch/simulation_launch.yaml | 30 + easyfleet_easynav_deployment/package.xml | 9 + .../rviz/easynav_costmap.rviz | 70 +- .../rviz/navigation_manager.rviz | 380 ++--------- .../easynav_navigation_capability.cpp | 40 +- .../CMakeLists.txt | 16 + easyfleet_easynav_farm_deployment/README.md | 67 ++ .../config/easynav_system.params.yaml | 312 +++++++++ .../config/navigation.json | 77 +++ .../config/navigation_params.yaml | 48 ++ .../launch/robot_1_launch.yaml | 71 ++ .../launch/simulation_launch.yaml | 35 + easyfleet_easynav_farm_deployment/package.xml | 48 ++ .../rviz/robot_1.rviz | 323 +++++++++ easyfleet_example_deployments.repos | 27 +- easyfleet_fake_alone_deployment/README.md | 68 ++ .../launch/alone_launch.yaml | 6 + .../launch/robot_1_launch.yaml | 1 + easyfleet_fake_alone_deployment/package.xml | 2 + .../README.md | 70 ++ .../launch/collaboration_launch.yaml | 6 + .../launch/robot_1_launch.yaml | 1 + .../launch/robot_2_launch.yaml | 1 + .../launch/robot_3_launch.yaml | 1 + .../package.xml | 2 + .../src/main_collaboration.cpp | 2 +- 115 files changed, 8231 insertions(+), 652 deletions(-) create mode 100644 easyfleet_easynav_collaboration_deployment/README.md create mode 100644 easyfleet_easynav_collaboration_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/CMakeLists.txt create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/README.md create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/navigation.json create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_manager.params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/dds_router.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception.json create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception_params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/robot_params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/dds_router.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation.json create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation_params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/robot_params.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/launch/easynav_collaboration_launch.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_1_launch.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_2_launch.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/package.xml create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_1.rviz create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_2.rviz create mode 100755 easyfleet_easynav_collaboration_fastdds_deployment/scripts/build_dds_router.sh create mode 100755 easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_central.sh create mode 100755 easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_1.sh create mode 100755 easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_2.sh create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/src/main_easynav_collaboration.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/src/robot_node.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/CMakeLists.txt create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_action_server.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_capability.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_action_server.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_capability.cpp create mode 100644 easyfleet_easynav_collaboration_fastdds_deployment/test/test_utils.hpp create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment/README.md create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/README.md create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/manipulation_fake_capability.py create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/perception_fake_capability.py create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_1_launch.yaml create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_2_launch.yaml create mode 100644 easyfleet_easynav_collaboration_simple_api_deployment_py/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_collaboration_zenoh_deployment/README.md create mode 100644 easyfleet_easynav_deployment/README.md create mode 100644 easyfleet_easynav_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_farm_deployment/CMakeLists.txt create mode 100644 easyfleet_easynav_farm_deployment/README.md create mode 100644 easyfleet_easynav_farm_deployment/config/easynav_system.params.yaml create mode 100644 easyfleet_easynav_farm_deployment/config/navigation.json create mode 100644 easyfleet_easynav_farm_deployment/config/navigation_params.yaml create mode 100644 easyfleet_easynav_farm_deployment/launch/robot_1_launch.yaml create mode 100644 easyfleet_easynav_farm_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_easynav_farm_deployment/package.xml create mode 100644 easyfleet_easynav_farm_deployment/rviz/robot_1.rviz create mode 100644 easyfleet_fake_alone_deployment/README.md create mode 100644 easyfleet_fake_collaboration_deployment/README.md diff --git a/README.md b/README.md index 9276b95..a557542 100644 --- a/README.md +++ b/README.md @@ -18,6 +18,7 @@ Web: [https://easynavigation.github.io/easyfleet_web/](https://easynavigation.gi | [**easyfleet_easynav_collaboration_simple_api_deployment_py**](easyfleet_easynav_collaboration_simple_api_deployment_py) | Python version of the simple API scenario. | | [**easyfleet_easynav_collaboration_zenoh_deployment**](easyfleet_easynav_collaboration_zenoh_deployment) | The simple API scenario over `rmw_zenoh`, one Zenoh island per robot plus a central one. | | [**easyfleet_easynav_collaboration_fastdds_deployment**](easyfleet_easynav_collaboration_fastdds_deployment) | The simple API scenario over Fast DDS, isolated per robot with DDS Router. | +| [**easyfleet_plansys2_warehouse_deployment**](easyfleet_plansys2_warehouse_deployment) | Four Kobukis in a large warehouse, driven by PlanSys2: the behavior tree of each plan action calls the robots' capabilities. | | [**easyfleet_easynav_farm_deployment**](easyfleet_easynav_farm_deployment) | One Summit robot navigating outdoors on a NavMap, localized with GPS. | ## Build diff --git a/easyfleet_easynav_collaboration_deployment/README.md b/easyfleet_easynav_collaboration_deployment/README.md new file mode 100644 index 0000000..41d3ffd --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/README.md @@ -0,0 +1,92 @@ +# easyfleet_easynav_collaboration_deployment + +Two Kobukis in Gazebo, both navigating with real EasyNav on one shared map that a central `navigation_manager_node` publishes. `robot_1` also has a fake perception capability, and `robot_2` a fake manipulation capability. A five-phase mission coordinates them. This is the multi-robot version of [easyfleet_easynav_deployment](../easyfleet_easynav_deployment), written with the **low-level API**: `robot_node.cpp` configures and activates the capabilities by hand, and the mission uses `discover_capabilities()`, `CapabilityClient` and `run_capability()` directly. [easyfleet_easynav_collaboration_simple_api_deployment](../easyfleet_easynav_collaboration_simple_api_deployment) runs the same scenario with the simple `Deployment`/`SimpleController` API. The zenoh and fastdds variants add network isolation for each robot. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Robot | Namespace | Spawn | Capabilities | +|---|---|---|---| +| `robot_1` | `/robot_1` | (0, 0) | `navigation` (EasyNav), `perception` (fake, `gato` detections) | +| `robot_2` | `/robot_2` | (2, 1) | `navigation` (EasyNav), `manipulation` (fake, 8 s execution) | + +Each robot runs these processes, all under its namespace: + +- `system_main`: EasyNav with costmap AMCL, costmap planner and RPP. It uses `tf_prefix=robot_N`. +- `navigation`: `easynav_navigation_capability_node`, from `easyfleet_easynav_deployment`. +- `robot_node`: hosts the fake capability. +- Its own RViz, off by default. + +Each robot has its own TF tree on `/robot_N/tf`. + +Central, unnamespaced processes: + +- `navigation_manager_node`: + - Publishes the `home2` map on `/global_map` and the routes on `/global_routes`. + - Relays `odom`, `base_footprint` and `base_link` from each robot's tree to `/tf` at 10 Hz (`robots.relay_tf*`), for the central RViz. +- `central_rviz`: `easyfleet_easynav_deployment/rviz/navigation_manager.rviz`, which shows the global map, the routes, each robot's `base_link` axes and each robot's planned path. It has no RobotModel display. +- `easynav_collaboration_mission_node`: started 5 s after the rest. +- `rmw_zenohd`: the Zenoh router, from `simulation_launch.yaml`. + +## How to run + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Zenoh router + Gazebo (small house) + both Kobukis +ros2 launch easyfleet_easynav_collaboration_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: both robots + navigation manager + central RViz + mission +ros2 launch easyfleet_easynav_collaboration_deployment easynav_collaboration_launch.yaml +``` + +Add `robot_rviz:=true` to also open each robot's RViz, which shows its costmap, scan, particles, path and the mission status text. One robot can also be started on its own, for example `ros2 launch easyfleet_easynav_collaboration_deployment robot_1_launch.yaml`. In that case its RViz is on by default (`rviz:=false` turns it off). + +## What you should see + +- **Gazebo:** both Kobukis moving in the small house. +- **Central RViz:** the global map, the routes, both robots' `base_link` axes moving and each robot's current path. +- **Mission output.** Goals run until they finish, with a 180 s safety cap: + 0. **Phase 0:** discovery of the four capabilities. + 1. **Phase 1:** `robot_1` goes to `kitchen` with perception running, while `robot_2` goes to `dock`. + 2. **Phase 2:** `robot_1` moves to `kitchen_standby` to make room, while `robot_2` goes to `kitchen`. + 3. **Phase 3:** `robot_1`'s perception stops, and `robot_2` runs manipulation for up to 10 s. + 4. **Phase 4:** both robots head to `dock`. 10 s in, `robot_1` is stopped and sent to `charging_station` instead. + 5. **Phase 5:** both robots return home (`robot_1_home`, `robot_2_home`). +- **Per-robot RViz** (`robot_rviz:=true`): the mission also publishes a status line above each robot on `/mission_status_markers`. + +## Warehouse A + +The same two robots and mission in Warehouse A of [large_warehouse_world](https://github.com/juanscelyg/large_warehouse_world), on easyfleet_easynav_deployment's Warehouse A map and routes (see its README): + +```bash +ros2 launch easyfleet_easynav_collaboration_deployment warehouse_simulation_launch.yaml +ros2 launch easyfleet_easynav_collaboration_deployment warehouse_launch.yaml +``` + +- `robot_1` spawns at (0, 0) and `robot_2` at (2, 1), in the central plaza. Of the mission's waypoints (`config/warehouse/navigation_params.yaml`), `dock`, `kitchen` and `kitchen_standby` are on the corridor between the two rack blocks, and `charging_station` and the homes in the plaza. +- Each Kobuki carries a cylinder at the height of its lidar (`lidar_target`), so that the other robot's lidar sees it; its lidar reaches 30 m. +- `robot_rviz:=true` opens each robot's RViz; `mission:=false` starts everything but the mission. +- With both robots the simulation keeps real time only in `large_warehouse_world_a.sdf` (the default): every lidar renders the whole world. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Zenoh router, world and two namespaced Kobukis (`gui` argument). | +| `launch/easynav_collaboration_launch.yaml` | Both robots + `navigation_manager_node` + central RViz + mission (`robot_rviz` argument). | +| `launch/robot_N_launch.yaml` | One robot's stack under `/robot_N` (`rviz` argument). | +| `config/navigation_manager.params.yaml` | Global map and routes, and the TF relay settings. | +| `config/navigation_params.yaml`, `navigation.json` | Waypoints shared by both robots, and the navigation capability description. | +| `config/robot_N/easynav_system.costmap_rpp.params.yaml` | EasyNav parameters. The only difference between the robots is the AMCL initial pose. | +| `config/robot_N/robot_params.yaml` | Fake capabilities that `robot_node` hosts. | +| `rviz/robot_N.rviz` | Per-robot views, with topics already resolved to `/robot_N/...`. | +| `src/robot_node.cpp` | Hand-written lifecycle management of the fake capabilities. | +| `src/main_easynav_collaboration.cpp` | The five-phase mission (low-level API, explicit status markers). | + +## Troubleshooting + +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). +- If the mission finds no capabilities, the robots took longer than 5 s to activate. Relaunch, or start the mission by hand: `RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 run easyfleet_easynav_collaboration_deployment easynav_collaboration_mission_node --ros-args -p use_sim_time:=true`. +- The central RViz only receives the three relayed frames of each robot. For a robot's full tree, use its own RViz (`robot_rviz:=true`). diff --git a/easyfleet_easynav_collaboration_deployment/config/navigation_manager.params.yaml b/easyfleet_easynav_collaboration_deployment/config/navigation_manager.params.yaml index 68770fb..f5536e1 100644 --- a/easyfleet_easynav_collaboration_deployment/config/navigation_manager.params.yaml +++ b/easyfleet_easynav_collaboration_deployment/config/navigation_manager.params.yaml @@ -34,3 +34,8 @@ map.map_path_file: "maps/home2.yaml" routes.package: "easyfleet_easynav_deployment" routes.map_path_file: "maps/routes_1.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 diff --git a/easyfleet_easynav_collaboration_deployment/config/navigation_params.yaml b/easyfleet_easynav_collaboration_deployment/config/navigation_params.yaml index 83f2e71..68a27ae 100644 --- a/easyfleet_easynav_collaboration_deployment/config/navigation_params.yaml +++ b/easyfleet_easynav_collaboration_deployment/config/navigation_params.yaml @@ -66,7 +66,7 @@ y: -0.15 yaw: -3.14 # Each robot's own Gazebo spawn pose (see - # easynav_playground_kobuki/config/multirobot_config.yaml), used by + # launch/simulation_launch.yaml), used by # main_easynav_collaboration.cpp to send each robot back to where it # started at the end of the mission. robot_1_home: diff --git a/easyfleet_easynav_collaboration_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml index 9620215..feedc3a 100644 --- a/easyfleet_easynav_collaboration_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml @@ -20,7 +20,7 @@ # copied from src/easynav_indoor_testcase/robots_params/costmap.rpp.params.yaml). # The only difference from that single-robot file is localizer_node's # initial_pose below, set to robot_1's actual spawn pose in -# easynav_playground_kobuki's multirobot_config.yaml (x=0.0, y=0.0, no +# launch/simulation_launch.yaml (x=0.0, y=0.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml index d0176ae..1002295 100644 --- a/easyfleet_easynav_collaboration_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml @@ -16,7 +16,7 @@ # Real EasyNav (easynav_system system_main) for robot_2: identical to # config/robot_1/easynav_system.costmap_rpp.params.yaml except # localizer_node's initial_pose below, set to robot_2's actual spawn pose -# in easynav_playground_kobuki's multirobot_config.yaml (x=2.0, y=1.0, no +# in launch/simulation_launch.yaml (x=2.0, y=1.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_deployment/launch/easynav_collaboration_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/easynav_collaboration_launch.yaml index d825f09..4decf1c 100644 --- a/easyfleet_easynav_collaboration_deployment/launch/easynav_collaboration_launch.yaml +++ b/easyfleet_easynav_collaboration_deployment/launch/easynav_collaboration_launch.yaml @@ -22,9 +22,9 @@ # own (e.g. `ros2 launch easyfleet_easynav_collaboration_deployment robot_1_launch.yaml`). # # Does NOT launch Gazebo -- start -# `ros2 launch easynav_playground_kobuki playgorund_multirobot_kobuki.launch.py` +# `ros2 launch easyfleet_easynav_collaboration_deployment simulation_launch.yaml` # separately first, so both robots are already spawned (namespaced -# "robot_1"/"robot_2", see multirobot_config.yaml) and publishing +# "robot_1"/"robot_2") and publishing # scan_raw/odom/TF before system_main tries to localize against them. # # easynav_collaboration_mission_node is delayed a few seconds so both @@ -33,10 +33,36 @@ # easyfleet_mission_manager::discover_capabilities()) opens -- launching # everything at literally the same instant risks discovering nothing. launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} + - arg: {name: "mission", default: "true", description: "Run the example mission"} + - arg: {name: "robot_1_easynav_params", default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_1/easynav_system.costmap_rpp.params.yaml"} + - arg: {name: "robot_2_easynav_params", default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_2/easynav_system.costmap_rpp.params.yaml"} + - arg: {name: "navigation_params", default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_params.yaml"} + - arg: {name: "navigation_manager_params", default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_manager.params.yaml"} + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/launch/robot_1_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} + - {name: "easynav_params", value: "$(var robot_1_easynav_params)"} + - {name: "navigation_params", value: "$(var navigation_params)"} - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/launch/robot_2_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} + - {name: "easynav_params", value: "$(var robot_2_easynav_params)"} + - {name: "navigation_params", value: "$(var navigation_params)"} + # Fleet view: global map and routes, each robot's base_link and path. + - node: + pkg: "rviz2" + exec: "rviz2" + name: "central_rviz" + output: "screen" + args: "-d $(find-pkg-share easyfleet_easynav_deployment)/rviz/navigation_manager.rviz" + param: + - name: "use_sim_time" + value: true # Fleet-wide navigation manager: publishes the same map/routes both # robots above would otherwise load locally on /global_map and @@ -49,9 +75,10 @@ launch: param: - name: "use_sim_time" value: true - - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_manager.params.yaml" + - from: "$(var navigation_manager_params)" - timer: + if: "$(var mission)" period: 5.0 children: - node: diff --git a/easyfleet_easynav_collaboration_deployment/launch/robot_1_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/robot_1_launch.yaml index 84e130f..720a1fe 100644 --- a/easyfleet_easynav_collaboration_deployment/launch/robot_1_launch.yaml +++ b/easyfleet_easynav_collaboration_deployment/launch/robot_1_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake perception capability (this package's own # robot_node), all namespaced under /robot_1 so they never collide with # robot_2's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_1's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # system_main's "tf_prefix" is set to "robot_1" to match: kobuki_description # prefixes every TF frame name with the robot's namespace unconditionally @@ -51,6 +51,16 @@ # Each robot gets its own window/config, so both can be watched navigating # at once. launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - arg: + name: "easynav_params" + default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_1/easynav_system.costmap_rpp.params.yaml" + description: "EasyNav parameters (map, routes, initial pose)" + - arg: + name: "navigation_params" + default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_params.yaml" + description: "Navigation capability parameters (waypoints)" + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_1" @@ -60,10 +70,15 @@ launch: exec: "system_main" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_1/easynav_system.costmap_rpp.params.yaml" + - from: "$(var easynav_params)" - name: "tf_prefix" value: "robot_1" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -91,7 +106,7 @@ launch: name: "navigation" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_params.yaml" + - from: "$(var navigation_params)" - name: "behavior_tree_xml" value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" - name: "bt_plugins" @@ -111,9 +126,15 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_1/perception_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_deployment)/rviz/robot_1.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_deployment/launch/robot_2_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/robot_2_launch.yaml index 969e2a3..eb4fe34 100644 --- a/easyfleet_easynav_collaboration_deployment/launch/robot_2_launch.yaml +++ b/easyfleet_easynav_collaboration_deployment/launch/robot_2_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake manipulation capability (this package's own # robot_node), all namespaced under /robot_2 so they never collide with # robot_1's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_2's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # See robot_1_launch.yaml for why "tf_prefix" is set to match the # namespace, and why it -- and the namespace itself -- are hardcoded here @@ -29,6 +29,16 @@ # below loads its own pre-resolved rviz/robot_2.rviz config instead of a # remapped copy of the shared easynav_costmap.rviz. launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - arg: + name: "easynav_params" + default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_2/easynav_system.costmap_rpp.params.yaml" + description: "EasyNav parameters (map, routes, initial pose)" + - arg: + name: "navigation_params" + default: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_params.yaml" + description: "Navigation capability parameters (waypoints)" + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_2" @@ -38,10 +48,15 @@ launch: exec: "system_main" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_2/easynav_system.costmap_rpp.params.yaml" + - from: "$(var easynav_params)" - name: "tf_prefix" value: "robot_2" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -69,7 +84,7 @@ launch: name: "navigation" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/navigation_params.yaml" + - from: "$(var navigation_params)" - name: "behavior_tree_xml" value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" - name: "bt_plugins" @@ -89,9 +104,15 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/robot_2/manipulation_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_deployment)/rviz/robot_2.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..fdf725c --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/launch/simulation_launch.yaml @@ -0,0 +1,43 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with the small house world and two Kobukis, robot_1 at (0, 0) and +# robot_2 at (2, 1) -- the initial poses in config/robot_N. Each robot's +# topics and TF tree are under its namespace (/robot_N/tf). Also starts the +# Zenoh router. Run it before easynav_collaboration_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "2.0"} + - {name: "y", value: "1.0"} diff --git a/easyfleet_easynav_collaboration_deployment/package.xml b/easyfleet_easynav_collaboration_deployment/package.xml index c115f3e..95f4bb7 100644 --- a/easyfleet_easynav_collaboration_deployment/package.xml +++ b/easyfleet_easynav_collaboration_deployment/package.xml @@ -13,11 +13,10 @@ manipulation (own copies of the PerceptionFakeCapability/ ManipulationFakeCapability reference implementations, mirroring easyfleet_fake_collaboration_deployment's own duplication of these - classes rather than sharing them). Both robots run against the same - Gazebo world/map (easynav_playground_kobuki's - playgorund_multirobot_kobuki.launch.py + src/easynav_indoor_testcase's - home2 map), each namespaced to match its actual spawn namespace and - initial pose. Also ships the example mission script built on + classes rather than sharing them). Both robots run in the same Gazebo + world (launch/simulation_launch.yaml, from easynav_playgrounds' small + house) on the home2 map, each under its own namespace, with its own TF + tree, at its initial pose. Uses rmw_zenoh_cpp by default. Also ships the example mission script built on easyfleet_mission_manager and the launch files and JSON/parameter configuration each robot is started with. @@ -48,6 +47,15 @@ easyfleet_easynav_navigation easyfleet_navigation_manager easynav_system + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + large_warehouse_world + rmw_zenoh_cpp + rviz2 ament_lint_auto ament_lint_common diff --git a/easyfleet_easynav_collaboration_deployment/src/main_easynav_collaboration.cpp b/easyfleet_easynav_collaboration_deployment/src/main_easynav_collaboration.cpp index 4312662..efcff55 100644 --- a/easyfleet_easynav_collaboration_deployment/src/main_easynav_collaboration.cpp +++ b/easyfleet_easynav_collaboration_deployment/src/main_easynav_collaboration.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -97,6 +98,32 @@ Navigation::Goal make_easynav_goal(const std::string & waypoint_id) return goal; } +using NavigationResponse = easyfleet_core::CapabilityClient::Response; + +/// Empty if the goal succeeded, else e.g. 'robot_1 did not reach "kitchen" (CANCELED)'. +std::string missed( + const std::string & robot, const std::string & waypoint, const NavigationResponse & response) +{ + if (response.outcome == easyfleet_core::CapabilityClient::Outcome::SUCCEEDED) { + return ""; + } + return robot + " did not reach \"" + waypoint + "\" (" + outcome_to_string(response.outcome) + + ")"; +} + +/// The end of a phase: `done` if no robot missed its waypoint, else which did. +std::string phase_end( + const std::string & phase, const std::string & done, const std::vector & misses) +{ + std::string failed; + for (const auto & miss : misses) { + if (!miss.empty()) { + failed += (failed.empty() ? "" : "; ") + miss; + } + } + return phase + " done: " + (failed.empty() ? done : failed + "."); +} + } // namespace int main(int argc, char ** argv) @@ -187,24 +214,27 @@ int main(int argc, char ** argv) print_step("[" + kRobot1 + "/perception] not active, skipping."); } + std::string miss1, miss2; { std::thread robot1_thread; if (robot1_navigation_client) { robot1_thread = std::thread( [&] { - run_capability( - robot1_navigation_client, robot1_navigation->get().action_name, - make_easynav_goal(kKitchen), kLongTimeout, - make_navigation_feedback_printer(robot1_navigation->get().action_name)); + miss1 = missed( + kRobot1, kKitchen, run_capability( + robot1_navigation_client, robot1_navigation->get().action_name, + make_easynav_goal(kKitchen), kLongTimeout, + make_navigation_feedback_printer(robot1_navigation->get().action_name))); }); } else { print_step("[" + kRobot1 + "/navigation] not active, skipping."); } if (robot2_navigation_client) { status.set_status(kRobot2, "Navigating -> " + kDock); - run_capability( - robot2_navigation_client, robot2_navigation->get().action_name, make_easynav_goal(kDock), - kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name)); + miss2 = missed( + kRobot2, kDock, run_capability( + robot2_navigation_client, robot2_navigation->get().action_name, make_easynav_goal(kDock), + kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name))); } else { print_step("[" + kRobot2 + "/navigation] not active, skipping."); } @@ -212,10 +242,12 @@ int main(int argc, char ** argv) robot1_thread.join(); } } - print_step("Phase 1 done: both robots reached their waypoint."); + print_step(phase_end("Phase 1", "both robots reached their waypoint.", {miss1, miss2})); // Phase 2: robot_1 -> "kitchen_standby" (still perceiving) while robot_2 // -> "kitchen", simultaneously. + miss1.clear(); + miss2.clear(); print_section( "Phase 2: " + kRobot1 + " -> \"" + kKitchenStandby + "\" (clearing space, still perceiving), " + kRobot2 + " -> \"" + kKitchen + "\" (simultaneously)"); @@ -228,19 +260,22 @@ int main(int argc, char ** argv) (robot1_perception_thread.joinable() ? "\nPerceiving" : "")); robot1_thread = std::thread( [&] { - run_capability( - robot1_navigation_client, robot1_navigation->get().action_name, - make_easynav_goal(kKitchenStandby), kLongTimeout, - make_navigation_feedback_printer(robot1_navigation->get().action_name)); + miss1 = missed( + kRobot1, kKitchenStandby, run_capability( + robot1_navigation_client, robot1_navigation->get().action_name, + make_easynav_goal(kKitchenStandby), kLongTimeout, + make_navigation_feedback_printer(robot1_navigation->get().action_name))); }); } else { print_step("[" + kRobot1 + "/navigation] not active, skipping."); } if (robot2_navigation_client) { status.set_status(kRobot2, "Navigating -> " + kKitchen); - run_capability( - robot2_navigation_client, robot2_navigation->get().action_name, make_easynav_goal(kKitchen), - kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name)); + miss2 = missed( + kRobot2, kKitchen, run_capability( + robot2_navigation_client, robot2_navigation->get().action_name, + make_easynav_goal(kKitchen), kLongTimeout, + make_navigation_feedback_printer(robot2_navigation->get().action_name))); } else { print_step("[" + kRobot2 + "/navigation] not active, skipping."); } @@ -248,7 +283,10 @@ int main(int argc, char ** argv) robot1_thread.join(); } } - print_step("Phase 2 done: " + kRobot1 + " clear of \"" + kKitchen + "\", " + kRobot2 + " there."); + print_step( + phase_end( + "Phase 2", kRobot1 + " clear of \"" + kKitchen + "\", " + kRobot2 + " there.", + {miss1, miss2})); // robot_1's perception has done its job (giving robot_2 room to work at // "kitchen") -- stop it now. @@ -279,6 +317,8 @@ int main(int argc, char ** argv) // simultaneously; 10s into robot_1's navigation it is explicitly stopped // (via run_capability's own timeout-then-cancel behavior) and redirected // to "charging_station" instead. + miss1.clear(); + miss2.clear(); print_section( "Phase 4: " + kRobot1 + " -> \"" + kDock + "\" (canceled after 10s) -> \"" + kChargingStation + "\", " + kRobot2 + " -> \"" + kDock + "\" (simultaneously)"); @@ -294,19 +334,22 @@ int main(int argc, char ** argv) make_navigation_feedback_printer(robot1_navigation->get().action_name)); print_step(kRobot1 + " redirected to \"" + kChargingStation + "\"."); status.set_status(kRobot1, "Navigating -> " + kChargingStation + "\n(redirected)"); - run_capability( - robot1_navigation_client, robot1_navigation->get().action_name + " (charging_station)", - make_easynav_goal(kChargingStation), kLongTimeout, - make_navigation_feedback_printer(robot1_navigation->get().action_name)); + miss1 = missed( + kRobot1, kChargingStation, run_capability( + robot1_navigation_client, + robot1_navigation->get().action_name + " (charging_station)", + make_easynav_goal(kChargingStation), kLongTimeout, + make_navigation_feedback_printer(robot1_navigation->get().action_name))); }); } else { print_step("[" + kRobot1 + "/navigation] not active, skipping."); } if (robot2_navigation_client) { status.set_status(kRobot2, "Navigating -> " + kDock); - run_capability( - robot2_navigation_client, robot2_navigation->get().action_name, make_easynav_goal(kDock), - kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name)); + miss2 = missed( + kRobot2, kDock, run_capability( + robot2_navigation_client, robot2_navigation->get().action_name, make_easynav_goal(kDock), + kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name))); } else { print_step("[" + kRobot2 + "/navigation] not active, skipping."); } @@ -315,12 +358,16 @@ int main(int argc, char ** argv) } } print_step( - "Phase 4 done: " + kRobot1 + " at \"" + kChargingStation + "\", " + kRobot2 + " at \"" + - kDock + "\"."); + phase_end( + "Phase 4", + kRobot1 + " at \"" + kChargingStation + "\", " + kRobot2 + " at \"" + kDock + "\".", + {miss1, miss2})); status.set_status(kRobot1, "At " + kChargingStation + "\nIdle"); status.set_status(kRobot2, "At " + kDock + "\nIdle"); // Phase 5: both robots return to their own starting pose, simultaneously. + miss1.clear(); + miss2.clear(); print_section( "Phase 5: " + kRobot1 + " -> \"" + kRobot1Home + "\", " + kRobot2 + " -> \"" + kRobot2Home + "\" (simultaneously)"); @@ -330,20 +377,22 @@ int main(int argc, char ** argv) status.set_status(kRobot1, "Navigating -> home"); robot1_thread = std::thread( [&] { - run_capability( - robot1_navigation_client, robot1_navigation->get().action_name, - make_easynav_goal(kRobot1Home), kLongTimeout, - make_navigation_feedback_printer(robot1_navigation->get().action_name)); + miss1 = missed( + kRobot1, kRobot1Home, run_capability( + robot1_navigation_client, robot1_navigation->get().action_name, + make_easynav_goal(kRobot1Home), kLongTimeout, + make_navigation_feedback_printer(robot1_navigation->get().action_name))); }); } else { print_step("[" + kRobot1 + "/navigation] not active, skipping."); } if (robot2_navigation_client) { status.set_status(kRobot2, "Navigating -> home"); - run_capability( - robot2_navigation_client, robot2_navigation->get().action_name, - make_easynav_goal(kRobot2Home), - kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name)); + miss2 = missed( + kRobot2, kRobot2Home, run_capability( + robot2_navigation_client, robot2_navigation->get().action_name, + make_easynav_goal(kRobot2Home), + kLongTimeout, make_navigation_feedback_printer(robot2_navigation->get().action_name))); } else { print_step("[" + kRobot2 + "/navigation] not active, skipping."); } @@ -361,7 +410,7 @@ int main(int argc, char ** argv) if (robot2_manipulation_client) { robot2_manipulation_client->cancel(); } - print_step("Phase 5 done: both robots back at their starting pose."); + print_step(phase_end("Phase 5", "both robots back at their starting pose.", {miss1, miss2})); status.set_status(kRobot1, "Mission complete"); status.set_status(kRobot2, "Mission complete"); diff --git a/easyfleet_easynav_collaboration_deployment/src/robot_node.cpp b/easyfleet_easynav_collaboration_deployment/src/robot_node.cpp index 36d81aa..f818b6f 100644 --- a/easyfleet_easynav_collaboration_deployment/src/robot_node.cpp +++ b/easyfleet_easynav_collaboration_deployment/src/robot_node.cpp @@ -94,6 +94,9 @@ int main(int argc, char ** argv) "capabilities", std::vector()); const auto config_subdir = config_node->declare_parameter( "config_subdir", std::string()); + // Package whose share/config/ holds the capability JSON files. + const auto config_package = config_node->declare_parameter( + "config_package", std::string("easyfleet_easynav_collaboration_deployment")); if (capability_names.empty()) { RCLCPP_FATAL( @@ -104,7 +107,7 @@ int main(int argc, char ** argv) } const std::filesystem::path share_dir = ament_index_cpp::get_package_share_path( - "easyfleet_easynav_collaboration_deployment"); + config_package); std::vector nodes; for (const auto & name : capability_names) { diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/CMakeLists.txt b/easyfleet_easynav_collaboration_fastdds_deployment/CMakeLists.txt new file mode 100644 index 0000000..5936aa2 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/CMakeLists.txt @@ -0,0 +1,127 @@ +cmake_minimum_required(VERSION 3.20) +project(easyfleet_easynav_collaboration_fastdds_deployment) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(easyfleet_core REQUIRED) +find_package(easyfleet_interfaces REQUIRED) +find_package(easyfleet_mission_manager REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(pluginlib REQUIRED) + +# perception + manipulation only: this deployment's navigation capability is +# real EasyNav, reused directly from easyfleet_easynav_deployment's own +# easynav_navigation_capability_node executable (see launch/robot_*_launch.yaml) +# -- no navigation_fake_capability here, unlike the sibling fake deployments. +add_library(${PROJECT_NAME} + src/${PROJECT_NAME}/perception_fake_capability.cpp + src/${PROJECT_NAME}/manipulation_fake_capability.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + rclcpp::rclcpp + rclcpp_action::rclcpp_action + rclcpp_lifecycle::rclcpp_lifecycle + easyfleet_core::easyfleet_core + easyfleet_interfaces::easyfleet_interfaces + vision_msgs::vision_msgs + ${builtin_interfaces_TARGETS} + pluginlib::pluginlib +) +# Makes perception and manipulation loadable by pluginlib lookup name (see +# easyfleet_core::Deployment::add_capability(), used by robot_node.cpp) -- +# registered in perception_fake_capability.cpp/manipulation_fake_capability.cpp +# via PLUGINLIB_EXPORT_CLASS. +pluginlib_export_plugin_description_file( + easyfleet_core easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml) + +# The same robot as easyfleet_easynav_collaboration_deployment's own +# robot_node.cpp, driven through easyfleet_core::Deployment's simpler API +# instead of that package's hand-rolled lifecycle dance -- see refactor.md. +# Produces identical ROS-level behavior (same capability/action names), so +# a launch file can point at either package interchangeably. +add_executable(robot_node src/robot_node.cpp) +target_link_libraries(robot_node + ${PROJECT_NAME} + easyfleet_core::easyfleet_core +) + +# The simple-API mission script (easyfleet_mission_manager::RobotHandle/ +# SimpleController/FleetSession) -- see refactor.md for the implementation +# history; produces identical mission behavior to +# easyfleet_easynav_collaboration_deployment's own +# main_easynav_collaboration.cpp, driven through the simpler API instead. +add_executable(easynav_collaboration_mission_node src/main_easynav_collaboration.cpp) +target_link_libraries(easynav_collaboration_mission_node + rclcpp::rclcpp + easyfleet_core::easyfleet_core + easyfleet_mission_manager::easyfleet_mission_manager +) + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install( + TARGETS + robot_node + easynav_collaboration_mission_node + DESTINATION lib/${PROJECT_NAME} +) +install( + DIRECTORY launch config rviz scripts + DESTINATION share/${PROJECT_NAME} +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rclcpp + rclcpp_action + rclcpp_lifecycle + lifecycle_msgs + easyfleet_core + easyfleet_interfaces + easyfleet_mission_manager + vision_msgs + builtin_interfaces + pluginlib +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + add_subdirectory(test) +endif() + +ament_package() diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/README.md b/easyfleet_easynav_collaboration_fastdds_deployment/README.md new file mode 100644 index 0000000..2c0b395 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/README.md @@ -0,0 +1,85 @@ +# easyfleet_easynav_collaboration_fastdds_deployment + +The [simple API two-robot scenario](../easyfleet_easynav_collaboration_simple_api_deployment) (same code, waypoints and mission) isolated per robot with **Fast DDS**, not Zenoh. Each robot runs in its own `ROS_DOMAIN_ID`, and the central part runs in domain 0. One eProsima **DDS Router** per robot bridges its domain with domain 0. An allowlist on each router decides which topics cross, so a robot's internal traffic never leaves its domain. It is the DDS version of the [zenoh variant](../easyfleet_easynav_collaboration_zenoh_deployment), and the only example that pins `rmw_fastrtps_cpp` instead of `rmw_zenoh_cpp`. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Domain | What runs there | +|---|---| +| 0 (central) | Gazebo world + `/clock` bridge, `navigation_manager_node`, central RViz, `easynav_collaboration_mission_node` | +| 1 (`robot_1`) | Kobuki 1, and under `/robot_1`: `system_main`, `navigation`, `robot_node` (`perception`), optional RViz | +| 2 (`robot_2`) | Kobuki 2, and under `/robot_2`: `system_main`, `navigation`, `robot_node` (`manipulation`), optional RViz | + +`ddsrouter` processes: one for each robot, started by `robot_N_launch.yaml` with `config/robot_N/dds_router.yaml`. Each joins its robot's domain and domain 0. Its allowlist lets these topics cross: + +- `capabilities`, `capabilities_status` +- `global_map`, `global_routes` +- `clock`, `mission_status_markers` +- The robot's capability actions, `planner_node` and `easynav_control`. +- `/robot_N/tf` and `/robot_N/tf_static`, routed only from the robot to the central domain, so `navigation_manager_node` can relay `odom`, `base_footprint` and `base_link` to `/tf` for the central RViz. + +Transient-local topics such as `/global_map` and `/capabilities` get explicit QoS overrides so that late joiners still receive them. + +Robots, spawn poses (0, 0) and (2, 1), and capabilities are the same as in the [simple API example](../easyfleet_easynav_collaboration_simple_api_deployment#what-runs-where). + +## How to run + +DDS Router is not a ROS package. Build it once with the script in this package. It installs to `$HOME/.local/dds_router`, which is the path the launch files use. Run it from a shell where your ROS environment is sourced, so it reuses the installed Fast DDS: + +```bash +$(ros2 pkg prefix easyfleet_easynav_collaboration_fastdds_deployment)/share/easyfleet_easynav_collaboration_fastdds_deployment/scripts/build_dds_router.sh +``` + +If you pass a different install prefix as the first argument, edit the `ddsrouter` path in `robot_N_launch.yaml` to match. + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Gazebo world in domain 0, Kobuki N in domain N +ros2 launch easyfleet_easynav_collaboration_fastdds_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: DDS Routers + both robots + navigation manager + central RViz + mission +ros2 launch easyfleet_easynav_collaboration_fastdds_deployment easynav_collaboration_launch.yaml +``` + +`robot_rviz:=true` also opens each robot's RViz, in that robot's domain. + +To inspect a domain from another terminal, source the script for that domain: + +```bash +SCRIPTS=$(ros2 pkg prefix easyfleet_easynav_collaboration_fastdds_deployment)/share/easyfleet_easynav_collaboration_fastdds_deployment/scripts +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +source $SCRIPTS/set_robot_1.sh # ROS_DOMAIN_ID=1; also set_robot_2.sh (2), set_central.sh (0) +ros2 topic list +``` + +The scripts set `ROS_DOMAIN_ID` and restart the `ros2` daemon. They do not set the RMW. When several terminals use different domains, use `--no-daemon`. + +## What you should see + +The same Gazebo scene, central RViz (global map, routes, both `base_link` axes, paths) and five-phase mission output as in the [simple API example](../easyfleet_easynav_collaboration_simple_api_deployment#what-you-should-see). + +The mission starts **about 40 s** after the robots. That margin is for EasyNav's first localization and costmap cycle, which can take more than 20 s on a loaded machine. It is not related to DDS Router. + +From domain 0, `ros2 topic list` shows only the bridged topics. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | World in domain 0 and the Kobukis in domains 1 and 2 (`gui`). Pins `rmw_fastrtps_cpp`. | +| `launch/easynav_collaboration_launch.yaml` | Both robots + central nodes in domain 0, with the mission delayed 40 s (`robot_rviz`). | +| `launch/robot_N_launch.yaml` | The robot's `ddsrouter` + its stack in domain N (`rviz`). | +| `config/robot_N/dds_router.yaml` | Bridge participants (domain N and domain 0), allowlist, QoS overrides and TF routes. | +| `config/robot_N/*`, `config/navigation*.{yaml,json}` | EasyNav, capability and navigation manager configuration, as in the simple API example. | +| `scripts/build_dds_router.sh` | Builds DDS Router from source into `$HOME/.local/dds_router`. | +| `scripts/set_central.sh`, `set_robot_N.sh` | Source these to set a shell's `ROS_DOMAIN_ID`. | +| `src/robot_node.cpp`, `src/main_easynav_collaboration.cpp` | `Deployment` and `SimpleController` code, same as the simple API example. | + +## Notes and troubleshooting + +- Each robot's `robot_N_launch.yaml` expects `ddsrouter` at `$HOME/.local/dds_router/bin/ddsrouter`. If that file is missing, the bridge does not start, the robot's capabilities never reach domain 0, and the mission discovers nothing. +- Domains 0, 1 and 2 are fixed in the launch files and the `dds_router.yaml` files. If other ROS 2 systems on your network use these domains, their traffic mixes with this example. +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation.json b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation.json new file mode 100644 index 0000000..de1bc78 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation.json @@ -0,0 +1,78 @@ +{ + "name": "navigation", + "display_name": "Navigate to a named waypoint (EasyNav-backed, real navigation)", + "description": "Moves the robot to one of a fixed set of named waypoints (configured on this node as ROS parameters) by driving a real EasyNav navigation stack through easynav::GoalManagerClient. Internally structured as a BehaviorTree.CPP tree (StartOff -> Navigate -> Finish). Unlike a mock navigation capability, this is a real easyfleet_core::NavigationActionServerBase backend -- see easyfleet_easynav_navigation.", + "action": { + "name": "/robot_1/navigation or /robot_2/navigation, depending on which robot announced it", + "type": "easyfleet_interfaces/action/Navigation", + "goal": { + "target_pose": "geometry_msgs/PoseStamped - unused by this backend.", + "waypoints": "geometry_msgs/PoseStamped[] - unused by this backend.", + "parameters_json": "string - REQUIRED for this backend: a JSON object with a string field \"goal_id\" naming one of the waypoints configured on this node (see 'parameters' below for the current list). Goals without a valid goal_id are rejected." + }, + "feedback": { + "current_pose": "geometry_msgs/PoseStamped - robot's current pose, as reported by EasyNav.", + "navigation_time": "builtin_interfaces/Duration - time elapsed since this navigation goal started, as reported by EasyNav.", + "estimated_time_remaining": "builtin_interfaces/Duration - as reported by EasyNav.", + "distance_remaining": "float32 - straight-line distance left to the goal, as reported by EasyNav.", + "number_of_recoveries": "int16 - always 0: EasyNav has no equivalent concept.", + "current_waypoint_index": "uint32 - always 0: this backend navigates to one waypoint per goal, not a waypoint list." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT, 10 NO_PATH_FOUND, 11 LOCALIZATION_LOST.", + "error_msg": "string - human-readable explanation, populated when error_code is non-zero.", + "final_pose": "geometry_msgs/PoseStamped - robot's pose when the goal ended, as reported by EasyNav.", + "total_time": "builtin_interfaces/Duration - not populated by this backend (left at its default)." + } + }, + "requirements": [ + "parameters_json must contain a \"goal_id\" naming one of this node's configured waypoints.", + "EasyNav (easynav_system system_main) must already be running and reachable on the easynav_control topic, localized on the home2 map, with this robot's Gazebo instance publishing scan_raw and the TF tree the costmap localizer/controller expect." + ], + "effects": [ + "On success (error_code == SUCCESS), the robot's physical location changes to be at (or very near) the requested waypoint.", + "A currently executing goal can be preempted by a newer navigation goal: EasyNav is redirected to the new waypoint without stopping first (see 'preemption' below).", + "A canceled or capability-shutdown goal sends EasyNav an explicit cancellation, stopping the robot." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "navigation.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the previous ROS goal, but EasyNav itself is redirected (not stopped) to the new waypoint -- the robot keeps moving, just towards the new target.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must wait for the current navigation to finish, or explicitly cancel it, before sending another." + }, + "parameters": { + "navigation.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new navigation goal is allowed to preempt (replace) one that is currently executing." + }, + "navigation.tick_rate_hz": { + "type": "double", + "unit": "hertz", + "default": 10.0, + "description": "Rate at which the internal behavior tree is ticked while a goal is executing." + }, + "navigation.waypoint_ids": { + "type": "string[]", + "default": ["dock", "kitchen", "kitchen_standby", "charging_station", "robot_1_home", "robot_2_home"], + "description": "The set of named waypoints this node accepts as parameters_json.goal_id, valid on the home2 map (see navigation_params.yaml). Each id must have a matching navigation.waypoints..{frame_id,x,y,yaw} sub-parameter." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + }, + "behavior_tree_xml": { + "type": "string", + "description": "Path to the BehaviorTree.CPP XML tree to run per goal (StartOff -> Navigate -> Finish, reused from easyfleet_easynav_deployment)." + }, + "bt_plugins": { + "type": "string[]", + "description": "Paths to the BT.CPP plugin shared libraries (BT_REGISTER_NODES) to load before building the tree -- every node the tree XML references must come from one of these. Reused directly from easyfleet_easynav_deployment (libstart_off_bt_node.so/libfinish_bt_node.so) and easyfleet_easynav_navigation (libeasynav_navigate_bt_node.so); this package supplies no BT plugins of its own." + } + }, + "notes": [ + "This backend drives a real EasyNav navigation stack (easynav_system system_main) with a real navigation configuration -- see config/robot_1|robot_2/easynav_system.costmap_rpp.params.yaml (AMCLLocalizer + CostmapMapsManager + RegulatedPurePursuitController, copied from src/easynav_indoor_testcase/robots_params/costmap.rpp.params.yaml). Goals actually complete once the robot reaches the waypoint (within system_node's position_tolerance/angle_tolerance).", + "Deployment: the two-robot 'easynav_collaboration' scenario -- system_main and this capability are namespaced under /robot_1 and /robot_2 respectively (each its own process, its own launch/robot_N_launch.yaml), on the same shared home2 map. system_main's own 'tf_prefix' parameter is set to match, so TF/goal frames stay consistent per robot -- see easyfleet_easynav_deployment's README section on tf_prefix for the full explanation.", + "The executable itself (easynav_navigation_capability_node) is not duplicated in this package: it is reused directly from easyfleet_easynav_deployment, launched twice (once per robot) with different namespaces/parameters." + ] +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_manager.params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_manager.params.yaml new file mode 100644 index 0000000..f5536e1 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_manager.params.yaml @@ -0,0 +1,41 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for easyfleet_navigation_manager's navigation_manager_node, +# launched once for this whole scenario, fleet-wide, not per robot (see +# launch/easynav_collaboration_launch.yaml) -- publishes the same +# map/routes both robots' own config/robot_1|robot_2/easynav_system.costmap_rpp.params.yaml +# point their maps_manager_node at locally (maps/home2.yaml, +# maps/routes_1.yaml), centrally on /global_map and /global_routes. +# Reuses easyfleet_easynav_deployment's own copies of those files (same +# package this scenario already depends on for the navigation capability +# itself) rather than duplicating them again here. +# +# Uses the "/**" wildcard, matching this package's other params files +# (see navigation_params.yaml's own comment for why): navigation_manager_node +# itself is never namespaced today, but this keeps the file correct if +# that ever changes. +/**: + ros__parameters: + map_type: "costmap" + map.package: "easyfleet_easynav_deployment" + map.map_path_file: "maps/home2.yaml" + routes.package: "easyfleet_easynav_deployment" + routes.map_path_file: "maps/routes_1.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_params.yaml new file mode 100644 index 0000000..68a27ae --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/navigation_params.yaml @@ -0,0 +1,81 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for the "navigation" capability, shared by both robot_1 and +# robot_2 (see launch/robot_1_launch.yaml / robot_2_launch.yaml, which both +# load this same file onto their own easynav_navigation_capability_node +# instance -- copied unchanged from +# easyfleet_easynav_deployment/config/easynav_robot/navigation_params.yaml, +# same executable, reused directly). `capabilities_file` and +# `behavior_tree_xml` are supplied by those launch files, not set here. +# +# Waypoints match src/easynav_indoor_testcase's "home2" map/world (see +# config/robot_1|robot_2/easynav_system.costmap_rpp.params.yaml's +# map_path_file), so they are valid, reachable targets, not placeholders. +# Each waypoint's frame_id ("map", unprefixed here) is namespace-prefixed +# automatically by EasynavNavigationActionServer's constructor (using +# whichever namespace, "robot_1" or "robot_2", the capability itself ends +# up under) -- no per-robot copy of this file is needed just for that. +# +# Uses the "/**" wildcard (rather than a bare "navigation:" node name) so +# this file matches the node regardless of namespace -- a bare node name +# only matches a node's *unqualified* name and silently drops every +# parameter this file sets once namespaced (arrays most noticeably, since +# their code-side default is an empty vector). +/**: + ros__parameters: + navigation: + allow_preemption: true + tick_rate_hz: 10.0 + waypoint_ids: ["dock", "kitchen", "kitchen_standby", "charging_station", "robot_1_home", "robot_2_home"] + waypoints: + dock: + frame_id: "map" + x: 0.0 + y: 2.5 + yaw: 1.57 + kitchen: + frame_id: "map" + x: 5.4 + y: -3.0 + yaw: 0.0 + # 1m short of "kitchen" along its own approach direction, facing it + # (same yaw) -- used by main_easynav_collaboration.cpp to have + # robot_1 clear space at "kitchen" for robot_2 while staying nearby + # with its perception capability still active. + kitchen_standby: + frame_id: "map" + x: 3.4 + y: -3.0 + yaw: 0.0 + charging_station: + frame_id: "map" + x: -7.86 + y: -0.15 + yaw: -3.14 + # Each robot's own Gazebo spawn pose (see + # launch/simulation_launch.yaml), used by + # main_easynav_collaboration.cpp to send each robot back to where it + # started at the end of the mission. + robot_1_home: + frame_id: "map" + x: 0.0 + y: 0.0 + yaw: 0.0 + robot_2_home: + frame_id: "map" + x: 2.0 + y: 1.0 + yaw: 0.0 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/dds_router.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/dds_router.yaml new file mode 100644 index 0000000..b2a8677 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/dds_router.yaml @@ -0,0 +1,152 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_1's DDS Router bridge: connects robot_1's own ROS_DOMAIN_ID (1) with +# the central island's (0), forwarding only the same boundary contract +# documented for the Zenoh-island sibling deployment (see +# easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/zenoh_router.json5 +# and easyfleet_ws/fastdds.md for the full design discussion and the +# empirical findings behind every choice below). +# +# Everything NOT listed in `allowlist` never crosses this bridge at all +# (odom, scan, local costmaps, this robot's own rviz displays, ...) -- DDS +# Router's default with an allowlist configured is to forward nothing except +# what matches it, so an unlisted topic fails *closed*, not open. +# +# Topic names use the ROS 2 DDS wire-mangled form (`rt/`), not the +# plain ROS name -- both `allowlist`/`topics` (QoS overrides) and +# `topic-routes` match against the literal DDS topic name, and a plain +# unmangled name here would simply never match anything (see fastdds.md, +# 2026-08-29, for how this was discovered and confirmed with DDS Router's +# own `kind: echo` participant). +participants: + - name: robot_domain + kind: local + domain: 1 + + - name: central_domain + kind: local + domain: 0 + +allowlist: + # Capability discovery rendezvous (robot -> central) and fleet-wide + # map/routes (central -> robot, published by navigation_manager_node) -- + # see robot_1_launch.yaml's incoming_map/incoming_routes remaps. + - name: "*capabilities" + - name: "*capabilities_status" + - name: "*global_map" + - name: "*global_routes" + # Sim time, bridged from Gazebo by a single ros_gz_bridge node living in + # the central island (every island using use_sim_time needs it). + - name: "*clock" + # mission_control's StatusMarkerPublisher (central -> robot); each + # robot_N.rviz already has a MarkerArray display subscribed to it. + - name: "*mission_status_markers" + # robot_1's own capability actions (navigation, perception), called + # directly by name from easyfleet_mission_manager's RunningCapability. + # The wildcard covers the whole family of hidden send_goal/cancel_goal/ + # get_result services and feedback/status topics an action decomposes + # into, regardless of the rq/rr/rt DDS-mangling prefix each gets. + - name: "*robot_1/navigation*" + - name: "*robot_1/perception*" + # navigation_manager_node's inter-robot collision arbitration + # (RobotNavigationWatcher): watches this robot's planned path and + # exchanges PAUSE/RESUME/ack over easynav_control. Both already + # per-robot-namespaced, so no cross-robot mixing risk from allowing them + # bidirectionally (unlike /tf below). + - name: "*robot_1/planner_node*" + - name: "*robot_1/easynav_control*" + # This robot's own TF (/robot_1/tf, /robot_1/tf_static): for the + # central navigation_manager_node's fleet view (robots.relay_tf). See + # topic-routes below for why it only flows robot -> central. + - name: "rt/robot_1/tf" + - name: "rt/robot_1/tf_static" + +# Manual QoS overrides: DDS Router does not auto-propagate a topic's +# discovered durability by default -- a TRANSIENT_LOCAL publisher's late +# joiners on the other side of the bridge get nothing unless the matching +# `topics:` entry here explicitly forces `durability: true` (+ `reliability: +# true`, since the two must be configured together -- an incompatible +# BEST_EFFORT/TRANSIENT_LOCAL combination is otherwise possible). Confirmed +# empirically (fastdds.md, 2026-08-29): without this, a late-joining +# subscriber across the bridge never receives the cached last sample, even +# though live delivery works fine. +topics: + - name: "*capabilities" + qos: + durability: true + reliability: true + - name: "*global_map" + qos: + durability: true + reliability: true + - name: "*global_routes" + qos: + durability: true + reliability: true + # Forces the whole navigation/perception action family transient_local -- + # slightly broader than strictly necessary (only the _action/status + # sub-topic is actually TRANSIENT_LOCAL by ROS 2 convention; the hidden + # services and the feedback topic are RELIABLE/VOLATILE by default), but + # harmless: a service/feedback consumer only ever waits for a fresh + # reply/feedback matched by its own sequence number, so caching old ones + # has no functional effect. + - name: "*robot_1/navigation*" + qos: + durability: true + reliability: true + - name: "*robot_1/perception*" + qos: + durability: true + reliability: true + - name: "rt/robot_1/tf_static" + qos: + durability: true + reliability: true + +# /tf and /tf_static are deliberately NOT bidirectional like everything +# above: they're absolute, unnamespaced topics, so if crossed both ways, +# whatever the central island forwards back in -- once the fleet-wide RViz2 +# also merges robot_2's /tf onto this very same "/tf" key -- would land +# right back in robot_1's own domain, defeating the whole point of keeping +# each robot's TF tree private (see zenoh.md's identical concern for the +# Zenoh-island sibling deployment, and fastdds.md 2026-08-29 for how this +# was validated here: a participant listed as `src` with no `dst` forwards +# nothing, so central_domain's own route below never sends data back to +# robot_domain). +# +# `type` is mandatory for `topic-routes` (unlike `allowlist`/`topics`, where +# it's optional) and, unlike there, does NOT accept a "*" wildcard -- an +# unmatched wildcard here silently falls back to full bidirectional +# forwarding, i.e. fails *open* on exactly the case we need to fail closed. +# The exact DDS type name (not the ROS "tf2_msgs/msg/TFMessage" name) was +# obtained empirically with a `kind: echo`, `discovery: true` participant +# (see fastdds.md). +topic-routes: + - name: "rt/robot_1/tf" + type: "tf2_msgs::msg::dds_::TFMessage_" + routes: + - src: robot_domain + dst: + - central_domain + - src: central_domain + + - name: "rt/robot_1/tf_static" + type: "tf2_msgs::msg::dds_::TFMessage_" + routes: + - src: robot_domain + dst: + - central_domain + - src: central_domain diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml new file mode 100644 index 0000000..feedc3a --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml @@ -0,0 +1,160 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Real EasyNav (easynav_system system_main) for robot_1: costmap +# localizer/maps-manager/planner + the regulated pure-pursuit controller, +# copied from easyfleet_easynav_deployment's own +# config/easynav_robot/easynav_system.costmap_rpp.params.yaml (itself +# copied from src/easynav_indoor_testcase/robots_params/costmap.rpp.params.yaml). +# The only difference from that single-robot file is localizer_node's +# initial_pose below, set to robot_1's actual spawn pose in +# launch/simulation_launch.yaml (x=0.0, y=0.0, no +# rotation) -- matching src/easynav_indoor_testcase's own multi-robot +# reference (costmap_multirobot.params.yaml), which seeds each robot's +# AMCL at its exact spawn pose rather than an arbitrary offset. +# +# Uses "/**/" so every sub-node's parameters load once +# launch/robot_1_launch.yaml pushes system_main under the "robot_1" +# namespace (a bare node name would silently drop everything here once +# namespaced -- see easyfleet_easynav_deployment's navigation_params.yaml +# for how this exact class of bug was first found). +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + std_dev_xy: 0.1 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/routes_1.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/home2.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception.json b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception.json new file mode 100644 index 0000000..895d064 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception.json @@ -0,0 +1,70 @@ +{ + "name": "perception", + "display_name": "Detect objects of a known class", + "description": "Runs a continuous object-detection stream for a requested object class, publishing detections (both 2D image-plane and 3D spatial) as feedback until the caller cancels the goal. This mock implementation only knows how to detect one class: cats ('gato'). Any goal that does not request that class is rejected outright.", + "action": { + "name": "/robot_1/perception", + "type": "easyfleet_interfaces/action/Perception", + "goal": { + "object_classes": "string[] - object classes to look for. Must include 'gato' (case-insensitive substring match); any other request is rejected since this mock does not detect other classes.", + "continuous": "bool - accepted but not honored by this mock: every goal streams until canceled/preempted regardless of this flag.", + "max_rate_hz": "float32 - accepted but not honored by this mock: the fixed 'perception.detection_rate_hz' parameter is used instead.", + "parameters_json": "string - optional JSON escape hatch for stack-specific perception parameters; empty for defaults." + }, + "feedback": { + "detections_3d": "vision_msgs/Detection3DArray - 1 to 3 randomly placed 3D detections per message, published continuously while the goal is active.", + "detections_2d": "vision_msgs/Detection2DArray - the same detections' 2D image-plane bounding boxes, published alongside detections_3d." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT. CANCELED is the expected way this capability's goals end (this mock never completes on its own); ABORTED means the goal was preempted or the capability shut down.", + "error_msg": "string - populated when error_code is not SUCCESS/CANCELED, explaining why.", + "detections_3d": "vision_msgs/Detection3DArray - final detection snapshot; empty in this mock implementation.", + "detections_2d": "vision_msgs/Detection2DArray - final detection snapshot; empty in this mock implementation." + } + }, + "requirements": [ + "The requested 'object_classes' list must include the target class this mock supports (default 'gato'); otherwise the goal is rejected immediately.", + "This action never completes on its own: it streams feedback indefinitely until the caller cancels the goal (or it is preempted by a new one)." + ], + "effects": [ + "None on the physical world: this is a perception-only capability that reports detections, it does not act on them.", + "While a goal is active, detections_3d/detections_2d feedback is published at a steady rate (see 'perception.detection_rate_hz')." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "perception.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the previous goal (error_code=ABORTED) and starts detecting for the new request instead.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must cancel the current goal first." + }, + "parameters": { + "perception.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new perception goal is allowed to preempt (replace) one that is currently executing." + }, + "perception.target_class": { + "type": "string", + "default": "gato", + "description": "The only object class this mock can 'detect'. A goal is only accepted if its 'objects' list contains this string (case-insensitive substring match)." + }, + "perception.detection_rate_hz": { + "type": "double", + "unit": "hertz", + "default": 20.0, + "description": "Rate at which detections_3d/detections_2d feedback is published while a goal is active." + }, + "perception.frame_id": { + "type": "string", + "default": "camera_link", + "description": "Frame id stamped on the published detections." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + } + }, + "notes": [ + "This is a mock implementation, part of the EasyFleet architecture, standing in for a real perception stack (a classic detector, a VLM-based one, or otherwise) during integration testing of multi-capability systems. Detections are random placeholder positions, not derived from any sensor data.", + "Deployment: the two-robot 'easynav_collaboration' scenario, robot_1 -- carried alongside this robot's real EasyNav navigation capability, which runs as a separate process (see navigation.json)." + ] +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception_params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception_params.yaml new file mode 100644 index 0000000..ba4f4ac --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/perception_params.yaml @@ -0,0 +1,27 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for the "perception" capability of robot_1 in this deployment. +# `capabilities_file` is computed in code (robot_node.cpp) via +# ament_index_cpp, not set here. Uses the "/**" wildcard (rather than a +# bare node name) since this file loads onto robot_node, namespaced under +# "robot_1" alongside (a separate process for) real EasyNav navigation. +/**: + ros__parameters: + perception: + allow_preemption: true + target_class: "gato" + detection_rate_hz: 20.0 + frame_id: "camera_link" diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/robot_params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/robot_params.yaml new file mode 100644 index 0000000..af1de7d --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_1/robot_params.yaml @@ -0,0 +1,25 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Tells robot_node which fake capabilities robot_1 hosts (real navigation +# is a separate process, see launch/robot_1_launch.yaml), and which +# config// its JSON capability descriptions live under. Read +# by robot_node's "robot_config" bootstrap node -- matched via the "/**" +# wildcard for the same reason as this directory's other params files (see +# perception_params.yaml). +/**: + ros__parameters: + capabilities: ["perception"] + config_subdir: "robot_1" diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/dds_router.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/dds_router.yaml new file mode 100644 index 0000000..ffb9018 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/dds_router.yaml @@ -0,0 +1,85 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_2's DDS Router bridge: connects robot_2's own ROS_DOMAIN_ID (2) with +# the central island's (0). See config/robot_1/dds_router.yaml for the full +# rationale behind every choice here (identical except for the domain ID, +# the "robot_1"->"robot_2" namespace, and "perception"->"manipulation") and +# fastdds.md for the design discussion and empirical findings. +participants: + - name: robot_domain + kind: local + domain: 2 + + - name: central_domain + kind: local + domain: 0 + +allowlist: + - name: "*capabilities" + - name: "*capabilities_status" + - name: "*global_map" + - name: "*global_routes" + - name: "*clock" + - name: "*mission_status_markers" + - name: "*robot_2/navigation*" + - name: "*robot_2/manipulation*" + - name: "*robot_2/planner_node*" + - name: "*robot_2/easynav_control*" + - name: "rt/robot_2/tf" + - name: "rt/robot_2/tf_static" + +topics: + - name: "*capabilities" + qos: + durability: true + reliability: true + - name: "*global_map" + qos: + durability: true + reliability: true + - name: "*global_routes" + qos: + durability: true + reliability: true + - name: "*robot_2/navigation*" + qos: + durability: true + reliability: true + - name: "*robot_2/manipulation*" + qos: + durability: true + reliability: true + - name: "rt/robot_2/tf_static" + qos: + durability: true + reliability: true + +topic-routes: + - name: "rt/robot_2/tf" + type: "tf2_msgs::msg::dds_::TFMessage_" + routes: + - src: robot_domain + dst: + - central_domain + - src: central_domain + + - name: "rt/robot_2/tf_static" + type: "tf2_msgs::msg::dds_::TFMessage_" + routes: + - src: robot_domain + dst: + - central_domain + - src: central_domain diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml new file mode 100644 index 0000000..1002295 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml @@ -0,0 +1,156 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Real EasyNav (easynav_system system_main) for robot_2: identical to +# config/robot_1/easynav_system.costmap_rpp.params.yaml except +# localizer_node's initial_pose below, set to robot_2's actual spawn pose +# in launch/simulation_launch.yaml (x=2.0, y=1.0, no +# rotation) -- matching src/easynav_indoor_testcase's own multi-robot +# reference (costmap_multirobot.params.yaml), which seeds each robot's +# AMCL at its exact spawn pose rather than an arbitrary offset. +# +# Uses "/**/" so every sub-node's parameters load once +# launch/robot_2_launch.yaml pushes system_main under the "robot_2" +# namespace (a bare node name would silently drop everything here once +# namespaced -- see easyfleet_easynav_deployment's navigation_params.yaml +# for how this exact class of bug was first found). +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 2.0 + y: 1.0 + yaw: 0.0 + std_dev_xy: 0.1 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 10.0 + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/routes_1.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 10.0 + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/home2.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation.json b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation.json new file mode 100644 index 0000000..460aeba --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation.json @@ -0,0 +1,69 @@ +{ + "name": "manipulation", + "display_name": "Execute a manipulator trajectory", + "description": "Executes a pre-planned joint-space (or Cartesian) trajectory on the robot's manipulator arm. This capability does not plan motions itself: the caller (typically a MoveIt planning step running elsewhere) must supply a complete, collision-free trajectory to execute. Use this once a target end-effector pose or joint configuration has already been resolved into concrete waypoints.", + "action": { + "name": "/robot_2/manipulation", + "type": "easyfleet_interfaces/action/Manipulation", + "goal": { + "mode": "uint8 - 0 MODE_JOINT_TARGET, 1 MODE_POSE_TARGET, 2 MODE_NAMED_TASK; selects which of the fields below is used.", + "joint_target": "sensor_msgs/JointState - target joint configuration, used when mode == MODE_JOINT_TARGET.", + "pose_target": "geometry_msgs/PoseStamped - target end-effector pose, used when mode == MODE_POSE_TARGET.", + "end_effector_link": "string - end-effector link name, used when mode == MODE_POSE_TARGET.", + "named_task": "string - name of a pre-programmed task to run, used when mode == MODE_NAMED_TASK.", + "parameters_json": "string - optional JSON escape hatch for stack-specific manipulation parameters (e.g. planner id, velocity scaling); empty for defaults." + }, + "feedback": { + "state": "string - human-readable execution state (e.g. 'EXECUTING').", + "current_joint_state": "sensor_msgs/JointState - manipulator's current joint state while executing.", + "progress": "float32 - execution progress, from 0.0 to 1.0." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT, 10 PLANNING_FAILED, 11 COLLISION_DETECTED.", + "error_msg": "string - human-readable explanation, populated when error_code is non-zero.", + "final_joint_state": "sensor_msgs/JointState - manipulator's joint state when the goal ended." + } + }, + "requirements": [ + "The trajectory must have already been planned (e.g. by a MoveIt planning pipeline) and be free of collisions; this capability performs no planning or collision checking of its own.", + "The trajectory's joint names must match the robot's actual manipulator joints.", + "The start state of the trajectory must be reasonably close to the robot's current state." + ], + "effects": [ + "On success (error_code == SUCCESS), the manipulator's joints end up at the requested target.", + "A currently executing trajectory can be preempted by a newer goal (see 'preemption' below): the previous goal is aborted and the manipulator starts executing the new trajectory instead." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "manipulation.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the current trajectory execution and immediately starts executing the new one.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must wait for the current execution to finish, or explicitly cancel it, before sending another." + }, + "parameters": { + "manipulation.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new trajectory goal is allowed to preempt (replace) one that is currently executing." + }, + "manipulation.mock_execution_duration": { + "type": "double", + "unit": "seconds", + "default": 3.0, + "description": "Mock/simulated time it takes to execute a trajectory, regardless of its actual number of waypoints. This implementation does not drive a real manipulator: it simulates progress over this duration for integration testing of the wider system." + }, + "manipulation.mock_feedback_period": { + "type": "double", + "unit": "seconds", + "default": 0.3, + "description": "How often feedback is published while a goal is executing." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + } + }, + "notes": [ + "This is a mock implementation, part of the EasyFleet architecture, standing in for a real manipulator stack (MoveIt or otherwise) during integration testing of multi-capability systems. It does not perform real trajectory execution or controller interaction: 'state' feedback simply reports 'EXECUTING' while progress is simulated on a timer.", + "Deployment: the two-robot 'easynav_collaboration' scenario, robot_2 -- carried alongside this robot's real EasyNav navigation capability, which runs as a separate process (see navigation.json)." + ] +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation_params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation_params.yaml new file mode 100644 index 0000000..2194028 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/manipulation_params.yaml @@ -0,0 +1,33 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for the "manipulation" capability of robot_2 in this +# deployment. `capabilities_file` is computed in code (robot_node.cpp) via +# ament_index_cpp, not set here. Uses the "/**" wildcard (rather than a +# bare node name) since this file loads onto robot_node, namespaced under +# "robot_2" alongside (a separate process for) real EasyNav navigation. +/**: + ros__parameters: + manipulation: + allow_preemption: true + # Long enough (with a safety margin under Phase 3's own 10s + # run_capability timeout in main_easynav_collaboration.cpp) for the + # "Manipulating" status marker above robot_2 to actually be + # noticeable -- at the mock's original 3.0s default, it and the + # "Manipulation done" message that follows it were easy to miss + # entirely mid-mission, since Phase 4 starts overwriting robot_2's + # status again almost immediately afterwards. + mock_execution_duration: 8.0 + mock_feedback_period: 0.3 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/robot_params.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/robot_params.yaml new file mode 100644 index 0000000..07872bd --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/config/robot_2/robot_params.yaml @@ -0,0 +1,25 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Tells robot_node which fake capabilities robot_2 hosts (real navigation +# is a separate process, see launch/robot_2_launch.yaml), and which +# config// its JSON capability descriptions live under. Read +# by robot_node's "robot_config" bootstrap node -- matched via the "/**" +# wildcard for the same reason as this directory's other params files (see +# manipulation_params.yaml). +/**: + ros__parameters: + capabilities: ["manipulation"] + config_subdir: "robot_2" diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml b/easyfleet_easynav_collaboration_fastdds_deployment/easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml new file mode 100644 index 0000000..a16c300 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml @@ -0,0 +1,28 @@ + + + + + + Fake/mock "perception" capability for this deployment scenario: streams randomly placed cat detections at a fixed rate, no real object detector. + + + + + Fake/mock "manipulation" capability for this deployment scenario: simulates joint-trajectory execution progress over a configurable duration, no real manipulator. + + + + diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp b/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp new file mode 100644 index 0000000..fab76eb --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp @@ -0,0 +1,91 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#ifndef EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__MANIPULATION_FAKE_CAPABILITY_HPP_ +#define EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__MANIPULATION_FAKE_CAPABILITY_HPP_ + +#include + +#include "easyfleet_interfaces/action/manipulation.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easyfleet_core/capability.hpp" +#include "easyfleet_core/manipulation_action_server_base.hpp" + +namespace easyfleet_easynav_collaboration_fastdds_deployment +{ + +/// @brief Fake/mock implementation of the easyfleet_interfaces/Manipulation action. +/** + * A reference implementation of `easyfleet_core::ManipulationActionServerBase`: + * it does not drive a real manipulator or interact with real controllers. It + * simulates trajectory execution progress over a configurable duration, + * publishing feedback at a configurable rate, so that the rest of a + * multi-capability system can be integrated and tested before a real + * manipulator stack is wired in through its own + * `ManipulationActionServerBase` subclass. + * + * Duplicated (not shared) from easyfleet_fake_collaboration_deployment's + * own copy of this same reference implementation, matching this project's + * established convention of keeping each deployment package self-contained + * rather than introducing a cross-deployment dependency for a mock. + */ +class ManipulationFakeActionServer : public easyfleet_core::ManipulationActionServerBase +{ +public: + /// @brief Constructs the action server. + /// @param node Lifecycle node that will host this action server. + /// @param action_name Name under which the action is advertised. + ManipulationFakeActionServer( + rclcpp_lifecycle::LifecycleNode & node, + const std::string & action_name); + +protected: + /// @brief Always accepts (this mock never rejects a goal). + /// @param uuid Id of the incoming goal. + /// @param goal Goal content (unused). + /// @return Always `ACCEPT_AND_EXECUTE`. + rclcpp_action::GoalResponse on_goal_received( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) override; + + /// @brief Simulates trajectory execution progress over + /// `mock_execution_duration_s_`, publishing feedback every + /// `mock_feedback_period_s_`. + /// @param goal_handle Handle of the accepted goal to run to completion. + void on_execute(const GoalHandleSharedPtr goal_handle) override; + +private: + double mock_execution_duration_s_; + double mock_feedback_period_s_; +}; + +/// @brief The "manipulation" capability, backed by the fake/mock action server: a +/// lifecycle node advertising a (mock) easyfleet_interfaces/Manipulation +/// action, described by config/robot_2/manipulation.json. +class ManipulationFakeCapability + : public easyfleet_core::Capability +{ +public: + /// @brief Constructs the capability node. + /// @param options Forwarded to the underlying `LifecycleNode`. + explicit ManipulationFakeCapability(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); +}; + +} // namespace easyfleet_easynav_collaboration_fastdds_deployment + +#endif // EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__MANIPULATION_FAKE_CAPABILITY_HPP_ diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp b/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp new file mode 100644 index 0000000..8a64050 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/include/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp @@ -0,0 +1,103 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#ifndef EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__PERCEPTION_FAKE_CAPABILITY_HPP_ +#define EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__PERCEPTION_FAKE_CAPABILITY_HPP_ + +#include +#include + +#include "easyfleet_interfaces/action/perception.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easyfleet_core/capability.hpp" +#include "easyfleet_core/perception_action_server_base.hpp" + +namespace easyfleet_easynav_collaboration_fastdds_deployment +{ + +/// @brief Fake/mock implementation of the easyfleet_interfaces/Perception action. +/** + * A reference implementation of `easyfleet_core::PerceptionActionServerBase`: + * it does not run a real object detector. When asked for the configured + * target class (a cat, "gato", by default) it publishes feedback at a + * configurable rate (20 Hz by default) with 1 to 3 randomly placed + * detections, in both 2D and 3D, until the goal is canceled, preempted or + * the node shuts down -- there is no other terminal condition, mirroring + * how a continuous perception stream behaves. Goals asking for any other + * class are rejected. `continuous`/`max_rate_hz` are accepted but not + * honored by this mock: every goal streams forever at the fixed + * `detection_rate_hz` parameter, since a mock has no notion of "detect once + * and stop". + * + * Duplicated (not shared) from easyfleet_fake_collaboration_deployment's + * own copy of this same reference implementation, matching this project's + * established convention of keeping each deployment package self-contained + * rather than introducing a cross-deployment dependency for a mock. + */ +class PerceptionFakeActionServer : public easyfleet_core::PerceptionActionServerBase +{ +public: + /// @brief Constructs the action server. + /// @param node Lifecycle node that will host this action server. + /// @param action_name Name under which the action is advertised. + PerceptionFakeActionServer( + rclcpp_lifecycle::LifecycleNode & node, + const std::string & action_name); + +protected: + /// @brief Accepts only goals asking for the configured `target_class_`. + /// @param uuid Id of the incoming goal. + /// @param goal Goal content to validate. + /// @return `ACCEPT_AND_EXECUTE` if `goal->target_class` matches + /// `target_class_`, `REJECT` otherwise. + rclcpp_action::GoalResponse on_goal_received( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) override; + + /// @brief Streams random detections at `detection_rate_hz_` until canceled, + /// preempted or shut down. + /// @param goal_handle Handle of the accepted goal to run to completion. + void on_execute(const GoalHandleSharedPtr goal_handle) override; + +private: + /// @brief One feedback message with 1 to 3 randomly placed detections of `target_class_`. + /// @return One feedback message with 1 to 3 randomly placed detections + /// of `target_class_`. + Feedback make_random_detections(); + + std::string target_class_; + std::string frame_id_; + double detection_rate_hz_; + + std::mt19937 random_engine_; +}; + +/// @brief The "perception" capability, backed by the fake/mock action server: a +/// lifecycle node advertising a (mock) easyfleet_interfaces/Perception +/// action, described by config/robot_1/perception.json. +class PerceptionFakeCapability : public easyfleet_core::Capability +{ +public: + /// @brief Constructs the capability node. + /// @param options Forwarded to the underlying `LifecycleNode`. + explicit PerceptionFakeCapability(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); +}; + +} // namespace easyfleet_easynav_collaboration_fastdds_deployment + +#endif // EASYFLEET_EASYNAV_COLLABORATION_SIMPLE_API_DEPLOYMENT__PERCEPTION_FAKE_CAPABILITY_HPP_ diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/launch/easynav_collaboration_launch.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/launch/easynav_collaboration_launch.yaml new file mode 100644 index 0000000..53db5fd --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/launch/easynav_collaboration_launch.yaml @@ -0,0 +1,118 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# This deployment's top-level launch file: two real-EasyNav-navigating +# robots plus mission control. +# - robot_1: navigation (real EasyNav), perception (mock) +# - robot_2: navigation (real EasyNav), manipulation (mock) +# Each robot is namespaced independently (see the per-robot launch files), +# so their capabilities never collide and can also be brought up on their +# own (e.g. `ros2 launch easyfleet_easynav_collaboration_fastdds_deployment robot_1_launch.yaml`). +# +# Does NOT launch Gazebo -- start +# `ros2 launch easyfleet_easynav_collaboration_fastdds_deployment simulation_launch.yaml` +# separately first, so both robots are already spawned (namespaced +# "robot_1"/"robot_2") and publishing +# scan_raw/odom/TF before system_main tries to localize against them. +# +# easynav_collaboration_mission_node is delayed a few seconds so both +# robots have finished activating (and published their /capabilities +# announcements) before mission control's discovery window (see +# easyfleet_mission_manager::discover_capabilities()) opens -- launching +# everything at literally the same instant risks discovering nothing. +# +# FastDDS multi-domain: this is the launcher for the "central island" too +# (see fastdds.md) -- unlike the Zenoh-island sibling deployment, the +# central island needs no router/bridge process of its own: it's just plain +# ROS_DOMAIN_ID=0, and robot_1_launch.yaml/robot_2_launch.yaml each start and +# connect to their OWN DDS Router bridge in turn (see those files), which is +# the thing that straddles domain 0 and each robot's own domain -- robot_1 +# and robot_2 never connect directly to each other. No allowlist needed on +# this side: each robot bridge's own config/robot_N/dds_router.yaml already +# enforces, per-topic, that only its documented boundary contract crosses. +launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_fastrtps_cpp"} + - timer: + period: 2.0 + children: + - include: + file: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/launch/robot_1_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} + - include: + file: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/launch/robot_2_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} + + # Fleet-wide navigation manager: publishes the same map/routes both + # robots above would otherwise load locally on /global_map and + # /global_routes (see robot_1_launch.yaml/robot_2_launch.yaml's + # "incoming_map"/"incoming_routes" remaps on system_main). + - node: + pkg: "easyfleet_navigation_manager" + exec: "navigation_manager_node" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "0" + param: + - name: "use_sim_time" + value: true + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/navigation_manager.params.yaml" + + # Fleet-wide view: navigation_manager_node's own topics + # (/global_map, /global_routes_markers, /global_routes_imarkers/...) + # are all unnamespaced, so this rviz2 needs no remap -- unlike + # robot_1_launch.yaml/robot_2_launch.yaml's own rviz2, which is + # per-robot-namespaced. + - node: + pkg: "rviz2" + exec: "rviz2" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "0" + args: "-d $(find-pkg-share easyfleet_easynav_deployment)/rviz/navigation_manager.rviz" + param: + - name: "use_sim_time" + value: true + + # 40s: NOT a DDS Router discovery margin (a nested-timer wall-clock + # test confirmed a 15s delay here really was elapsing in real time) + # -- it's EasyNav's own AMCL/costmap stack taking far longer than + # its 0.2s target cycle time to complete its FIRST cycle on this + # sandbox's CPU-bound hardware (2 full navigation stacks + 2 Gazebo + # robots + software-rendered EGL + DDS Router bridges, all on one + # machine). Confirmed empirically (fastdds.md, 2026-08-29): both + # robots' localizer_node logged "[costmap] target cycle time + # exceeded by more than 1.5x (22.3s / 22.6s > 0.200 s)" for their + # first cycle -- so a 15s margin still wasn't enough, symmetrically + # for both robots, regardless of which one's capability mission + # control happens to check first. This affects any RMW/bridge + # strategy identically; it's not specific to FastDDS/DDS Router. + - timer: + period: 40.0 + children: + - node: + pkg: "easyfleet_easynav_collaboration_fastdds_deployment" + exec: "easynav_collaboration_mission_node" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "0" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_1_launch.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_1_launch.yaml new file mode 100644 index 0000000..0c85192 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_1_launch.yaml @@ -0,0 +1,179 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_1 in this scenario: real EasyNav navigation (easynav_system +# system_main + easyfleet_easynav_deployment's own +# easynav_navigation_capability_node, reused directly -- not duplicated in +# this package) plus a fake perception capability (this package's own +# robot_node), all namespaced under /robot_1 so they never collide with +# robot_2's identically-shaped setup. Can be run standalone, against +# this package's simulation_launch.yaml +# already running (robot_1's Kobuki is spawned there, namespaced the same +# way). +# +# system_main's "tf_prefix" is set to "robot_1" to match: kobuki_description +# prefixes every TF frame name with the robot's namespace unconditionally +# (independent of topic namespacing), so system_main's own frames need the +# same prefix to see the same TF tree the robot publishes -- see +# easyfleet_easynav_deployment's launch/easynav_robot_gazebo_launch.yaml +# for the full explanation of this fix (this file applies it directly, +# hardcoded to "robot_1", rather than through a robot_namespace argument, +# since a robot_N_launch.yaml is inherently robot-specific -- matching +# easyfleet_fake_collaboration_deployment's own robot_1_launch.yaml +# convention). Kept as-is under this multi-domain setup even though it +# would not be strictly needed for a robot with no external namespace +# coupling: kobuki_description's spawn (a *different* package, +# easynav_playground_kobuki, out of scope here) bakes topic namespace and TF +# frame prefix together into the same "namespace" arg, so removing either +# here would desync system_main from what the simulated Kobuki actually +# publishes -- see zenoh.md, refinement of preguntas 2/4 (same underlying +# constraint, orthogonal to which RMW/bridge strategy is used). +# +# FastDDS multi-domain: this group runs entirely on ROS_DOMAIN_ID=1, its own +# private DDS domain (see fastdds.md), then -- after a short delay -- starts +# robot_1's own DDS Router bridge process, which is the only thing that +# straddles both this domain and the central island's (domain 0). Only the +# small boundary contract in config/robot_1/dds_router.yaml (capabilities, +# global_map/routes, this robot's own capability actions, /tf egress-only, +# ...) crosses that bridge; everything else (odom, scan, local costmaps, +# this robot's own rviz, ...) never leaves ROS_DOMAIN_ID=1 at all, since +# domain isolation is real DDS multicast/discovery separation, not an +# access-control layer bolted on top like the Zenoh-island sibling +# deployment. +# +# DDS Router is not an ament/colcon package and ships as a standalone +# binary built once via scripts/build_dds_router.sh (see that script and +# fastdds.md for why) -- defaults to $HOME/.local/dds_router/bin/ddsrouter. +# +# Also launches its own rviz2, with its own rviz/robot_1.rviz config: a +# copy of easynav_indoor_testcase's easynav_costmap.rviz with every Topic +# Value/Fixed Frame already rewritten to the fully-resolved "/robot_1/..." +# path, rather than an absolute-to-relative remap on an unmodified shared +# config (which is what easyfleet_easynav_deployment's +# easynav_robot_gazebo_launch.yaml still does, since *that* file's +# namespace is a runtime argument, not fixed at authoring time like this +# one). Static, pre-resolved values sidestep a real, reproduced problem +# with the remap approach here: RViz's own topic-selector/status UI +# doesn't reliably recognize the display as "connected" against a saved +# Topic value it needs a launch-time remap to resolve -- the Map display +# kept showing no map (and the unremapped "/maps_manager_node/..." string) +# until manually retyped in the GUI, even though the underlying +# subscription was, per `ros2 topic info -v`, already correctly connected. +# Each robot gets its own window/config, so both can be watched navigating +# at once. +launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_fastrtps_cpp"} + - group: + - executable: + cmd: "$(env HOME)/.local/dds_router/bin/ddsrouter -c $(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_1/dds_router.yaml" + output: "screen" + env: + - name: "LD_LIBRARY_PATH" + value: "$(env HOME)/.local/dds_router/lib:$(env LD_LIBRARY_PATH)" + + - timer: + period: 2.0 + children: + - group: + - push_ros_namespace: + namespace: "robot_1" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "1" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_1/easynav_system.costmap_rpp.params.yaml" + - name: "tf_prefix" + value: "robot_1" + remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + # Fleet-wide map/routes from easyfleet_navigation_manager's + # navigation_manager_node. Direction is "from what the code + # itself names its topic, to the name we actually want" -- + # see + # easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml's + # own copy of this remap for the full explanation. This copy + # previously had `from`/`to` swapped, which is a silent + # no-op in ROS 2 (a remap only fires when some code actually + # requests the exact `from` name; CostmapMapsManager's/ + # RoutesMapsManager's incoming_map/incoming_routes + # subscriptions build their own fully-qualified name + # directly via get_fully_qualified_name() and never request + # "/global_map"/"/global_routes" themselves, so that + # direction never had any effect) -- confirmed empirically + # with a minimal rclpy repro outside this codebase before + # fixing. + - from: "maps_manager_node/costmap/incoming_map" + to: "/global_map" + - from: "maps_manager_node/routes/incoming_routes" + to: "/global_routes" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "1" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/navigation.json" + + - node: + pkg: "easyfleet_easynav_collaboration_fastdds_deployment" + exec: "robot_node" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "1" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_1/robot_params.yaml" + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_1/perception_params.yaml" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "1" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + args: "-d $(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/rviz/robot_1.rviz" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_2_launch.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_2_launch.yaml new file mode 100644 index 0000000..d6d1be8 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/launch/robot_2_launch.yaml @@ -0,0 +1,144 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_2 in this scenario: real EasyNav navigation (easynav_system +# system_main + easyfleet_easynav_deployment's own +# easynav_navigation_capability_node, reused directly -- not duplicated in +# this package) plus a fake manipulation capability (this package's own +# robot_node), all namespaced under /robot_2 so they never collide with +# robot_1's identically-shaped setup. Can be run standalone, against +# this package's simulation_launch.yaml +# already running (robot_2's Kobuki is spawned there, namespaced the same +# way). +# +# See robot_1_launch.yaml for why "tf_prefix" (and the namespace itself) are +# kept hardcoded here even under this multi-domain setup -- driven by how +# easynav_playground_kobuki's Kobuki spawn couples topic namespace and TF +# frame prefix together (see zenoh.md), orthogonal to which RMW/bridge +# strategy is used. Also see that file for why rviz2 below loads its own +# pre-resolved rviz/robot_2.rviz config instead of a remapped copy of the +# shared easynav_costmap.rviz. +# +# FastDDS multi-domain: see robot_1_launch.yaml for the full rationale -- +# this group runs entirely on ROS_DOMAIN_ID=2, then starts robot_2's own DDS +# Router bridge process. Boundary contract (what can cross into the central +# island, domain 0) is in config/robot_2/dds_router.yaml: capabilities, +# global_map/routes, this robot's own capability actions +# (/robot_2/navigation, /robot_2/manipulation), and /tf/tf_static +# egress-only. +launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_fastrtps_cpp"} + - group: + - executable: + cmd: "$(env HOME)/.local/dds_router/bin/ddsrouter -c $(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_2/dds_router.yaml" + output: "screen" + env: + - name: "LD_LIBRARY_PATH" + value: "$(env HOME)/.local/dds_router/lib:$(env LD_LIBRARY_PATH)" + + - timer: + period: 2.0 + children: + - group: + - push_ros_namespace: + namespace: "robot_2" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "2" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_2/easynav_system.costmap_rpp.params.yaml" + - name: "tf_prefix" + value: "robot_2" + remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + # Fleet-wide map/routes from easyfleet_navigation_manager's + # navigation_manager_node. Direction is "from what the code + # itself names its topic, to the name we actually want" -- + # see + # easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml's + # own copy of this remap for the full explanation. This copy + # previously had `from`/`to` swapped, which is a silent + # no-op in ROS 2 (a remap only fires when some code actually + # requests the exact `from` name; CostmapMapsManager's/ + # RoutesMapsManager's incoming_map/incoming_routes + # subscriptions build their own fully-qualified name + # directly via get_fully_qualified_name() and never request + # "/global_map"/"/global_routes" themselves, so that + # direction never had any effect) -- confirmed empirically + # with a minimal rclpy repro outside this codebase before + # fixing. + - from: "maps_manager_node/costmap/incoming_map" + to: "/global_map" + - from: "maps_manager_node/routes/incoming_routes" + to: "/global_routes" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "2" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/navigation.json" + + - node: + pkg: "easyfleet_easynav_collaboration_fastdds_deployment" + exec: "robot_node" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "2" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_2/robot_params.yaml" + - from: "$(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/config/robot_2/manipulation_params.yaml" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + env: + - name: "ROS_DOMAIN_ID" + value: "2" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + args: "-d $(find-pkg-share easyfleet_easynav_collaboration_fastdds_deployment)/rviz/robot_2.rviz" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_collaboration_fastdds_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..ac35374 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/launch/simulation_launch.yaml @@ -0,0 +1,52 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo (small house world, from easynav_playground_kobuki_worlds), the +# /clock bridge and both Kobukis -- robot_1 at (0, 0), robot_2 at (2, 1), +# matching config/robot_N -- each in its own ROS_DOMAIN_ID (see fastdds.md): +# the world and its /clock bridge in the central domain 0, each Kobuki's nodes +# in its robot's domain (1, 2). Each part is a group, so the domain set for +# one never leaks into the next. This example is about Fast DDS + DDS Router, +# so it pins rmw_fastrtps_cpp. system_main, the capabilities, the DDS Routers +# and rviz2 come from robot_N_launch.yaml / easynav_collaboration_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_fastrtps_cpp"} + - group: + - set_env: {name: "ROS_DOMAIN_ID", value: "0"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} + - timer: + period: 2.0 + children: + - group: + - set_env: {name: "ROS_DOMAIN_ID", value: "1"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - group: + - set_env: {name: "ROS_DOMAIN_ID", value: "2"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "2.0"} + - {name: "y", value: "1.0"} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/package.xml b/easyfleet_easynav_collaboration_fastdds_deployment/package.xml new file mode 100644 index 0000000..e1cb138 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/package.xml @@ -0,0 +1,65 @@ + + + + easyfleet_easynav_collaboration_fastdds_deployment + 0.1.0 + + Same two-robot scenario as easyfleet_easynav_collaboration_deployment / + easyfleet_easynav_collaboration_zenoh_deployment (real EasyNav + navigation on both robots, robot_1 also carrying a fake perception + capability, robot_2 a fake manipulation one, same Gazebo world/map, + same launch/config/rviz shape, robot_node.cpp written against + easyfleet_core::Deployment's simpler API), but the multi-robot isolation + strategy is the default RMW (FastDDS) with a separate ROS_DOMAIN_ID per + robot and per the central island, bridged by eProsima DDS Router + instead of rmw_zenoh + ACLs -- see fastdds.md at the workspace root for + the full design discussion, the empirical findings behind every DDS + Router config choice, and the comparison against the Zenoh-island + sibling deployment. DDS Router is not an ament/colcon package and must + be built once via scripts/build_dds_router.sh before this deployment's + launch files (which invoke the resulting `ddsrouter` executable + directly) will work. + + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_action + rclcpp_lifecycle + lifecycle_msgs + easyfleet_core + easyfleet_interfaces + easyfleet_mission_manager + vision_msgs + builtin_interfaces + ament_index_cpp + pluginlib + + launch + launch_ros + launch_yaml + easyfleet_easynav_deployment + easyfleet_easynav_navigation + easyfleet_navigation_manager + easynav_system + + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + rviz2 + rmw_fastrtps_cpp + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + geometry_msgs + + + ament_cmake + + diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_1.rviz b/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_1.rviz new file mode 100644 index 0000000..56b282c --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_1.rviz @@ -0,0 +1,637 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Map1 + - /TF1/Frames1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 711 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/maps_manager_node/routes/routes_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/maps_manager_node/routes/routes_map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + robot_1/base_footprint: + Value: true + robot_1/base_link: + Value: true + robot_1/caster_back_link: + Value: true + robot_1/caster_front_link: + Value: true + robot_1/cliff_sensor_front_link: + Value: true + robot_1/cliff_sensor_left_link: + Value: true + robot_1/cliff_sensor_right_link: + Value: true + robot_1/gyro_link: + Value: true + robot_1/laser_link: + Value: true + robot_1/map: + Value: true + robot_1/odom: + Value: true + robot_1/plate_bottom_link: + Value: true + robot_1/plate_middle_link: + Value: true + robot_1/plate_top_link: + Value: true + robot_1/standoff_1in_0_link: + Value: true + robot_1/standoff_1in_1_link: + Value: true + robot_1/standoff_1in_2_link: + Value: true + robot_1/standoff_1in_3_link: + Value: true + robot_1/standoff_1in_4_link: + Value: true + robot_1/standoff_1in_5_link: + Value: true + robot_1/standoff_2in_0_link: + Value: true + robot_1/standoff_2in_1_link: + Value: true + robot_1/standoff_2in_2_link: + Value: true + robot_1/standoff_2in_3_link: + Value: true + robot_1/standoff_8in_0_link: + Value: true + robot_1/standoff_8in_1_link: + Value: true + robot_1/standoff_8in_2_link: + Value: true + robot_1/standoff_8in_3_link: + Value: true + robot_1/wheel_left_link: + Value: true + robot_1/wheel_right_link: + Value: true + robot_2/base_footprint: + Value: true + robot_2/base_link: + Value: true + robot_2/caster_back_link: + Value: true + robot_2/caster_front_link: + Value: true + robot_2/cliff_sensor_front_link: + Value: true + robot_2/cliff_sensor_left_link: + Value: true + robot_2/cliff_sensor_right_link: + Value: true + robot_2/gyro_link: + Value: true + robot_2/laser_link: + Value: true + robot_2/map: + Value: true + robot_2/odom: + Value: true + robot_2/plate_bottom_link: + Value: true + robot_2/plate_middle_link: + Value: true + robot_2/plate_top_link: + Value: true + robot_2/standoff_1in_0_link: + Value: true + robot_2/standoff_1in_1_link: + Value: true + robot_2/standoff_1in_2_link: + Value: true + robot_2/standoff_1in_3_link: + Value: true + robot_2/standoff_1in_4_link: + Value: true + robot_2/standoff_1in_5_link: + Value: true + robot_2/standoff_2in_0_link: + Value: true + robot_2/standoff_2in_1_link: + Value: true + robot_2/standoff_2in_2_link: + Value: true + robot_2/standoff_2in_3_link: + Value: true + robot_2/standoff_8in_0_link: + Value: true + robot_2/standoff_8in_1_link: + Value: true + robot_2/standoff_8in_2_link: + Value: true + robot_2/standoff_8in_3_link: + Value: true + robot_2/wheel_left_link: + Value: true + robot_2/wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + robot_1/map: + robot_1/odom: + robot_1/base_footprint: + robot_1/base_link: + robot_1/caster_back_link: + {} + robot_1/caster_front_link: + {} + robot_1/cliff_sensor_front_link: + {} + robot_1/cliff_sensor_left_link: + {} + robot_1/cliff_sensor_right_link: + {} + robot_1/gyro_link: + {} + robot_1/laser_link: + {} + robot_1/plate_bottom_link: + {} + robot_1/plate_middle_link: + {} + robot_1/plate_top_link: + {} + robot_1/standoff_1in_0_link: + {} + robot_1/standoff_1in_1_link: + {} + robot_1/standoff_1in_2_link: + {} + robot_1/standoff_1in_3_link: + {} + robot_1/standoff_1in_4_link: + {} + robot_1/standoff_1in_5_link: + {} + robot_1/standoff_2in_0_link: + {} + robot_1/standoff_2in_1_link: + {} + robot_1/standoff_2in_2_link: + {} + robot_1/standoff_2in_3_link: + {} + robot_1/standoff_8in_0_link: + {} + robot_1/standoff_8in_1_link: + {} + robot_1/standoff_8in_2_link: + {} + robot_1/standoff_8in_3_link: + {} + robot_1/wheel_left_link: + {} + robot_1/wheel_right_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/scan_raw + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Particles + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/localizer_node/costmap/particles + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/planner_node/simple/path + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.699999988079071 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/curvature_lookahead_point + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/lookahead_point + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + robot_1/base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_1/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/caster_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/cliff_sensor_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_1/cliff_sensor_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_1/cliff_sensor_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_1/gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_1/laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/plate_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/plate_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/plate_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_1in_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_2in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_2in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_2in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_2in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_8in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_8in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_8in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/standoff_8in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_1/wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MissionStatus + Namespaces: + Value: true + mission_status: true + Topic: + Depth: 10 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mission_status_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: robot_1/map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 9.66158676147461 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.630398154258728 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.9203978776931763 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1178 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000001000000000000030300000440fc0200000005fb000000100044006900730070006c006100790073010000003d00000352000000c900fffffffb0000001200530065006c0065006300740069006f006e00000002a7000001250000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000030b000000e60000005c00fffffffb0000000a00560069006500770073000000030e00000103000000a400fffffffb0000000800540069006d00650100000395000000e80000003900ffffff0000070d0000044000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2582 + X: 100 + Y: 91 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_2.rviz b/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_2.rviz new file mode 100644 index 0000000..bafb0af --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/rviz/robot_2.rviz @@ -0,0 +1,637 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Map1 + - /TF1/Frames1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 711 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/maps_manager_node/routes/routes_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/maps_manager_node/routes/routes_map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + robot_1/base_footprint: + Value: true + robot_1/base_link: + Value: true + robot_1/caster_back_link: + Value: true + robot_1/caster_front_link: + Value: true + robot_1/cliff_sensor_front_link: + Value: true + robot_1/cliff_sensor_left_link: + Value: true + robot_1/cliff_sensor_right_link: + Value: true + robot_1/gyro_link: + Value: true + robot_1/laser_link: + Value: true + robot_1/map: + Value: true + robot_1/odom: + Value: true + robot_1/plate_bottom_link: + Value: true + robot_1/plate_middle_link: + Value: true + robot_1/plate_top_link: + Value: true + robot_1/standoff_1in_0_link: + Value: true + robot_1/standoff_1in_1_link: + Value: true + robot_1/standoff_1in_2_link: + Value: true + robot_1/standoff_1in_3_link: + Value: true + robot_1/standoff_1in_4_link: + Value: true + robot_1/standoff_1in_5_link: + Value: true + robot_1/standoff_2in_0_link: + Value: true + robot_1/standoff_2in_1_link: + Value: true + robot_1/standoff_2in_2_link: + Value: true + robot_1/standoff_2in_3_link: + Value: true + robot_1/standoff_8in_0_link: + Value: true + robot_1/standoff_8in_1_link: + Value: true + robot_1/standoff_8in_2_link: + Value: true + robot_1/standoff_8in_3_link: + Value: true + robot_1/wheel_left_link: + Value: true + robot_1/wheel_right_link: + Value: true + robot_2/base_footprint: + Value: true + robot_2/base_link: + Value: true + robot_2/caster_back_link: + Value: true + robot_2/caster_front_link: + Value: true + robot_2/cliff_sensor_front_link: + Value: true + robot_2/cliff_sensor_left_link: + Value: true + robot_2/cliff_sensor_right_link: + Value: true + robot_2/gyro_link: + Value: true + robot_2/laser_link: + Value: true + robot_2/map: + Value: true + robot_2/odom: + Value: true + robot_2/plate_bottom_link: + Value: true + robot_2/plate_middle_link: + Value: true + robot_2/plate_top_link: + Value: true + robot_2/standoff_1in_0_link: + Value: true + robot_2/standoff_1in_1_link: + Value: true + robot_2/standoff_1in_2_link: + Value: true + robot_2/standoff_1in_3_link: + Value: true + robot_2/standoff_1in_4_link: + Value: true + robot_2/standoff_1in_5_link: + Value: true + robot_2/standoff_2in_0_link: + Value: true + robot_2/standoff_2in_1_link: + Value: true + robot_2/standoff_2in_2_link: + Value: true + robot_2/standoff_2in_3_link: + Value: true + robot_2/standoff_8in_0_link: + Value: true + robot_2/standoff_8in_1_link: + Value: true + robot_2/standoff_8in_2_link: + Value: true + robot_2/standoff_8in_3_link: + Value: true + robot_2/wheel_left_link: + Value: true + robot_2/wheel_right_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + robot_2/map: + robot_2/odom: + robot_2/base_footprint: + robot_2/base_link: + robot_2/caster_back_link: + {} + robot_2/caster_front_link: + {} + robot_2/cliff_sensor_front_link: + {} + robot_2/cliff_sensor_left_link: + {} + robot_2/cliff_sensor_right_link: + {} + robot_2/gyro_link: + {} + robot_2/laser_link: + {} + robot_2/plate_bottom_link: + {} + robot_2/plate_middle_link: + {} + robot_2/plate_top_link: + {} + robot_2/standoff_1in_0_link: + {} + robot_2/standoff_1in_1_link: + {} + robot_2/standoff_1in_2_link: + {} + robot_2/standoff_1in_3_link: + {} + robot_2/standoff_1in_4_link: + {} + robot_2/standoff_1in_5_link: + {} + robot_2/standoff_2in_0_link: + {} + robot_2/standoff_2in_1_link: + {} + robot_2/standoff_2in_2_link: + {} + robot_2/standoff_2in_3_link: + {} + robot_2/standoff_8in_0_link: + {} + robot_2/standoff_8in_1_link: + {} + robot_2/standoff_8in_2_link: + {} + robot_2/standoff_8in_3_link: + {} + robot_2/wheel_left_link: + {} + robot_2/wheel_right_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/scan_raw + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Particles + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/localizer_node/costmap/particles + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/planner_node/simple/path + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.699999988079071 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/curvature_lookahead_point + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/lookahead_point + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + robot_2/base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_2/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/caster_back_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/caster_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/cliff_sensor_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_2/cliff_sensor_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_2/cliff_sensor_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_2/gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + robot_2/laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/plate_bottom_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/plate_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/plate_top_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_1in_5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_2in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_2in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_2in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_2in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_8in_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_8in_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_8in_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/standoff_8in_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robot_2/wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MissionStatus + Namespaces: + Value: true + mission_status: true + Topic: + Depth: 10 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mission_status_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: robot_2/map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.876926898956299 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.1879083663225174 + Y: 0.037696853280067444 + Z: -0.1949782520532608 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.000397801399231 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1178 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000001000000000000030300000440fc0200000005fb000000100044006900730070006c006100790073010000003d00000352000000c900fffffffb0000001200530065006c0065006300740069006f006e00000002a7000001250000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000030b000000e60000005c00fffffffb0000000a00560069006500770073000000030e00000103000000a400fffffffb0000000800540069006d00650100000395000000e80000003900ffffff0000070d0000044000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2582 + X: 1707 + Y: 91 diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/scripts/build_dds_router.sh b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/build_dds_router.sh new file mode 100755 index 0000000..9758f79 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/build_dds_router.sh @@ -0,0 +1,85 @@ +#!/usr/bin/env bash +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. +# +# Bootstraps eProsima DDS Router from source. This deployment bridges each +# robot's ROS_DOMAIN_ID with the central island's using DDS Router (see +# fastdds.md at the workspace root for the full design discussion). DDS +# Router is not available as a binary package in any conda-forge/robostack +# channel and is not itself an ament/colcon package, so it can't just be a +# in package.xml -- it has to be built once, out-of-tree, from its +# upstream sources, before this deployment's launch files can find the +# `ddsrouter` executable they invoke. +# +# Usage: run once (from inside this workspace's ROS environment, so the +# already-installed ros-rolling-fastdds/fastcdr get found and reused rather +# than rebuilt): +# ./build_dds_router.sh [install_prefix] +# Default install_prefix: $HOME/.local/dds_router +# +# Every module below is plain CMake (no ament/colcon), so each is +# configured/built/installed into the same prefix in dependency order, each +# one finding the previous ones via -DCMAKE_PREFIX_PATH. This exact sequence +# was validated from scratch on 2026-08-29 against +# ros-rolling-fastdds==3.6.1 (see fastdds.md) with zero source patches needed +# anywhere in the chain -- the only non-default flag required was linking +# pthread explicitly for the final tool (this toolchain's conda-forge glibc +# doesn't fold pthread into libc the way a recent system glibc does). + +set -euo pipefail + +INSTALL_PREFIX="${1:-$HOME/.local/dds_router}" +WORK_DIR="$(mktemp -d)" +trap 'rm -rf "$WORK_DIR"' EXIT + +echo "Building eProsima DDS Router into: $INSTALL_PREFIX" +mkdir -p "$INSTALL_PREFIX" + +build_module() { + local src_dir="$1" + shift + echo "=== Building $(basename "$src_dir") ===" + cmake -B "$src_dir/build" -S "$src_dir" \ + -DCMAKE_INSTALL_PREFIX="$INSTALL_PREFIX" \ + -DCMAKE_PREFIX_PATH="$INSTALL_PREFIX" \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_TESTS=OFF \ + "$@" + cmake --build "$src_dir/build" -j"$(nproc)" + cmake --install "$src_dir/build" +} + +cd "$WORK_DIR" +git clone --depth 1 --branch 1.x https://github.com/eProsima/dev-utils.git dev_utils +git clone --depth 1 --branch 1.x https://github.com/eProsima/DDS-Pipe.git ddspipe +git clone --depth 1 https://github.com/eProsima/DDS-Router.git ddsrouter + +build_module "$WORK_DIR/dev_utils/cmake_utils" +build_module "$WORK_DIR/dev_utils/cpp_utils" +build_module "$WORK_DIR/ddspipe/ddspipe_core" +build_module "$WORK_DIR/ddspipe/ddspipe_participants" +build_module "$WORK_DIR/ddspipe/ddspipe_yaml" +build_module "$WORK_DIR/ddsrouter/ddsrouter_core" +build_module "$WORK_DIR/ddsrouter/ddsrouter_yaml" +build_module "$WORK_DIR/ddsrouter/tools/ddsrouter_tool" -DCMAKE_EXE_LINKER_FLAGS="-lpthread" + +echo "" +echo "DDS Router installed at: $INSTALL_PREFIX/bin/ddsrouter" +if [ "$INSTALL_PREFIX" != "$HOME/.local/dds_router" ]; then + echo "NOTE: this deployment's launch files hardcode \$HOME/.local/dds_router" + echo "as the install location -- since you passed a different prefix, edit" + echo "the 'cmd'/'LD_LIBRARY_PATH' entries in robot_1_launch.yaml and" + echo "robot_2_launch.yaml to point at $INSTALL_PREFIX instead." +fi diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_central.sh b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_central.sh new file mode 100755 index 0000000..cad2bb0 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_central.sh @@ -0,0 +1,38 @@ +#!/usr/bin/env bash +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Connects THIS shell to the central island's DDS domain, for introspection +# (`ros2 topic list`, `ros2 topic echo`, ...) -- see fastdds.md. Must be +# SOURCED, not executed, since it needs to export a variable into your +# current shell rather than a throwaway subshell: +# +# source set_central.sh + +if ! (return 0 2>/dev/null); then + echo "set_central.sh must be sourced, not executed: 'source ${BASH_SOURCE[0]:-$0}'" >&2 + exit 1 +fi + +export ROS_DOMAIN_ID=0 + +# See set_robot_1.sh for why the daemon needs restarting here. +ros2 daemon stop >/dev/null 2>&1 + +echo "This shell is now connected to the central island's ROS_DOMAIN_ID=0." +echo "If you also have another terminal open on a different domain, add" +echo "--no-daemon to 'ros2 topic list'/'ros2 node list' etc. in both, or" +echo "they'll fight over the one shared daemon and one of them will see the" +echo "wrong domain." diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_1.sh b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_1.sh new file mode 100755 index 0000000..1337102 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_1.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Connects THIS shell to robot_1's DDS domain, for introspection +# (`ros2 topic list`, `ros2 topic echo`, ...) -- see fastdds.md. Must be +# SOURCED, not executed, since it needs to export a variable into your +# current shell rather than a throwaway subshell: +# +# source set_robot_1.sh + +if ! (return 0 2>/dev/null); then + echo "set_robot_1.sh must be sourced, not executed: 'source ${BASH_SOURCE[0]:-$0}'" >&2 + exit 1 +fi + +export ROS_DOMAIN_ID=1 + +# `ros2 topic list`/`node list`/etc. talk to the `ros2 daemon`, a background +# process -- ONE per user/machine, shared by every terminal, not per-shell -- +# that keeps a single rmw context alive for as long as it runs, fixed to +# whatever ROS_DOMAIN_ID was set when it first started. It does not re-read +# any shell's environment on later `ros2` calls, and it is unrelated to the +# actual simulation (Gazebo, system_main, the DDS Router bridges, ...), +# which never talks to it -- stopping it here is harmless to whatever else +# is running. Restarting it means THIS shell's next `ros2` command gets a +# fresh context using the domain ID just exported above. +ros2 daemon stop >/dev/null 2>&1 + +echo "This shell is now connected to robot_1's ROS_DOMAIN_ID=1." +echo "If you also have another terminal open on a different domain, add" +echo "--no-daemon to 'ros2 topic list'/'ros2 node list' etc. in both, or" +echo "they'll fight over the one shared daemon and one of them will see the" +echo "wrong domain." diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_2.sh b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_2.sh new file mode 100755 index 0000000..ddd0c04 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/scripts/set_robot_2.sh @@ -0,0 +1,38 @@ +#!/usr/bin/env bash +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Connects THIS shell to robot_2's DDS domain, for introspection +# (`ros2 topic list`, `ros2 topic echo`, ...) -- see fastdds.md. Must be +# SOURCED, not executed, since it needs to export a variable into your +# current shell rather than a throwaway subshell: +# +# source set_robot_2.sh + +if ! (return 0 2>/dev/null); then + echo "set_robot_2.sh must be sourced, not executed: 'source ${BASH_SOURCE[0]:-$0}'" >&2 + exit 1 +fi + +export ROS_DOMAIN_ID=2 + +# See set_robot_1.sh for why the daemon needs restarting here. +ros2 daemon stop >/dev/null 2>&1 + +echo "This shell is now connected to robot_2's ROS_DOMAIN_ID=2." +echo "If you also have another terminal open on a different domain, add" +echo "--no-daemon to 'ros2 topic list'/'ros2 node list' etc. in both, or" +echo "they'll fight over the one shared daemon and one of them will see the" +echo "wrong domain." diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.cpp new file mode 100644 index 0000000..4c4a326 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.cpp @@ -0,0 +1,129 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include "easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp" + +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#include "easyfleet_core/capability_factory.hpp" + +namespace easyfleet_easynav_collaboration_fastdds_deployment +{ + +namespace +{ +bool goal_target_is_empty(const ManipulationFakeActionServer::Goal & goal) +{ + switch (goal.mode) { + case easyfleet_interfaces::action::Manipulation::Goal::MODE_JOINT_TARGET: + return goal.joint_target.name.empty(); + case easyfleet_interfaces::action::Manipulation::Goal::MODE_POSE_TARGET: + return goal.pose_target.header.frame_id.empty(); + case easyfleet_interfaces::action::Manipulation::Goal::MODE_NAMED_TASK: + return goal.named_task.empty(); + default: + return true; + } +} +} // namespace + +ManipulationFakeActionServer::ManipulationFakeActionServer( + rclcpp_lifecycle::LifecycleNode & node, + const std::string & action_name) +: easyfleet_core::ManipulationActionServerBase(node, action_name) +{ + mock_execution_duration_s_ = + node.declare_parameter(action_name + ".mock_execution_duration", 3.0); + mock_feedback_period_s_ = + node.declare_parameter(action_name + ".mock_feedback_period", 0.3); +} + +rclcpp_action::GoalResponse ManipulationFakeActionServer::on_goal_received( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal_target_is_empty(*goal)) { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +void ManipulationFakeActionServer::on_execute(const GoalHandleSharedPtr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + + auto feedback = std::make_shared(); + feedback->state = "EXECUTING"; + feedback->current_joint_state = goal->joint_target; + + const int num_steps = std::max( + 1, static_cast(mock_execution_duration_s_ / mock_feedback_period_s_)); + + for (int i = 0; i < num_steps; ++i) { + if (goal_handle->is_canceling()) { + auto result = std::make_shared(); + result->error_code = Result::CANCELED; + result->error_msg = "Manipulation canceled by client."; + goal_handle->canceled(result); + return; + } + if (is_preempt_requested()) { + auto result = std::make_shared(); + result->error_code = Result::ABORTED; + result->error_msg = "Manipulation preempted by a newer goal."; + goal_handle->abort(result); + return; + } + if (is_shutdown_requested()) { + auto result = std::make_shared(); + result->error_code = Result::ABORTED; + result->error_msg = "Manipulation capability is shutting down."; + goal_handle->abort(result); + return; + } + + std::this_thread::sleep_for(std::chrono::duration(mock_feedback_period_s_)); + feedback->progress = static_cast((i + 1) / static_cast(num_steps)); + goal_handle->publish_feedback(feedback); + } + + auto result = std::make_shared(); + result->error_code = Result::SUCCESS; + result->final_joint_state = goal->joint_target; + goal_handle->succeed(result); +} + +ManipulationFakeCapability::ManipulationFakeCapability(const rclcpp::NodeOptions & options) +: easyfleet_core::Capability("manipulation", options) +{ +} + +// See the matching comment in perception_fake_capability.cpp for why this +// is a non-template alias rather than registering +// easyfleet_core::CapabilityFactoryFor directly. +using ManipulationFakeCapabilityFactory = + easyfleet_core::CapabilityFactoryFor; + +} // namespace easyfleet_easynav_collaboration_fastdds_deployment + +PLUGINLIB_EXPORT_CLASS( + easyfleet_easynav_collaboration_fastdds_deployment::ManipulationFakeCapabilityFactory, + easyfleet_core::CapabilityFactory) diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.cpp new file mode 100644 index 0000000..674910e --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/src/easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.cpp @@ -0,0 +1,184 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include "easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "pluginlib/class_list_macros.hpp" + +#include "easyfleet_core/capability_factory.hpp" + +namespace easyfleet_easynav_collaboration_fastdds_deployment +{ + +namespace +{ +bool requests_target_class( + const std::vector & object_classes, const std::string & target_class) +{ + return std::any_of( + object_classes.begin(), object_classes.end(), + [&target_class](const std::string & requested) { + std::string lower = requested; + std::transform(lower.begin(), lower.end(), lower.begin(), ::tolower); + return lower.find(target_class) != std::string::npos; + }); +} +} // namespace + +PerceptionFakeActionServer::PerceptionFakeActionServer( + rclcpp_lifecycle::LifecycleNode & node, + const std::string & action_name) +: easyfleet_core::PerceptionActionServerBase(node, action_name), + random_engine_(std::random_device{}()) +{ + target_class_ = node.declare_parameter(action_name + ".target_class", std::string("gato")); + frame_id_ = node.declare_parameter(action_name + ".frame_id", std::string("camera_link")); + detection_rate_hz_ = node.declare_parameter(action_name + ".detection_rate_hz", 20.0); + + std::transform(target_class_.begin(), target_class_.end(), target_class_.begin(), ::tolower); +} + +rclcpp_action::GoalResponse PerceptionFakeActionServer::on_goal_received( + const rclcpp_action::GoalUUID & /*uuid*/, + std::shared_ptr goal) +{ + if (goal->object_classes.empty() || + !requests_target_class(goal->object_classes, target_class_)) + { + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +PerceptionFakeActionServer::Feedback PerceptionFakeActionServer::make_random_detections() +{ + std::uniform_int_distribution count_dist(1, 3); + std::uniform_real_distribution xy_dist(-3.0, 3.0); + std::uniform_real_distribution z_dist(0.0, 1.0); + std::uniform_real_distribution pixel_x_dist(0.0, 640.0); + std::uniform_real_distribution pixel_y_dist(0.0, 480.0); + std::uniform_real_distribution score_dist(0.6, 0.99); + + Feedback feedback; + feedback.detections_3d.header.frame_id = frame_id_; + feedback.detections_2d.header.frame_id = frame_id_; + + const int num_cats = count_dist(random_engine_); + for (int i = 0; i < num_cats; ++i) { + const double score = score_dist(random_engine_); + const std::string id = "gato_" + std::to_string(i); + + vision_msgs::msg::Detection3D detection_3d; + detection_3d.header.frame_id = frame_id_; + detection_3d.id = id; + vision_msgs::msg::ObjectHypothesisWithPose hypothesis_3d; + hypothesis_3d.hypothesis.class_id = target_class_; + hypothesis_3d.hypothesis.score = score; + hypothesis_3d.pose.pose.position.x = xy_dist(random_engine_); + hypothesis_3d.pose.pose.position.y = xy_dist(random_engine_); + hypothesis_3d.pose.pose.position.z = z_dist(random_engine_); + detection_3d.results.push_back(hypothesis_3d); + detection_3d.bbox.center = hypothesis_3d.pose.pose; + detection_3d.bbox.size.x = 0.3; + detection_3d.bbox.size.y = 0.3; + detection_3d.bbox.size.z = 0.2; + feedback.detections_3d.detections.push_back(detection_3d); + + vision_msgs::msg::Detection2D detection_2d; + detection_2d.header.frame_id = frame_id_; + detection_2d.id = id; + vision_msgs::msg::ObjectHypothesisWithPose hypothesis_2d; + hypothesis_2d.hypothesis.class_id = target_class_; + hypothesis_2d.hypothesis.score = score; + detection_2d.results.push_back(hypothesis_2d); + detection_2d.bbox.center.position.x = pixel_x_dist(random_engine_); + detection_2d.bbox.center.position.y = pixel_y_dist(random_engine_); + detection_2d.bbox.size_x = 100.0; + detection_2d.bbox.size_y = 100.0; + feedback.detections_2d.detections.push_back(detection_2d); + } + + return feedback; +} + +void PerceptionFakeActionServer::on_execute(const GoalHandleSharedPtr goal_handle) +{ + // `continuous`/`max_rate_hz` on the goal are accepted but not honored by + // this mock: it always streams forever at detection_rate_hz_ until + // canceled, preempted or shut down, regardless of what the goal requested. + const auto period = std::chrono::duration(1.0 / detection_rate_hz_); + + while (true) { + if (goal_handle->is_canceling()) { + auto result = std::make_shared(); + result->error_code = Result::CANCELED; + goal_handle->canceled(result); + return; + } + if (is_preempt_requested()) { + auto result = std::make_shared(); + result->error_code = Result::ABORTED; + result->error_msg = "Perception goal preempted by a newer goal."; + goal_handle->abort(result); + return; + } + if (is_shutdown_requested()) { + auto result = std::make_shared(); + result->error_code = Result::ABORTED; + result->error_msg = "Perception capability is shutting down."; + goal_handle->abort(result); + return; + } + + auto feedback = std::make_shared(make_random_detections()); + goal_handle->publish_feedback(feedback); + + std::this_thread::sleep_for(period); + } +} + +PerceptionFakeCapability::PerceptionFakeCapability(const rclcpp::NodeOptions & options) +: easyfleet_core::Capability("perception", options) +{ +} + +// A non-template alias, registered below, so a Deployment can load this +// backend by pluginlib lookup name ("perception", see +// easyfleet_easynav_collaboration_fastdds_deployment_plugins.xml) instead of +// needing PerceptionFakeCapability's own fixed-name constructor above -- +// see easyfleet_core/capability_factory.hpp for why the factory is what's +// plugin-loaded, not the capability node itself. Registering the raw +// template instantiation directly (`easyfleet_core::CapabilityFactoryFor`) +// would work too, but PLUGINLIB_EXPORT_CLASS registers classes by the +// *stringified* macro argument, which must then match the plugins.xml +// `type=` attribute character-for-character (including template angle +// brackets and any incidental whitespace) -- a non-template alias avoids +// that fragility entirely. +using PerceptionFakeCapabilityFactory = + easyfleet_core::CapabilityFactoryFor; + +} // namespace easyfleet_easynav_collaboration_fastdds_deployment + +PLUGINLIB_EXPORT_CLASS( + easyfleet_easynav_collaboration_fastdds_deployment::PerceptionFakeCapabilityFactory, + easyfleet_core::CapabilityFactory) diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/src/main_easynav_collaboration.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/src/main_easynav_collaboration.cpp new file mode 100644 index 0000000..0eb49c7 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/src/main_easynav_collaboration.cpp @@ -0,0 +1,261 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +// The simple-API mission script for this scenario, written against +// easyfleet_mission_manager's RobotHandle/SimpleController -- see +// easyfleet_easynav_collaboration_deployment/src/main_easynav_collaboration.cpp +// for the low-level-API sibling this is meant to behave identically to. +// Status markers above each robot in RViz are not set here explicitly the +// way the low-level-API sibling does: RobotHandle::run_capability() +// publishes them automatically for the life of each goal, which is exactly +// the boilerplate this simpler API exists to remove. +// +// Mission control demo for this deployment scenario (see +// src/EasyFleet/easyfleet_example_deployments/easyfleet_easynav_collaboration_fastdds_deployment/launch): +// two robots, both real-EasyNav-navigating on the same shared map -- +// - robot_1: navigation (real EasyNav), perception (mock) +// - robot_2: navigation (real EasyNav), manipulation (mock) +// +// A longer, five-phase choreography exercising real navigation end to end +// (goals are left to actually SUCCEED, not stopped early -- see +// kLongTimeout below -- except where phase 4 deliberately cancels one): +// 1. robot_1 -> "kitchen" (perceiving throughout) while robot_2 -> "dock", +// simultaneously. Waits for both navigations to actually finish (not +// a timeout). +// 2. robot_1 -> "kitchen_standby" (1m short of "kitchen", still facing +// it, perception still running from phase 1) to clear space, while +// robot_2 -> "kitchen", simultaneously. +// 3. Once robot_2 is at "kitchen" (and robot_1's perception is stopped, +// its job here done), robot_2 runs manipulation for up to 10s. +// 4. robot_1 -> "dock" while robot_2 -> "dock" too, simultaneously; 10s +// into robot_1's navigation it is explicitly stopped and redirected to +// "charging_station" instead. +// 5. Once both navigations from phase 4 finish, both robots return to +// their own starting pose ("robot_1_home"/"robot_2_home") and any +// still-running capability goal is stopped. +// +// Also publishes a short status line above each robot in RViz +// (easyfleet_mission_manager::StatusMarkerPublisher, a floating +// TEXT_VIEW_FACING marker at each robot's own base_link) at every +// meaningful transition above, so someone watching only RViz -- not this +// program's terminal output -- can follow which capability each robot is +// using and what it's doing/its result, without reading code or logs. + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "easyfleet_core/init.hpp" +#include "easyfleet_mission_manager/mission_helpers.hpp" +#include "easyfleet_mission_manager/output.hpp" +#include "easyfleet_mission_manager/robot_handle.hpp" +#include "easyfleet_mission_manager/simple_controller.hpp" + +using namespace easyfleet_mission_manager; +using namespace std::chrono_literals; + +namespace +{ + +// This demo is specific to this deployment scenario, which defines exactly +// these two robots (see .../easyfleet_easynav_collaboration_fastdds_deployment/launch/). +const std::string kRobot1 = "robot_1"; +const std::string kRobot2 = "robot_2"; + +// Named waypoints configured on both robots' navigation capability, see +// config/navigation_params.yaml -- real, reachable points on the shared +// home2 map. +const std::string kDock = "dock"; +const std::string kKitchen = "kitchen"; +const std::string kKitchenStandby = "kitchen_standby"; +const std::string kChargingStation = "charging_station"; +const std::string kRobot1Home = "robot_1_home"; +const std::string kRobot2Home = "robot_2_home"; + +// Real navigation goals are meant to run to actual completion in this +// mission (see the file's own header comment) -- this is a generous safety +// net, not the expected way a goal ends, unlike easyfleet_mission_manager's +// own kRunTimeout (10s), which is both too short for real navigation and +// -- for every navigation goal below except one -- not what's wanted here. +constexpr std::chrono::seconds kLongTimeout(180); + +} // namespace + +int main(int argc, char ** argv) +{ + easyfleet::init(argc, argv); + + easyfleet::SimpleController controller; + + easyfleet::RobotHandle robot_1(kRobot1); + easyfleet::RobotHandle robot_2(kRobot2); + + controller.add_robot(robot_1); + controller.add_robot(robot_2); + + safe_print( + std::string(ansi::bold) + ansi::cyan + + "Control Center -- two-robot \"easynav\" mission (" + kRobot1 + ", " + kRobot2 + ")" + + ansi::reset); + + print_section("Phase 0: Discovering capabilities"); + print_step("listening on /capabilities and /capabilities_status ..."); + controller.discover_capabilities(); + + robot_1.print_capabilities(); + robot_2.print_capabilities(); + + if (!robot_1.has_capability("navigation") || !robot_2.has_capability("navigation")) { + safe_print( + std::string(ansi::bold) + ansi::red + + "Both robots must have a navigation capability for this demo to work." + ansi::reset); + return 1; + } + + if (!robot_1.has_capability("perception")) { + safe_print( + std::string(ansi::bold) + ansi::red + + "Robot 1 must have a perception capability for this demo to work." + ansi::reset); + return 1; + } + + if (!robot_2.has_capability("manipulation")) { + safe_print( + std::string(ansi::bold) + ansi::red + + "Robot 2 must have a manipulation capability for this demo to work." + ansi::reset); + return 1; + } + + // Phase 1: robot_1 -> "kitchen" (perceiving throughout) while robot_2 -> + // "dock", simultaneously. robot_1's perception is started here and kept + // running (untouched) across phase 2 too -- it's only stopped once + // robot_1 actually reaches "kitchen_standby" at the end of phase 2, see + // there. + + print_section( + "Phase 1: " + kRobot1 + " -> \"" + kKitchen + "\" (perceiving), " + kRobot2 + " -> \"" + + kDock + "\" (simultaneously)"); + + robot_1.run_capability("navigation", make_navigation_goal(kKitchen), kLongTimeout); + robot_1.run_capability("perception", make_perception_goal(), kLongTimeout); + robot_2.run_capability("navigation", make_navigation_goal(kDock), kLongTimeout); + + while (robot_1.is_capability_running("navigation") || + robot_2.is_capability_running("navigation")) + { + controller.spin_some(); + } + + print_step("Phase 1 done: both robots reached their waypoint."); + + // Phase 2: robot_1 -> "kitchen_standby" (still perceiving) while robot_2 + // -> "kitchen", simultaneously. + print_section( + "Phase 2: " + kRobot1 + " -> \"" + kKitchenStandby + "\" (clearing space, still perceiving), " + + kRobot2 + " -> \"" + kKitchen + "\" (simultaneously)"); + + // robot_1's perception keeps running, untouched, from phase 1 -- no need + // to (re-)issue it here; doing so would just preempt the still-running + // goal with an identical one. + robot_1.run_capability( + "navigation", make_navigation_goal(kKitchenStandby), kLongTimeout); + + robot_2.run_capability("navigation", make_navigation_goal(kKitchen), kLongTimeout); + + while (robot_1.is_capability_running("navigation") || + robot_2.is_capability_running("navigation")) + { + controller.spin_some(); + } + + robot_1.stop_capability("perception"); + + print_step("Phase 2 done: " + kRobot1 + " clear of \"" + kKitchen + "\", " + kRobot2 + " there."); + + // Phase 3: robot_2, now at "kitchen", runs manipulation for up to 10s + // (the mock's own configured duration finishes well within that). + print_section("Phase 3: " + kRobot2 + " manipulation at \"" + kKitchen + "\""); + + // Default timeout (kRunTimeout, 10s) already matches what this phase + // wants. + robot_2.run_capability("manipulation", make_manipulation_goal()); + controller.spin_for(std::chrono::seconds(10)); + + robot_2.stop_capability("manipulation"); + + print_step("Phase 3 done."); + + // Phase 4: robot_1 -> "dock" while robot_2 -> "dock" too (robot_2 was + // still at "kitchen" from phase 3, so this is a real move, not a no-op), + // simultaneously; 10s into robot_1's navigation it is explicitly stopped + // (via run_capability's own timeout-then-cancel behavior) and redirected + // to "charging_station" instead. + print_section( + "Phase 4: " + kRobot1 + " -> \"" + kDock + "\" (canceled after 10s) -> \"" + + kChargingStation + "\", " + kRobot2 + " -> \"" + kDock + "\" (simultaneously)"); + + // Default timeout (kRunTimeout, 10s) here is deliberate: this is the + // goal meant to be capped and redirected below, unlike every other + // navigation goal in this mission. + robot_1.run_capability("navigation", make_navigation_goal(kDock)); + robot_2.run_capability("navigation", make_navigation_goal(kDock), kLongTimeout); + + controller.spin_for(std::chrono::seconds(10)); + + // A new goal on the same capability preempts the one still in flight + // (ActionServerBase's own preemption handling) -- no explicit + // stop_capability() needed first. + robot_1.run_capability( + "navigation", make_navigation_goal(kChargingStation), kLongTimeout); + + while (robot_1.is_capability_running("navigation") || + robot_2.is_capability_running("navigation")) + { + controller.spin_some(); + } + print_step( + "Phase 4 done: " + kRobot1 + " at \"" + kChargingStation + "\", " + kRobot2 + " at \"" + + kDock + "\"."); + + // Phase 5: both robots return to their own starting pose, simultaneously. + print_section( + "Phase 5: " + kRobot1 + " -> \"" + kRobot1Home + "\", " + kRobot2 + " -> \"" + kRobot2Home + + "\" (simultaneously)"); + + robot_1.run_capability("navigation", make_navigation_goal(kRobot1Home), kLongTimeout); + robot_2.run_capability("navigation", make_navigation_goal(kRobot2Home), kLongTimeout); + + while (robot_1.is_capability_running("navigation") || + robot_2.is_capability_running("navigation")) + { + controller.spin_some(); + } + + // Safety net: nothing should still be running by this point (perception + // was stopped after phase 2, manipulation after phase 3, and every + // navigation goal above already ran to completion or was explicitly + // redirected), but stop anything left active regardless before + // declaring the mission over. + robot_1.stop_capability("perception"); + robot_2.stop_capability("manipulation"); + + print_step("Phase 5 done: both robots back at their starting pose."); + print_section("Mission complete"); + + controller.shutdown(); + + return 0; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/src/robot_node.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/src/robot_node.cpp new file mode 100644 index 0000000..a690917 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/src/robot_node.cpp @@ -0,0 +1,44 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +// The "simple API" mirror of easyfleet_easynav_collaboration_deployment's +// own robot_node.cpp: hosts the exact same fake capabilities, reads the +// exact same "capabilities"/"config_subdir" parameters (e.g. +// config/robot_1/robot_params.yaml), and produces identical resulting +// capability/action/node names -- but driven through +// easyfleet_core::Deployment instead of that package's hand-rolled +// configure/activate/spin/deactivate/cleanup/shutdown dance and hardcoded +// if-chain factory. The two packages are meant to be interchangeable at +// the launch level (see refactor.md for the full design history): point a +// launch file at this package instead of +// easyfleet_easynav_collaboration_deployment and everything else -- Gazebo, +// navigation, RViz, the mission script -- behaves the same. + +#include "easyfleet_core/deployment.hpp" +#include "easyfleet_core/init.hpp" + +int main(int argc, char ** argv) +{ + easyfleet::init(argc, argv); + + easyfleet::Deployment deployment; + deployment.add_capabilities_from_parameters( + "easyfleet_easynav_collaboration_fastdds_deployment"); + + deployment.start(); + deployment.run(); + deployment.shutdown(); + return 0; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/CMakeLists.txt b/easyfleet_easynav_collaboration_fastdds_deployment/test/CMakeLists.txt new file mode 100644 index 0000000..984123d --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/CMakeLists.txt @@ -0,0 +1,47 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(geometry_msgs REQUIRED) + +# Confine DDS discovery to localhost (subnet-wide traffic is noticeably +# slower and, on loaded/shared machines, occasionally flaky) and give each +# test binary its own ROS_DOMAIN_ID: these tests exercise +# easyfleet_core::Capability, which publishes on the *global* topics +# /capabilities and /capabilities_status, so without domain isolation they +# could cross-talk with any other test binary or capability node using +# those same topics. IDs 175-178 are used here specifically to stay clear +# of easyfleet_easynav_collaboration_deployment's own 171-174: this +# package started as a straight copy of that one (see refactor.md), so its +# test suite originally carried the exact same IDs -- harmless standalone, +# but colcon can run both packages' test binaries concurrently, and same +# domain ID across two *different* packages is exactly the cross-talk this +# comment already warned about, just not for the sibling pointed to here. +ament_add_gtest(test_perception_fake_action_server test_perception_fake_action_server.cpp + TIMEOUT 60 SKIP_LINKING_MAIN_LIBRARIES + ENV ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST ROS_DOMAIN_ID=175) +target_link_libraries(test_perception_fake_action_server + ${PROJECT_NAME} + GTest::gtest +) + +ament_add_gtest(test_perception_fake_capability test_perception_fake_capability.cpp + TIMEOUT 60 SKIP_LINKING_MAIN_LIBRARIES + ENV ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST ROS_DOMAIN_ID=176) +target_link_libraries(test_perception_fake_capability + ${PROJECT_NAME} + GTest::gtest +) + +ament_add_gtest(test_manipulation_fake_action_server test_manipulation_fake_action_server.cpp + TIMEOUT 60 SKIP_LINKING_MAIN_LIBRARIES + ENV ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST ROS_DOMAIN_ID=177) +target_link_libraries(test_manipulation_fake_action_server + ${PROJECT_NAME} + GTest::gtest +) + +ament_add_gtest(test_manipulation_fake_capability test_manipulation_fake_capability.cpp + TIMEOUT 60 SKIP_LINKING_MAIN_LIBRARIES + ENV ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST ROS_DOMAIN_ID=178) +target_link_libraries(test_manipulation_fake_capability + ${PROJECT_NAME} + GTest::gtest +) diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_action_server.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_action_server.cpp new file mode 100644 index 0000000..715a91a --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_action_server.cpp @@ -0,0 +1,279 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp" +#include "test_utils.hpp" + +using namespace std::chrono_literals; +using Manipulation = easyfleet_interfaces::action::Manipulation; +using easyfleet_easynav_collaboration_fastdds_deployment::ManipulationFakeActionServer; +using easyfleet_easynav_collaboration_fastdds_deployment_test::spin_in_background; +using easyfleet_easynav_collaboration_fastdds_deployment_test::unique_test_name; +using easyfleet_easynav_collaboration_fastdds_deployment_test::wait_until; + +namespace +{ + +Manipulation::Goal make_joint_target_goal() +{ + Manipulation::Goal goal; + goal.mode = Manipulation::Goal::MODE_JOINT_TARGET; + goal.joint_target.name = {"joint1", "joint2"}; + goal.joint_target.position = {1.0, -1.0}; + return goal; +} + +} // namespace + +// Mock timing is fixed short and fine-grained for the whole suite (via +// parameter overrides at node construction) so tests run quickly while +// still leaving enough steps to observe intermediate feedback reliably. +class ManipulationFakeActionServerTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + action_name_ = unique_test_name("manipulation_test"); + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter(action_name_ + ".mock_execution_duration", 1.0), + rclcpp::Parameter(action_name_ + ".mock_feedback_period", 0.1), + }); + server_node_ = std::make_shared( + unique_test_name("test_manip_server"), options); + action_server_ = std::make_shared( + *server_node_, action_name_); + + client_node_ = std::make_shared(unique_test_name("test_manip_client")); + client_ = rclcpp_action::create_client(client_node_, action_name_); + + executor_ = std::make_shared(); + executor_->add_node(server_node_->get_node_base_interface()); + executor_->add_node(client_node_); + spin_thread_ = spin_in_background(*executor_); + + ASSERT_TRUE(client_->wait_for_action_server(5s)); + } + + static void TearDownTestSuite() + { + executor_->cancel(); + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + executor_->remove_node(server_node_->get_node_base_interface()); + executor_->remove_node(client_node_); + executor_.reset(); + client_.reset(); + client_node_.reset(); + action_server_.reset(); + server_node_.reset(); + } + + inline static std::string action_name_; + inline static rclcpp_lifecycle::LifecycleNode::SharedPtr server_node_; + inline static std::shared_ptr action_server_; + inline static rclcpp::Node::SharedPtr client_node_; + inline static rclcpp_action::Client::SharedPtr client_; + inline static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + inline static std::thread spin_thread_; +}; + +TEST_F(ManipulationFakeActionServerTest, ExposesConfiguredActionName) +{ + EXPECT_EQ(action_server_->get_action_name(), action_name_); +} + +TEST_F(ManipulationFakeActionServerTest, RejectsGoalWithEmptyJointTarget) +{ + Manipulation::Goal goal; + goal.mode = Manipulation::Goal::MODE_JOINT_TARGET; + + auto future = client_->async_send_goal(goal); + ASSERT_EQ(future.wait_for(2s), std::future_status::ready); + EXPECT_FALSE(future.get()); +} + +TEST_F(ManipulationFakeActionServerTest, AcceptsGoalAndSucceedsWithNoError) +{ + auto goal = make_joint_target_goal(); + + auto goal_handle_future = client_->async_send_goal(goal); + ASSERT_EQ(goal_handle_future.wait_for(2s), std::future_status::ready); + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle); + + auto result_future = client_->async_get_result(goal_handle); + ASSERT_EQ(result_future.wait_for(3s), std::future_status::ready); + auto wrapped = result_future.get(); + EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED); + ASSERT_TRUE(wrapped.result); + EXPECT_EQ(wrapped.result->error_code, Manipulation::Result::SUCCESS); +} + +TEST_F(ManipulationFakeActionServerTest, FeedbackReportsExecutingState) +{ + auto goal = make_joint_target_goal(); + + std::mutex mutex; + std::vector states; + rclcpp_action::Client::SendGoalOptions options; + options.feedback_callback = + [&mutex, &states]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + std::lock_guard lock(mutex); + states.push_back(feedback->state); + }; + + auto goal_handle_future = client_->async_send_goal(goal, options); + ASSERT_EQ(goal_handle_future.wait_for(2s), std::future_status::ready); + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle); + + auto result_future = client_->async_get_result(goal_handle); + ASSERT_EQ(result_future.wait_for(3s), std::future_status::ready); + EXPECT_EQ(result_future.get().code, rclcpp_action::ResultCode::SUCCEEDED); + + std::lock_guard lock(mutex); + ASSERT_GE(states.size(), 2u); + for (const auto & state : states) { + EXPECT_EQ(state, "EXECUTING"); + } +} + +TEST_F(ManipulationFakeActionServerTest, ClientCancelResultsInCanceled) +{ + auto goal = make_joint_target_goal(); + + auto goal_handle_future = client_->async_send_goal(goal); + ASSERT_EQ(goal_handle_future.wait_for(2s), std::future_status::ready); + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle); + + EXPECT_TRUE(wait_until([this] {return action_server_->is_active();}, 1s)); + + auto cancel_future = client_->async_cancel_goal(goal_handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + + auto result_future = client_->async_get_result(goal_handle); + ASSERT_EQ(result_future.wait_for(3s), std::future_status::ready); + auto wrapped = result_future.get(); + EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::CANCELED); + ASSERT_TRUE(wrapped.result); + EXPECT_EQ(wrapped.result->error_code, Manipulation::Result::CANCELED); +} + +TEST_F(ManipulationFakeActionServerTest, NewGoalPreemptsRunningGoalByDefault) +{ + EXPECT_TRUE(action_server_->is_preemptable()); + + auto first_goal = make_joint_target_goal(); + auto first_handle_future = client_->async_send_goal(first_goal); + ASSERT_EQ(first_handle_future.wait_for(2s), std::future_status::ready); + auto first_handle = first_handle_future.get(); + ASSERT_TRUE(first_handle); + EXPECT_TRUE(wait_until([this] {return action_server_->is_active();}, 1s)); + + auto first_result_future = client_->async_get_result(first_handle); + + auto second_goal = make_joint_target_goal(); + auto second_handle_future = client_->async_send_goal(second_goal); + ASSERT_EQ(second_handle_future.wait_for(2s), std::future_status::ready); + auto second_handle = second_handle_future.get(); + ASSERT_TRUE(second_handle) << "second goal should be accepted since preemption is allowed"; + + ASSERT_EQ(first_result_future.wait_for(3s), std::future_status::ready); + EXPECT_EQ(first_result_future.get().code, rclcpp_action::ResultCode::ABORTED); + + auto second_result_future = client_->async_get_result(second_handle); + ASSERT_EQ(second_result_future.wait_for(3s), std::future_status::ready); + EXPECT_EQ(second_result_future.get().code, rclcpp_action::ResultCode::SUCCEEDED); +} + +TEST(ManipulationFakeActionServerStandaloneTest, NonPreemptableRejectsSecondGoalWhileBusy) +{ + const auto action_name = unique_test_name("manipulation_np"); + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter(action_name + ".allow_preemption", false), + rclcpp::Parameter(action_name + ".mock_execution_duration", 1.0), + rclcpp::Parameter(action_name + ".mock_feedback_period", 0.1), + }); + auto server_node = std::make_shared( + unique_test_name("test_manip_server_np"), options); + auto action_server = std::make_shared( + *server_node, action_name); + ASSERT_FALSE(action_server->is_preemptable()); + + auto client_node = std::make_shared(unique_test_name("test_manip_client_np")); + auto client = rclcpp_action::create_client(client_node, action_name); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(server_node->get_node_base_interface()); + executor.add_node(client_node); + auto spin_thread = spin_in_background(executor); + + ASSERT_TRUE(client->wait_for_action_server(5s)); + + auto first_goal = make_joint_target_goal(); + auto first_handle_future = client->async_send_goal(first_goal); + ASSERT_EQ(first_handle_future.wait_for(2s), std::future_status::ready); + auto first_handle = first_handle_future.get(); + ASSERT_TRUE(first_handle); + ASSERT_TRUE(wait_until([&] {return action_server->is_active();}, 1s)); + + auto second_goal = make_joint_target_goal(); + auto second_handle_future = client->async_send_goal(second_goal); + ASSERT_EQ(second_handle_future.wait_for(2s), std::future_status::ready); + EXPECT_FALSE(second_handle_future.get()) + << "second goal should have been rejected while the first is busy"; + + auto first_result_future = client->async_get_result(first_handle); + ASSERT_EQ(first_result_future.wait_for(3s), std::future_status::ready); + EXPECT_EQ(first_result_future.get().code, rclcpp_action::ResultCode::SUCCEEDED); + + executor.cancel(); + spin_thread.join(); + executor.remove_node(server_node->get_node_base_interface()); + executor.remove_node(client_node); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_capability.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_capability.cpp new file mode 100644 index 0000000..3ee4f33 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_manipulation_fake_capability.cpp @@ -0,0 +1,353 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "easyfleet_easynav_collaboration_fastdds_deployment/manipulation_fake_capability.hpp" +#include "easyfleet_interfaces/msg/capability_description.hpp" +#include "easyfleet_interfaces/msg/capability_status.hpp" +#include "test_utils.hpp" + +using namespace std::chrono_literals; +using easyfleet_interfaces::msg::CapabilityDescription; +using easyfleet_interfaces::msg::CapabilityStatus; +using Manipulation = easyfleet_interfaces::action::Manipulation; +using easyfleet_easynav_collaboration_fastdds_deployment::ManipulationFakeCapability; +using easyfleet_easynav_collaboration_fastdds_deployment_test::spin_in_background; +using easyfleet_easynav_collaboration_fastdds_deployment_test::unique_test_name; +using easyfleet_easynav_collaboration_fastdds_deployment_test::wait_until; + +namespace +{ + +// Capability packages no longer ship a default capabilities JSON of their +// own (that lives under the deployment scenarios in +// src/EasyFleet/easyfleet_easynav_collaboration_fastdds_deployment), so tests +// exercise the publish-file-verbatim behavior against a JSON they write +// themselves. +constexpr char kSampleCapabilitiesJson[] = + R"json({ + "name": "manipulation", + "display_name": "Reach a joint target", + "action": { + "name": "/manipulation", + "type": "easyfleet_interfaces/action/Manipulation" + }, + "requirements": [ + "The target joint configuration must already be free of collisions." + ], + "effects": [ + "On success, the manipulator's joints end up at the requested target." + ], + "parameters": { + "manipulation.allow_preemption": { + "type": "bool", + "default": true + } + } +} +)json"; + +std::string write_sample_capabilities_file() +{ + const auto path = "/tmp/" + unique_test_name("manipulation_capabilities") + ".json"; + std::ofstream file(path); + file << kSampleCapabilitiesJson; + return path; +} + +Manipulation::Goal make_joint_target_goal() +{ + Manipulation::Goal goal; + goal.mode = Manipulation::Goal::MODE_JOINT_TARGET; + goal.joint_target.name = {"joint1"}; + goal.joint_target.position = {1.0}; + return goal; +} + +template +class Collector +{ +public: + void callback(const typename MsgT::SharedPtr msg) + { + std::lock_guard lock(mutex_); + messages_.push_back(*msg); + } + + std::vector messages() const + { + std::lock_guard lock(mutex_); + return messages_; + } + + std::size_t count() const + { + std::lock_guard lock(mutex_); + return messages_.size(); + } + +private: + mutable std::mutex mutex_; + std::vector messages_; +}; + +} // namespace + +// ManipulationFakeCapability's node/action name ("manipulation") is fixed by +// design (it isn't parameterized), so unlike other fixtures in this project +// the capability is created and fully torn down per test rather than shared +// across the suite. +class ManipulationFakeCapabilityTest : public ::testing::Test +{ +protected: + void SetUp() override + { + // ManipulationFakeActionServer reads its mock timing parameters once, at + // construction time, so they must be overridden via NodeOptions (not + // set_parameter() afterwards) to actually take effect. Keeping the + // whole suite on a short mock duration also keeps it fast. + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("manipulation.mock_execution_duration", 0.5), + rclcpp::Parameter("manipulation.mock_feedback_period", 0.1), + }); + capability_ = std::make_shared(options); + sub_node_ = std::make_shared(unique_test_name("test_capability_subscriber")); + + executor_.add_node(capability_->get_node_base_interface()); + executor_.add_node(sub_node_); + spin_thread_ = spin_in_background(executor_); + } + + void TearDown() override + { + executor_.cancel(); + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + description_subs_.clear(); + status_subs_.clear(); + executor_.remove_node(capability_->get_node_base_interface()); + executor_.remove_node(sub_node_); + capability_.reset(); + sub_node_.reset(); + } + + std::shared_ptr> subscribe_descriptions() + { + auto collector = std::make_shared>(); + auto sub = sub_node_->create_subscription( + "/capabilities", rclcpp::QoS(1).reliable().transient_local(), + [collector](const CapabilityDescription::SharedPtr msg) {collector->callback(msg);}); + description_subs_.push_back(sub); + return collector; + } + + std::shared_ptr> subscribe_status() + { + auto collector = std::make_shared>(); + auto sub = sub_node_->create_subscription( + "/capabilities_status", rclcpp::QoS(10).reliable(), + [collector](const CapabilityStatus::SharedPtr msg) {collector->callback(msg);}); + status_subs_.push_back(sub); + return collector; + } + + std::shared_ptr capability_; + rclcpp::Node::SharedPtr sub_node_; + rclcpp::executors::SingleThreadedExecutor executor_; + std::thread spin_thread_; + std::vector description_subs_; + std::vector status_subs_; +}; + +TEST_F(ManipulationFakeCapabilityTest, StartsUnconfigured) +{ + EXPECT_EQ( + capability_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(ManipulationFakeCapabilityTest, NodeAndActionAreNamedManipulation) +{ + EXPECT_EQ(std::string(capability_->get_name()), "manipulation"); + EXPECT_EQ(capability_->get_capability_name(), "manipulation"); + ASSERT_TRUE(capability_->get_action_server()); + EXPECT_EQ(capability_->get_action_server()->get_action_name(), "manipulation"); + EXPECT_EQ(capability_->get_resolved_action_name(), "/manipulation"); + EXPECT_EQ(capability_->get_robot_name(), ""); +} + +TEST_F(ManipulationFakeCapabilityTest, ActivateWithoutCapabilitiesFileParameterFails) +{ + capability_->configure(); + auto cb = ManipulationFakeCapability::CallbackReturn::SUCCESS; + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, ManipulationFakeCapability::CallbackReturn::FAILURE); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} + +TEST_F(ManipulationFakeCapabilityTest, ActivatePublishesTheProvidedCapabilitiesJsonVerbatim) +{ + const auto path = write_sample_capabilities_file(); + + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_descriptions(); + + auto cb = ManipulationFakeCapability::CallbackReturn::FAILURE; + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, ManipulationFakeCapability::CallbackReturn::SUCCESS); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); + const CapabilityDescription msg = collector->messages().front(); + EXPECT_EQ(msg.description_json, kSampleCapabilitiesJson); + EXPECT_EQ(msg.capability, "manipulation"); + EXPECT_EQ(msg.robot, ""); + EXPECT_EQ(msg.action_name, "/manipulation"); +} + +TEST_F(ManipulationFakeCapabilityTest, PublishedCapabilitiesJsonIsWellFormed) +{ + // Not a full JSON parser: checks the shape an LLM/planner consuming this + // file would expect, without adding a JSON parsing dependency. + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_descriptions(); + capability_->activate(); + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); + + const std::string json = collector->messages().front().description_json; + EXPECT_NE(json.find("\"name\""), std::string::npos); + EXPECT_NE(json.find("\"action\""), std::string::npos); + EXPECT_NE(json.find("Manipulation"), std::string::npos); + EXPECT_NE(json.find("\"requirements\""), std::string::npos); + EXPECT_NE(json.find("\"effects\""), std::string::npos); + EXPECT_NE(json.find("\"parameters\""), std::string::npos); + const auto last_non_space = json.find_last_not_of(" \t\r\n"); + EXPECT_EQ(json.front(), '{'); + ASSERT_NE(last_non_space, std::string::npos); + EXPECT_EQ(json[last_non_space], '}'); +} + +TEST_F(ManipulationFakeCapabilityTest, HeartbeatPublishesIdentityEverySecondWhileActive) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_status(); + capability_->activate(); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 2;}, 3s)); + for (const auto & msg : collector->messages()) { + EXPECT_EQ(msg.capability, "manipulation"); + EXPECT_EQ(msg.robot, ""); + EXPECT_EQ(msg.action_name, "/manipulation"); + EXPECT_FALSE(msg.busy); + } +} + +TEST_F(ManipulationFakeCapabilityTest, DeactivateStopsTheHeartbeat) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_status(); + capability_->activate(); + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); + + capability_->deactivate(); + const auto count_after_deactivate = collector->count(); + std::this_thread::sleep_for(1500ms); + EXPECT_EQ(collector->count(), count_after_deactivate); +} + +TEST_F(ManipulationFakeCapabilityTest, CleanupThenReconfigureAndActivateAgainWorks) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + capability_->activate(); + capability_->deactivate(); + capability_->cleanup(); + EXPECT_EQ( + capability_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto collector = subscribe_descriptions(); + + auto cb = ManipulationFakeCapability::CallbackReturn::FAILURE; + capability_->configure(); + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, ManipulationFakeCapability::CallbackReturn::SUCCESS); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); +} + +TEST_F(ManipulationFakeCapabilityTest, ActiveCapabilityCompletesARealJointTargetGoal) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + capability_->activate(); + + auto client = rclcpp_action::create_client(sub_node_, "manipulation"); + ASSERT_TRUE(client->wait_for_action_server(5s)); + + auto goal = make_joint_target_goal(); + + auto goal_handle_future = client->async_send_goal(goal); + ASSERT_EQ(goal_handle_future.wait_for(2s), std::future_status::ready); + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle); + + auto result_future = client->async_get_result(goal_handle); + ASSERT_EQ(result_future.wait_for(3s), std::future_status::ready); + auto wrapped = result_future.get(); + EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED); + ASSERT_TRUE(wrapped.result); + EXPECT_EQ(wrapped.result->error_code, Manipulation::Result::SUCCESS); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_action_server.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_action_server.cpp new file mode 100644 index 0000000..8ed88f4 --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_action_server.cpp @@ -0,0 +1,278 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp" +#include "test_utils.hpp" + +using namespace std::chrono_literals; +using Perception = easyfleet_interfaces::action::Perception; +using easyfleet_easynav_collaboration_fastdds_deployment::PerceptionFakeActionServer; +using easyfleet_easynav_collaboration_fastdds_deployment_test::spin_in_background; +using easyfleet_easynav_collaboration_fastdds_deployment_test::unique_test_name; +using easyfleet_easynav_collaboration_fastdds_deployment_test::wait_until; + +namespace +{ + +Perception::Goal make_goal(const std::string & object_class = "gato") +{ + Perception::Goal goal; + goal.object_classes.push_back(object_class); + return goal; +} + +} // namespace + +// Mock detection rate is fixed fast (via parameter overrides at node +// construction) for the whole suite, so tests collecting several feedback +// messages stay quick. +class PerceptionFakeActionServerTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + action_name_ = unique_test_name("perception_test"); + + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter(action_name_ + ".detection_rate_hz", 50.0), + rclcpp::Parameter(action_name_ + ".target_class", "gato"), + }); + server_node_ = std::make_shared( + unique_test_name("test_perception_server"), options); + action_server_ = std::make_shared(*server_node_, action_name_); + + client_node_ = std::make_shared(unique_test_name("test_perception_client")); + client_ = rclcpp_action::create_client(client_node_, action_name_); + + executor_ = std::make_shared(); + executor_->add_node(server_node_->get_node_base_interface()); + executor_->add_node(client_node_); + spin_thread_ = spin_in_background(*executor_); + + ASSERT_TRUE(client_->wait_for_action_server(5s)); + } + + static void TearDownTestSuite() + { + executor_->cancel(); + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + executor_->remove_node(server_node_->get_node_base_interface()); + executor_->remove_node(client_node_); + executor_.reset(); + client_.reset(); + client_node_.reset(); + action_server_.reset(); + server_node_.reset(); + } + + inline static std::string action_name_; + inline static rclcpp_lifecycle::LifecycleNode::SharedPtr server_node_; + inline static std::shared_ptr action_server_; + inline static rclcpp::Node::SharedPtr client_node_; + inline static rclcpp_action::Client::SharedPtr client_; + inline static rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + inline static std::thread spin_thread_; +}; + +TEST_F(PerceptionFakeActionServerTest, ExposesConfiguredActionName) +{ + EXPECT_EQ(action_server_->get_action_name(), action_name_); +} + +TEST_F(PerceptionFakeActionServerTest, RejectsGoalWithEmptyObjectClassesList) +{ + Perception::Goal goal; + auto future = client_->async_send_goal(goal); + ASSERT_EQ(future.wait_for(2s), std::future_status::ready); + EXPECT_FALSE(future.get()); +} + +TEST_F(PerceptionFakeActionServerTest, RejectsGoalForUnsupportedClass) +{ + auto future = client_->async_send_goal(make_goal("perro")); + ASSERT_EQ(future.wait_for(2s), std::future_status::ready); + EXPECT_FALSE(future.get()); +} + +TEST_F(PerceptionFakeActionServerTest, AcceptsTargetClassCaseInsensitiveAndAsSubstring) +{ + auto handle_future = client_->async_send_goal(make_goal("Gatos")); + ASSERT_EQ(handle_future.wait_for(2s), std::future_status::ready); + auto handle = handle_future.get(); + ASSERT_TRUE(handle); + + ASSERT_TRUE(wait_until([this] {return action_server_->is_active();}, 1s)); + auto cancel_future = client_->async_cancel_goal(handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + auto result_future = client_->async_get_result(handle); + ASSERT_EQ(result_future.wait_for(2s), std::future_status::ready); + EXPECT_EQ(result_future.get().code, rclcpp_action::ResultCode::CANCELED); +} + +TEST_F(PerceptionFakeActionServerTest, StreamsOneToThreeDetectionsInBoth2dAnd3dUntilCanceled) +{ + std::mutex mutex; + std::vector> feedbacks; + rclcpp_action::Client::SendGoalOptions options; + options.feedback_callback = + [&mutex, &feedbacks]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) + { + std::lock_guard lock(mutex); + feedbacks.push_back(feedback); + }; + + auto handle_future = client_->async_send_goal(make_goal(), options); + ASSERT_EQ(handle_future.wait_for(2s), std::future_status::ready); + auto handle = handle_future.get(); + ASSERT_TRUE(handle); + + ASSERT_TRUE( + wait_until( + [&] { + std::lock_guard lock(mutex); + return feedbacks.size() >= 5; + }, 2s)); + + auto cancel_future = client_->async_cancel_goal(handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + auto result_future = client_->async_get_result(handle); + ASSERT_EQ(result_future.wait_for(2s), std::future_status::ready); + auto wrapped = result_future.get(); + EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::CANCELED); + ASSERT_TRUE(wrapped.result); + EXPECT_EQ(wrapped.result->error_code, Perception::Result::CANCELED); + + std::lock_guard lock(mutex); + for (const auto & feedback : feedbacks) { + ASSERT_GE(feedback->detections_3d.detections.size(), 1u); + ASSERT_LE(feedback->detections_3d.detections.size(), 3u); + EXPECT_EQ(feedback->detections_2d.detections.size(), feedback->detections_3d.detections.size()); + for (const auto & detection : feedback->detections_3d.detections) { + ASSERT_FALSE(detection.results.empty()); + EXPECT_EQ(detection.results.front().hypothesis.class_id, "gato"); + } + for (const auto & detection : feedback->detections_2d.detections) { + ASSERT_FALSE(detection.results.empty()); + EXPECT_EQ(detection.results.front().hypothesis.class_id, "gato"); + } + } +} + +TEST_F(PerceptionFakeActionServerTest, NewGoalPreemptsRunningGoalByDefault) +{ + EXPECT_TRUE(action_server_->is_preemptable()); + + auto first_handle_future = client_->async_send_goal(make_goal()); + ASSERT_EQ(first_handle_future.wait_for(2s), std::future_status::ready); + auto first_handle = first_handle_future.get(); + ASSERT_TRUE(first_handle); + ASSERT_TRUE(wait_until([this] {return action_server_->is_active();}, 1s)); + + auto first_result_future = client_->async_get_result(first_handle); + + auto second_handle_future = client_->async_send_goal(make_goal()); + ASSERT_EQ(second_handle_future.wait_for(2s), std::future_status::ready); + auto second_handle = second_handle_future.get(); + ASSERT_TRUE(second_handle) << "second goal should be accepted since preemption is allowed"; + + ASSERT_EQ(first_result_future.wait_for(2s), std::future_status::ready); + auto first_wrapped = first_result_future.get(); + EXPECT_EQ(first_wrapped.code, rclcpp_action::ResultCode::ABORTED); + ASSERT_TRUE(first_wrapped.result); + EXPECT_EQ(first_wrapped.result->error_code, Perception::Result::ABORTED); + + ASSERT_TRUE(wait_until([this] {return action_server_->is_active();}, 1s)); + auto cancel_future = client_->async_cancel_goal(second_handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + auto second_result_future = client_->async_get_result(second_handle); + ASSERT_EQ(second_result_future.wait_for(2s), std::future_status::ready); + EXPECT_EQ(second_result_future.get().code, rclcpp_action::ResultCode::CANCELED); +} + +TEST(PerceptionFakeActionServerStandaloneTest, NonPreemptableRejectsSecondGoalWhileBusy) +{ + const auto action_name = unique_test_name("perception_np"); + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter(action_name + ".allow_preemption", false), + rclcpp::Parameter(action_name + ".detection_rate_hz", 50.0), + }); + auto server_node = std::make_shared( + unique_test_name("test_perception_server_np"), options); + auto action_server = std::make_shared(*server_node, action_name); + ASSERT_FALSE(action_server->is_preemptable()); + + auto client_node = std::make_shared(unique_test_name("test_perception_client_np")); + auto client = rclcpp_action::create_client(client_node, action_name); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(server_node->get_node_base_interface()); + executor.add_node(client_node); + auto spin_thread = spin_in_background(executor); + + ASSERT_TRUE(client->wait_for_action_server(5s)); + + auto first_handle_future = client->async_send_goal(make_goal()); + ASSERT_EQ(first_handle_future.wait_for(2s), std::future_status::ready); + auto first_handle = first_handle_future.get(); + ASSERT_TRUE(first_handle); + ASSERT_TRUE(wait_until([&] {return action_server->is_active();}, 1s)); + + auto second_handle_future = client->async_send_goal(make_goal()); + ASSERT_EQ(second_handle_future.wait_for(2s), std::future_status::ready); + EXPECT_FALSE(second_handle_future.get()) + << "second goal should have been rejected while the first is busy"; + + auto cancel_future = client->async_cancel_goal(first_handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + auto first_result_future = client->async_get_result(first_handle); + ASSERT_EQ(first_result_future.wait_for(2s), std::future_status::ready); + EXPECT_EQ(first_result_future.get().code, rclcpp_action::ResultCode::CANCELED); + + executor.cancel(); + spin_thread.join(); + executor.remove_node(server_node->get_node_base_interface()); + executor.remove_node(client_node); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_capability.cpp b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_capability.cpp new file mode 100644 index 0000000..a6b36bf --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_perception_fake_capability.cpp @@ -0,0 +1,314 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +#include "easyfleet_easynav_collaboration_fastdds_deployment/perception_fake_capability.hpp" +#include "easyfleet_interfaces/msg/capability_description.hpp" +#include "easyfleet_interfaces/msg/capability_status.hpp" +#include "test_utils.hpp" + +using namespace std::chrono_literals; +using easyfleet_interfaces::msg::CapabilityDescription; +using easyfleet_interfaces::msg::CapabilityStatus; +using Perception = easyfleet_interfaces::action::Perception; +using easyfleet_easynav_collaboration_fastdds_deployment::PerceptionFakeCapability; +using easyfleet_easynav_collaboration_fastdds_deployment_test::spin_in_background; +using easyfleet_easynav_collaboration_fastdds_deployment_test::unique_test_name; +using easyfleet_easynav_collaboration_fastdds_deployment_test::wait_until; + +namespace +{ + +// Capability packages no longer ship a default capabilities JSON of their +// own (that lives under the deployment scenarios in +// src/EasyFleet/easyfleet_easynav_collaboration_fastdds_deployment), so tests +// exercise the publish-file-verbatim behavior against a JSON they write +// themselves. +constexpr char kSampleCapabilitiesJson[] = + R"json({ + "name": "perception", + "display_name": "Detect objects of a known class", + "action": { + "name": "/perception", + "type": "easyfleet_interfaces/action/Perception" + }, + "requirements": [ + "The requested 'object_classes' list must include the target class this mock supports." + ], + "effects": [ + "None on the physical world: this is a perception-only capability." + ], + "parameters": { + "perception.allow_preemption": { + "type": "bool", + "default": true + } + } +} +)json"; + +std::string write_sample_capabilities_file() +{ + const auto path = "/tmp/" + unique_test_name("perception_capabilities") + ".json"; + std::ofstream file(path); + file << kSampleCapabilitiesJson; + return path; +} + +template +class Collector +{ +public: + void callback(const typename MsgT::SharedPtr msg) + { + std::lock_guard lock(mutex_); + messages_.push_back(*msg); + } + + std::vector messages() const + { + std::lock_guard lock(mutex_); + return messages_; + } + + std::size_t count() const + { + std::lock_guard lock(mutex_); + return messages_.size(); + } + +private: + mutable std::mutex mutex_; + std::vector messages_; +}; + +} // namespace + +// PerceptionFakeCapability's node/action name ("perception") is fixed by design +// (it isn't parameterized), so the capability is created and fully torn +// down per test rather than shared across the suite. +class PerceptionFakeCapabilityTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::NodeOptions options; + options.parameter_overrides( + { + rclcpp::Parameter("perception.detection_rate_hz", 50.0), + }); + capability_ = std::make_shared(options); + sub_node_ = std::make_shared(unique_test_name("test_capability_subscriber")); + + executor_.add_node(capability_->get_node_base_interface()); + executor_.add_node(sub_node_); + spin_thread_ = spin_in_background(executor_); + } + + void TearDown() override + { + executor_.cancel(); + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + description_subs_.clear(); + status_subs_.clear(); + executor_.remove_node(capability_->get_node_base_interface()); + executor_.remove_node(sub_node_); + capability_.reset(); + sub_node_.reset(); + } + + std::shared_ptr> subscribe_descriptions() + { + auto collector = std::make_shared>(); + auto sub = sub_node_->create_subscription( + "/capabilities", rclcpp::QoS(1).reliable().transient_local(), + [collector](const CapabilityDescription::SharedPtr msg) {collector->callback(msg);}); + description_subs_.push_back(sub); + return collector; + } + + std::shared_ptr> subscribe_status() + { + auto collector = std::make_shared>(); + auto sub = sub_node_->create_subscription( + "/capabilities_status", rclcpp::QoS(10).reliable(), + [collector](const CapabilityStatus::SharedPtr msg) {collector->callback(msg);}); + status_subs_.push_back(sub); + return collector; + } + + std::shared_ptr capability_; + rclcpp::Node::SharedPtr sub_node_; + rclcpp::executors::SingleThreadedExecutor executor_; + std::thread spin_thread_; + std::vector description_subs_; + std::vector status_subs_; +}; + +TEST_F(PerceptionFakeCapabilityTest, StartsUnconfigured) +{ + EXPECT_EQ( + capability_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(PerceptionFakeCapabilityTest, NodeAndActionAreNamedPerception) +{ + EXPECT_EQ(std::string(capability_->get_name()), "perception"); + EXPECT_EQ(capability_->get_capability_name(), "perception"); + ASSERT_TRUE(capability_->get_action_server()); + EXPECT_EQ(capability_->get_action_server()->get_action_name(), "perception"); + EXPECT_EQ(capability_->get_resolved_action_name(), "/perception"); + EXPECT_EQ(capability_->get_robot_name(), ""); +} + +TEST_F(PerceptionFakeCapabilityTest, ActivateWithoutCapabilitiesFileParameterFails) +{ + capability_->configure(); + auto cb = PerceptionFakeCapability::CallbackReturn::SUCCESS; + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, PerceptionFakeCapability::CallbackReturn::FAILURE); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} + +TEST_F(PerceptionFakeCapabilityTest, ActivatePublishesTheProvidedCapabilitiesJsonVerbatim) +{ + const auto path = write_sample_capabilities_file(); + + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_descriptions(); + + auto cb = PerceptionFakeCapability::CallbackReturn::FAILURE; + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, PerceptionFakeCapability::CallbackReturn::SUCCESS); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); + const CapabilityDescription msg = collector->messages().front(); + EXPECT_EQ(msg.description_json, kSampleCapabilitiesJson); + EXPECT_EQ(msg.capability, "perception"); + EXPECT_EQ(msg.robot, ""); + EXPECT_EQ(msg.action_name, "/perception"); +} + +TEST_F(PerceptionFakeCapabilityTest, HeartbeatPublishesIdentityEverySecondWhileActive) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + + auto collector = subscribe_status(); + capability_->activate(); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 2;}, 3s)); + for (const auto & msg : collector->messages()) { + EXPECT_EQ(msg.capability, "perception"); + EXPECT_EQ(msg.robot, ""); + EXPECT_EQ(msg.action_name, "/perception"); + EXPECT_FALSE(msg.busy); + } +} + +TEST_F(PerceptionFakeCapabilityTest, CleanupThenReconfigureAndActivateAgainWorks) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + capability_->activate(); + capability_->deactivate(); + capability_->cleanup(); + EXPECT_EQ( + capability_->get_current_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + auto collector = subscribe_descriptions(); + + auto cb = PerceptionFakeCapability::CallbackReturn::FAILURE; + capability_->configure(); + const auto & state = capability_->activate(cb); + EXPECT_EQ(cb, PerceptionFakeCapability::CallbackReturn::SUCCESS); + EXPECT_EQ(state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + ASSERT_TRUE(wait_until([&] {return collector->count() >= 1;}, 2s)); +} + +TEST_F(PerceptionFakeCapabilityTest, ActiveCapabilityStreamsDetectionsUntilCanceled) +{ + const auto path = write_sample_capabilities_file(); + capability_->set_parameter(rclcpp::Parameter("capabilities_file", path)); + capability_->configure(); + capability_->activate(); + + auto client = rclcpp_action::create_client(sub_node_, "perception"); + ASSERT_TRUE(client->wait_for_action_server(5s)); + + Perception::Goal goal; + goal.object_classes.push_back("gato"); + + std::atomic feedback_count{0}; + rclcpp_action::Client::SendGoalOptions options; + options.feedback_callback = + [&feedback_count]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) + { + feedback_count.fetch_add(1); + }; + + auto goal_handle_future = client->async_send_goal(goal, options); + ASSERT_EQ(goal_handle_future.wait_for(2s), std::future_status::ready); + auto goal_handle = goal_handle_future.get(); + ASSERT_TRUE(goal_handle); + + ASSERT_TRUE(wait_until([&] {return feedback_count.load() >= 5;}, 2s)); + + auto cancel_future = client->async_cancel_goal(goal_handle); + ASSERT_EQ(cancel_future.wait_for(2s), std::future_status::ready); + + auto result_future = client->async_get_result(goal_handle); + ASSERT_EQ(result_future.wait_for(2s), std::future_status::ready); + auto wrapped = result_future.get(); + EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::CANCELED); + ASSERT_TRUE(wrapped.result); + EXPECT_EQ(wrapped.result->error_code, Perception::Result::CANCELED); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/easyfleet_easynav_collaboration_fastdds_deployment/test/test_utils.hpp b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_utils.hpp new file mode 100644 index 0000000..2068c0a --- /dev/null +++ b/easyfleet_easynav_collaboration_fastdds_deployment/test/test_utils.hpp @@ -0,0 +1,72 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +#ifndef EASYFLEET_EASYNAV_COLLABORATION_DEPLOYMENT__TEST__TEST_UTILS_HPP_ +#define EASYFLEET_EASYNAV_COLLABORATION_DEPLOYMENT__TEST__TEST_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/executor.hpp" + +namespace easyfleet_easynav_collaboration_fastdds_deployment_test +{ + +/// Appends a process-wide unique suffix to `base`, so tests that create +/// their own throwaway nodes/actions never collide with each other. +inline std::string unique_test_name(const std::string & base) +{ + static std::atomic counter{0}; + return base + "_" + std::to_string(counter.fetch_add(1)); +} + +/// Polls `pred` until it returns true or `timeout` elapses. Returns the last +/// value of `pred`. Used instead of fixed sleeps to keep tests fast and +/// non-flaky. +template +bool wait_until( + Predicate pred, + std::chrono::milliseconds timeout, + std::chrono::milliseconds poll = std::chrono::milliseconds(10)) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (pred()) { + return true; + } + std::this_thread::sleep_for(poll); + } + return pred(); +} + +/// Starts `executor.spin()` on a background thread and blocks until it has +/// actually begun spinning before returning it. `Executor::cancel()` only +/// reliably interrupts a `spin()` that has already started: calling it any +/// earlier races with the thread below and can block `join()` forever. +inline std::thread spin_in_background(rclcpp::Executor & executor) +{ + std::thread thread([&executor] {executor.spin();}); + while (!executor.is_spinning()) { + std::this_thread::yield(); + } + return thread; +} + +} // namespace easyfleet_easynav_collaboration_fastdds_deployment_test + +#endif // EASYFLEET_EASYNAV_COLLABORATION_DEPLOYMENT__TEST__TEST_UTILS_HPP_ diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/README.md b/easyfleet_easynav_collaboration_simple_api_deployment/README.md new file mode 100644 index 0000000..4876044 --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment/README.md @@ -0,0 +1,80 @@ +# easyfleet_easynav_collaboration_simple_api_deployment + +The two-robot EasyNav scenario of [easyfleet_easynav_collaboration_deployment](../easyfleet_easynav_collaboration_deployment): same robots, world, waypoints and five-phase mission. This version is written with the **simple API**, which is the one to copy for new deployments: + +- **Robot side:** `robot_node.cpp` is about ten lines built on `easyfleet::Deployment`. It loads the fake capabilities as pluginlib plugins, named in the `capabilities` parameter, and takes care of their lifecycle. +- **Mission side:** the mission uses `easyfleet::SimpleController` and `RobotHandle` (`has_capability()`, `run_capability()`, ...). Status markers in RViz are published automatically for each goal. + +There is also a Python version: [easyfleet_easynav_collaboration_simple_api_deployment_py](../easyfleet_easynav_collaboration_simple_api_deployment_py). The [zenoh](../easyfleet_easynav_collaboration_zenoh_deployment) and [fastdds](../easyfleet_easynav_collaboration_fastdds_deployment) variants are built on this package's code. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Robot | Namespace | Spawn | Capabilities | +|---|---|---|---| +| `robot_1` | `/robot_1` | (0, 0) | `navigation` (EasyNav), `perception` (fake, `gato` detections) | +| `robot_2` | `/robot_2` | (2, 1) | `navigation` (EasyNav), `manipulation` (fake, 8 s execution) | + +Each robot runs these processes, all under its namespace: + +- `system_main`: EasyNav with costmap AMCL, costmap planner and RPP. It uses `tf_prefix=robot_N`. +- `navigation`: `easynav_navigation_capability_node`, from `easyfleet_easynav_deployment`. +- `robot_node`: the `Deployment` that hosts the fake capability. +- Its own RViz, off by default. + +Each robot has its own TF tree on `/robot_N/tf`. + +Central, unnamespaced processes: + +- `navigation_manager_node`: + - Publishes `/global_map` and `/global_routes`. + - Relays `odom`, `base_footprint` and `base_link` of each robot to `/tf` at 10 Hz. +- The central RViz: `navigation_manager.rviz` from `easyfleet_easynav_deployment`, showing the global map, the routes, each robot's `base_link` axes and each robot's path. +- `easynav_collaboration_mission_node`: started 5 s after the rest. +- `rmw_zenohd`: the Zenoh router, from `simulation_launch.yaml`. + +## How to run + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Zenoh router + Gazebo (small house) + both Kobukis +ros2 launch easyfleet_easynav_collaboration_simple_api_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: both robots + navigation manager + central RViz + mission +ros2 launch easyfleet_easynav_collaboration_simple_api_deployment easynav_collaboration_launch.yaml +``` + +`robot_rviz:=true` also opens each robot's RViz. `robot_N_launch.yaml` starts a single robot, with its RViz on by default (`rviz:=false`). + +## What you should see + +The output is the same as in [easyfleet_easynav_collaboration_deployment](../easyfleet_easynav_collaboration_deployment#what-you-should-see): + +- **Gazebo:** both Kobukis moving. +- **Central RViz:** the global map, the routes, both `base_link` axes and the planned paths. +- **Mission output:** + - **Phase 0:** discovery, with each robot's capabilities printed by `RobotHandle::print_capabilities()`. + - **Phases 1-5:** `kitchen`/`dock` → `kitchen_standby`/`kitchen` → manipulation at `kitchen` → `dock` with `robot_1` redirected to `charging_station` → back home. Goals have a 180 s safety cap. +- If a required capability is missing, the mission stops at the start with a red error. Both robots need `navigation`, `robot_1` needs `perception` and `robot_2` needs `manipulation`. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Zenoh router, world and two namespaced Kobukis. | +| `launch/easynav_collaboration_launch.yaml` | Both robots + `navigation_manager_node` + central RViz + mission (`robot_rviz`). | +| `launch/robot_N_launch.yaml` | One robot's stack under `/robot_N` (`rviz`). | +| `config/robot_N/robot_params.yaml` | `capabilities` (pluginlib lookup names) and `config_subdir` for the `Deployment`. | +| `config/robot_N/*` | EasyNav parameters, fake capability parameters and JSON descriptions. | +| `config/navigation_params.yaml`, `navigation.json` | Shared waypoints and the navigation capability description. | +| `config/navigation_manager.params.yaml` | Global map and routes, and the TF relay. | +| `easyfleet_easynav_collaboration_simple_api_deployment_plugins.xml` | Registers the `perception` and `manipulation` capability factories. | +| `src/robot_node.cpp` | `Deployment`: `add_capabilities_from_parameters()`, then `start()`, `run()` and `shutdown()`. | +| `src/main_easynav_collaboration.cpp` | The five-phase mission on `SimpleController`/`RobotHandle`. | + +## Troubleshooting + +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). +- Status text markers (`/mission_status_markers`) are shown only in the per-robot RViz (`robot_rviz:=true`). diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_manager.params.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_manager.params.yaml index 68770fb..f5536e1 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_manager.params.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_manager.params.yaml @@ -34,3 +34,8 @@ map.map_path_file: "maps/home2.yaml" routes.package: "easyfleet_easynav_deployment" routes.map_path_file: "maps/routes_1.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_params.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_params.yaml index 83f2e71..68a27ae 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_params.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/config/navigation_params.yaml @@ -66,7 +66,7 @@ y: -0.15 yaw: -3.14 # Each robot's own Gazebo spawn pose (see - # easynav_playground_kobuki/config/multirobot_config.yaml), used by + # launch/simulation_launch.yaml), used by # main_easynav_collaboration.cpp to send each robot back to where it # started at the end of the mission. robot_1_home: diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml index 6556958..d7dc772 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml @@ -20,7 +20,7 @@ # copied from src/easynav_indoor_testcase/robots_params/costmap.rpp.params.yaml). # The only difference from that single-robot file is localizer_node's # initial_pose below, set to robot_1's actual spawn pose in -# easynav_playground_kobuki's multirobot_config.yaml (x=0.0, y=0.0, no +# launch/simulation_launch.yaml (x=0.0, y=0.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml index 2b8c53d..964276e 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml @@ -16,7 +16,7 @@ # Real EasyNav (easynav_system system_main) for robot_2: identical to # config/robot_1/easynav_system.costmap_rpp.params.yaml except # localizer_node's initial_pose below, set to robot_2's actual spawn pose -# in easynav_playground_kobuki's multirobot_config.yaml (x=2.0, y=1.0, no +# in launch/simulation_launch.yaml (x=2.0, y=1.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/easyfleet_easynav_collaboration_simple_api_deployment_plugins.xml b/easyfleet_easynav_collaboration_simple_api_deployment/easyfleet_easynav_collaboration_simple_api_deployment_plugins.xml index 286500f..7166d2d 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/easyfleet_easynav_collaboration_simple_api_deployment_plugins.xml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/easyfleet_easynav_collaboration_simple_api_deployment_plugins.xml @@ -7,7 +7,7 @@ sibling easyfleet_easynav_collaboration_deployment package's own hardcoded factory already expects means the exact same "capabilities" parameter value (e.g. ["perception"]) works unchanged for either - package's robot_node.cpp -- the two are meant to be interchangeable + package's robot_node.cpp: the two are meant to be interchangeable at the launch level with no other change (see refactor.md). A package offering more than one alternative implementation of the same capability type would register each under diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/launch/easynav_collaboration_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/launch/easynav_collaboration_launch.yaml index 7569ed1..d227263 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/launch/easynav_collaboration_launch.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/launch/easynav_collaboration_launch.yaml @@ -22,9 +22,9 @@ # own (e.g. `ros2 launch easyfleet_easynav_collaboration_simple_api_deployment robot_1_launch.yaml`). # # Does NOT launch Gazebo -- start -# `ros2 launch easynav_playground_kobuki playgorund_multirobot_kobuki.launch.py` +# `ros2 launch easyfleet_easynav_collaboration_simple_api_deployment simulation_launch.yaml` # separately first, so both robots are already spawned (namespaced -# "robot_1"/"robot_2", see multirobot_config.yaml) and publishing +# "robot_1"/"robot_2") and publishing # scan_raw/odom/TF before system_main tries to localize against them. # # easynav_collaboration_mission_node is delayed a few seconds so both @@ -33,10 +33,17 @@ # easyfleet_mission_manager::discover_capabilities()) opens -- launching # everything at literally the same instant risks discovering nothing. launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_1_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_2_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} # Fleet-wide navigation manager: publishes the same map/routes both # robots above would otherwise load locally on /global_map and diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_1_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_1_launch.yaml index 2d8edff..27fd8de 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_1_launch.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_1_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake perception capability (this package's own # robot_node), all namespaced under /robot_1 so they never collide with # robot_2's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_1's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # system_main's "tf_prefix" is set to "robot_1" to match: kobuki_description # prefixes every TF frame name with the robot's namespace unconditionally @@ -51,6 +51,8 @@ # Each robot gets its own window/config, so both can be watched navigating # at once. launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_1" @@ -64,6 +66,11 @@ launch: - name: "tf_prefix" value: "robot_1" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -111,9 +118,15 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_1/perception_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/rviz/robot_1.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_2_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_2_launch.yaml index f73e68e..66d75f7 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_2_launch.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/launch/robot_2_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake manipulation capability (this package's own # robot_node), all namespaced under /robot_2 so they never collide with # robot_1's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_2's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # See robot_1_launch.yaml for why "tf_prefix" is set to match the # namespace, and why it -- and the namespace itself -- are hardcoded here @@ -29,6 +29,8 @@ # below loads its own pre-resolved rviz/robot_2.rviz config instead of a # remapped copy of the shared easynav_costmap.rviz. launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_2" @@ -42,6 +44,11 @@ launch: - name: "tf_prefix" value: "robot_2" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -89,9 +96,15 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_2/manipulation_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/rviz/robot_2.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..fdf725c --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment/launch/simulation_launch.yaml @@ -0,0 +1,43 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with the small house world and two Kobukis, robot_1 at (0, 0) and +# robot_2 at (2, 1) -- the initial poses in config/robot_N. Each robot's +# topics and TF tree are under its namespace (/robot_N/tf). Also starts the +# Zenoh router. Run it before easynav_collaboration_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "2.0"} + - {name: "y", value: "1.0"} diff --git a/easyfleet_easynav_collaboration_simple_api_deployment/package.xml b/easyfleet_easynav_collaboration_simple_api_deployment/package.xml index f081cc7..01b264d 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment/package.xml +++ b/easyfleet_easynav_collaboration_simple_api_deployment/package.xml @@ -44,6 +44,15 @@ easyfleet_navigation_manager easynav_system + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + rmw_zenoh_cpp + rviz2 + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/README.md b/easyfleet_easynav_collaboration_simple_api_deployment_py/README.md new file mode 100644 index 0000000..d655987 --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/README.md @@ -0,0 +1,65 @@ +# easyfleet_easynav_collaboration_simple_api_deployment_py + +The Python version of [easyfleet_easynav_collaboration_simple_api_deployment](../easyfleet_easynav_collaboration_simple_api_deployment): same two robots, world, waypoints, central view and five-phase mission. The fake perception and manipulation capabilities and the mission are written in Python on `easyfleet_mission_manager_py`, with `Capability`/`ActionServerBase` for the capabilities and `RobotHandle`/`SimpleController` for the mission. Navigation is still real EasyNav, using the C++ `easynav_navigation_capability_node` from `easyfleet_easynav_deployment`. Use this package as a template if your mission or your capabilities are in Python. It is also an example of C++ and Python capabilities in the same fleet. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Robot | Namespace | Spawn | Capabilities | +|---|---|---|---| +| `robot_1` | `/robot_1` | (0, 0) | `navigation` (EasyNav, C++), `perception` (fake, Python) | +| `robot_2` | `/robot_2` | (2, 1) | `navigation` (EasyNav, C++), `manipulation` (fake, Python) | + +Each robot runs these processes, all under its namespace: + +- `system_main`: EasyNav, with `tf_prefix=robot_N` and its own TF tree on `/robot_N/tf`. +- `navigation`: the C++ capability node. +- One Python process for its fake capability: `perception_fake_capability_node` on `robot_1`, `manipulation_fake_capability_node` on `robot_2`. +- Its own RViz, off by default. + +Central processes: `navigation_manager_node`, the central RViz (`navigation_manager.rviz`), the Python `easynav_collaboration_mission_node` (5 s delay) and the Zenoh router started by `simulation_launch.yaml`. `navigation_manager_node` publishes `/global_map` and `/global_routes`, and relays `odom`, `base_footprint` and `base_link` of each robot to `/tf` at 10 Hz. + +This package has no `config/` or `rviz/` folder of its own. The launch files take the EasyNav, capability, navigation manager and RViz configuration from `easyfleet_easynav_collaboration_simple_api_deployment`, so that package must be built too. + +## How to run + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Zenoh router + Gazebo (small house) + both Kobukis +ros2 launch easyfleet_easynav_collaboration_simple_api_deployment_py simulation_launch.yaml # gui:=false for headless + +# Terminal 2: both robots + navigation manager + central RViz + Python mission +ros2 launch easyfleet_easynav_collaboration_simple_api_deployment_py easynav_collaboration_launch.yaml +``` + +`robot_rviz:=true` also opens each robot's RViz. `robot_N_launch.yaml` starts one robot, with its RViz on by default (`rviz:=false`). + +## What you should see + +The same as the [C++ version](../easyfleet_easynav_collaboration_simple_api_deployment#what-you-should-see): + +- **Gazebo:** both Kobukis moving. +- **Central RViz:** the global map, the routes, both `base_link` axes and the paths. +- **Mission output:** + - **Phase 0:** discovery. + - **Phases 1-5:** `kitchen`/`dock` → `kitchen_standby`/`kitchen` → manipulation → `dock` with `robot_1` redirected to `charging_station` → home. 180 s safety cap per goal. +- **Per-robot RViz:** status markers, published automatically by `RobotHandle.run_capability()`. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Zenoh router, world and two namespaced Kobukis. | +| `launch/easynav_collaboration_launch.yaml` | Both robots + `navigation_manager_node` + central RViz + mission (`robot_rviz`). | +| `launch/robot_N_launch.yaml` | One robot: `system_main`, the C++ navigation capability and the Python fake capability (`rviz`). | +| `/mission.py` | The five-phase mission on `SimpleController`/`RobotHandle`. | +| `/perception_fake_capability.py` | Python fake perception (`gato` detections). | +| `/manipulation_fake_capability.py` | Python fake manipulation. | +| `setup.py` | Console scripts: `easynav_collaboration_mission_node`, `perception_fake_capability_node`, `manipulation_fake_capability_node`. | + +## Troubleshooting + +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). +- Because there is one process per Python capability, a robot that needs more capabilities needs one more node in its `robot_N_launch.yaml` for each. diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/manipulation_fake_capability.py b/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/manipulation_fake_capability.py new file mode 100644 index 0000000..3d324ab --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/manipulation_fake_capability.py @@ -0,0 +1,109 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Fake/mock implementation of the easyfleet_interfaces/Manipulation action, in Python. + +A Python port of this deployment's own C++ +manipulation_fake_capability.hpp/.cpp (a reference implementation of +easyfleet_core::ManipulationActionServerBase there, +easyfleet_mission_manager_py's ActionServerBase here): it does not drive +a real manipulator or interact with real controllers. It simulates +trajectory execution progress over a configurable duration, publishing +feedback at a configurable rate, so that the rest of a multi-capability +system can be integrated and tested before a real manipulator stack is +wired in through its own ActionServerBase subclass. +""" + +import time + +from easyfleet_interfaces.action import Manipulation +from easyfleet_mission_manager_py import ActionServerBase, Capability, spin_capability +import rclpy + + +def _goal_target_is_empty(goal) -> bool: + if goal.mode == Manipulation.Goal.MODE_JOINT_TARGET: + return not goal.joint_target.name + if goal.mode == Manipulation.Goal.MODE_POSE_TARGET: + return not goal.pose_target.header.frame_id + if goal.mode == Manipulation.Goal.MODE_NAMED_TASK: + return not goal.named_task + return True + + +class ManipulationFakeActionServer(ActionServerBase): + """Reference mock implementation of the Manipulation action, in Python.""" + + def __init__(self, node, action_name: str): + super().__init__(node, Manipulation, action_name) + self.mock_execution_duration_s = node.declare_parameter( + f'{action_name}.mock_execution_duration', 3.0).value + self.mock_feedback_period_s = node.declare_parameter( + f'{action_name}.mock_feedback_period', 0.3).value + + def on_goal_received(self, goal) -> bool: + return not _goal_target_is_empty(goal) + + def on_execute(self, goal_handle): + goal = goal_handle.request + feedback = Manipulation.Feedback() + feedback.state = 'EXECUTING' + feedback.current_joint_state = goal.joint_target + + num_steps = max(1, int(self.mock_execution_duration_s / self.mock_feedback_period_s)) + + for i in range(num_steps): + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return Manipulation.Result( + error_code=Manipulation.Result.CANCELED, + error_msg='Manipulation canceled by client.') + if self.is_preempt_requested(): + goal_handle.abort() + return Manipulation.Result( + error_code=Manipulation.Result.ABORTED, + error_msg='Manipulation preempted by a newer goal.') + if self.is_shutdown_requested(): + goal_handle.abort() + return Manipulation.Result( + error_code=Manipulation.Result.ABORTED, + error_msg='Manipulation capability is shutting down.') + + time.sleep(self.mock_feedback_period_s) + feedback.progress = (i + 1) / num_steps + goal_handle.publish_feedback(feedback) + + goal_handle.succeed() + return Manipulation.Result( + error_code=Manipulation.Result.SUCCESS, final_joint_state=goal.joint_target) + + +class ManipulationFakeCapability(Capability): + """The "manipulation" capability, backed by the fake/mock action server.""" + + def __init__(self, **kwargs): + super().__init__('manipulation', ManipulationFakeActionServer, **kwargs) + + +def main(args=None): + """Run the "manipulation" capability as a standalone node.""" + rclpy.init(args=args) + spin_capability(ManipulationFakeCapability()) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/perception_fake_capability.py b/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/perception_fake_capability.py new file mode 100644 index 0000000..43c4a9d --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/perception_fake_capability.py @@ -0,0 +1,151 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Fake/mock implementation of the easyfleet_interfaces/Perception action, in Python. + +A Python port of this deployment's own C++ +perception_fake_capability.hpp/.cpp (a reference implementation of +easyfleet_core::PerceptionActionServerBase there, easyfleet_mission_manager_py's +ActionServerBase here): it does not run a real object detector. When +asked for the configured target class (a cat, "gato", by default) it +publishes feedback at a configurable rate (20 Hz by default) with 1 to 3 +randomly placed detections, in both 2D and 3D, until the goal is +canceled, preempted or the node shuts down -- there is no other terminal +condition, mirroring how a continuous perception stream behaves. Goals +asking for any other class are rejected. `continuous`/`max_rate_hz` are +accepted but not honored by this mock: every goal streams forever at the +fixed `detection_rate_hz` parameter, since a mock has no notion of +"detect once and stop". +""" + +import random +import time + +from easyfleet_interfaces.action import Perception +from easyfleet_mission_manager_py import ActionServerBase, Capability, spin_capability +import rclpy +from vision_msgs.msg import ( + BoundingBox2D, + BoundingBox3D, + Detection2D, + Detection3D, + ObjectHypothesisWithPose, +) + + +def _requests_target_class(object_classes, target_class: str) -> bool: + return any(target_class in requested.lower() for requested in object_classes) + + +class PerceptionFakeActionServer(ActionServerBase): + """Reference mock implementation of the Perception action, in Python.""" + + def __init__(self, node, action_name: str): + super().__init__(node, Perception, action_name) + self.target_class = node.declare_parameter( + f'{action_name}.target_class', 'gato').value.lower() + self.frame_id = node.declare_parameter(f'{action_name}.frame_id', 'camera_link').value + self.detection_rate_hz = node.declare_parameter( + f'{action_name}.detection_rate_hz', 20.0).value + self._random = random.Random() + + def on_goal_received(self, goal) -> bool: + return bool(goal.object_classes) and _requests_target_class( + goal.object_classes, self.target_class) + + def _make_random_detections(self) -> Perception.Feedback: + feedback = Perception.Feedback() + feedback.detections_3d.header.frame_id = self.frame_id + feedback.detections_2d.header.frame_id = self.frame_id + + for i in range(self._random.randint(1, 3)): + score = self._random.uniform(0.6, 0.99) + object_id = f'{self.target_class}_{i}' + + detection_3d = Detection3D() + detection_3d.header.frame_id = self.frame_id + detection_3d.id = object_id + hypothesis_3d = ObjectHypothesisWithPose() + hypothesis_3d.hypothesis.class_id = self.target_class + hypothesis_3d.hypothesis.score = score + hypothesis_3d.pose.pose.position.x = self._random.uniform(-3.0, 3.0) + hypothesis_3d.pose.pose.position.y = self._random.uniform(-3.0, 3.0) + hypothesis_3d.pose.pose.position.z = self._random.uniform(0.0, 1.0) + detection_3d.results.append(hypothesis_3d) + detection_3d.bbox = BoundingBox3D() + detection_3d.bbox.center = hypothesis_3d.pose.pose + detection_3d.bbox.size.x = 0.3 + detection_3d.bbox.size.y = 0.3 + detection_3d.bbox.size.z = 0.2 + feedback.detections_3d.detections.append(detection_3d) + + detection_2d = Detection2D() + detection_2d.header.frame_id = self.frame_id + detection_2d.id = object_id + hypothesis_2d = ObjectHypothesisWithPose() + hypothesis_2d.hypothesis.class_id = self.target_class + hypothesis_2d.hypothesis.score = score + detection_2d.results.append(hypothesis_2d) + detection_2d.bbox = BoundingBox2D() + detection_2d.bbox.center.position.x = self._random.uniform(0.0, 640.0) + detection_2d.bbox.center.position.y = self._random.uniform(0.0, 480.0) + detection_2d.bbox.size_x = 100.0 + detection_2d.bbox.size_y = 100.0 + feedback.detections_2d.detections.append(detection_2d) + + return feedback + + def on_execute(self, goal_handle): + # continuous/max_rate_hz on the goal are accepted but not honored + # by this mock: it always streams forever at self.detection_rate_hz + # until canceled, preempted or shut down, regardless of what the + # goal requested. + period = 1.0 / self.detection_rate_hz + while True: + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return Perception.Result(error_code=Perception.Result.CANCELED) + if self.is_preempt_requested(): + goal_handle.abort() + return Perception.Result( + error_code=Perception.Result.ABORTED, + error_msg='Perception goal preempted by a newer goal.') + if self.is_shutdown_requested(): + goal_handle.abort() + return Perception.Result( + error_code=Perception.Result.ABORTED, + error_msg='Perception capability is shutting down.') + + goal_handle.publish_feedback(self._make_random_detections()) + time.sleep(period) + + +class PerceptionFakeCapability(Capability): + """The "perception" capability, backed by the fake/mock action server.""" + + def __init__(self, **kwargs): + super().__init__('perception', PerceptionFakeActionServer, **kwargs) + + +def main(args=None): + """Run the "perception" capability as a standalone node.""" + rclpy.init(args=args) + spin_capability(PerceptionFakeCapability()) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml index 95845ad..69f9ac8 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml @@ -13,17 +13,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -# This deployment's top-level launch file -- identical to +# This deployment's top-level launch file -- similar to # easyfleet_easynav_collaboration_simple_api_deployment's own -# easynav_collaboration_launch.yaml except for the very last node: the -# mission controller here is easyfleet_easynav_collaboration_simple_api_deployment_py's -# Python one instead of that package's C++ one. Everything else (both -# robots, the navigation manager, the fleet-wide RViz) is reused -# directly from the sibling C++ package -- only the mission controller -# differs, per this package's own purpose. +# easynav_collaboration_launch.yaml, but both robots' fake capabilities +# (robot_1's perception, robot_2's manipulation) and the mission +# controller are this package's own Python implementations, not that +# sibling package's C++ ones -- see robot_1_launch.yaml/ +# robot_2_launch.yaml here (real EasyNav navigation, config, rviz are +# still reused directly from the C++ package; only the fake-capability +# hosting differs). # # Does NOT launch Gazebo -- start -# `ros2 launch easynav_playground_kobuki playgorund_multirobot_kobuki.launch.py` +# `ros2 launch easyfleet_easynav_collaboration_simple_api_deployment_py simulation_launch.yaml` # separately first, so both robots are already spawned (namespaced # "robot_1"/"robot_2") and publishing scan_raw/odom/TF before # system_main tries to localize against them. @@ -34,10 +35,17 @@ # launching everything at literally the same instant risks discovering # nothing. launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - include: - file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_1_launch.yaml" + file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment_py)/launch/robot_1_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} - include: - file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_2_launch.yaml" + file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment_py)/launch/robot_2_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} # Fleet-wide navigation manager: publishes the same map/routes both # robots above would otherwise load locally on /global_map and diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_1_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_1_launch.yaml new file mode 100644 index 0000000..381439c --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_1_launch.yaml @@ -0,0 +1,91 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# This deployment's own robot_1: identical to the C++ sibling package's +# robot_1_launch.yaml (real EasyNav navigation, same config/rviz, reused +# directly) except the fake perception capability is this package's own +# Python perception_fake_capability_node instead of that package's +# pluginlib-loaded C++ "robot_node" -- see +# perception_fake_capability.py for the reference implementation and +# easyfleet_mission_manager_py's Capability/ActionServerBase for what it's +# built on. Config files (perception_params.yaml, perception.json) are +# reused directly from the C++ sibling package: they are plain +# YAML/JSON, not C++-specific. +launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - group: + - push_ros_namespace: + namespace: "robot_1" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_1/easynav_system.costmap_rpp.params.yaml" + - name: "tf_prefix" + value: "robot_1" + remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + - from: "maps_manager_node/costmap/incoming_map" + to: "/global_map" + - from: "maps_manager_node/routes/incoming_routes" + to: "/global_routes" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/navigation.json" + + - node: + pkg: "easyfleet_easynav_collaboration_simple_api_deployment_py" + exec: "perception_fake_capability_node" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_1/perception_params.yaml" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_1/perception.json" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + args: "-d $(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/rviz/robot_1.rviz" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_2_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_2_launch.yaml new file mode 100644 index 0000000..dce3d78 --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/robot_2_launch.yaml @@ -0,0 +1,89 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# This deployment's own robot_2: identical to the C++ sibling package's +# robot_2_launch.yaml (real EasyNav navigation, same config/rviz, reused +# directly) except the fake manipulation capability is this package's own +# Python manipulation_fake_capability_node instead of that package's +# pluginlib-loaded C++ "robot_node" -- see manipulation_fake_capability.py +# for the reference implementation. Config files (manipulation_params.yaml, +# manipulation.json) are reused directly from the C++ sibling package: +# they are plain YAML/JSON, not C++-specific. See robot_1_launch.yaml. +launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - group: + - push_ros_namespace: + namespace: "robot_2" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_2/easynav_system.costmap_rpp.params.yaml" + - name: "tf_prefix" + value: "robot_2" + remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + - from: "maps_manager_node/costmap/incoming_map" + to: "/global_map" + - from: "maps_manager_node/routes/incoming_routes" + to: "/global_routes" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/navigation.json" + + - node: + pkg: "easyfleet_easynav_collaboration_simple_api_deployment_py" + exec: "manipulation_fake_capability_node" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_2/manipulation_params.yaml" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/robot_2/manipulation.json" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + args: "-d $(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/rviz/robot_2.rviz" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/simulation_launch.yaml b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/simulation_launch.yaml new file mode 100644 index 0000000..fdf725c --- /dev/null +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/simulation_launch.yaml @@ -0,0 +1,43 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with the small house world and two Kobukis, robot_1 at (0, 0) and +# robot_2 at (2, 1) -- the initial poses in config/robot_N. Each robot's +# topics and TF tree are under its namespace (/robot_N/tf). Also starts the +# Zenoh router. Run it before easynav_collaboration_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "2.0"} + - {name: "y", value: "1.0"} diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml b/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml index ca47d00..2b0cce1 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml @@ -4,17 +4,17 @@ easyfleet_easynav_collaboration_simple_api_deployment_py 0.1.0 - The Python-mission-controller mirror of - easyfleet_easynav_collaboration_simple_api_deployment: the exact same - two-robot scenario (real EasyNav navigation on both robots, robot_1 - also carrying a fake perception capability, robot_2 a fake - manipulation one, same Gazebo world/map, same launch/config/rviz - shape, reused directly from that sibling package -- robot_node, - perception/manipulation capabilities, navigation, launch files), but - the mission script is written in Python against - easyfleet_mission_manager_py's RobotHandle/SimpleController instead - of the sibling package's C++ one. Only the mission controller - differs; everything else is the same package, included directly. + The Python mirror of easyfleet_easynav_collaboration_simple_api_deployment: + the exact same two-robot scenario (real EasyNav navigation on both + robots, robot_1 also carrying a fake perception capability, robot_2 a + fake manipulation one, same Gazebo world/map, same rviz shape). Real + EasyNav navigation, its config, and rviz configs are reused directly + from that sibling package; the fake perception/manipulation + capabilities and the mission script are this package's own Python + implementations, built on easyfleet_mission_manager_py's + Capability/ActionServerBase (capabilities) and RobotHandle/ + SimpleController (mission control) instead of the sibling package's + C++ ones. Francisco Martín Rico @@ -25,14 +25,27 @@ rclpy easyfleet_mission_manager_py + easyfleet_interfaces + vision_msgs easyfleet_easynav_collaboration_simple_api_deployment easyfleet_navigation_manager + easyfleet_easynav_deployment + easyfleet_easynav_navigation launch launch_ros launch_yaml rviz2 + easynav_system + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + rmw_zenoh_cpp + ament_copyright ament_flake8 ament_pep257 diff --git a/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py b/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py index d5098ff..645eeb1 100644 --- a/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py +++ b/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py @@ -9,21 +9,32 @@ data_files=[ ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/launch', ['launch/easynav_collaboration_launch.yaml']), + ('share/' + package_name + '/launch', [ + 'launch/easynav_collaboration_launch.yaml', + 'launch/robot_1_launch.yaml', + 'launch/robot_2_launch.yaml', + 'launch/simulation_launch.yaml', + ]), ], install_requires=['setuptools'], zip_safe=True, maintainer='Francisco Martín Rico', maintainer_email='fmrico@gmail.com', description=( - 'Python-mission-controller mirror of ' - 'easyfleet_easynav_collaboration_simple_api_deployment.' + 'Python mirror of easyfleet_easynav_collaboration_simple_api_deployment: ' + 'mission controller and fake perception/manipulation capabilities.' ), license='Apache-2.0', entry_points={ 'console_scripts': [ 'easynav_collaboration_mission_node = ' 'easyfleet_easynav_collaboration_simple_api_deployment_py.mission:main', + 'perception_fake_capability_node = ' + 'easyfleet_easynav_collaboration_simple_api_deployment_py' + '.perception_fake_capability:main', + 'manipulation_fake_capability_node = ' + 'easyfleet_easynav_collaboration_simple_api_deployment_py' + '.manipulation_fake_capability:main', ], }, ) diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/README.md b/easyfleet_easynav_collaboration_zenoh_deployment/README.md new file mode 100644 index 0000000..cc8eb30 --- /dev/null +++ b/easyfleet_easynav_collaboration_zenoh_deployment/README.md @@ -0,0 +1,84 @@ +# easyfleet_easynav_collaboration_zenoh_deployment + +The [simple API two-robot scenario](../easyfleet_easynav_collaboration_simple_api_deployment) (same code, waypoints and mission) deployed as separate **Zenoh islands**: + +- Each robot has its own `rmw_zenohd` router, and its nodes connect only to that router. +- Each robot router connects to a central router. +- An access-control list (ACL) on each robot router lets only a small set of topics cross to the central island. Internal topics such as odometry, scan and costmaps stay on the robot. + +In the other EasyNav examples, every node shares one router, so every topic is visible everywhere. This example shows how to keep each robot's traffic local while the fleet still works, as you would with separate machines on a shared network. [The fastdds variant](../easyfleet_easynav_collaboration_fastdds_deployment) does the same with DDS domains and DDS Router. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Island | Router (config) | Endpoints | What runs there | +|---|---|---|---| +| central | `config/zenoh_router_central.json5` | `tcp/127.0.0.1:7447` | Gazebo world + `/clock` bridge, `navigation_manager_node`, central RViz, mission | +| `robot_1` | `config/robot_1/zenoh_router.json5` | unix socket `/tmp/easyfleet_robot_1_zenoh.sock`, `tcp/127.0.0.1:7448` | Kobuki 1, and under `/robot_1`: `system_main`, `navigation`, `robot_node` (`perception`), optional RViz | +| `robot_2` | `config/robot_2/zenoh_router.json5` | unix socket `/tmp/easyfleet_robot_2_zenoh.sock`, `tcp/127.0.0.1:7449` | Kobuki 2, and under `/robot_2`: `system_main`, `navigation`, `robot_node` (`manipulation`), optional RViz | + +How the islands connect: + +- Nodes connect to their island's router through a session config: `zenoh_session_central.json5` or `robot_N/zenoh_session.json5`. +- Each robot router connects to the central router. `robot_1` and `robot_2` never connect directly to each other. + +The robot routers deny by default. Clients on the local unix socket get everything. Over the TCP link to the central router, only these topics cross, in both directions: + +- `/capabilities`, `/capabilities_status` +- `/global_map`, `/global_routes` +- `/clock`, `/mission_status_markers` +- The robot's capability actions, `planner_node` and `easynav_control` topics. +- `/robot_N/tf` and `/robot_N/tf_static`, outbound only, so `navigation_manager_node` can relay `odom`, `base_footprint` and `base_link` to `/tf` for the central RViz. + +Robots, spawn poses (0, 0) and (2, 1), and capabilities are the same as in the [simple API example](../easyfleet_easynav_collaboration_simple_api_deployment#what-runs-where). + +## How to run + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: the three Zenoh routers + Gazebo (small house) + both Kobukis, each in its island +ros2 launch easyfleet_easynav_collaboration_zenoh_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: robots (each with its router) + navigation manager + central RViz + mission +ros2 launch easyfleet_easynav_collaboration_zenoh_deployment easynav_collaboration_launch.yaml +``` + +`robot_rviz:=true` also opens each robot's RViz, inside that robot's island. + +To inspect an island from another terminal, source its script (sourcing, not executing it, is required) and use the `ros2` CLI: + +```bash +SCRIPTS=$(ros2 pkg prefix easyfleet_easynav_collaboration_zenoh_deployment)/share/easyfleet_easynav_collaboration_zenoh_deployment/scripts +source $SCRIPTS/set_robot_1.sh +ros2 topic list # robot_1's full graph +source $SCRIPTS/set_central.sh +ros2 topic list # only what the ACLs let through +``` + +The scripts set `RMW_IMPLEMENTATION` and `ZENOH_SESSION_CONFIG_URI`, and restart the `ros2` daemon. When several terminals are on different islands, use `--no-daemon`. + +## What you should see + +The same Gazebo scene, central RViz (global map, routes, both `base_link` axes, paths) and five-phase mission output as in the [simple API example](../easyfleet_easynav_collaboration_simple_api_deployment#what-you-should-see). The difference shows in `ros2 topic list`: from the central island you do not see `/robot_N/scan_raw`, `/robot_N/odom` and similar topics, but from a robot's island you do. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Starts the three routers, then the world (central session) and each Kobuki (in its robot's session). | +| `launch/easynav_collaboration_launch.yaml` | Central router + both robots + central nodes, with the central session config. | +| `launch/robot_N_launch.yaml` | The robot's router + its stack, with its session config (`rviz` argument). | +| `config/zenoh_router_central.json5`, `zenoh_session_central.json5` | Central router and client sessions. | +| `config/robot_N/zenoh_router.json5` | Robot router: link to the central router, listen endpoints, and the ACL that defines what crosses. | +| `config/robot_N/zenoh_session.json5` | Client session over the robot's unix socket. | +| `config/robot_N/*`, `config/navigation*.{yaml,json}` | EasyNav, capability and navigation manager configuration, as in the simple API example. | +| `scripts/set_central.sh`, `set_robot_N.sh` | Source these to attach a shell to an island. | +| `src/robot_node.cpp`, `src/main_easynav_collaboration.cpp` | `Deployment` and `SimpleController` code, same as the simple API example. | + +## Notes and troubleshooting + +- Both `simulation_launch.yaml` and `easynav_collaboration_launch.yaml` start routers on the same endpoints. When both launch files are running, the routers from the second one find those endpoints already in use. +- The routers listen only on `127.0.0.1` and unix sockets, so everything runs on one machine. To spread islands across hosts, change the endpoints. +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_manager.params.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_manager.params.yaml index 68770fb..f5536e1 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_manager.params.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_manager.params.yaml @@ -34,3 +34,8 @@ map.map_path_file: "maps/home2.yaml" routes.package: "easyfleet_easynav_deployment" routes.map_path_file: "maps/routes_1.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_params.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_params.yaml index 83f2e71..68a27ae 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_params.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/navigation_params.yaml @@ -66,7 +66,7 @@ y: -0.15 yaw: -3.14 # Each robot's own Gazebo spawn pose (see - # easynav_playground_kobuki/config/multirobot_config.yaml), used by + # launch/simulation_launch.yaml), used by # main_easynav_collaboration.cpp to send each robot back to where it # started at the end of the mission. robot_1_home: diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml index 9620215..feedc3a 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/easynav_system.costmap_rpp.params.yaml @@ -20,7 +20,7 @@ # copied from src/easynav_indoor_testcase/robots_params/costmap.rpp.params.yaml). # The only difference from that single-robot file is localizer_node's # initial_pose below, set to robot_1's actual spawn pose in -# easynav_playground_kobuki's multirobot_config.yaml (x=0.0, y=0.0, no +# launch/simulation_launch.yaml (x=0.0, y=0.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/zenoh_router.json5 b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/zenoh_router.json5 index 18dfb36..7dca209 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/zenoh_router.json5 +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_1/zenoh_router.json5 @@ -174,11 +174,6 @@ "*/robot_1/navigation/**/@adv/**", "*/robot_1/perception/**", "*/robot_1/perception/**/@adv/**", - // robot -> central, TRANSIENT_LOCAL (robot_state_publisher): - // needed for the fleet-wide RViz2 (navigation_manager.rviz) to - // render this robot's RobotModel display. - "*/robot_1/robot_description/**", - "*/robot_1/robot_description/**/@adv/**", // navigation_manager_node's inter-robot collision arbitration // (RobotNavigationWatcher, easyfleet_navigation_manager): watches // this robot's planned path (robot -> central, @@ -233,33 +228,22 @@ key_exprs: ["@ros2_lv/**"], }, { - // /tf and /tf_static are deliberately NOT in allow_boundary_data: - // they're absolute, unnamespaced topics (see zenoh.md pregunta 4), - // so if crossed bidirectionally like everything else here, whatever - // central forwards back in -- which, once the fleet-wide RViz2 also - // wants robot_2's TF merged onto the very same "/tf" key, would - // include robot_2's frames too -- would land right back in this - // island, defeating the entire point of keeping each robot's TF - // tree private. Interest/query propagation (this rule) still needs - // to flow both ways for routing to work at all (this is how a - // *subscriber* on the other end tells this router "please forward - // me matching data"), but the actual DATA-carrying message types - // (next rule) are deliberately egress-only: robot_1's own /tf can - // leave towards the central island's merged view, but nothing - // coming back over this same link can ever re-enter robot_1's own - // island. Confirmed empirically: a subscriber connected to this - // island only ever received robot_1's own /tf, never a second - // publisher's /tf injected directly on the central side (simulating - // robot_2's), while a subscriber on the central side correctly saw - // both. See zenoh.md (2026-08-29). + // This robot's TF (/robot_1/tf, /robot_1/tf_static, robot_1/ frames) + // stays out of allow_boundary_data: only what crosses is what the + // central navigation_manager_node subscribes to for its fleet view + // (robots.relay_tf, which keeps just the frames that place this + // robot's base_link in "map"). Interest/query propagation (this rule) + // flows both ways for routing to work; the data itself (next rule) + // is egress-only, so nothing from the central side ever enters this + // island's TF tree. See zenoh.md. id: "allow_tf_boundary_meta", messages: ["declare_subscriber", "query", "declare_queryable"], flows: ["egress", "ingress"], permission: "allow", - key_exprs: ["*/tf/**", "*/tf_static/**", "*/tf_static/**/@adv/**"], + key_exprs: ["*/robot_1/tf/**", "*/robot_1/tf_static/**", "*/robot_1/tf_static/**/@adv/**"], }, { - // The actual DATA for /tf, /tf_static -- egress only, see + // The actual DATA for /robot_1/tf, /robot_1/tf_static -- egress only, see // allow_tf_boundary_meta above for why. /tf_static is // TRANSIENT_LOCAL (standard ROS 2 convention), hence its "@adv" // twin, same reasoning as every other latched topic in this file; @@ -271,7 +255,7 @@ messages: ["put", "delete", "reply"], flows: ["egress"], permission: "allow", - key_exprs: ["*/tf/**", "*/tf_static/**", "*/tf_static/**/@adv/**"], + key_exprs: ["*/robot_1/tf/**", "*/robot_1/tf_static/**", "*/robot_1/tf_static/**/@adv/**"], }, ], subjects: [ diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml index d0176ae..1002295 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/easynav_system.costmap_rpp.params.yaml @@ -16,7 +16,7 @@ # Real EasyNav (easynav_system system_main) for robot_2: identical to # config/robot_1/easynav_system.costmap_rpp.params.yaml except # localizer_node's initial_pose below, set to robot_2's actual spawn pose -# in easynav_playground_kobuki's multirobot_config.yaml (x=2.0, y=1.0, no +# in launch/simulation_launch.yaml (x=2.0, y=1.0, no # rotation) -- matching src/easynav_indoor_testcase's own multi-robot # reference (costmap_multirobot.params.yaml), which seeds each robot's # AMCL at its exact spawn pose rather than an arbitrary offset. diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/zenoh_router.json5 b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/zenoh_router.json5 index eb843a1..ed5d044 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/zenoh_router.json5 +++ b/easyfleet_easynav_collaboration_zenoh_deployment/config/robot_2/zenoh_router.json5 @@ -85,11 +85,6 @@ "*/robot_2/navigation/**/@adv/**", "*/robot_2/manipulation/**", "*/robot_2/manipulation/**/@adv/**", - // robot -> central, TRANSIENT_LOCAL (robot_state_publisher): - // needed for the fleet-wide RViz2 (navigation_manager.rviz) to - // render this robot's RobotModel display. - "*/robot_2/robot_description/**", - "*/robot_2/robot_description/**/@adv/**", // navigation_manager_node's inter-robot collision arbitration -- // see robot_1/zenoh_router.json5's identical entry for the full // explanation (RobotNavigationWatcher watches this robot's path @@ -114,25 +109,22 @@ key_exprs: ["@ros2_lv/**"], }, { - // /tf, /tf_static: interest/query propagation bidirectional (needed - // for routing), but see allow_tf_boundary_data_out below for why - // the actual data is egress-only -- full rationale in - // robot_1/zenoh_router.json5's identical rule (confirmed - // empirically that this prevents a robot from ever receiving - // another robot's TF back through the central island, while still - // letting the central island see everyone's merged). + // /robot_2/tf, /robot_2/tf_static: interest/query propagation + // bidirectional (needed for routing), data egress-only (next rule), + // for the central navigation_manager_node's fleet view -- full + // rationale in robot_1/zenoh_router.json5's identical rule. id: "allow_tf_boundary_meta", messages: ["declare_subscriber", "query", "declare_queryable"], flows: ["egress", "ingress"], permission: "allow", - key_exprs: ["*/tf/**", "*/tf_static/**", "*/tf_static/**/@adv/**"], + key_exprs: ["*/robot_2/tf/**", "*/robot_2/tf_static/**", "*/robot_2/tf_static/**/@adv/**"], }, { id: "allow_tf_boundary_data_out", messages: ["put", "delete", "reply"], flows: ["egress"], permission: "allow", - key_exprs: ["*/tf/**", "*/tf_static/**", "*/tf_static/**/@adv/**"], + key_exprs: ["*/robot_2/tf/**", "*/robot_2/tf_static/**", "*/robot_2/tf_static/**/@adv/**"], }, ], subjects: [ diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/easyfleet_easynav_collaboration_zenoh_deployment_plugins.xml b/easyfleet_easynav_collaboration_zenoh_deployment/easyfleet_easynav_collaboration_zenoh_deployment_plugins.xml index a5cf410..4d54b11 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/easyfleet_easynav_collaboration_zenoh_deployment_plugins.xml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/easyfleet_easynav_collaboration_zenoh_deployment_plugins.xml @@ -7,7 +7,7 @@ sibling easyfleet_easynav_collaboration_deployment package's own hardcoded factory already expects means the exact same "capabilities" parameter value (e.g. ["perception"]) works unchanged for either - package's robot_node.cpp -- the two are meant to be interchangeable + package's robot_node.cpp: the two are meant to be interchangeable at the launch level with no other change (see refactor.md). A package offering more than one alternative implementation of the same capability type would register each under diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/launch/easynav_collaboration_launch.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/launch/easynav_collaboration_launch.yaml index 299f15b..ee1a7a2 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/launch/easynav_collaboration_launch.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/launch/easynav_collaboration_launch.yaml @@ -22,9 +22,9 @@ # own (e.g. `ros2 launch easyfleet_easynav_collaboration_zenoh_deployment robot_1_launch.yaml`). # # Does NOT launch Gazebo -- start -# `ros2 launch easynav_playground_kobuki playgorund_multirobot_kobuki.launch.py` +# `ros2 launch easyfleet_easynav_collaboration_zenoh_deployment simulation_launch.yaml` # separately first, so both robots are already spawned (namespaced -# "robot_1"/"robot_2", see multirobot_config.yaml) and publishing +# "robot_1"/"robot_2") and publishing # scan_raw/odom/TF before system_main tries to localize against them. # # easynav_collaboration_mission_node is delayed a few seconds so both @@ -45,6 +45,7 @@ # contract crosses -- see zenoh.md "Punto 4" for why that's sufficient even # without a central-side ACL. launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" output: "screen" @@ -57,8 +58,12 @@ launch: children: - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/launch/robot_1_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} - include: file: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/launch/robot_2_launch.yaml" + arg: + - {name: "rviz", value: "$(var robot_rviz)"} # Fleet-wide navigation manager: publishes the same map/routes both # robots above would otherwise load locally on /global_map and diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_1_launch.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_1_launch.yaml index eee7826..8749d58 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_1_launch.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_1_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake perception capability (this package's own # robot_node), all namespaced under /robot_1 so they never collide with # robot_2's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_1's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # system_main's "tf_prefix" is set to "robot_1" to match: kobuki_description # prefixes every TF frame name with the robot's namespace unconditionally @@ -68,6 +68,7 @@ # Each robot gets its own window/config, so both can be watched navigating # at once. launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} - group: - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" @@ -97,6 +98,11 @@ launch: - name: "tf_prefix" value: "robot_1" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -154,6 +160,7 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_1/perception_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" @@ -162,6 +169,11 @@ launch: value: "rmw_zenoh_cpp" - name: "ZENOH_SESSION_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_1/zenoh_session.json5" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/rviz/robot_1.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_2_launch.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_2_launch.yaml index 5e74672..e2d7879 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_2_launch.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/launch/robot_2_launch.yaml @@ -19,9 +19,9 @@ # this package) plus a fake manipulation capability (this package's own # robot_node), all namespaced under /robot_2 so they never collide with # robot_1's identically-shaped setup. Can be run standalone, against -# easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py +# this package's simulation_launch.yaml # already running (robot_2's Kobuki is spawned there, namespaced the same -# way -- see multirobot_config.yaml). +# way). # # See robot_1_launch.yaml for why "tf_prefix" (and the namespace itself) are # kept hardcoded here even under the Zenoh-island setup below -- unrelated to @@ -37,6 +37,7 @@ # capabilities, global_map/routes, and this robot's own capability actions # (/robot_2/navigation, /robot_2/manipulation). launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's own rviz2"} - group: - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" @@ -66,6 +67,11 @@ launch: - name: "tf_prefix" value: "robot_2" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes from easyfleet_navigation_manager's # navigation_manager_node. Direction is "from what the code # itself names its topic, to the name we actually want" -- @@ -123,6 +129,7 @@ launch: - from: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_2/manipulation_params.yaml" - node: + if: "$(var rviz)" pkg: "rviz2" exec: "rviz2" output: "screen" @@ -131,6 +138,11 @@ launch: value: "rmw_zenoh_cpp" - name: "ZENOH_SESSION_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_2/zenoh_session.json5" + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" args: "-d $(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/rviz/robot_2.rviz" param: - name: "use_sim_time" diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_collaboration_zenoh_deployment/launch/simulation_launch.yaml index 58214f2..e455ce1 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/launch/simulation_launch.yaml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/launch/simulation_launch.yaml @@ -13,142 +13,66 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Zenoh-aware equivalent of easynav_playground_kobuki's own -# playgorund_multirobot_kobuki.launch.py, hardcoded for this deployment's -# two robots (mirrors that package's config/multirobot_config.yaml: kobuki_1 -# -> robot_1, lidar only; kobuki_2 -> robot_2, lidar only, offset x=2.0 -# y=1.0) instead of reading it generically, matching this package's existing -# robot_N_launch.yaml convention of being robot-specific rather than -# parameterized. Launches Gazebo (server + client), the shared /clock -# bridge, and both robots' Kobuki spawns (robot_state_publisher, -# joint_state_publisher, the per-robot Gazebo<->ROS bridge) -- but NOT -# system_main/the capability nodes/rviz2, which come from -# robot_1_launch.yaml/robot_2_launch.yaml (or easynav_collaboration_launch.yaml) -# as before. Run this file, then that one, in either order (see below). -# -# Zenoh wiring: every node here needs to land in the *same* Zenoh island as -# the rest of that robot's stack (see zenoh.md), not on the default rmw -- -# - The Kobuki spawn nodes for robot_1/robot_2 are plain rclcpp nodes with -# no zenoh-specific launch arguments of their own (kobuki_description's -# spawn.launch.py/kobuki_description.launch.py are generic, unaware of -# this deployment's Zenoh setup), so RMW_IMPLEMENTATION/ -# ZENOH_SESSION_CONFIG_URI are injected via `set_env` immediately before -# each robot's `include`, rather than per-node `env:` (which isn't -# available to us here, since we don't individually author those nodes). -# `set_env` mutates the whole launch context's environment from that point -# on, so it must be re-set to robot_2's session file before robot_2's -# include, or robot_2's nodes would inherit robot_1's session by mistake. -# - The /clock bridge is the one node here that must NOT go into either -# robot's island: it's the single shared source of sim time for the whole -# fleet, so it connects to the *central* router instead, and each robot -# router's ACL explicitly whitelists "*/clock/**" inbound (see -# config/robot_N/zenoh_router.json5) so it still reaches system_main in -# every island. -# - This file also starts all three routers itself (central + both robots), -# so it does not depend on run order relative to -# easynav_collaboration_launch.yaml/robot_N_launch.yaml, which start the -# very same routers: whichever of the two runs second just gets a -# harmless "address already in use" from its own duplicate router -# attempt (confirmed empirically -- see zenoh.md) while the one already -# running keeps serving; every other node here only ever *connects* to a -# router (with zenoh's own connect-retry), so it doesn't matter which -# file's router instance actually "won". +# Gazebo (small house world, from easynav_playground_kobuki_worlds), the +# /clock bridge and both Kobukis -- robot_1 at (0, 0), robot_2 at (2, 1), +# matching config/robot_N -- each in its own Zenoh island (see zenoh.md): +# the world and its /clock bridge use the central session, each Kobuki's +# nodes use its robot's session. Each part is a group, so the RMW/session +# environment set for one never leaks into the next. Also starts the three +# Zenoh routers (central, robot_1, robot_2). system_main, the capabilities and +# rviz2 come from robot_N_launch.yaml / easynav_collaboration_launch.yaml. + launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" output: "screen" env: - name: "ZENOH_ROUTER_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/zenoh_router_central.json5" - - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" output: "screen" env: - name: "ZENOH_ROUTER_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_1/zenoh_router.json5" - - executable: cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" output: "screen" env: - name: "ZENOH_ROUTER_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_2/zenoh_router.json5" - - # Gazebo world (server + GUI client), same world/args as - # easynav_playground_kobuki's playgorund_multirobot_kobuki.launch.py. - # gz-sim itself doesn't speak ROS/rmw, so no Zenoh env needed here. - - include: - file: "$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py" - arg: - - name: "gz_args" - value: "-r -s $(find-pkg-share aws_robomaker_small_house_world)/worlds/small_house.world" - - - include: - file: "$(find-pkg-share ros_gz_sim)/launch/gz_sim.launch.py" - arg: - - name: "gz_args" - value: " -g " - - timer: period: 2.0 children: - # Shared /clock: central island, so it's visible fleet-wide without - # namespacing; whitelisted into each robot island explicitly (see - # config/robot_N/zenoh_router.json5). - - node: - pkg: "ros_gz_bridge" - exec: "parameter_bridge" - name: "bridge_ros_gz" - output: "screen" - env: - - name: "RMW_IMPLEMENTATION" - value: "rmw_zenoh_cpp" - - name: "ZENOH_SESSION_CONFIG_URI" + - group: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - set_env: + name: "ZENOH_SESSION_CONFIG_URI" value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/zenoh_session_central.json5" - param: - - name: "config_file" - value: "$(find-pkg-share easynav_playground_kobuki)/config/bridge/clock_bridge.yaml" - - name: "use_sim_time" - value: true - - - set_env: - name: "RMW_IMPLEMENTATION" - value: "rmw_zenoh_cpp" - - set_env: - name: "ZENOH_SESSION_CONFIG_URI" - value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_1/zenoh_session.json5" - - include: - file: "$(find-pkg-share kobuki_description)/launch/spawn.launch.py" - arg: - - name: "name" - value: "kobuki_1" - - name: "namespace" - value: "robot_1" - - name: "camera" - value: "false" - - name: "lidar" - value: "true" - - name: "use_sim_time" - value: "true" - - - set_env: - name: "ZENOH_SESSION_CONFIG_URI" - value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_2/zenoh_session.json5" - - include: - file: "$(find-pkg-share kobuki_description)/launch/spawn.launch.py" - arg: - - name: "name" - value: "kobuki_2" - - name: "namespace" - value: "robot_2" - - name: "camera" - value: "false" - - name: "lidar" - value: "true" - - name: "x" - value: "2.0" - - name: "y" - value: "1.0" - - name: "use_sim_time" - value: "true" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} + - group: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - set_env: + name: "ZENOH_SESSION_CONFIG_URI" + value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_1/zenoh_session.json5" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - group: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - set_env: + name: "ZENOH_SESSION_CONFIG_URI" + value: "$(find-pkg-share easyfleet_easynav_collaboration_zenoh_deployment)/config/robot_2/zenoh_session.json5" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "2.0"} + - {name: "y", value: "1.0"} diff --git a/easyfleet_easynav_collaboration_zenoh_deployment/package.xml b/easyfleet_easynav_collaboration_zenoh_deployment/package.xml index 6aaadb6..223644b 100644 --- a/easyfleet_easynav_collaboration_zenoh_deployment/package.xml +++ b/easyfleet_easynav_collaboration_zenoh_deployment/package.xml @@ -44,6 +44,15 @@ easyfleet_navigation_manager easynav_system + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + rviz2 + rmw_zenoh_cpp + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/easyfleet_easynav_deployment/README.md b/easyfleet_easynav_deployment/README.md new file mode 100644 index 0000000..ac772b8 --- /dev/null +++ b/easyfleet_easynav_deployment/README.md @@ -0,0 +1,105 @@ +# easyfleet_easynav_deployment + +One Kobuki in Gazebo whose `navigation` capability is backed by real [EasyNavigation](https://easynavigation.github.io/). It uses a costmap AMCL localizer, a costmap planner and a regulated pure-pursuit controller. The robot's own navigation action server runs a BehaviorTree.CPP tree (`StartOff -> Navigate -> Finish`), and the `Navigate` node drives EasyNav. The fake examples simulate navigation; this one drives a simulated robot to named waypoints. The package also provides the parts the multi-robot EasyNav examples reuse: `easynav_navigation_capability_node`, the BT plugins, the `home2` map, the routes and the central `navigation_manager.rviz`. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +By default everything is unnamespaced, since there is a single robot. `robot_namespace:=` pushes the robot nodes under a namespace. + +| Process | What it is | +|---|---| +| Gazebo + Kobuki | Small house world from [easynav_playgrounds](https://github.com/EasyNavigation/easynav_playgrounds) (`easynav_playground_kobuki_worlds`), with one robot at (0, 0). | +| `system_main` | EasyNav: costmap maps manager plus routes maps manager, AMCL localizer, costmap planner, RPP controller. | +| `navigation` (`easynav_navigation_capability_node`) | The EasyFleet `navigation` capability. Waypoints: `dock`, `kitchen`, `charging_station`. | +| `rviz2` | Robot view (`rviz/easynav_costmap.rviz`): costmap, scan, particles, path. | +| `navigation_manager_node` | Publishes the map on `/global_map` and the routes on `/global_routes` (transient local). `system_main` takes them through remaps. | +| `easynav_mission_node` | Mission control, started 3 s after the rest. | +| `rmw_zenohd` | Zenoh router, started by `simulation_launch.yaml`. | + +## How to run + +Build the workspace as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Zenoh router + Gazebo + Kobuki +ros2 launch easyfleet_easynav_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: EasyNav + capability + RViz + navigation manager + mission +ros2 launch easyfleet_easynav_deployment easynav_gazebo_launch.yaml +``` + +`easynav_robot_gazebo_launch.yaml` starts only the robot side (`system_main`, the capability and RViz). Use it if you want to send goals yourself. + +## What you should see + +- **Gazebo:** the small house with the Kobuki. +- **RViz:** the costmap, the laser scan, the AMCL particles and the planned path while the robot moves. +- **Mission output:** + 1. **Phase 1:** discovery and the description of the capability. + 2. **Phase 2:** navigation to `dock`. `StartOff` and `Finish` print a message before and after the trip. The phase is capped at 10 s, so the goal may be stopped before the robot arrives. + 3. **Phase 3:** a goal to `dock`, then 3 s later a goal to `kitchen`. The first goal is aborted at the ROS level, and EasyNav is redirected to the new target without stopping. + +To send a goal by hand: + +```bash +RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 action send_goal /navigation easyfleet_interfaces/action/Navigation \ + "{parameters_json: '{\"goal_id\": \"kitchen\"}'}" +``` + +## The EasyNav-backed navigation capability + +This backend ignores `target_pose`. It reads a waypoint id from `parameters_json` (`{"goal_id": ""}`) and looks it up in `navigation.waypoints` (`config/easynav_robot/navigation_params.yaml`). The action runs the BT given in `behavior_tree_xml` with the plugins listed in `bt_plugins`: + +- `Navigate`, from `easyfleet_easynav_navigation`. +- `StartOff` and `Finish`, from this package. + +The capability keeps a persistent EasyNav goal client, so a new goal redirects EasyNav instead of stopping it first. + +## tf_prefix and namespaces + +Each robot has its own TF tree under its namespace (`/robot_N/tf`, remapped from `/tf`). The Kobuki publishes frames prefixed with that namespace (`robot_N/base_link`, ...). So when `robot_namespace` is set, `system_main` gets `tf_prefix:=` and uses the same frame names. With the default empty namespace, frames are plain `map`/`odom`/`base_link`. + +## Warehouse A + +The same robot in Warehouse A of [large_warehouse_world](https://github.com/juanscelyg/large_warehouse_world), a 200 x 200 m warehouse with rack aisles: + +```bash +# Terminal 1: Gazebo with Warehouse A and a Kobuki at (0, 0) of the map +ros2 launch easyfleet_easynav_deployment warehouse_simulation_launch.yaml + +# Terminal 2: EasyNav and its RViz, the navigation capability and the navigation manager +ros2 launch easyfleet_easynav_deployment warehouse_launch.yaml +``` + +Goals come from RViz's 2D Goal Pose. `mission:=true` also runs `easynav_mission_node`, whose 10 s per goal suit the small house rather than this warehouse. + +- **World**: `large_warehouse_world_a.sdf` by default (`world:=` for the whole one), with only Warehouse A furnished. The Kobuki's lidar reaches 30 m (`lidar_range`): at 12 m it sees almost nothing in the open areas. +- **Map** (`maps/warehouse_a.yaml`, 0.2 m, origin at the center of Warehouse A): what the lidar sees at its height (rack legs, the packages under the racks, walls). `scripts/sdf_to_costmap.py` generates it from the world; `maps/warehouse_a_collision.yaml` is the same band from the collision geometry (solid racks). +- **Routes** (`maps/warehouse_a_routes.yaml`): one per rack aisle, two along the central aisle, the edges of the rack area, the cross corridors and spurs to the docks. `scripts/aisle_routes.py` generates them from the maps. +- **EasyNav** (`config/warehouse/`): the costmap/RPP stack of this package on those maps; the map server's update rate is 2 Hz, as 10 Hz is not kept with 1010 x 1010 cells. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Zenoh router, Gazebo and the Kobuki (`gui` argument). | +| `launch/easynav_gazebo_launch.yaml` | Robot side + `navigation_manager_node` + mission (`robot_namespace` argument). | +| `launch/easynav_robot_gazebo_launch.yaml` | `system_main`, the capability and RViz, with the TF and global map remaps. | +| `config/easynav_robot/easynav_system.costmap_rpp.params.yaml` | EasyNav stack parameters. | +| `config/easynav_robot/navigation_params.yaml` | Named waypoints and BT tick rate. | +| `config/easynav_robot/navigation_manager.params.yaml` | Map and routes that `navigation_manager_node` publishes. | +| `config/easynav_robot/navigation_gazebo.json` | Capability description. | +| `behavior_trees/navigate.xml` | The `StartOff -> Navigate -> Finish` tree. | +| `maps/` | `home1`/`home2` occupancy maps, `routes_1.yaml`. | +| `rviz/easynav_costmap.rviz` | Single-robot view. | +| `rviz/navigation_manager.rviz` | Fleet view, used by the multi-robot examples. | +| `src//easynav_navigation_capability.cpp` | Capability implementation. | +| `src//start_off.cpp`, `finish.cpp` | Demo BT nodes. | +| `src/main_easynav.cpp` | The mission. | + +## Troubleshooting + +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). +- Mission phases are capped at 10 s, which is shorter than a full trip across the house. The multi-robot examples let goals run until they finish. diff --git a/easyfleet_easynav_deployment/include/easyfleet_easynav_deployment/easynav_navigation_capability.hpp b/easyfleet_easynav_deployment/include/easyfleet_easynav_deployment/easynav_navigation_capability.hpp index 2697e47..24b47b5 100644 --- a/easyfleet_easynav_deployment/include/easyfleet_easynav_deployment/easynav_navigation_capability.hpp +++ b/easyfleet_easynav_deployment/include/easyfleet_easynav_deployment/easynav_navigation_capability.hpp @@ -19,7 +19,6 @@ #include #include #include -#include #include "behaviortree_cpp/bt_factory.h" #include "easynav_system/GoalManagerClient.hpp" @@ -84,14 +83,15 @@ class EasynavNavigationActionServer : public easyfleet_core::NavigationActionSer std::string behavior_tree_xml_; double tick_rate_hz_; + /// Cancel EasyNav's navigation and process its answer (up to 1 s). + void cancel_and_wait(); + // GoalManagerClient needs a plain rclcpp::Node::SharedPtr, but this // capability is an rclcpp_lifecycle::LifecycleNode -- so, same pattern as // easyfleet_core::ActionClient, it owns a small internal node dedicated - // to talking to EasyNav, spun on its own background thread for the - // capability's whole lifetime. + // to talking to EasyNav, spun from on_execute() (no extra thread). rclcpp::Node::SharedPtr internal_node_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; - std::thread spin_thread_; // One instance for the capability's entire lifetime (not per ROS goal): // this is what lets a new goal preempt an in-flight one at the EasyNav diff --git a/easyfleet_easynav_deployment/launch/easynav_gazebo_launch.yaml b/easyfleet_easynav_deployment/launch/easynav_gazebo_launch.yaml index bffcb66..b38c28d 100644 --- a/easyfleet_easynav_deployment/launch/easynav_gazebo_launch.yaml +++ b/easyfleet_easynav_deployment/launch/easynav_gazebo_launch.yaml @@ -29,19 +29,37 @@ # # robot_namespace forwards to easynav_robot_gazebo_launch.yaml's own # argument of the same name (default "", unnamespaced, for a single real -# robot); pass robot_namespace:="robot_1" to match -# easynav_playground_kobuki's playground_monorobot_kobuki.launch.py. +# robot, and simulation_launch.yaml's unnamespaced Kobuki). launch: + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - arg: name: "robot_namespace" default: "" description: "Namespace the robot is pushed under. Empty (default) for a single unnamespaced real robot/Gazebo instance." + - arg: + name: "easynav_params" + default: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/easynav_system.costmap_rpp.params.yaml" + description: "EasyNav (system_main) parameters file" + - arg: + name: "navigation_params" + default: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/navigation_params.yaml" + description: "Navigation capability parameters file (waypoints)" + - arg: + name: "navigation_manager_params" + default: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/navigation_manager.params.yaml" + description: "navigation_manager parameters file (fleet map and routes)" + - arg: {name: "mission", default: "true", description: "Run the mission (easynav_mission_node)"} - include: file: "$(find-pkg-share easyfleet_easynav_deployment)/launch/easynav_robot_gazebo_launch.yaml" arg: - name: "robot_namespace" value: "$(var robot_namespace)" + - name: "easynav_params" + value: "$(var easynav_params)" + - name: "navigation_params" + value: "$(var navigation_params)" # Fleet-wide navigation manager: publishes the same map/routes the robot # above would otherwise load locally on /global_map and /global_routes @@ -54,9 +72,10 @@ launch: exec: "navigation_manager_node" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/navigation_manager.params.yaml" + - from: "$(var navigation_manager_params)" - timer: + if: "$(var mission)" period: 3.0 children: - node: diff --git a/easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml b/easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml index bbef3a4..94a979b 100644 --- a/easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml +++ b/easyfleet_easynav_deployment/launch/easynav_robot_gazebo_launch.yaml @@ -36,10 +36,9 @@ # including map_frame -- see easynav_common/RTTFBuffer.hpp) is set below to # the same robot_namespace value so its TF lookups match; empty by default, # a no-op, so the unnamespaced case is unaffected. Set -# robot_namespace:="robot_1" (matching easynav_playground_kobuki's -# playground_monorobot_kobuki.launch.py, whose monorobot_config.yaml spawns -# the Kobuki under that same namespace) to exercise EasyFleet's -# namespace-based robot-id handling here too. +# robot_namespace:="robot_1" (and spawn the Kobuki with +# easynav_playground_kobuki_worlds' kobuki.launch.yaml namespace:=robot_1) to +# exercise EasyFleet's namespace-based robot-id handling here too. # # rviz2 is inside the same namespace group, with an explicit "remap" entry # per absolute ("/foo") topic reference baked into @@ -56,11 +55,9 @@ # which is exactly the relative topic easynav_system's GoalManager # subscribes to (GoalManager.cpp) -- so once both are in the same # namespace, the RViz arrow drives EasyNav directly, no GoalManagerClient -# needed. /tf and /tf_static are deliberately *not* remapped: this scenario -# uses tf_prefix (a shared /tf topic, namespace-prefixed frame *names*), -# not kobuki_description's separate do_tf_remapping (per-robot /tf -# *topics*) -- see the tf_prefix comment above -- so rviz2 needs to see the -# same shared /tf topic system_main/Kobuki do. Fixed Frame is overridden +# needed. /tf and /tf_static are remapped to the namespace too, as +# easynav_playground_kobuki_worlds' kobuki.launch.yaml does for a namespaced +# Kobuki (//tf); unnamespaced, that is plain /tf. Fixed Frame is overridden # via rviz2's own "-f" CLI flag rather than editing the .rviz file, using # "$(path-join ...)" so it comes out as "map" when unnamespaced and # "/map" when namespaced (path-join drops an empty first @@ -68,10 +65,20 @@ # leading-slash "/map" -- not a valid unprefixed frame id -- in the # unnamespaced case). launch: + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - arg: name: "robot_namespace" default: "" description: "Namespace system_main + the navigation capability are pushed under. Empty (default) for a single unnamespaced real robot/Gazebo instance." + - arg: + name: "easynav_params" + default: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/easynav_system.costmap_rpp.params.yaml" + description: "EasyNav (system_main) parameters file" + - arg: + name: "navigation_params" + default: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/navigation_params.yaml" + description: "Navigation capability parameters file (waypoints)" - group: - push_ros_namespace: @@ -82,10 +89,15 @@ launch: exec: "system_main" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/easynav_system.costmap_rpp.params.yaml" + - from: "$(var easynav_params)" - name: "tf_prefix" value: "$(var robot_namespace)" remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" # Fleet-wide map/routes, published once by # easyfleet_navigation_manager's navigation_manager_node. # Remap direction is "from what the code itself names its @@ -110,7 +122,7 @@ launch: name: "navigation" output: "screen" param: - - from: "$(find-pkg-share easyfleet_easynav_deployment)/config/easynav_robot/navigation_params.yaml" + - from: "$(var navigation_params)" - name: "behavior_tree_xml" value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" - name: "bt_plugins" @@ -130,6 +142,11 @@ launch: - name: "use_sim_time" value: true remap: + # Each robot has its own TF tree, under its namespace (/robot_N/tf). + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" - from: "/maps_manager_node/costmap/dynamic_map" to: "maps_manager_node/costmap/dynamic_map" - from: "/maps_manager_node/costmap/dynamic_map_updates" diff --git a/easyfleet_easynav_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..5b52223 --- /dev/null +++ b/easyfleet_easynav_deployment/launch/simulation_launch.yaml @@ -0,0 +1,30 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with the small house world and one unnamespaced Kobuki (from +# easynav_playground_kobuki_worlds), plus the Zenoh router. Run it before +# easynav_gazebo_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/gazebo_sim.launch.yaml" + arg: + - {name: "gui", value: "$(var gui)"} diff --git a/easyfleet_easynav_deployment/package.xml b/easyfleet_easynav_deployment/package.xml index 76d83b7..535cafb 100644 --- a/easyfleet_easynav_deployment/package.xml +++ b/easyfleet_easynav_deployment/package.xml @@ -44,6 +44,15 @@ easyfleet_navigation_manager rviz2 + easynav_costmap_localizer + easynav_costmap_maps_manager + easynav_costmap_planner + easynav_regulated_pp_controller + easynav_routes_maps_manager + easynav_playground_kobuki_worlds + large_warehouse_world + rmw_zenoh_cpp + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/easyfleet_easynav_deployment/rviz/easynav_costmap.rviz b/easyfleet_easynav_deployment/rviz/easynav_costmap.rviz index 555f702..2cbb5fd 100644 --- a/easyfleet_easynav_deployment/rviz/easynav_costmap.rviz +++ b/easyfleet_easynav_deployment/rviz/easynav_costmap.rviz @@ -6,10 +6,11 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Map1 - /TF1/Frames1 - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 711 + Tree Height: 708 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -53,7 +54,7 @@ Visualization Manager: Binary representation: false Binary threshold: 100 Class: rviz_default_plugins/Map - Color Scheme: map + Color Scheme: costmap Draw Behind: false Enabled: true Name: Map @@ -63,13 +64,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /maps_manager_node/costmap/dynamic_map + Value: /maps_manager_node/routes/routes_map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /maps_manager_node/costmap/dynamic_map_updates + Value: /maps_manager_node/routes/routes_map_updates Use Timestamp: false Value: true - Class: rviz_default_plugins/TF @@ -83,16 +84,6 @@ Visualization Manager: Value: true base_link: Value: false - camera_depth_frame: - Value: false - camera_depth_optical_frame: - Value: false - camera_link: - Value: false - camera_rgb_frame: - Value: false - camera_rgb_optical_frame: - Value: false caster_back_link: Value: false caster_front_link: @@ -107,6 +98,8 @@ Visualization Manager: Value: false laser_link: Value: false + lidar_target_link: + Value: true map: Value: true odom: @@ -159,14 +152,6 @@ Visualization Manager: odom: base_footprint: base_link: - camera_rgb_frame: - camera_depth_frame: - camera_depth_optical_frame: - {} - camera_link: - {} - camera_rgb_optical_frame: - {} caster_back_link: {} caster_front_link: @@ -180,7 +165,8 @@ Visualization Manager: gyro_link: {} laser_link: - {} + lidar_target_link: + {} plate_bottom_link: {} plate_middle_link: @@ -361,27 +347,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_rgb_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_rgb_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false caster_back_link: Alpha: 1 Show Axes: false @@ -413,6 +378,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + lidar_target_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true plate_bottom_link: Alpha: 1 Show Axes: false @@ -562,7 +532,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.1107726097106934 + Distance: 23.658164978027344 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -577,10 +547,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6353981494903564 + Pitch: 0.680398166179657 Target Frame: Value: Orbit (rviz) - Yaw: 0.9203978776931763 + Yaw: 0.5753951072692871 Saved: ~ Window Geometry: Displays: @@ -588,7 +558,7 @@ Window Geometry: Height: 1178 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000001000000000000030300000440fc0200000005fb000000100044006900730070006c006100790073010000003d00000352000000c900fffffffb0000001200530065006c0065006300740069006f006e00000002a7000001250000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000030b000000e60000005c00fffffffb0000000a00560069006500770073000000030e00000103000000a400fffffffb0000000800540069006d00650100000395000000e80000003900ffffff0000070d0000044000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000003030000043efc0200000005fb000000100044006900730070006c006100790073010000003e00000350000000ca00fffffffb0000001200530065006c0065006300740069006f006e00000002a7000001250000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000030b000000e60000005c00fffffffb0000000a00560069006500770073000000030e00000103000000a600fffffffb0000000800540069006d00650100000394000000e80000003a00ffffff000006d20000043e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -597,6 +567,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2582 - X: 1707 + Width: 2523 + X: 37 Y: 91 diff --git a/easyfleet_easynav_deployment/rviz/navigation_manager.rviz b/easyfleet_easynav_deployment/rviz/navigation_manager.rviz index b0ffea8..f1963a5 100644 --- a/easyfleet_easynav_deployment/rviz/navigation_manager.rviz +++ b/easyfleet_easynav_deployment/rviz/navigation_manager.rviz @@ -10,9 +10,6 @@ Panels: - /MarkerArray1 - /MarkerArray1/Topic1 - /InteractiveMarkers1 - - /RobotModel1 - - /RobotModel1/Mass Properties1 - - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 Tree Height: 553 - Class: rviz_common/Selection @@ -100,328 +97,87 @@ Visualization Manager: Show Descriptions: true Show Visual Aids: false Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: robot_1 base_link + Radius: 0.05 + Reference Frame: robot_1/base_link + Value: true - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_1 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: Depth: 5 - Durability Policy: Transient Local + Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_1/robot_description + Value: /robot_1/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/Axes Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - robot_1/base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_1/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/caster_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/caster_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/cliff_sensor_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_1/cliff_sensor_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_1/cliff_sensor_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_1/gyro_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_1/laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/plate_bottom_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/plate_middle_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/plate_top_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_1in_5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_2in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_2in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_2in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_2in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_8in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_8in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_8in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/standoff_8in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/wheel_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_1/wheel_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 + Length: 0.5 + Name: robot_2 base_link + Radius: 0.05 + Reference Frame: robot_2/base_link Value: true - Visual Enabled: true - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_2 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_2/robot_description + Value: /robot_2/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/MarkerArray Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - robot_2/base_footprint: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_2/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/caster_back_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/caster_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/cliff_sensor_front_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_2/cliff_sensor_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_2/cliff_sensor_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_2/gyro_link: - Alpha: 1 - Show Axes: false - Show Trail: false - robot_2/laser_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/plate_bottom_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/plate_middle_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/plate_top_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_4_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_1in_5_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_2in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_2in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_2in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_2in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_8in_0_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_8in_1_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_8in_2_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/standoff_8in_3_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/wheel_left_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - robot_2/wheel_right_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 + Name: MissionStatus + Namespaces: + {} + Topic: + Depth: 10 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mission_status_markers Value: true - Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/easyfleet_easynav_deployment/src/easyfleet_easynav_deployment/easynav_navigation_capability.cpp b/easyfleet_easynav_deployment/src/easyfleet_easynav_deployment/easynav_navigation_capability.cpp index 341329b..e04c1ae 100644 --- a/easyfleet_easynav_deployment/src/easyfleet_easynav_deployment/easynav_navigation_capability.cpp +++ b/easyfleet_easynav_deployment/src/easyfleet_easynav_deployment/easynav_navigation_capability.cpp @@ -133,12 +133,10 @@ EasynavNavigationActionServer::EasynavNavigationActionServer( gm_client_ = easynav::GoalManagerClient::make_shared(internal_node_); + // Spun from on_execute() only (the one thread that also ticks the tree), + // so gm_client_ is never touched from two threads at once. executor_ = std::make_shared(); executor_->add_node(internal_node_); - spin_thread_ = std::thread([this] {executor_->spin();}); - while (!executor_->is_spinning()) { - std::this_thread::yield(); - } // Every BT node this tree can use -- including this package's own // Navigate -- is loaded as a plugin by path, so this class has no @@ -151,12 +149,8 @@ EasynavNavigationActionServer::EasynavNavigationActionServer( EasynavNavigationActionServer::~EasynavNavigationActionServer() { - if (executor_) { - executor_->cancel(); - } - if (spin_thread_.joinable()) { - spin_thread_.join(); - } + // on_execute() uses executor_ and gm_client_: stop it first. + stop(); } rclcpp_action::GoalResponse EasynavNavigationActionServer::on_goal_received( @@ -201,9 +195,13 @@ void EasynavNavigationActionServer::on_execute(const GoalHandleSharedPtr goal_ha rclcpp::Rate rate(tick_rate_hz_); BT::NodeStatus status = BT::NodeStatus::RUNNING; + // Process what EasyNav sent since the last goal before the tree looks at it. + executor_->spin_some(); + while (status == BT::NodeStatus::RUNNING) { + executor_->spin_some(); if (goal_handle->is_canceling()) { - gm_client_->cancel(); + cancel_and_wait(); auto result = std::make_shared(); result->error_code = Result::CANCELED; result->error_msg = "Navigation canceled by client."; @@ -221,7 +219,7 @@ void EasynavNavigationActionServer::on_execute(const GoalHandleSharedPtr goal_ha return; } if (is_shutdown_requested()) { - gm_client_->cancel(); + cancel_and_wait(); auto result = std::make_shared(); result->error_code = Result::ABORTED; result->error_msg = "Navigation capability is shutting down."; @@ -253,6 +251,24 @@ void EasynavNavigationActionServer::on_execute(const GoalHandleSharedPtr goal_ha } } +void EasynavNavigationActionServer::cancel_and_wait() +{ + // Let EasyNav's answer (CANCELLED) be processed now: otherwise it stays + // queued until the next goal, whose send it would then confuse. + using State = easynav::GoalManagerClient::State; + gm_client_->cancel(); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (std::chrono::steady_clock::now() < deadline) { + executor_->spin_some(std::chrono::milliseconds(10)); + const auto state = gm_client_->get_state(); + if (state != State::ACCEPTED_AND_NAVIGATING && state != State::SENT_GOAL && + state != State::SENT_PREEMPT) + { + return; + } + } +} + EasynavNavigationCapability::EasynavNavigationCapability(const rclcpp::NodeOptions & options) : easyfleet_core::Capability("navigation", options) { diff --git a/easyfleet_easynav_farm_deployment/CMakeLists.txt b/easyfleet_easynav_farm_deployment/CMakeLists.txt new file mode 100644 index 0000000..5956dbf --- /dev/null +++ b/easyfleet_easynav_farm_deployment/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.20) +project(easyfleet_easynav_farm_deployment) + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch config rviz + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/easyfleet_easynav_farm_deployment/README.md b/easyfleet_easynav_farm_deployment/README.md new file mode 100644 index 0000000..f9a8365 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/README.md @@ -0,0 +1,67 @@ +# easyfleet_easynav_farm_deployment + +One Summit XL navigating outdoors on a farm (the `granja_leon` world). Its EasyFleet `navigation` capability is backed by EasyNav with a different stack from the indoor examples: + +- **Localization:** GPS + IMU + wheel odometry fusion (`easynav_fusion_localizer/FusionLocalizer`), with no laser or AMCL. +- **Map:** a **NavMap** (`easynav_navmap_maps_manager`), with preferred routes overlaid by `easynav_navmap_routes_maps_manager`. +- **Planning and control:** an A* planner on the NavMap (`easynav_navmap_planner`) and the regulated pure-pursuit controller. + +It shows that the capability, the BT and the waypoint interface from [easyfleet_easynav_deployment](../easyfleet_easynav_deployment) work unchanged on a different robot, environment and EasyNav configuration. This package has only launch and config files, and no code of its own. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Process | Namespace | What it is | +|---|---|---| +| Gazebo + Summit XL | none | `easynav_playground_summit_worlds` from [easynav_playgrounds](https://github.com/EasyNavigation/easynav_playgrounds), loaded with the `granja_leon` world. The robot is not namespaced: it publishes `/robotnik_base_control/odom`, `/gps/fix`, `/imu/data`, `/tf` and listens on `/cmd_vel`. | +| `system_main` | `/robot_1` | EasyNav: `FusionLocalizer`, NavMap and NavMap routes maps managers, A* planner, RPP controller. Its `cmd_vel` is remapped to `/cmd_vel`. | +| `navigation` | `/robot_1` | `easynav_navigation_capability_node` from `easyfleet_easynav_deployment`, with the waypoints `north_point` (0, 10), `east_point` (10, 0) and `home` (0, 0) in `map`. | +| `rviz2` | `/robot_1` | `rviz/robot_1.rviz`: NavMap, filtered odometry, planned path. | +| `rmw_zenohd` | none | Zenoh router, started by `simulation_launch.yaml`. | + +The `navigation` capability is `/robot_1/navigation`. There is no mission node and no `navigation_manager_node` in this example, so you send the goals yourself. + +## Requirements + +`granja_leon`, the world, its models and `maps/granja_leon.navmap`, is a separate package that must be in your workspace. It is **not publicly released yet**, so `easyfleet_example_deployments.repos` does not include it. The fusion localizer also needs `robot_localization` (rolling-devel), which that `.repos` file does include. + +## How to run + +Build the workspace (with `granja_leon` in it) as described in the [top-level README](../README.md). Then: + +```bash +# Terminal 1: Zenoh router + Gazebo with granja_leon + Summit XL +ros2 launch easyfleet_easynav_farm_deployment simulation_launch.yaml # gui:=false for headless + +# Terminal 2: EasyNav + navigation capability + RViz (rviz:=false to skip RViz) +ros2 launch easyfleet_easynav_farm_deployment robot_1_launch.yaml + +# Terminal 3: send a goal to a named waypoint +RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 action send_goal --feedback /robot_1/navigation \ + easyfleet_interfaces/action/Navigation "{parameters_json: '{\"goal_id\": \"north_point\"}'}" +``` + +The capability ignores `target_pose` and navigates to the waypoint named in `goal_id`. + +## What you should see + +- **Gazebo:** the farm and the Summit XL, which drives once a goal is sent. +- **RViz:** the NavMap, the filtered odometry from the fusion localizer and the planned A* path. +- **Terminal 3:** navigation feedback and a result with `error_code` 0 when the robot gets within `position_tolerance` (0.75 m) of the waypoint. + +## Key files + +| File | Purpose | +|---|---| +| `launch/simulation_launch.yaml` | Zenoh router, `GZ_SIM_RESOURCE_PATH` for the `granja_leon` models, and the Summit XL in the `granja_leon` world (`gui`). | +| `launch/robot_1_launch.yaml` | `system_main`, the navigation capability and RViz under `/robot_1` (`rviz`). | +| `config/easynav_system.params.yaml` | EasyNav stack. The GPS origin (42.630258, -5.551252), the local and global filters, the NavMap from `granja_leon`, A* and RPP. | +| `config/navigation_params.yaml` | Named waypoints. | +| `config/navigation.json` | Capability description, published on `/capabilities`. | +| `rviz/robot_1.rviz` | NavMap view (needs `navmap_rviz_plugin`). | + +## Notes and troubleshooting + +- Only the EasyFleet and EasyNav nodes are under `robot_1`. The simulated robot is not namespaced, and `tf_prefix` is not set, so EasyNav uses plain `map`, `odom` and `base_link`. The fixed frame in `rviz/robot_1.rviz` is `robot_1/map`. If RViz shows nothing, set Fixed Frame to `map`. +- In some environments (for example RoboStack/pixi rolling), EasyNav binaries currently fail with `undefined symbol: PyObject_GetAttrString`. This is an EasyNav linking issue. Workaround: `export LD_PRELOAD=$CONDA_PREFIX/lib/libpython3.14.so` (adjust the path to your Python). diff --git a/easyfleet_easynav_farm_deployment/config/easynav_system.params.yaml b/easyfleet_easynav_farm_deployment/config/easynav_system.params.yaml new file mode 100644 index 0000000..7de6bbf --- /dev/null +++ b/easyfleet_easynav_farm_deployment/config/easynav_system.params.yaml @@ -0,0 +1,312 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# GPS-localized, NavMap-mapped, no-distance-sensor EasyNav configuration for +# a Summit robot spawned under the "robot_1" namespace (see +# easynav_playground_summit/launch/playground_monorobot_summit.launch.py). +# Adapted from easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml +# (already tuned for Summit): +# - localizer_node: kept as-is except latitude_origin/longitude_origin/ +# altitude_origin, fixed to match src/granja_leon/worlds/granja_leon.world's +# own (42.630258, -5.551252, 842.3611450195312) -- +# the example's own origin (~40.28, -3.82) is a different location, and +# must match the world's GPS anchor or the GPS->local-XY conversion this +# plugin does internally would be offset from where system_main's map +# frame actually is. +# - maps_manager_node: swapped from costmap+routes to navmap (routes are +# explicitly deferred to later), pointing at the granja_leon package's +# already-generated maps/granja_leon.navmap. +# - planner_node: swapped from CostmapPlanner to easynav_navmap_planner's +# AStarPlanner (the NavMap-paired planner; naming convention matches +# easynav_indoor_testcase/robots_params/navmap.kobuki.params.yaml's own +# "planner_types: [simple]" block). No obstacles/inflated_obstacles NavMap +# layer exists (no distance sensor to populate one, by design), so +# AStarPlanner falls back to FREE_SPACE for every NavCel and degrades to +# pure geometric shortest-path -- verified against navmap_core's +# layer_get() default-on-missing-layer behavior. +# - sensors_node: "laser1" dropped entirely -- GPS + IMU + wheel odometry +# only, no distance/range sensor. +# - controller_node: RegulatedPurePursuitController. +# - Sensor topics are absolute: the Summit from easynav_playground_summit is +# not namespaced (/gps/fix, /imu/data, /robotnik_base_control/odom, /tf), +# while system_main runs under "robot_1" (see launch/robot_1_launch.yaml). +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [Ukf] + dummy: + rt_freq: 30.0 + plugin: easynav_localizer/DummyLocalizer + cycle_time_nort: 0.01 + cycle_time_rt: 0.001 + Ukf: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 0.1 + plugin: easynav_fusion_localizer/FusionLocalizer + latitude_origin: 42.630258 + longitude_origin: -5.551252 + altitude_origin: 842.3611450195312 + local_filter: + two_d_mode: true + publish_tf: false + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + dynamic_process_noise_covariance: true + + #--- INPUT: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [true, true, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + global_filter: + two_d_mode: true + publish_tf: true + debug: false + # transform_timeout is no longer a valid parameter here: ukf_wrapper.cpp + # now hardcodes it to zero (see UkfWrapper::loadParams()) so a config + # value can never make the real-time thread block on a TF lookup again. + smooth_lagged_data: true + dynamic_process_noise_covariance: true + + # State the filter assumes before its first correction (map frame: + # [X, Y, Z, roll, pitch, yaw, Vx, Vy, Vz, Vroll, Vpitch, Vyaw, Ax, Ay, Az]). + # Matches the robot's Gazebo spawn pose (see + # easynav_playground_summit/launch/playground_monorobot_summit.launch.py) + # so map->odom is correct from the very first cycle instead of + # starting at (0,0,0) and jumping once the first GPS fix lands. + initial_state: [29.406, 5.22624, -0.427925, 0.0, 0.0, -0.527741, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + # --- Reference Frames --- + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # --- INPUT 1: GPS --- + gps0: /gps/fix + gps0_config: + [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + gps0_differential: false + + # --- INPUT 2: Wheel Odometry --- + odom0: /robotnik_base_control/odom + odom0_config: + [false, false, false, + false, false, false, + true, true, false, + false, false, true, + false, false, false] + odom0_differential: false + + # --- INPUT 3: IMU --- + imu0: /imu/data + imu0_config: + [false, false, false, + false, false, false, + false, false, false, + false, false, true, + false, false, false] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # --- Process Variances --- + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] + + initial_estimate_covariance: [ + 5.0, 5.0, 1e-3, 1e-3, 1e-3, 1.0, + 0.5, 0.5, 1e-3, 1e-3, 1e-3, 0.1, + 0.1, 0.1, 0.1 + ] + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [navmap, navmap_routes] + navmap: + freq: 10.0 + plugin: easynav_navmap_maps_manager/NavMapMapsManager + package: granja_leon + navmap_path_file: maps/granja_leon.navmap + # No filters: ObstaclesFilter needs point-cloud "points" perceptions, + # which this deployment deliberately has none of (GPS-only, no + # distance/range sensor) -- with it configured, it logged "There are + # no points perceptions" every cycle and appeared to block the + # NavMap itself from ever being published. + # Second maps manager, run after "navmap" above so RoutesNavMapFilter's + # own filter chain sees that manager's already-populated shared + # "navigation" layer (obstacles/inflation) before adding route costs on + # top of it -- see easynav_navmap_routes_maps_manager's README. + navmap_routes: + freq: 10.0 + plugin: easynav_navmap_routes_maps_manager/NavMapRoutesMapsManager + # No package/map_path_file: falls back to the manager's own single + # default segment ((0,0,0) -> (1,0,0)); edit routes live via RViz's + # interactive markers (or the /.../save_routes service once real + # farm routes are drawn) rather than a placeholder YAML. + filters: [routes_navmap] + routes_navmap: + plugin: easynav_navmap_routes_maps_manager/RoutesNavMapFilter + min_cost: 200 + route_width: 1.0 + dummy: + freq: 10.0 + plugin: easynav_maps_manager/DummyMapsManager + cycle_time_nort: 0.1 + cycle_time_rt: 0.0001 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + dummy: + freq: 1.0 + plugin: easynav_planner/DummyPlanner + cycle_time_nort: 0.2 + cycle_time_rt: 0.0001 + simple: + freq: 0.5 + plugin: easynav_navmap_planner/AStarPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits for the Summit XL (outdoor base, much + # larger/faster than the indoor Kobuki these were copied from) -- + # matches the envelope the localizer's own + # dynamic_process_noise_covariance already assumes for this robot (see + # max_linear_velocity/max_angular_velocity in + # easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml). + max_linear_vel: 1.5 + min_linear_vel: -0.5 + max_angular_vel: 1.0 + min_angular_vel: -1.0 + max_linear_accel: 0.8 + max_linear_decel: 0.8 + max_angular_accel: 1.0 + max_angular_decel: 1.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [gps, imu] + perception_default_frame: odom + imu: + topic: /imu/data + type: sensor_msgs/msg/Imu + gps: + topic: /gps/fix + type: sensor_msgs/msg/NavSatFix + # Required: FusionLocalizer::update_rt() looks up GPS perceptions via + # nav_state.has_group("gnss")/get_group("gnss") + # specifically -- without this, the sensor registers under the empty + # default group and GPS is silently never fused into the global + # filter (it just free-runs on odom/IMU velocity dead-reckoning, + # drifting away from the true, GPS-confirmed position). + group: gnss + +/**/system_node: + ros__parameters: + use_sim_time: true + position_tolerance: 0.75 + angle_tolerance: 0.25 diff --git a/easyfleet_easynav_farm_deployment/config/navigation.json b/easyfleet_easynav_farm_deployment/config/navigation.json new file mode 100644 index 0000000..d7e02e6 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/config/navigation.json @@ -0,0 +1,77 @@ +{ + "name": "navigation", + "display_name": "Navigate to a named waypoint (GPS-localized EasyNav, NavMap)", + "description": "Moves the robot to one of a fixed set of named waypoints (configured on this node as ROS parameters) by driving a real EasyNav navigation stack through easynav::GoalManagerClient. Internally structured as a BehaviorTree.CPP tree (StartOff -> Navigate -> Finish). Localization is GPS-based (easynav_fusion_localizer/FusionLocalizer, fusing GPS + wheel odometry + IMU -- no distance/range sensor), and the map is a NavMap (easynav_navmap_maps_manager/NavMapMapsManager) rather than an occupancy grid.", + "action": { + "name": "/robot_1/navigation", + "type": "easyfleet_interfaces/action/Navigation", + "goal": { + "target_pose": "geometry_msgs/PoseStamped - unused by this backend.", + "waypoints": "geometry_msgs/PoseStamped[] - unused by this backend.", + "parameters_json": "string - REQUIRED for this backend: a JSON object with a string field \"goal_id\" naming one of the waypoints configured on this node (see 'parameters' below for the current list). Goals without a valid goal_id are rejected." + }, + "feedback": { + "current_pose": "geometry_msgs/PoseStamped - robot's current pose, as reported by EasyNav.", + "navigation_time": "builtin_interfaces/Duration - time elapsed since this navigation goal started, as reported by EasyNav.", + "estimated_time_remaining": "builtin_interfaces/Duration - as reported by EasyNav.", + "distance_remaining": "float32 - straight-line distance left to the goal, as reported by EasyNav.", + "number_of_recoveries": "int16 - always 0: EasyNav has no equivalent concept.", + "current_waypoint_index": "uint32 - always 0: this backend navigates to one waypoint per goal, not a waypoint list." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT, 10 NO_PATH_FOUND, 11 LOCALIZATION_LOST.", + "error_msg": "string - human-readable explanation, populated when error_code is non-zero.", + "final_pose": "geometry_msgs/PoseStamped - robot's pose when the goal ended, as reported by EasyNav.", + "total_time": "builtin_interfaces/Duration - not populated by this backend (left at its default)." + } + }, + "requirements": [ + "parameters_json must contain a \"goal_id\" naming one of this node's configured waypoints.", + "EasyNav (easynav_system system_main) must already be running and reachable on the robot_1/easynav_control topic, GPS-localized (FusionLocalizer), with the granja_leon NavMap loaded and a Summit (Gazebo-simulated by launch/simulation_launch.yaml, not namespaced) publishing /gps/fix, /robotnik_base_control/odom and /imu/data -- no laser/scan topic is required or used." + ], + "effects": [ + "On success (error_code == SUCCESS), the robot's physical location changes to be at (or very near) the requested waypoint.", + "A currently executing goal can be preempted by a newer navigation goal: EasyNav is redirected to the new waypoint without stopping first (see 'preemption' below).", + "A canceled or capability-shutdown goal sends EasyNav an explicit cancellation, stopping the robot." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "navigation.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the previous ROS goal, but EasyNav itself is redirected (not stopped) to the new waypoint -- the robot keeps moving, just towards the new target.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must wait for the current navigation to finish, or explicitly cancel it, before sending another." + }, + "parameters": { + "navigation.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new navigation goal is allowed to preempt (replace) one that is currently executing." + }, + "navigation.tick_rate_hz": { + "type": "double", + "unit": "hertz", + "default": 10.0, + "description": "Rate at which the internal behavior tree is ticked while a goal is executing." + }, + "navigation.waypoint_ids": { + "type": "string[]", + "default": ["north_point", "east_point", "home"], + "description": "The set of named waypoints this node accepts as parameters_json.goal_id, valid on the granja_leon map (see navigation_params.yaml). Each id must have a matching navigation.waypoints..{frame_id,x,y,yaw} sub-parameter." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + }, + "behavior_tree_xml": { + "type": "string", + "description": "Path to the BehaviorTree.CPP XML tree to run per goal (StartOff -> Navigate -> Finish for this deployment)." + }, + "bt_plugins": { + "type": "string[]", + "description": "Paths to the BT.CPP plugin shared libraries (BT_REGISTER_NODES) to load before building the tree -- every node the tree XML references must come from one of these. This deployment reuses easyfleet_easynav_deployment's StartOff/Finish and easyfleet_easynav_navigation's real Navigate node." + } + }, + "notes": [ + "This backend drives a real EasyNav navigation stack (easynav_system system_main) with a GPS-localized, NavMap-mapped configuration -- see config/easynav_system.params.yaml (easynav_fusion_localizer/FusionLocalizer + easynav_navmap_maps_manager/NavMapMapsManager + easynav_navmap_planner/AStarPlanner + easynav_regulated_pp_controller/RegulatedPurePursuitController).", + "Deployment: Gazebo test; the EasyFleet/EasyNav nodes run under \"robot_1\" (its identity in the fleet), the Summit is not namespaced and its frames are unprefixed. Start the simulation first: ros2 launch easyfleet_easynav_farm_deployment simulation_launch.yaml." + ] +} diff --git a/easyfleet_easynav_farm_deployment/config/navigation_params.yaml b/easyfleet_easynav_farm_deployment/config/navigation_params.yaml new file mode 100644 index 0000000..7f5e213 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/config/navigation_params.yaml @@ -0,0 +1,48 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for the "navigation" capability, reusing +# easyfleet_easynav_deployment's own easynav_navigation_capability_node as-is +# (see launch/robot_1_launch.yaml) -- see that package's own +# navigation_params.yaml for the full explanation of why "/**" is used here +# instead of a bare node name (this node is namespaced under "robot_1"). +# +# Waypoints are local-ENU coordinates on src/granja_leon's map (a 400x400 m +# square centered on the world's GPS anchor, see +# src/granja_leon/worlds/granja_leon.world's ), kept +# modest and close to the robot's spawn point (0, 0) to stay well within the +# navigable area regardless of local terrain slope. +/**: + ros__parameters: + navigation: + allow_preemption: true + tick_rate_hz: 10.0 + waypoint_ids: ["north_point", "east_point", "home"] + waypoints: + north_point: + frame_id: "map" + x: 0.0 + y: 10.0 + yaw: 1.57 + east_point: + frame_id: "map" + x: 10.0 + y: 0.0 + yaw: 0.0 + home: + frame_id: "map" + x: 0.0 + y: 0.0 + yaw: 0.0 diff --git a/easyfleet_easynav_farm_deployment/launch/robot_1_launch.yaml b/easyfleet_easynav_farm_deployment/launch/robot_1_launch.yaml new file mode 100644 index 0000000..e194837 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/launch/robot_1_launch.yaml @@ -0,0 +1,71 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_1 in this scenario: a real EasyNav navigation stack (easynav_system +# system_main + easyfleet_easynav_deployment's own +# easynav_navigation_capability_node, reused directly -- not duplicated in +# this package), configured for GPS localization (easynav_fusion_localizer) +# and a NavMap (easynav_navmap_maps_manager), no distance/range sensor. +# Namespaced under /robot_1, the robot's identity in the fleet; the Summit +# itself (launch/simulation_launch.yaml, from easynav_playground_summit) is +# not namespaced, so its sensor topics are absolute in the parameters file. +# +# No tf_prefix: the playground Summit's frames are not prefixed either. +launch: + - arg: {name: "rviz", default: "true", description: "Open this robot's rviz2"} + # Zenoh by default; needs the router started by simulation_launch.yaml. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - group: + - push_ros_namespace: + namespace: "robot_1" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_farm_deployment)/config/easynav_system.params.yaml" + # The playground's Summit is not namespaced: its frames have no + # prefix and it listens on /cmd_vel. + remap: + - from: "cmd_vel" + to: "/cmd_vel" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_easynav_farm_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_easynav_farm_deployment)/config/navigation.json" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + args: "-d $(find-pkg-share easyfleet_easynav_farm_deployment)/rviz/robot_1.rviz" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_easynav_farm_deployment/launch/simulation_launch.yaml b/easyfleet_easynav_farm_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..9c20b30 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/launch/simulation_launch.yaml @@ -0,0 +1,35 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with the granja_leon world (a farm terrain, GPS-referenced) and the +# Summit XL from easynav_playground_summit_worlds, not namespaced, plus the +# Zenoh router. Run it before robot_1_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + # Gazebo finds the farm models here (the playground adds its own). + - set_env: + name: "GZ_SIM_RESOURCE_PATH" + value: "$(find-pkg-share granja_leon)/models:$(env GZ_SIM_RESOURCE_PATH '')" + - include: + file: "$(find-pkg-share easynav_playground_summit_worlds)/launch/gazebo_sim.launch.yaml" + arg: + - {name: "world", value: "$(find-pkg-share granja_leon)/worlds/granja_leon.world"} + - {name: "gui", value: "$(var gui)"} diff --git a/easyfleet_easynav_farm_deployment/package.xml b/easyfleet_easynav_farm_deployment/package.xml new file mode 100644 index 0000000..86d1c0d --- /dev/null +++ b/easyfleet_easynav_farm_deployment/package.xml @@ -0,0 +1,48 @@ + + + + easyfleet_easynav_farm_deployment + 0.1.0 + + Single-robot deployment scenario: a Summit XL on a farm (the granja_leon + world), whose EasyFleet/EasyNav nodes run under "robot_1", with a real + EasyNav navigation stack (easynav_system system_main + + easyfleet_easynav_deployment's navigation capability, reused as-is) + localized via GPS (easynav_fusion_localizer/FusionLocalizer, fusing GPS + + wheel odometry + IMU -- no distance/range sensor) against a NavMap + (easynav_navmap_maps_manager/NavMapMapsManager, granja_leon.navmap), with + a second maps manager (easynav_navmap_routes_maps_manager) overlaying + preferred routes on top of it. launch/simulation_launch.yaml starts the + Summit from easynav_playground_summit_worlds in that world. Uses + rmw_zenoh_cpp by default. Pure launch/config package -- no C++ of its own. + + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + launch + launch_ros + launch_yaml + easynav_system + easyfleet_easynav_deployment + easyfleet_easynav_navigation + easynav_fusion_localizer + easynav_navmap_maps_manager + easynav_navmap_routes_maps_manager + easynav_navmap_planner + rviz2 + navmap_rviz_plugin + granja_leon + + easynav_regulated_pp_controller + easynav_playground_summit_worlds + rmw_zenoh_cpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/easyfleet_easynav_farm_deployment/rviz/robot_1.rviz b/easyfleet_easynav_farm_deployment/rviz/robot_1.rviz new file mode 100644 index 0000000..bdc5483 --- /dev/null +++ b/easyfleet_easynav_farm_deployment/rviz/robot_1.rviz @@ -0,0 +1,323 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /TF1 + - /Path1 + - /NavMapDisplay1 + Splitter Ratio: 0.5 + Tree Height: 617 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + - /NavMap Goal Pose1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 400 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + robot_1/base_docking_contact_link: + Value: true + robot_1/base_footprint: + Value: true + robot_1/base_link: + Value: true + robot_1/front_camera_base_link: + Value: true + robot_1/front_camera_depth_frame: + Value: true + robot_1/front_camera_depth_optical_frame: + Value: true + robot_1/front_camera_left_camera_frame: + Value: true + robot_1/front_camera_left_camera_optical_frame: + Value: true + robot_1/front_camera_right_camera_frame: + Value: true + robot_1/front_camera_right_camera_optical_frame: + Value: true + robot_1/front_laser_base_link: + Value: true + robot_1/front_laser_link: + Value: true + robot_1/front_left_wheel_link: + Value: true + robot_1/front_right_wheel_link: + Value: true + robot_1/gps__link: + Value: true + robot_1/gps_base_link: + Value: true + robot_1/imu_link: + Value: true + robot_1/map: + Value: true + robot_1/odom: + Value: true + robot_1/rear_left_wheel_link: + Value: true + robot_1/rear_right_wheel_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + robot_1/map: + robot_1/odom: + robot_1/base_footprint: + robot_1/base_link: + robot_1/base_docking_contact_link: + {} + robot_1/front_camera_base_link: + robot_1/front_camera_depth_frame: + robot_1/front_camera_depth_optical_frame: + {} + robot_1/front_camera_left_camera_frame: + robot_1/front_camera_left_camera_optical_frame: + {} + robot_1/front_camera_right_camera_frame: + robot_1/front_camera_right_camera_optical_frame: + {} + robot_1/front_laser_base_link: + robot_1/front_laser_link: + {} + robot_1/front_left_wheel_link: + {} + robot_1/front_right_wheel_link: + {} + robot_1/gps__link: + {} + robot_1/gps_base_link: + {} + robot_1/imu_link: + {} + robot_1/rear_left_wheel_link: + {} + robot_1/rear_right_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 97; 53; 131 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/planner_node/simple/path + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry (filtered) + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/localizer_node/odometry/filtered + Value: true + - Alpha: 1 + Class: navmap_rviz_plugin/NavMapDisplay + Color Scheme: Heat + Draw Normals: false + Enabled: true + Layer: Color (vertex RGBA) + Name: NavMapDisplay + Normal Scale: 0.15000000596046448 + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/maps_manager_node/navmap/map + Update Layer Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/maps_manager_node/navmap/map_updates + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/initialpose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/clicked_point + - Class: navmap_rviz_plugin/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/goal_pose + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 34.190940856933594 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 21.665428161621094 + Y: 1.7618165016174316 + Z: 1.3571733236312866 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9649996757507324 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.5399839878082275 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000100000000000001b0000002f4fc0200000005fb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000001200530065006c0065006300740069006f006e000000018d0000009d0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001ea000000cb0000005c00fffffffb0000000a005600690065007700730000000232000000b2000000a400fffffffb0000000800540069006d0065000000028f0000009d0000003900ffffff000002fa000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 1068 + Y: 157 diff --git a/easyfleet_example_deployments.repos b/easyfleet_example_deployments.repos index 74c6609..c20cc35 100644 --- a/easyfleet_example_deployments.repos +++ b/easyfleet_example_deployments.repos @@ -19,14 +19,33 @@ repositories: type: git url: https://github.com/fmrico/yaets.git version: rolling - easynav_playground_kobuki: + easynav_playgrounds: type: git - url: https://github.com/EasyNavigation/easynav_playground_kobuki.git + url: https://github.com/EasyNavigation/easynav_playgrounds.git version: rolling - easynav_playground_summit: + # fmrico's branch until juanscelyg/large_warehouse_world#1 is merged: packages under + # the Warehouse A racks, ODE collisions and large_warehouse_world_a.sdf. + large_warehouse_world: type: git - url: https://github.com/EasyNavigation/easynav_playground_summit.git + url: https://github.com/fmrico/large_warehouse_world.git + version: fill_racks + # PlanSys2 with Kairos, for easyfleet_plansys2_warehouse_deployment. + ros2_planning_system: + type: git + url: https://github.com/PlanSys2/ros2_planning_system.git version: rolling + kairos: + type: git + url: https://github.com/PlanSys2/kairos.git + version: main + popf: + type: git + url: https://github.com/fmrico/popf.git + version: rolling-devel + cascade_lifecycle: + type: git + url: https://github.com/fmrico/cascade_lifecycle.git + version: rolling-devel # No released .deb for rolling (needed by easynav_fusion_localizer). robot_localization: type: git diff --git a/easyfleet_fake_alone_deployment/README.md b/easyfleet_fake_alone_deployment/README.md new file mode 100644 index 0000000..14d2971 --- /dev/null +++ b/easyfleet_fake_alone_deployment/README.md @@ -0,0 +1,68 @@ +# easyfleet_fake_alone_deployment + +The smallest EasyFleet example: a single robot, `robot_1`, with fake (mock) navigation, perception and manipulation capabilities, plus a mission node that discovers them and runs them. Nothing moves and there is no simulator. Progress is simulated on timers, so you can see the whole capability lifecycle in seconds: announcement on `/capabilities`, discovery, goals, feedback, results, preemption and cancel. Start here. [easyfleet_fake_collaboration_deployment](../easyfleet_fake_collaboration_deployment) adds more robots, and the `easyfleet_easynav_*` examples replace the fake navigation with real EasyNav in Gazebo. + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Process | Namespace | What it is | +|---|---|---| +| `robot_node` | `/robot_1` | One process that hosts the three capabilities as lifecycle nodes. It configures and activates them, and each one announces itself on `/capabilities`. | +| `alone_mission_node` | (none) | Mission control. Starts 3 s after the robot. | +| `rmw_zenohd` | (none) | Zenoh router, started by `alone_launch.yaml`. | + +Capabilities of `robot_1`: + +- `navigation` (`/robot_1/navigation`): simulates 5 s of travel, with `distance_remaining` going down from 5 m. +- `manipulation` (`/robot_1/manipulation`): simulates a 3 s trajectory. +- `perception` (`/robot_1/perception`): streams random detections of the class `gato` at 20 Hz until the goal is canceled, and rejects any other class. + +## How to run + +Build the workspace as described in the [top-level README](../README.md), then run: + +```bash +source install/setup.bash +ros2 launch easyfleet_fake_alone_deployment alone_launch.yaml +``` + +This starts the Zenoh router, the robot and, after 3 s, the mission. `RMW_IMPLEMENTATION=rmw_zenoh_cpp` is set by the launch files. + +To run the robot and the mission separately, start a router first: + +```bash +# Terminal 1 +ros2 run rmw_zenoh_cpp rmw_zenohd +# Terminal 2 +ros2 launch easyfleet_fake_alone_deployment robot_1_launch.yaml +# Terminal 3 +RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 run easyfleet_fake_alone_deployment alone_mission_node +``` + +## What you should see + +Only terminal output. The mission prints: + +1. **Phase 1, discovery:** a summary line and the full JSON description of each of `robot_1`'s three capabilities. +2. **Phase 2, sequential execution:** navigation, then manipulation, then perception. Each one runs until it finishes or for at most 10 s, and prints its feedback. Perception never finishes on its own, so it is canceled at 10 s. +3. **Phase 3, parallel execution:** the three capabilities run at the same time, with the same 10 s cap. +4. `Mission complete`. + +## Key files + +| File | Purpose | +|---|---| +| `launch/alone_launch.yaml` | Starts the Zenoh router, the robot and the delayed mission. | +| `launch/robot_1_launch.yaml` | Starts `robot_node` under `/robot_1`. Can be run on its own if a router is already running. | +| `config/robot_1/robot_params.yaml` | Which capabilities the robot hosts (`capabilities`) and the folder their JSON files are in (`config_subdir`). | +| `config/robot_1/*_params.yaml` | Mock parameters: preemption, durations, feedback periods, target class. | +| `config/robot_1/*.json` | Capability descriptions, published as they are on `/capabilities`. | +| `src/robot_node.cpp` | Creates, configures and activates the capabilities, then spins them. | +| `src//*_fake_capability.cpp` | Mock implementations of the `easyfleet_core` action-server bases. | +| `src/main_alone.cpp` | The mission (`alone_mission_node`), built on `easyfleet_mission_manager`. | + +## Notes + +- The mission only talks to the capabilities through their ROS actions. It does not link against their implementation. +- Mission control waits 3 s before discovery so the robot has time to announce itself. If you start the mission by hand before the robot is active, it finds nothing. diff --git a/easyfleet_fake_alone_deployment/launch/alone_launch.yaml b/easyfleet_fake_alone_deployment/launch/alone_launch.yaml index 601a5a1..23c0c12 100644 --- a/easyfleet_fake_alone_deployment/launch/alone_launch.yaml +++ b/easyfleet_fake_alone_deployment/launch/alone_launch.yaml @@ -23,6 +23,12 @@ # easyfleet_mission_manager::discover_capabilities()) opens -- launching # both at literally the same instant risks discovering nothing. launch: + # Zenoh by default. The router is started here; when a robot_N_launch.yaml + # is run on its own instead, start one first (ros2 run rmw_zenoh_cpp rmw_zenohd). + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" - include: file: "$(find-pkg-share easyfleet_fake_alone_deployment)/launch/robot_1_launch.yaml" diff --git a/easyfleet_fake_alone_deployment/launch/robot_1_launch.yaml b/easyfleet_fake_alone_deployment/launch/robot_1_launch.yaml index 2c2f1e8..777d366 100644 --- a/easyfleet_fake_alone_deployment/launch/robot_1_launch.yaml +++ b/easyfleet_fake_alone_deployment/launch/robot_1_launch.yaml @@ -24,6 +24,7 @@ # remap to -- each capability already carries its own explicit name (set # by its own constructor). launch: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_1" diff --git a/easyfleet_fake_alone_deployment/package.xml b/easyfleet_fake_alone_deployment/package.xml index 2601134..7889be9 100644 --- a/easyfleet_fake_alone_deployment/package.xml +++ b/easyfleet_fake_alone_deployment/package.xml @@ -33,6 +33,8 @@ launch_ros launch_yaml + rmw_zenoh_cpp + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/easyfleet_fake_collaboration_deployment/README.md b/easyfleet_fake_collaboration_deployment/README.md new file mode 100644 index 0000000..d2178d0 --- /dev/null +++ b/easyfleet_fake_collaboration_deployment/README.md @@ -0,0 +1,70 @@ +# easyfleet_fake_collaboration_deployment + +Three robots with fake capabilities and one mission node that coordinates them. This is [easyfleet_fake_alone_deployment](../easyfleet_fake_alone_deployment) with more than one robot. Each robot carries a different set of capabilities under its own namespace, and mission control finds each capability by robot and by type, then runs some of them in parallel across robots and some in sequence on one robot. There is no simulator and nothing moves, so it starts in seconds. For the same kind of coordination with real navigation in Gazebo, see [easyfleet_easynav_collaboration_deployment](../easyfleet_easynav_collaboration_deployment). + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Robot | Namespace | Capabilities | +|---|---|---| +| `robot_1` | `/robot_1` | `navigation`, `perception` | +| `robot_2` | `/robot_2` | `navigation`, `perception` | +| `robot_3` | `/robot_3` | `navigation`, `manipulation` | + +Each robot runs as one `robot_node` process that hosts its capabilities. The capabilities to host come from `config/robot_N/robot_params.yaml`. Central processes: + +- `collaboration_mission_node`: mission control, started 3 s after the robots. +- `rmw_zenohd`: the Zenoh router, started by `collaboration_launch.yaml`. + +The capabilities are the same mocks as in the alone example: + +- `navigation`: simulates 5 s of travel. +- `perception`: streams `gato` detections at 20 Hz until it is canceled. +- `manipulation`: simulates a 3 s trajectory. + +## How to run + +Build the workspace as described in the [top-level README](../README.md), then run: + +```bash +source install/setup.bash +ros2 launch easyfleet_fake_collaboration_deployment collaboration_launch.yaml +``` + +To bring the robots up one by one, for example in separate terminals or on separate machines that share one router, start the router first: + +```bash +ros2 run rmw_zenoh_cpp rmw_zenohd # Terminal 1 +ros2 launch easyfleet_fake_collaboration_deployment robot_1_launch.yaml # Terminal 2 +ros2 launch easyfleet_fake_collaboration_deployment robot_2_launch.yaml # Terminal 3 +ros2 launch easyfleet_fake_collaboration_deployment robot_3_launch.yaml # Terminal 4 +RMW_IMPLEMENTATION=rmw_zenoh_cpp ros2 run easyfleet_fake_collaboration_deployment collaboration_mission_node +``` + +## What you should see + +Only terminal output. The mission prints: + +1. **Phase 0, discovery:** the capabilities of all three robots, with their descriptions. +2. **Phase 1:** `robot_1` and `robot_2` run navigation and perception, all four at once, for up to 10 s. Navigation finishes on its own after about 5 s, and perception is canceled at the cap. +3. **Phase 2:** `robot_3` runs navigation until it finishes, then manipulation. +4. `Mission complete`. + +If a capability is missing, for example because you did not start `robot_3`, its step is reported as `not active, skipping` and the mission goes on. + +## Key files + +| File | Purpose | +|---|---| +| `launch/collaboration_launch.yaml` | Starts the Zenoh router, the three robots and the delayed mission. | +| `launch/robot_N_launch.yaml` | Starts one robot under `/robot_N`. Needs a router that is already running. | +| `config/robot_N/robot_params.yaml` | The capabilities this robot hosts. | +| `config/robot_N/*_params.yaml`, `*.json` | Mock parameters and the capability descriptions published on `/capabilities`. | +| `src/robot_node.cpp` | Process that hosts the capabilities. | +| `src//*_fake_capability.cpp` | Mock capability implementations. | +| `src/main_collaboration.cpp` | The two-phase mission (`collaboration_mission_node`). | + +## Notes + +- Each robot is a set of namespaced actions. Mission control looks robots up by the namespace that each capability announces, so adding a robot means adding a `config/robot_N/` folder and a `robot_N_launch.yaml`. diff --git a/easyfleet_fake_collaboration_deployment/launch/collaboration_launch.yaml b/easyfleet_fake_collaboration_deployment/launch/collaboration_launch.yaml index 701c9fd..b92f8ff 100644 --- a/easyfleet_fake_collaboration_deployment/launch/collaboration_launch.yaml +++ b/easyfleet_fake_collaboration_deployment/launch/collaboration_launch.yaml @@ -28,6 +28,12 @@ # easyfleet_mission_manager::discover_capabilities()) opens -- launching # everything at literally the same instant risks discovering nothing. launch: + # Zenoh by default. The router is started here; when a robot_N_launch.yaml + # is run on its own instead, start one first (ros2 run rmw_zenoh_cpp rmw_zenohd). + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" - include: file: "$(find-pkg-share easyfleet_fake_collaboration_deployment)/launch/robot_1_launch.yaml" - include: diff --git a/easyfleet_fake_collaboration_deployment/launch/robot_1_launch.yaml b/easyfleet_fake_collaboration_deployment/launch/robot_1_launch.yaml index bda0226..9b3fa5b 100644 --- a/easyfleet_fake_collaboration_deployment/launch/robot_1_launch.yaml +++ b/easyfleet_fake_collaboration_deployment/launch/robot_1_launch.yaml @@ -23,6 +23,7 @@ # remap to -- each capability already carries its own explicit name (set # by its own constructor). launch: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_1" diff --git a/easyfleet_fake_collaboration_deployment/launch/robot_2_launch.yaml b/easyfleet_fake_collaboration_deployment/launch/robot_2_launch.yaml index 52a3e5d..9bc407c 100644 --- a/easyfleet_fake_collaboration_deployment/launch/robot_2_launch.yaml +++ b/easyfleet_fake_collaboration_deployment/launch/robot_2_launch.yaml @@ -23,6 +23,7 @@ # remap to -- each capability already carries its own explicit name (set # by its own constructor). launch: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_2" diff --git a/easyfleet_fake_collaboration_deployment/launch/robot_3_launch.yaml b/easyfleet_fake_collaboration_deployment/launch/robot_3_launch.yaml index d2ba34e..0d284b6 100644 --- a/easyfleet_fake_collaboration_deployment/launch/robot_3_launch.yaml +++ b/easyfleet_fake_collaboration_deployment/launch/robot_3_launch.yaml @@ -23,6 +23,7 @@ # remap to -- each capability already carries its own explicit name (set # by its own constructor). launch: + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} - group: - push_ros_namespace: namespace: "robot_3" diff --git a/easyfleet_fake_collaboration_deployment/package.xml b/easyfleet_fake_collaboration_deployment/package.xml index 968cf92..e210091 100644 --- a/easyfleet_fake_collaboration_deployment/package.xml +++ b/easyfleet_fake_collaboration_deployment/package.xml @@ -36,6 +36,8 @@ launch_ros launch_yaml + rmw_zenoh_cpp + ament_lint_auto ament_lint_common ament_cmake_gtest diff --git a/easyfleet_fake_collaboration_deployment/src/main_collaboration.cpp b/easyfleet_fake_collaboration_deployment/src/main_collaboration.cpp index 81b96f2..0337123 100644 --- a/easyfleet_fake_collaboration_deployment/src/main_collaboration.cpp +++ b/easyfleet_fake_collaboration_deployment/src/main_collaboration.cpp @@ -62,7 +62,7 @@ std::string run_and_describe( std::function on_feedback) { auto response = run_capability(client, label, goal, kRunTimeout, on_feedback); - return outcome_to_string(static_cast(response.outcome)); + return outcome_to_string(response.outcome); } } // namespace From 816d139be67cb63a65363c22c49a689b829f4da1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 25 Oct 2026 20:49:16 +0100 Subject: [PATCH 2/3] Warehouse A deployments and a PlanSys2 fleet MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - easyfleet_easynav_deployment and the collaboration deployment in Warehouse A of large_warehouse_world: a 0.2 m costmap of what the lidar sees at its height, routes along every rack aisle, the corridors and to the docks, and the scripts that generate them from the world (sdf_to_costmap.py, aisle_routes.py). - AMCL with reseeding, a 30 m lidar (at 12 m it sees almost nothing in the open areas) and a cylinder at lidar height on each Kobuki so the others see it. - easyfleet_plansys2_warehouse_deployment: four Kobukis, two scouts and two carriers, driven by PlanSys2 (Kairos). The controller builds the problem from the capabilities the robots announce; each action's behavior tree calls them with easyfleet_bt_nodes. The robots share each other as obstacles. Signed-off-by: Francisco Martín Rico --- .../warehouse/navigation_manager.params.yaml | 41 ++ .../config/warehouse/navigation_params.yaml | 55 ++ .../easynav_system.costmap_rpp.params.yaml | 153 ++++++ .../easynav_system.costmap_rpp.params.yaml | 153 ++++++ .../launch/warehouse_launch.yaml | 34 ++ .../launch/warehouse_simulation_launch.yaml | 67 +++ .../easynav_system.costmap_rpp.params.yaml | 148 +++++ .../warehouse/navigation_manager.params.yaml | 33 ++ .../config/warehouse/navigation_params.yaml | 45 ++ .../launch/warehouse_launch.yaml | 30 ++ .../launch/warehouse_simulation_launch.yaml | 52 ++ .../maps/warehouse_a.pgm | Bin 0 -> 1020117 bytes .../maps/warehouse_a.yaml | 10 + .../maps/warehouse_a_collision.pgm | Bin 0 -> 1020117 bytes .../maps/warehouse_a_collision.yaml | 10 + .../maps/warehouse_a_routes.yaml | 404 ++++++++++++++ .../scripts/aisle_routes.py | 238 ++++++++ .../scripts/sdf_to_costmap.py | 508 ++++++++++++++++++ .../CMakeLists.txt | 46 ++ .../README.md | 77 +++ .../behavior_trees/drop.xml | 6 + .../behavior_trees/inspect.xml | 6 + .../behavior_trees/move.xml | 6 + .../behavior_trees/pick.xml | 6 + .../config/capabilities/manipulation.json | 69 +++ .../config/capabilities/navigation.json | 90 ++++ .../config/capabilities/perception.json | 70 +++ .../config/carrier.yaml | 25 + .../config/controller.yaml | 29 + .../config/easynav_system.params.yaml | 151 ++++++ .../config/initial_poses.yaml | 45 ++ .../config/navigation_manager.params.yaml | 33 ++ .../config/navigation_params.yaml | 87 +++ .../config/plansys2_params.yaml | 27 + .../config/scout.yaml | 26 + .../launch/robot_launch.yaml | 99 ++++ .../launch/simulation_launch.yaml | 85 +++ .../launch/warehouse_launch.yaml | 176 ++++++ .../package.xml | 49 ++ .../pddl/warehouse.pddl | 122 +++++ .../rviz/warehouse.rviz | 332 ++++++++++++ .../src/warehouse_controller.cpp | 362 +++++++++++++ 42 files changed, 4005 insertions(+) create mode 100644 easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_manager.params.yaml create mode 100644 easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_params.yaml create mode 100644 easyfleet_easynav_collaboration_deployment/config/warehouse/robot_1/easynav_system.costmap_rpp.params.yaml create mode 100644 easyfleet_easynav_collaboration_deployment/config/warehouse/robot_2/easynav_system.costmap_rpp.params.yaml create mode 100644 easyfleet_easynav_collaboration_deployment/launch/warehouse_launch.yaml create mode 100644 easyfleet_easynav_collaboration_deployment/launch/warehouse_simulation_launch.yaml create mode 100644 easyfleet_easynav_deployment/config/warehouse/easynav_system.costmap_rpp.params.yaml create mode 100644 easyfleet_easynav_deployment/config/warehouse/navigation_manager.params.yaml create mode 100644 easyfleet_easynav_deployment/config/warehouse/navigation_params.yaml create mode 100644 easyfleet_easynav_deployment/launch/warehouse_launch.yaml create mode 100644 easyfleet_easynav_deployment/launch/warehouse_simulation_launch.yaml create mode 100644 easyfleet_easynav_deployment/maps/warehouse_a.pgm create mode 100644 easyfleet_easynav_deployment/maps/warehouse_a.yaml create mode 100644 easyfleet_easynav_deployment/maps/warehouse_a_collision.pgm create mode 100644 easyfleet_easynav_deployment/maps/warehouse_a_collision.yaml create mode 100644 easyfleet_easynav_deployment/maps/warehouse_a_routes.yaml create mode 100755 easyfleet_easynav_deployment/scripts/aisle_routes.py create mode 100755 easyfleet_easynav_deployment/scripts/sdf_to_costmap.py create mode 100644 easyfleet_plansys2_warehouse_deployment/CMakeLists.txt create mode 100644 easyfleet_plansys2_warehouse_deployment/README.md create mode 100644 easyfleet_plansys2_warehouse_deployment/behavior_trees/drop.xml create mode 100644 easyfleet_plansys2_warehouse_deployment/behavior_trees/inspect.xml create mode 100644 easyfleet_plansys2_warehouse_deployment/behavior_trees/move.xml create mode 100644 easyfleet_plansys2_warehouse_deployment/behavior_trees/pick.xml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/capabilities/manipulation.json create mode 100644 easyfleet_plansys2_warehouse_deployment/config/capabilities/navigation.json create mode 100644 easyfleet_plansys2_warehouse_deployment/config/capabilities/perception.json create mode 100644 easyfleet_plansys2_warehouse_deployment/config/carrier.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/controller.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/easynav_system.params.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/initial_poses.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/navigation_manager.params.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/navigation_params.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/plansys2_params.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/config/scout.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/launch/robot_launch.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/launch/simulation_launch.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/launch/warehouse_launch.yaml create mode 100644 easyfleet_plansys2_warehouse_deployment/package.xml create mode 100644 easyfleet_plansys2_warehouse_deployment/pddl/warehouse.pddl create mode 100644 easyfleet_plansys2_warehouse_deployment/rviz/warehouse.rviz create mode 100644 easyfleet_plansys2_warehouse_deployment/src/warehouse_controller.cpp diff --git a/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_manager.params.yaml b/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_manager.params.yaml new file mode 100644 index 0000000..245eea8 --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_manager.params.yaml @@ -0,0 +1,41 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for easyfleet_navigation_manager's navigation_manager_node, +# launched once for this whole scenario, fleet-wide, not per robot (see +# launch/easynav_collaboration_launch.yaml) -- publishes the same +# map/routes both robots' own config/robot_1|robot_2/easynav_system.costmap_rpp.params.yaml +# point their maps_manager_node at locally (maps/home2.yaml, +# maps/routes_1.yaml), centrally on /global_map and /global_routes. +# Reuses easyfleet_easynav_deployment's own copies of those files (same +# package this scenario already depends on for the navigation capability +# itself) rather than duplicating them again here. +# +# Uses the "/**" wildcard, matching this package's other params files +# (see navigation_params.yaml's own comment for why): navigation_manager_node +# itself is never namespaced today, but this keeps the file correct if +# that ever changes. +/**: + ros__parameters: + map_type: "costmap" + map.package: "easyfleet_easynav_deployment" + map.map_path_file: "maps/warehouse_a.yaml" + routes.package: "easyfleet_easynav_deployment" + routes.map_path_file: "maps/warehouse_a_routes.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 diff --git a/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_params.yaml b/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_params.yaml new file mode 100644 index 0000000..8ab3ab7 --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/config/warehouse/navigation_params.yaml @@ -0,0 +1,55 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Waypoints in Warehouse A (map frame, origin at the warehouse center), with +# the names easynav_collaboration_mission_node uses: the robots' homes in the +# central plaza, the rest on the corridor between the two rack blocks. +/**: + ros__parameters: + navigation: + allow_preemption: true + tick_rate_hz: 10.0 + waypoint_ids: ["dock", "kitchen", "kitchen_standby", "charging_station", "robot_1_home", "robot_2_home"] + waypoints: + dock: + frame_id: "map" + x: 0.0 + y: 13.9 + yaw: 1.57 + kitchen: + frame_id: "map" + x: -6.0 + y: 13.9 + yaw: 3.14 + kitchen_standby: + frame_id: "map" + x: -4.0 + y: 13.9 + yaw: 3.14 + charging_station: + frame_id: "map" + x: 5.0 + y: -6.0 + yaw: 0.0 + robot_1_home: + frame_id: "map" + x: 0.0 + y: 0.0 + yaw: 0.0 + robot_2_home: + frame_id: "map" + x: 2.0 + y: 1.0 + yaw: 0.0 diff --git a/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_1/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_1/easynav_system.costmap_rpp.params.yaml new file mode 100644 index 0000000..a35dc3b --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_1/easynav_system.costmap_rpp.params.yaml @@ -0,0 +1,153 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_1 in large_warehouse_world's Warehouse A (launch/warehouse_launch.yaml): +# same stack as config/robot_1, on easyfleet_easynav_deployment's maps/warehouse_a.yaml +# (what the lidar sees at its height, origin at the warehouse center, world (100, 300)) +# and maps/warehouse_a_routes.yaml; initial pose = spawn pose in launch/warehouse_simulation_launch.yaml. + +# Uses "/**/" so every sub-node's parameters load once +# launch/robot_1_launch.yaml pushes system_main under the "robot_1" +# namespace (a bare node name would silently drop everything here once +# namespaced -- see easyfleet_easynav_deployment's navigation_params.yaml +# for how this exact class of bug was first found). +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + std_dev_xy: 0.02 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a_routes.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_2/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_2/easynav_system.costmap_rpp.params.yaml new file mode 100644 index 0000000..fdef223 --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/config/warehouse/robot_2/easynav_system.costmap_rpp.params.yaml @@ -0,0 +1,153 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# robot_2 in large_warehouse_world's Warehouse A (launch/warehouse_launch.yaml): +# same stack as config/robot_2, on easyfleet_easynav_deployment's maps/warehouse_a.yaml +# (what the lidar sees at its height, origin at the warehouse center, world (100, 300)) +# and maps/warehouse_a_routes.yaml; initial pose = spawn pose in launch/warehouse_simulation_launch.yaml. + +# Uses "/**/" so every sub-node's parameters load once +# launch/robot_2_launch.yaml pushes system_main under the "robot_2" +# namespace (a bare node name would silently drop everything here once +# namespaced -- see easyfleet_easynav_deployment's navigation_params.yaml +# for how this exact class of bug was first found). +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 2.0 + y: 1.0 + yaw: 0.0 + std_dev_xy: 0.02 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a_routes.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_easynav_collaboration_deployment/launch/warehouse_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/warehouse_launch.yaml new file mode 100644 index 0000000..3937443 --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/launch/warehouse_launch.yaml @@ -0,0 +1,34 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# The collaboration example in large_warehouse_world's Warehouse A: robot_1 and +# robot_2 with their EasyNav stacks on the warehouse map and aisle routes +# (easyfleet_easynav_deployment's maps/warehouse_a*.yaml), the navigation +# manager, the central rviz2 and, unless mission:=false, the example mission +# on waypoints placed in the warehouse (config/warehouse/). Run +# warehouse_simulation_launch.yaml first. + +launch: + - arg: {name: "robot_rviz", default: "false", description: "Also open each robot's own rviz2"} + - arg: {name: "mission", default: "true", description: "Run the example mission"} + - include: + file: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/launch/easynav_collaboration_launch.yaml" + arg: + - {name: "robot_rviz", value: "$(var robot_rviz)"} + - {name: "mission", value: "$(var mission)"} + - {name: "robot_1_easynav_params", value: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/warehouse/robot_1/easynav_system.costmap_rpp.params.yaml"} + - {name: "robot_2_easynav_params", value: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/warehouse/robot_2/easynav_system.costmap_rpp.params.yaml"} + - {name: "navigation_params", value: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/warehouse/navigation_params.yaml"} + - {name: "navigation_manager_params", value: "$(find-pkg-share easyfleet_easynav_collaboration_deployment)/config/warehouse/navigation_manager.params.yaml"} diff --git a/easyfleet_easynav_collaboration_deployment/launch/warehouse_simulation_launch.yaml b/easyfleet_easynav_collaboration_deployment/launch/warehouse_simulation_launch.yaml new file mode 100644 index 0000000..c90dc56 --- /dev/null +++ b/easyfleet_easynav_collaboration_deployment/launch/warehouse_simulation_launch.yaml @@ -0,0 +1,67 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with large_warehouse_world (github.com/juanscelyg/large_warehouse_world) +# and two Kobukis in its Warehouse A central plaza: robot_1 at world (100, 300) +# and robot_2 at world (102, 301), i.e. (0, 0) and (2, 1) in the warehouse map +# (easyfleet_easynav_deployment's maps/warehouse_a.yaml, origin at the center of +# Warehouse A). Each robot's topics and TF tree are under its namespace. Also +# starts the Zenoh router. Run it before warehouse_launch.yaml. The world is +# large (about 700 x 450 m): only Warehouse A is mapped and navigated. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # At 12 m the lidar sees almost nothing in the open areas of the warehouse; at 30 m, 200+ of 360 rays hit. + - arg: {name: "lidar_range", default: "30.0", description: "Maximum lidar range (m)"} + # Each Kobuki carries a cylinder at lidar height (lidar_target) so the others' lidars see it. + # Warehouse A furniture only: with the whole world's, two lidars halve the real time factor. + - arg: + name: "world" + default: "$(find-pkg-share large_warehouse_world)/worlds/large_warehouse_world_a.sdf" + description: "World file (large_warehouse_world.sdf for the whole furnished world)" + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + # Gazebo finds the warehouse models here (the playground adds its own). + - set_env: + name: "GZ_SIM_RESOURCE_PATH" + value: "$(find-pkg-share large_warehouse_world)/models:$(env GZ_SIM_RESOURCE_PATH '')" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "world", value: "$(var world)"} + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - {name: "x", value: "100.0"} + - {name: "y", value: "300.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "102.0"} + - {name: "y", value: "301.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} diff --git a/easyfleet_easynav_deployment/config/warehouse/easynav_system.costmap_rpp.params.yaml b/easyfleet_easynav_deployment/config/warehouse/easynav_system.costmap_rpp.params.yaml new file mode 100644 index 0000000..89f8106 --- /dev/null +++ b/easyfleet_easynav_deployment/config/warehouse/easynav_system.costmap_rpp.params.yaml @@ -0,0 +1,148 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# One Kobuki in large_warehouse_world's Warehouse A (launch/warehouse_launch.yaml): +# same stack as config/easynav_robot on maps/warehouse_a.yaml (origin at the +# warehouse center, world (100, 300)) and maps/warehouse_a_routes.yaml; initial +# pose = spawn pose in launch/warehouse_simulation_launch.yaml. + +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + std_dev_xy: 0.02 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a_routes.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_easynav_deployment/config/warehouse/navigation_manager.params.yaml b/easyfleet_easynav_deployment/config/warehouse/navigation_manager.params.yaml new file mode 100644 index 0000000..6c51251 --- /dev/null +++ b/easyfleet_easynav_deployment/config/warehouse/navigation_manager.params.yaml @@ -0,0 +1,33 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Parameters for easyfleet_navigation_manager's navigation_manager_node, +# launched once for this scenario (see launch/warehouse_launch.yaml) +# -- publishes the same map/routes the robot's own +# easynav_system.costmap_rpp.params.yaml points its maps_manager_node at +# locally (maps/warehouse_a.yaml, maps/warehouse_a_routes.yaml, this package's own local +# copies), centrally on /global_map and /global_routes. +# +# Uses the "/**" wildcard, matching this package's other params files +# (see navigation_params.yaml's own comment for why): navigation_manager_node +# itself is never namespaced today, but this keeps the file correct if +# that ever changes. +/**: + ros__parameters: + map_type: "costmap" + map.package: "easyfleet_easynav_deployment" + map.map_path_file: "maps/warehouse_a.yaml" + routes.package: "easyfleet_easynav_deployment" + routes.map_path_file: "maps/warehouse_a_routes.yaml" diff --git a/easyfleet_easynav_deployment/config/warehouse/navigation_params.yaml b/easyfleet_easynav_deployment/config/warehouse/navigation_params.yaml new file mode 100644 index 0000000..6b1b1e8 --- /dev/null +++ b/easyfleet_easynav_deployment/config/warehouse/navigation_params.yaml @@ -0,0 +1,45 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Waypoints in Warehouse A (map frame, origin at the warehouse center), with +# the names easynav_mission_node uses: home in the central plaza, the rest +# on the corridor between the two rack blocks. +/**: + ros__parameters: + navigation: + allow_preemption: true + tick_rate_hz: 10.0 + waypoint_ids: ["dock", "kitchen", "charging_station", "home"] + waypoints: + dock: + frame_id: "map" + x: 0.0 + y: 13.9 + yaw: 1.57 + kitchen: + frame_id: "map" + x: -6.0 + y: 13.9 + yaw: 3.14 + charging_station: + frame_id: "map" + x: 5.0 + y: -6.0 + yaw: 0.0 + home: + frame_id: "map" + x: 0.0 + y: 0.0 + yaw: 0.0 diff --git a/easyfleet_easynav_deployment/launch/warehouse_launch.yaml b/easyfleet_easynav_deployment/launch/warehouse_launch.yaml new file mode 100644 index 0000000..94c57d2 --- /dev/null +++ b/easyfleet_easynav_deployment/launch/warehouse_launch.yaml @@ -0,0 +1,30 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# easynav_gazebo_launch.yaml in Warehouse A: EasyNav + its RViz, the navigation +# capability and navigation_manager, with +# config/warehouse (maps/warehouse_a.yaml and maps/warehouse_a_routes.yaml). +# Send goals with RViz's 2D Goal Pose. Run warehouse_simulation_launch.yaml first. + +launch: + # The mission gives each goal 10 s (sized for the small house): off by default here. + - arg: {name: "mission", default: "false", description: "Run the mission (dock, then kitchen)"} + - include: + file: "$(find-pkg-share easyfleet_easynav_deployment)/launch/easynav_gazebo_launch.yaml" + arg: + - {name: "easynav_params", value: "$(find-pkg-share easyfleet_easynav_deployment)/config/warehouse/easynav_system.costmap_rpp.params.yaml"} + - {name: "navigation_params", value: "$(find-pkg-share easyfleet_easynav_deployment)/config/warehouse/navigation_params.yaml"} + - {name: "navigation_manager_params", value: "$(find-pkg-share easyfleet_easynav_deployment)/config/warehouse/navigation_manager.params.yaml"} + - {name: "mission", value: "$(var mission)"} diff --git a/easyfleet_easynav_deployment/launch/warehouse_simulation_launch.yaml b/easyfleet_easynav_deployment/launch/warehouse_simulation_launch.yaml new file mode 100644 index 0000000..9ac36ea --- /dev/null +++ b/easyfleet_easynav_deployment/launch/warehouse_simulation_launch.yaml @@ -0,0 +1,52 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with large_warehouse_world (github.com/juanscelyg/large_warehouse_world) +# and one unnamespaced Kobuki in its Warehouse A central plaza, at world +# (100, 300), i.e. (0, 0) in maps/warehouse_a.yaml (origin at the center of +# Warehouse A). Also starts the Zenoh router. Run it before warehouse_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # At 12 m the lidar sees almost nothing in the open areas of the warehouse; at 30 m, 200+ of 360 rays hit. + - arg: {name: "lidar_range", default: "30.0", description: "Maximum lidar range (m)"} + # Each Kobuki carries a cylinder at lidar height (lidar_target) so the others' lidars see it. + # Warehouse A furniture only: with the whole world's, two lidars halve the real time factor. + - arg: + name: "world" + default: "$(find-pkg-share large_warehouse_world)/worlds/large_warehouse_world_a.sdf" + description: "World file (large_warehouse_world.sdf for the whole furnished world)" + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + # Gazebo finds the warehouse models here (the playground adds its own). + - set_env: + name: "GZ_SIM_RESOURCE_PATH" + value: "$(find-pkg-share large_warehouse_world)/models:$(env GZ_SIM_RESOURCE_PATH '')" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "world", value: "$(var world)"} + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "x", value: "100.0"} + - {name: "y", value: "300.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} diff --git a/easyfleet_easynav_deployment/maps/warehouse_a.pgm b/easyfleet_easynav_deployment/maps/warehouse_a.pgm new file mode 100644 index 0000000000000000000000000000000000000000..afcbd7bdb38da668fd25080a860d01b03922fc21 GIT binary patch literal 1020117 zcmeEvYmzgouI2uHM$tpKwu_3d*b`v3j+ zKmYqb|NB4x2jBklfB*a6|NOI`t-w}bE3g&V3Ty?o0$YKtz*b-@uoc(}Yz4LgTY;^> zR^Y7){QJ+p``HTo4F&#r{ItWhcck^LPT(@rTfcrbudTq(Dd61NkM{WZx1J-wHf;sA z0=Ft)zGL={zg6iCz7=?b0{h0lVG5hWR^TZG_Kkl^>0M_l@CF6;jeo-wHixaiQwr=G z|CG|Z&Q{X;1Yz3ZDVBh$sl-_l=0&h@Y-}pC7VRP6DJf*ud$ypuoQIZuCo<*g97`;zhMfS!&cxa1@?`9 zO6gr^EAR#d_KknT6gG#gz*7qB8~>EjyUteN4GQcV|Ar}S4qJhz6xcWZDW!Lvt-u=; z*f;(SQ`j7~0#7NhZ~RkA?>bw7Hz=@g{2QjQIcx=>K}-(!0)9;0+4w z8~=tWYz|w2rxe&X{wbw*ovpwd6xcWZ4O7@0wgOKnuy6cRO7A*bfj20yZ~PmkusLi6 zo>E}n_@|WKb+!U;P+;HqH%wu3*a|$Qz`pTMDZT4#1)fuY?zLNP-*{J+ZQKfcwF3VD zd;BS%%QqeX5~?iz)s@y$8^=E!Rv~ep?E*WUYV=dR_5{aq>jEYv20uKl`h`(VQM4c4iy#(+*NbF-lK{5Wf0_{e^hU%2 z@q#@DX&1{8$wi!*&8ni1MR3} zdD7||ANm@T7I=_3E8SzL?z$_CrTCGgGip$g1(nWq$97giyY8a&^%S@!mZ#QOtwm8EMlPEEqEhW8j$w?lGYa=(q!0A=VY!D-_%w@M+$=W(= zP6^TQv7$F#O8GE1TctV*DNmqi_t?o~V9|KUi*HU?%jZKv2eBJJ(yCwTX{#{)z>Rli z1M`w2Amj7mR44GtORYmI$?sHBv7+z@NT6-x7F;UYiXh*DO5GZb04t4MH&T*I#$Ahq z#^hCt0eh=x%jZKv1|vEF=uOXuRd={`O)5rLdLK&xd_P-L9PXMK$l{PQs3wHTi;E3@ zNN_7HP`@kw%DH;cg9Augq=G$$6 zY$dc=sgkqMY|GpAYzffj5HUF4U|5ikw$MIR8W#lqe3(*}Q#wW}sx-lqd{VMg2!m5X zGf?0twi$ZWc`lW94kor5Arp;rN|YujD6i6cyG5meuH>07izvtRV`$QRMEO21DcXiZ zFGaM&`t_cR(9RBlM@vJi@@nO#LKq<>Tj3aY{3J;`!H>9wIn9>@qg7U%!ZMq;jmCMo zg-wRwF)E^$8&sO4jsLfLylle5c*G;Lc4JB)UZjWwki!;)BXXF+c?IO_4BFYJv6Qr^`!62sD9(6)iyZ#7nGHzQvGK7KIF zPyUQmDNmk01!y;*$6GVn$6XF^JFH!HsaKgJY}z>mv{lDxOwl-#QeU3A)#L8-FoPu( z=rq#c8Mbrx)>S2>1yuoJ38}>(+8JrQ7~Q~8p<^#ajlW#BZv1D|6yE759=}uz=C;+s zX$;Xglk$OOSn8z*JV}l1qSgfc!2Qu)Q+<4&mi*=mHOosodyt?MZL$Wf<&_6MNY<1V z-gFFz!~B6TxUXJ*MrxG=6@Kc*o2oKvwGFO5EnJ1JGzOwRc6zaBg^w{SHDf%MA%HUN zy2VgW{H{a39N}Y9AG#!&jqXqBb+z%U`8V~cdS1Tqa*Nxo2E*k0r;Qd4JHBFiX#7po zj{Aikh7VM*>0nrRaC-4ujJaxiZt0H&#A(MdS|{w)J=D~K&iN7kwOH2<*~u;oS0?K{ zaMYy0YGc?{Xa!3xswHfBzWW+yz2Dk{b`xAl)3Q*x421Q5t2V#TgBeL>30?AY`NmgD zkQs^`ZvJ$mO;{_ADdVS$lZ@(ZhUpOwhP9OnR$Utw?V+KT?-H7W+DP6BR;^9AwP8_h zOw|+VY-7R}9KIl0w1*%|9Qcu}fDKxcraADst744szA~u!FBv z^Q-M)PFPNPJvkz%daYN!?H6ILk+__FS%%9D?gM!8I6)VBj_+$pkVA8$qsiw~Qu_<) z#0^7_>liW;etLWAcf6DwB6<#9fpJFs#O~o+@o0-Mp-8=8SQknTM5t;Pt{o7X8xg^` zK}HDfq%3Ptf!sRn&gZ;ZaxNdUSV>2kdC(g&+r5JKhMHdmx$4xlg$_!RK>EQU(zA>Z z+&LDra78x?sea!G^OvjT;Bb!0*a&s?=>$!bW~u>zx8WI+HZBd-oq9Jj3#WuFp5f6>A5T{l_OU#JAN{$7O8_cKpI`WV%-*DrlA=Bj8a#Y?i^wH?lPnlJ-XOSXDXTDil zf2+V3km;&j$UoC$H29R~9O3O!Xz9ry5PXiQ0L|a{l3m+kOXiW(1ip2;0k^zzj5qD2 zl}l{`GUJK%Fg9jnR@ciQuW+&?dVz2tLQu z0?i=|@y1|VGEb&c8aNRUBCi}{Vo#JN^13O~3ND^~W=}+$B_3bzsMYKl8p$A{WX_9Q z9OA_rzw{iCPS7inB4j$+m?TAuJn?k6MB|DAz5$2Rspu#6 ztc!W}%2_{@w%bMP)KCeA_JD*boq|h@i#ponv50t0O35N4XF_D-50kLS3Mekg-9QdM(YB56g@rOa0@NP-K^K%`89Cp7G4I946o84)?}3jIxKZd0dhX1Z^^eoPH*sFV}wFfQnBm8Tzt{t+IT^6pI z)_dTnNrBbIu&dAtmReLx*z)Cxu- ziit^t6U&8#6inp_dmb*_L$n0&iH2%ibS#>2tVG!dkCoIc*qjU!HlfD}sJ|JP4>-}Q z!;Qm2za@R1z%6u{{Os#i8bbgE;`&k1nMULK6*1zBc2!Vn6W;>;hWJg5mwF1pXF)2) zRNV}%zSTN>Rl4;=)K$uM*wx*R2RPBI!;Ql(oQ|ZNL*XG4&Z*O7oCyKpTL;k|mjQH2 zE90f$QU2Oj=p46_l~@+)E130#(qC2=Aj^7IbXY^NZat+|!hPIAJ@oGahxJZ)qaV@< z<@?Zs^_P_=Abjf}&dt6il@v_n34C5n0FhS&aeaEBkd~2PH z*5Se)01U@A0+rV#6>J5Unbx82w6wKZ$C?~z{q7qt4LMCrnQD^M>{CTPp2U|jaCo8d zLAo`m07qW9{L&lW`HBgOl|-WVNx+obD~H3Q0n%GN8c)O#qSWf)Jr7{?LgIc4hOh*^ z4FBpujQ8=$NSZEarm|U8FETTS=Y*8%an*FLP60!kl#lPtv|v~hN}$(}UL7ri8Km#y-etdjux zb?9ro|22y&7S09Oov1AXM_7VBF*|6HXjEN61_&hq^bCM5GS zl!xbP{ul&GF{{WQDjpn=i!h`x?4;8SVs%201+ezWbZxVF1+bB=@I?*hPH8vSRkYxa zYb$UbNbF!&5%q+O4t3cf^|>jr$W1FTpq*T(GKelZMfQpi9S`7aiEEHTzfnySmwIUZcS3>+_n%;zamv#WS9WWLC2`UIr@*_aE8Td$gq{ z?Ne>?OI%Ht!j8WQJTxV&UR_tEz*Yx$a8xP+*Z}{eU z&3qgpUv|Py9nf(*SuSn{#W2S`P&jrLV^M)}&x9BW-f+`Uy*Kc9M*P25W$k?B@P>t? zvxRToAuGN;3WHnc%TBC__72`e$JKQkn+n7ZXA|NH=S?@>yT5Kb=D3&s$t&9~x*2OvM7P}U1WcaRhXqKdZCO!sc!QX9#_`QN>;M;pnG3rcKgP<4c5}O- zzt``)@z`oqlvv!0qQ2zD<5dd|At0_fyn$#8q1yO%fkgNi<(%BsrH4HFuDuj==`5^@ z_(^6d>7bL(v0pWD%^prPu7~ruABaC*vtBUCSo(n@pMugkZsBRgTbGP@NX2xc>kS-oZL?jK0+dCs#Z5yiS5%_h^C#veYI$jG$HAPp3{Rs@w~-$$CWQ{FAxt zHdhn{i8gI<>{XZqIKx$UhOx}48r*XT!f{6;?MqG@JQ&sn33;JXC~i0iVC6NOTqI1< zq|>;vJ)eaPD=ei5vyxn)*@yv&@J}NT6g^$>$dUKqRJ40OhXFin_Vod!6K;v|A5@^ajRrYV|~G(X=s#nEv|{1fCtZ3G#CqNhnvTBtxCWC}hd z0@mT5oKAEvv?&3#=&+XrCk}jEw-v@Qi$1%QXX`*S@|wlayeXPVMg_TT#q+wY!0o^? zDmjC0$z0LEk#}+6Y{=8QGd@7h#E&{zwc^PnHbGK8Jpsz$^JQv5fZ3xbJhqiHneiAbB<7NhbpsIPw)Va}x|+CD<_&+p$cH<{t+! zVlu2F;aE_fG67s{lqj*V=X*Yh`bB{s>(*$0z2;J|t8{|jWx*4d!me_{d{c1hkWVm? zlJoopqQe>Dv2!9LUTVIq&04_>loc)f~@X_*fPT!zqfFsZbJw8_jI zX=HC$H`6{qt;y#RC(RMxrUv>5#hZ$}YTR%}Z))=zthqZ=1V3}*>1!sS=v!64HnUDP z%@NIy0z&)gq@ zkX)s1ib@#%J)S3M+~ zSxp+Jws6xyv<2GSNA9^ERO;Fay5wi>#@B$iy{iEJRaJhkbklagR^WmHeBI&m%T6CM zPS4T91r|wh8p#%&o63u}^PbbmNtElK)jRU~)b257m2N znZNoq0dQI9v)i@5F{*7o+mu`F&D>nVCnFzEJTIR;F(+D`jk136jqk|`{t3;i$87?u zqjK&xs^>1)udQEg)2aNl>C7#u@+1eI!(2_yhR=QD=OoOttfaZm?k8_N!dn<#SIq2Q z@JVIy?Sc9v;sVY$KoB*R09FrQokYMt1^s?G8$SI(_93DXN2zEy#UU-{QPyw%9q)6O zc2X0v4K64v7`r%(MuBhcOx6}V{~le7eGv_)*}DQ_*zHB|NV+-wk?<@=v+t#?but3g zEo9Jj_5uu=S$dJ5u-!@t$MnFB^K={Q3!*)cpzVWg_88)^G$Tl=^paNk$L5e%)@<8F z9(8@Mb)s01@z{+|?4rziaapgl?d631IHvwpLh+VA1c`4U>9?-UE`e?)x4gad+>a7Ht6$-RxLShA~Xk^h-*o{nYPx+8PxWgQ8s3L zW?4h(+i7HFnSP=hp_Ist;8(B{oLk8^N`2owZyf@L;ASQCBVaSA<3xlpZLwl{9B6Vl z_NJ+dX0ncw@L`PpatO|cXj_%bpmc56!mD)(SIy8)K`)*d$mYNrP~V;q`f;%c=5x%K zwy{h%YWP@7eHZS65yYlb(m%I>BEa%>H~!*xbG2(arReP|7Z6zH+edR4X*QK|BG#C+ zP8hG9g5GsEe(9();wbFYY!0Qc%^?7(AC@{82wf19E8R$`V}3|hY@>_R=}=Zm@rz<} z^JE${5R`!X=mCMHg+7|YNwaTi_NZy=I5a$yrc}J1dCsz|Ke**UCjn}G9`%2DnUNPZCr^_C=@qsTfX<~+xBoF@OI8lpB zMN>Kc!ik8MdUf3VjqjhY#~U}7YQcOH#7Gm(qK4cEWSh$C!6~7X*|{U2{C=+2mb?IweuuR z)qQe6>}fx}k?Xz)Z<+`s^4Y-VUO?wcD?3*dz^TgSRSP*T$uAD8SNZr*2BIyaCf#!`kz&X6K@S=lZd%jk@ zG^8{sR@wUZdMgl70M3rQBJn%sQwr=Gf2;B$@2yoH0+(i-Uk!5I9p6Lr9xIb{c&yyw zLOR8-`Y(6iB08Rb{#i4*4|d+V)h*vdx=odW%$+`fBX&EZtnA5JQw^k z%Z5fI$sas!RJtO3%O~9R|NO(4+~uo({Y;JEgWuF=h4+m=>#=wz@?OBj(+`L2c!J06 z{58SJ*FIafS{hZRUNH{yeAZ?=w)R)7Qxk(!f^!>0oe1Xd#5lR6cGZc)>@)nw{-st^u_p#gDb_uLZsb*q84eweVX@YPHlPg7}6f zTOLW3g}AgM@qK!6E4Sah@i_8Nk51~RBS<$&Z2iT;+X4jS)hH}6NH}}&9F9x6um|Bo z)mY|e)wtLZ zfvE9Q;MG)KG3`7IYrCeBMOv}e?g%YNE*KUem=UO@wC++Z=Xrau8T~ch`DR$r0dNYy z#JHE_~y?Z`vAa-LAYA-FToCnL(Y=N)IrR7vNqS;Ywl|7Ab~rw8)%a zx79OMQg8Xn=v9{3;FVh%LoBPtQPFA<$qu#T)rtDD33Y;UP(x2E!@;}XoO!75rUBH6Pu+74deRbGp3JOVD@wFcs>X6khf z@&XhiR61dR`5x&mG?;Y8_X{8^Q|TCl$S`%R9?x>QGE`56Xq;PQ!l?Du-1rF!H>PNJ zSmfdeSMxN2(`m#vTFe!KUgr{*XPs4F0)MSJ5XOkzBE-}&CZ6RYGE{FYI5nCay}3|J zjmj&i$hl9yP?tMjHU)N5i*AA(&oprT)31@ z@2;PX`^K{p>p$xw1W%}3^x%cS76-kV+vQfja6sv_(R;IfjcbN2b85YxJ~17=qP<7s zI^}M0!z6FP{VLFR4G1q+_wT~}JN^?VEP7D(t@^}t25P=*K)-(D<#$Y5_F$O&N31ql zIPCZZ=%a}>NG=!_CU=}^q2Zr#t{M#M{&w-JRIOaTgBuKcwKh-o0MnEwX%E}#5Wpec zPFw{;iUCfuP9U0k$TVfkVzELyXOhGHY|EZJtk)x z@Zx9g#v3>WYM%|0@W!Py@kVlq6HsiDlNQ=xO^ogQRR#2PO#Y5H!oUHOmob0W5^o9r zyp(xwQZfYW~IT>l;a`!^n?>PdRLzsKQ2 zH(vkn3@LzJT$ZyZMPjm~RH&(DQ16?%nY_%2njg=;@u(`R;aqHSIlg5BSpf908$Wb{ zKus2VzUSB_T2gABBxgp*rZdJFcYNw8j__$S_{>Rba~k&IZRVdWS+&X>FWY`*4s@*s zlu$KM?#Ty>_MauC`ek|%5wbM9eov{RUP0Gy#poAuA;hRyV)~AS)elABpN##t^!FK_ zR5eqqxqXo4gQc1+_nmO`BENcrVO~AeB2vghFgCs^`1wtb)lUQ|8xYqU`vY?P&tjs7 zZ;$fC^D|ViB`0>X@M)D=SY`oKV%h})bq3WINY!E%L7?<^V`y57hiYh?mXR`6KVCaM zs8**6@7Q3UND$Z%by++`56_PyTD^Qr&Os<_S{|pO_=X5NK=+suJ;KHz24plDqm)#O zNTCk8fOX=yw@BEfm>?mmADjtN!Z{m)=J+g4inE3%)=<=(`h)5dg}Fwk>w_1PHEbC| zKPf=BffdaS?}|4ZTQ-38Xvmox4$*Dl%qbQsAGnN2eh)p>h_peyiOzv2VWdl}@8y42O2j z4#26slCG6G^iXd0D@jXuMN#NiUIm^lTR1;#nBqpQ;)ur|t+(RFGg`?*rZGpe1Poc2 zPv^@eg(XBuDGP`z0%y`I%HDS4_3|4M?^x3G@eR)dJ*#->a2&+7rJMp6Oiki7+zSn9 zy~??))f}|rg!wg?dLdl8yMS>@Wx$g$-{e=hqAX$Mx#d2UEbY04sdc`N^0a67c-iqF z(9hb^j7lZeLf?uebGx;Qf=fL(D0gb5b+q>VE2+Aw6azN#iW~1=!aS^k31RS5oW{)5 zBC#gz+2xHS_l=K@Qb%y}jR!h(76WHD#ypF1Z-sW3MX%YqHL=f%<6Jd<<3qm^scNwF znaGnnI%RMM)*l1u|H*b1)DqqgeO23f>!+ZCAojgC-aACvoibUty7nbW96TGo4rbRxrnnjUWmv7|6NkFr8aS@Nu$-aI_#jN z1+=IVR!8M5W!Ig!wRL%R7HMSxN|TWv8BMt!o{`TU^i>S7HIPeJ@khP zUGg(`xX|EVk$>cd?QPp;yZ4<@VT+)S!m%#H*XeYG|yBu%TbgS@p5aN z{ufgNxD|{KjK$;@GW!6Oit*@;Pi4z~Y0mf-8u-#YPyA4KZ*%uCZ0Vvo?vP>48giVZwg55Ck^gou63yU>W!DUrbw}uWVI)ItB4mf`mv?a{Y9GoGO!eLs2o7_E#8>({-ma1&_Cm>R_B726Zzg&Xz*(jT%l=ov?9JuxsQl zukk8tGn8{p(_TG7=mnX>ddOcXN9LqX1JkmGu(WN&cU%gGfmEGXW@(9*kVC3oSe8}Y zVlMws5YVUwk2?b7sP6$}X6w)-7yIjwlzD`S>h*B;)TIt|nN&Wc9w^Gi8=rzkebocO zEw6giGals}(#|1OpGFqUrO;eWA{I%VEd9J?Fp?UQDDNB+-wD`(M)b;b85)Jvdnhwq zi&S;JmyH15#Xt((APs0RMi|0s;_lE#j@Hu;;)pd)Iz43U53p$8!pp-7r{> z1aOqsY^_y!%6azAU|v9VJP_$Y>a_BD;2=hXy%#26);lEToet9(eYnW^_mLGc~Vp8A< zR@w+p6)%XXXf&F?bK_CFo^EEqqEk6j5-zD%I*o-#MW{Hr00Rtx$DqtFrG+JMiq?jc zHrrwIZ4G2v&Qo+CDZApUhC^Lcz?3#d@i7|c`xL5ZX*9S$1$dj3%O^$W!xH#PKNGL)q=||@nXyBv_-v~#Ct(bPUO>m? zSTO?iif7y`GK{DGdIpI6bw{H$6CQOdX22JM!-KkS{D7(tgoXz90FU>T__Z6qSmDsG z9X?&5;Kc-UIOXzFU*vh4dq1r}Iw8~PJMm=-T)y!DkXn)j{@m9X^h z8T6=0yA$0xY?7Ua5rk%aTza;6S}E&eBof^QG16XoVJ{#Gk=O~2#uv-1RQ zJZsoNddAIUsKx6gL7z+C9F+go-|?!|b)ilOQD8cK_(qEp#BCcY_i#=8tKxV@`ht{^ z@K!9B4?w2S7MOI?40_6GTU($PK)m$EYopWBl0Nu0C{*{@M7c%draN>Jso#mF9jE~l z|KbcQy;_OCAp0pB`YhED_n%B#ns;#aLI0f!mLoelxyxd~#A z0|O3X`FL)Tt40xRq^5K>TYY%GQ3;U{wbW^+ftM*lk?lSetW&iO!`_Sln}eU0%n~}) zx;Vsc1q#5oPUP^3NMkpSxmhV4a*TRRiI zn%xzl57vx8{H{hyr-`Ms+hN8_rFmAyo-JDhAX{Ep+G$IDQoWP8zR{xXEUE5<0Y<&R zqaN*MzN}zGAtb?*fpk-OU_RZWnDG4Sh{QjBL~d1fxo2iios1DMWt2--1e0}I0>o)w z*&u!>u#$f=bH8ywZS(|`Z(3ncOkR1O-6srd>94tNGg7L@+iv3Bb{+Z+ue)b*dg}+pB;Adf!w+=R|5)0CQDHVzdhF<2Wkt3>D2@`7JoZ zWGF{y4`l^{1=u;Qf%Lx{VvmoA_;=-aZ|5?c@A@5opP(YheIBI~^fDzHTh3iau*%Qs#oEerR*ms;=9vzk-{waKkh)1|QE5_@P$SiQQsRCR5u z3wqFQf-4b17Alv4u->&z2hkR2^9wzgkyMt@B|p>u=`TS8OmH|K3EL;4asoPL8LKSz zjn{*|_9!nnqn&Ajo3A_0lN;%0eP@K8*|+i6DZGu@Yxw;B@q9>XJ!`L%RH&_xLcl^_ zYx3vHiqZiLBt^tR>;izl7xKE}NfOJk`X0fB^80?r!|A7&dkcv7fXv!wB?$w-#c`D= zYfhooGQFnCnCMQU?|JXF-VDj!bvX+#N%>2uz0J|OfmoD^Nn3dA9>J%y-_KOrqrAaWZUP95$UH5X1ua= zik?jiM=eGBEuA^|J7Bhq98mL%%vq(=x%9VXePKI7`Q{rxF)F^U?#;#%-9L&9fmc8t zhl==2pc8gB5rIC3bCSOU6nXg(LssnK036Emf(giT%-M7zI+8i0S7b>$xOn)1LpxJZ z&-$v-P7*1}vzqSBCf=Nrn&6%JlLCrE~>9V6SDpX`9( z)~h<+tF+_5fr(y@Ye~D~Y^khL_C@G&E7B&3l!Z_!QmZQ&aqyxTBEcXtpPf-bmL;KC zOCeDLGE{({ycSeBwLOb>Pad_Ie!FnT$ac+R6g>lG2L!iXRo{AsI`0sLejGS3(aUk2 zL4vz#cftH4Ob&RO!dNm!kE;}|fl85DUCB}t4z6^EKHNAl?2_1HQ3;(e_a)NW6Yxih zsUmRmXm@481b`@U6_&NB40Awa7c7+$Jle`m7nN)cC{_s=-wRi|?hEgS8E{!FZc)mU zfNrya`u#km)`LUpj^y-(0rra1pq@+hshQNkay6b`(o{h$_{R_am^F=qCL#=RvE*r- z1FFkKs&O2!_?N~@T47tKM7q-Mu^A8@R9Mm0D=@GV{#i>X zos|oieAv=t(ve#xJkHPsBeQyNtGf%`G*hM3`%5)>WKN~^gb1ei%5v0%!ImXxO|N%p zF7)nN$WWITSgEG4TsEEhIXHXHyX7JsfY=tbwyeJ;OjY)kS1>>g3J0PthDdi8ad!Hl z0L*OJIl=kU8CZ4@klHe^%QIi!-(^Y&Aa|nn2hox$8{|h6NXKPOGwW}}%Wk~ftu=Zr zLJ?N`#xGLzr(xhuE&Da9M{kv6gt!XqGS2#Mw9AwbKxJPx$tAot$SW0CdE;N%;6hS* zwHAV2hf-Sm#$Q+S=MmCz`S~_~xxs}uUj9=YU^XNZXFE*(6U?HpgHjKTnYZJ9p@+tZ zd{?mPVA!5-p)xDiK707@s`PZ?3Vg|eM0#Wfl~CUdse*Jwh{JB$36>YGXSLPGoJ^BlCqW`MG@KTSo|rusXpoj`zY1S~QHi z^x_3hNP0!hN%ue`MbIEYjOAi&n38K2Eot;DLbHu4VeM07!ay6?`3kAM5|CI2!=lWsDImGto9@gg3$oOkp0t*|u-|Sr31gi|$8FSZ@QK;4+r- zz0BKvj`UTzd+h?Qm#cbCx}tnfA)rzfBHV9+9Fj{R;)@=kwg0MLH5HI_0xAu{Nz@5B z&M!8#`y#J&SGjxQk1twP>9W&X!aH-28W0h?@sMHABtKiU7@5#un8~Ot6)4n21!b98 z=?p=pv{s4&;}C9r=7gjykDD+bB@KzADNn{rUoK0OLmVlzPmbc+VXdmU@5Di2!)zc0 zTaY#s!mcEw9TyC<$o^WpGz$BnPgs@CP&MG*p|8MgVWBL94i(RX=kkfaS6G892gZiFn7x!xRfd@S&M|VnWL8?+F=&i zUu(Tk7e2^3E3_l80o48wjy`+WS>+h0z9ckNcA%8f;H+OUP^Y28)b5MCoJ>a|&^>nC z3zM=8uAfxqIDcq8aj;>*8;@r>g-TI}UCXCM_4~ttFk$_r*6WMaN4p%6iyj0_U37vm z=tDOiD|a50-wjH=(R&9Xa|eoM8SXaKcU={uFfRS#!d7Kd#*r8zN0mb6psfRDO^u>Z zPhK`9TW3WxuWkwJxbbMkcJL>A#O(M?XfVuVRF)z@qE?A|%nW_WYoI7ymbPL549}-- zyf^A|f9r4j8Gm{g()NVThtnAdnZ(bB@06W3RXI+vfJF2TV<3r3dSAhiemtilvztVY z_?_C$@VKVO$b_xnc^)7 z6V_iUP*n5P=$hWz;E1cFIkWu{mp#(|Ka$U+r}+^hg%K=lBM3F+%;g0tmtPbx=- z^!_dMI^1rgD*_!lW*E#rSp~wW__EN*cA{sQa0EXV)0e%~Hu&V#s3$Rm)d=jlM1WPt z1>291x^j_wO^H87MZ#gBdRS{^{YVd$+JzZ<9dEZlDGdY~nW&^>#tuK4)RDl}uYV<5IsNR)Fx%7F5>L5&nR4l4iGB~tgn5m+A7-MaDt%n-wq&+d8 zOm3u&`Z|bIa}SGIpwbZQ6orlqbl82Ol>;f>5%7X$?E&^*dN-zsck;r58U`mztpqw z-+0$56wOao4w?XXw(P-_ob;z#(`VqH6AC%Y_MHLdr+QIi6zjbT@z6)d%=mayf^ z6JfpI+Josjwv?u2p>i1n>-|=3exV05lFAagBF4KZY&xS&ZA2c5NYTr(zy8&+ke*v{!IK!d6dXL z@F+OuTTF87pyaJZ=WhJW*ThMuiLO8fKg()G|Mvyg=7e(d&x!Nid`1B}|EG!g=OD_C z&Pz9#PgOT?Q2^J&!%H1s$3>{}p+cp(Z9z^RSUbylC-R}7pJlYtr7bqL#>V((Ec1J# zS+`eqz~uD&`sW}g4|%zX_L%K^Y5)ej{s#KFB zdmlyDcmd+S7to;YpImXzuWXLQ$2XZmY#{D|CzzVYw$GA0HG8cWVP~0E*RD(;;zR*V z5&5eH7dxP2rK%B2mh;8=Y>_m_F>{(tmFCO5DpO=j=X+sqiagygiO*9RK!MU;bg(%k z?xJ|emu$A)ofQGQa*a;(na*Lvlsj3SYV!t4R;n^P`^j13pV-*(qktt0aTyY!g;wG} zYEf#`(RgoFwg=wv%~=g=Ih0&03jfE+<7tNBY%AyJoX2SX_r^JP!A{GC*iBbVb-L+7 zCq>e>o3tPR3JFS`s<6|AWQ{Kb1WtxLCCvD(pre)!TnYDCi}acF9`CJ6f5DAU7c{9? zt#f>(U^yyT4ke z8_oeqa4tf{NJs6RH{P6YBtPC;rDcXfz-ma>!Lw*XkQd+DVd3VI2-lc*Thjn77d!Nt zdCSVuxurk_G}gRfr^^Bn4uZT2r!i2_apMKf_>UzY!_srI2m;3^4yTwmY=kG0L9W1a z#X~Uob^*^w*PtXFT&QH>1t)?7OlNFlXBA%`Bg{)MR~9~ZPw5b+}oRGm6P1p*~ge;RjGWXsFr)jjaNRc zuAp^!wc<+OvMz;aQA0d5pu+7Lq?v9CUMBab*{X=bKVG>J8{m$FW98$Dp>~D7PUri)-$+HCy3tf*GjovyzxX;G*H-AtW?q)=zwtk9kf+hnX88b z`2+4~8B^snPai+M9k8MlXRr& znVQAAJx|HrUTRT+W%JePeAkU<-i!47ei#-+?%n5FdJ&b+iLkozGEo(1^2P%|YEBmJ zf8Mp;Lku;kI%;FsRcHlEEvhAKdH#*1an}2-J!m(1_r54q*Y0EQBjkDfw?LoThVKy%ui1@sY`Uw9ux+A5yADWZMT7h#e}O?;!kFg=?2@x$(&8J_)aMX|-xGytRQ= z;q9v^fD`E-DvH3jerTo6H<;}yzQ0kr%Tm(SC-!yhrbdicZle(Id?iIrSA^@1S)~9? zt_$`>H$EN3Zbx2#J;it4c&-B+c9DTyu#3M?9sB;LhqNa{n#&Vq~!f@ zhB9AJN5B#mRrDv|lUYFF8z4j`UmI>g!@M~1a%lAvGUZhb`jkVMQGco3DK1ti!kR+q zC)qmsTfU!HiNH$`qYH&A7*#Vs`{>$4H8<{1ur$Ntv^fA1EjTD&YEqe+4QSZ#xCIw7 zhN|>Zi;_Xd&}={Tviz;f)N?m7+t1QkIh)&>+=^c#Qma$y6K>&_PD|~VQlFXvg|3hTNC=KSGif1kz$SAKZfA-% zN}S=UU!X@14WJ>{0hhE0euoVecw=zZXZO^JNWBvyfp6^kIwx%Zxa_#1X9`txs{A3_ z2hB!UOVC2Kh!OkZqGveRq2cyNB*98&3b)t(Sb?lh(4b94L|PQru4cWdvbB72jqQ^J z9UZzHRnXZ(o{g|pQ47@~M(m4=p5Y!E^;r?xjY#mFvBTP)RjM}~jjJQ%9ykra7ZyCa0YELx}^+m9G6+CdD$bx;P~YZVzR{;BZcYosc*FQ3M(YHgV6s=_V~c25el zWpHy(MipxM#?v39Cs1#rg*@*~HYwV{$KJ0h*(p)Fh1}znOD>ODhOd#T)Rsx(R`rq) zSD^t80D5Rt$8(%)m`dp`#woPDQK4qfeEhaxtu~w0UhS@p9PgoxGvtku8v=cMaB6yt zB@5NeI)a)pW6t3(ryhLehQ$sai*Wf0DNuz^uZ{Gd4ZBgF39#L@7*Jd*;1*wEcMJ4S zf$|g|ZfgCF_ity4|`g9}Rd3-evjL;pm;8Ruv2#`jB% zMj)TjoE^UvSgk<17pqY}j>p#>Z`lj(8PoZ=E}OugR{*ZVEAGVNLGTrO*^Pg}m(OCM zH{7==uy6bm6SH4HSTF6LsJylY@?ML$(e78^eA>>t`>Ruh`_D~ki{9V#S3Nfle^!0n zUsf#XLucfx$|P-=GVXZ&hR^e9JMSLg#|Ot3WtDVVop+XGZSpn-)~UnH*x0reA>HU4a9jhb;o@xge8cbfLnUNI zq!Yb@e==)1&Yx{X6piD~+0h+mBk2KhF1%v7?vOFV&^Axm#1!AIQa+&$ZjhT-Ut93JgGKl^ z1%eKzel8(tP}Vp0w8Zx^%vz#xtdz|iT*Ey5k!Cpg$%{MUIqfQ*W6xXKN z720l=ZB%Y&iO)?SJXK5BcUb5! zIfTK^=PcS6q7ae;r_DyuG{adX5XX_3^2tM97D3xQ%!^6Ar5@Wo7&Hm=5!+TtmPxeZ z3$FsSzk3Yf0>c;#oU!IhIRUQHdGyF00Oi%mt9GP4=&HnO`E0brS>xN+iFC=**rl(| z3}bPzsFKi>r8@aDS}aVMoBQKQLUE~prko3gc9J}OWoOn+=NrK2b!m1{W`HQ z?hhnO4}LbmkGNh~1w*r=7cv6^a{N-PEC#pabnH{6W+jU_+*BCZR>{_ip{pzewk@BH z*0}aLusI)Jzx>97rXQDx2_Td71Eg@+KXc=uEh96XV{Ur<6OaOj>IfQvCyHqRBWb5t z>MeUa1<*zVN}si8oRQB4ym6gKcbG|M%{Yl_w%?2@*qn}Hv-Q7G@J*}d2Vgzt#u<55iD4Mjl^dCDAdbt-9GC$KE>b_=)G zT* zy_WA>nj_EFzp_i`DR8P#rpk>c1z#!{_eUr10gil{NUfQA3efx;HiyQ$^Hh&{?Q*YN zh!eiKHjF!=V)*=eJ6EU)2psyy4t(R_)9$iX*4^@?VsMbtPO2W6X zY^*kxF+(nzgJ&f@s)v`Wah}(;F`(x}pMOTZ=)Yu}5V|1`9W|bCBkoVQ_(0Oxo9UQ@ z#J4d;sqRd@U&=udeyN{zy{!FpK!Ip!dN$dyLlru%=*Y%wC-BFk!PO?4Z;` zW9IF+&-Bm`^+m2F+Nk`ofxur%NdirDCE>8nUsl1eSD#V0e=veV{0RSAtZRqtWS51j zru7~;YEodeG3+X|f~6MK61F@aHI1|0kMy8aUc6d8xS-O%Fy93|^bZD{Vty{)_*OM2 z!lMg@al999&=!Q*8}C(Po2kks@6&|!$fZz`<^r0R`a;`)Hi`WbZKP@wO^-qQCI}iN zG$dXdrsSGM8+_OWy!dJN$q6@q5}BK8yaS$PzdFHpbTh$11enO(Dbosw>~Qc}-&mmn z0v3vPgOEjl$AU_3`3H^CLZICTY293kx=uB)Z~n=via5EWw}-@D$RGEMw6ix}0dRgG zcY4jrYHqwm&f)l^d~m#1&sM9wyxs5Hr|wGLcD~@{Ip%?EAD(aM_eEZf_v0h|!XYZ< zNv&We<|RuOaq!a~6r48xq_;B=`^bNGopu)Q6O>hL+-E;S|%(?WKWO*06l!#YvIMfM2o%${$d zsmZ=_r1M_tJKWBZ)tv)@L->)0R4=@U0AYsXV{8E)z81GY zI)*q9E>(!5*dXjn!cuny89NW2-!bk}Y|D2FoZ1jMCljR}dmvt3W0?}2wQ3cT7-xZJ zWt1(!q{$=+<^W938F`1NntXK=ofO4rUSsU*QMPzm`}!*3;`=m~LI{RCL#%PB&@{s< ziIoG1t?)cmfU;0@zGY^_LX{(d-I!Jn-Q*OIwQ3b=Ayfozr5yDPXE}NOG>tI%G)itj zBeFt=Lk6D*%3hEhUQ$(}{l&B{KAc#i^(CQP2!wuXfXR(J{vIfO@3mBfPfaJ<(5t-i zMpT^KvtnN%Hvx_FqM6ZK3&3`Mij!88wHkLyx^N<-o__b9p?f>(`Dk3g$|P~IG+-#B zPL?9Ycp(?YhLdYQ>t=hdBizpBpu19c9sTEBx#z>P*?>ThKm&XS7Nka*wt?aHWbT~bf>P^N2#6Pt3 z70K`^2-Aiir96A%1wn~BOeD(jRY+4)t=|EG8()FGY}R(-{gg!!rL^W2t(AMed*AA- zHT#LKtJiG;>y(1yqY-xJn7TbBr;W&9w@%#IRE0Z-IXkI0dwr$hX7d(jSA%dZz6KGHL@yg`@|G`^@}#B;(B!6JPj%+dp%z!wKh4uACZ_7FC8E7h>DqBW?7b{ux>Tza zqKS0D&KIhhL)X3kjRyJMilG2KPG^FlgKz^r51NE0GAjHBxIo|((tq5#t#?$u>>02dwP1=7tok6d=2q1PaNm5 zV&5J;B~eoN+gsoKjRO0|Co$v~Abxw)A91$Bg>#?0L~F@C0px~6aPd`26t zDkJhG;jqsCZF46Kilm6J{*n|Ky?~#QS+N^65kISZ_)A$I7iK#HXc$ zM!(?j4G!J$xEVKJjG`u!fzO+N#}C6RwSskNh_S~g=T8kV$*Y9^utj9}FpoRR!!+E1 z+)-;8rVOP0+ndfAzG0K;aVdbywcllAGFbQ4%Hxdz9eEY)=WzMHNFW15dHPD3jsle4 ziSY|xu%@Cu@e8o`rV{CXiKlM-(!uFKnnyvy-kugTpJ%IoGEKi&waBd;Pm$d=mE zWXy6zmokn|lMgzb7#rVSi}b;;Wrf@ImUt^d=h#Vt+&Z!cgg`h=JrXEbt>K8TiAYX- zgW~!~fsZYei+dLWyZ|8(H4y@^@+#7UY%$Ex%eIWFBADT@(|i&g17}eCOW@JPj{8GC zCguScTa=Hc@DVVFN1O=_-^JuCY?NI)q1HqPq6uuHC)Ua(rd?TLHm^j=EJ`hN#J35e z8;=5$$VV@-&oaRt=r+ompa?Q6bKVNv+5GBVOep%^hW5%j&{?}A+STKyHN7? z9i|I|0?YW?kWf-a+=4q-0L%#2bJEC8s3dkHsYmQd^8^<_p}=+=>Pg3;Eo=)UD4&X; zllF?n;0#(5PL4yXgYfxkgFNhw2~{U$C9FrqJt(y|&@%#8MJq=0&p9eQ5hy7mX6Pu1 zyyU{YP?^5*Co?oRi%$C_cujQ=v$R2Ww+v)Skt1048DbJEGZtHwl#q#o*Q>-8 z2q%MtA*(NWVnWrMndrHhLe6@*!=n)C9JG%CMmHe=gmCZx$kWU~Cufy`y?SMaYY{3h zyz#=#Wgammgl?paHceW;0lJx9fY8QKn^zvoRq?a&Shz;mPq`*~CKXxIIWe>+ZH6_PnO#K~nN0>F zTZ6XZ#+&mZC4PNjB@FgTcj?+`(W-P804%+N3>skvaw9@qDO9#*ak`+y4BH)kvMz`7 zvjmnZGZ)(kVeE(gaGGkYC>b*lv1^l7N%_bm(42t8<&J#Mfw}#6_uO&k?`A8&M2bzP zwZT|$4hr~-24JTM#Lw*2(_3kir>~G+TFt-o#tXMvwHxdUCM~q_A4>9+Tyb9fQ+ofj zMN~_NJZ;;G(CNP@rgCj5i{Q&qSMCbK?omFJ-Osge}SqYdACVnuI?j#oMko znhxX{@QAbD+~Jeo9t>j1OIh7 zs>c=Th|<0BR|ZupdmgwQHn;%?rG!A)b}xSxISc+GveX_J!x&o=rc)lk)R<3-;u zVSYDF6ejeDgb3ZjjH< z^nVCQyZ~lx|7+0}sA)S~OKuG9h<@qEhmOH7%y&T#{X@am9Zxgg*nZZI*a~a~zEgpH zLXVBh%foZDu-6_`_C-}pJ*cGgzlI~CYB{yXQkS#Jd< z70@fc|6Bc}ggb33@I?yj8~;Va+Elj!mlfDI{<4G{U@P!N3hW#IMZ?-uw*r?H*f;*N zgd1Qh@I?yj8~;Va+Elj!mlfDI{<4G{U@P!N3hW#IMZ?-uw*r?H*f;*Ngd1Qh@I?yj z8~;Va+Elj!mlfDI{<4G{U@P!N3hW#IMZ?-uw*r?H*f;*Ngd1Qh@I?yj8~;Va+Elj! zmlfDI{<4G{U@P!N3hW#IMZ?-uw*r?H*f;*Ngd1Qh@I?yj8~;Va+Elj!mlfDI{<4G{ zU@P!N3hW#IMZ?-uw*r?H*f;*Ngd1Qh@I?yj8~;Va+Elj!mlfDI{<4G{U@P!N3hW#I zMZ?-uw*r?H*f;*Ngd1Qh@I?yj8~;Va+Elj!mlfDI{<4G{U@P!N3hW#IMZ?-uw*r?H z*f;*Ngd1Qh@I4CXh5u*&|1|kMW7|x(0+R~t8$T)BPTLB6r2_lLf92RV>8-$|0{g~K z3b)g?0$-`XZd)|hngZO6~aCnpDc4++f} z>iNcPh?Ji3+?`QT00ebBV3U{Q{;Uaq$9tsPtnegGq_sh9wFu@uIRhM5RY+KE+P6)kGA6UuvCSA-g~S_>;@$`zo1`^7!N+XC^J)xoS392YaHv z;EmU`k8`L*Ccp!>64NnjcPX%Pc{FSV^98(5ZA}j5gz=tDWduEfz6dxj8rh1u^(c(` z=~Rs-9ltfpG}H|dOV6N%Xr;G_FC-|Y0AFz9YvR}@Fy@l5Fcf*!3Z^1hwiS_lp`bp@ zvk_I&{?&EJoe|DE4oCG!!GMgMbNv&zMCNwAkVU|fOX?!X@l4)$bG{jAjsAclFWWn= zp;B)0_EAC-CkqcPXcLSMMV#QA=0PGK3WNR|XESRVgUcr@p#+u~_gSz7kM(^O0hv|x z1sfYpeZOwvM-Wi3TQ~6pyal@ z7QTs-Wf{+|(sQy<_xRS4A;FncFqqxfF0qY6s(ufC&quoNG@zn#Tex7D1!e}uCQI_k z?x6(}>T1&CV}aVTRge?_jyOM+*mY(o6aiyCy>JsJ%MxE)?cy3%LRbrZYuPq}Ut<>I zjce`SHaEmnA=+udIOIWG!i1W@IHxdDy8-k)*l`qvRom2t8( z-SEL|5+LWPKwy&)y+H7I=G@k#lm|EVit~~izxMmH(YfzZJsf}2DekMd$;MV z0RBPqb(s`l$yrdiG!58y#v&P=ICA&lAZ+!G4;A&;OKU4`yjNh*MtlZyR)(t{4rGbODb7+eL;5Wt%BoNYrpr9^3=qFw>TnTGIS zr)|h0Ao4VRFH!YyBAZSpUQD0uNoYK@n3l=%V-RbbUCVVpP8MK$P-aS8y%KrOjs-0^ z*tU=%duG$y+16YcdH{?4*vLnD;NLp_f#C0Hlx(o;5BD_4c0u^KM0wE(m2(K$!?@%n z#&U6*64s}ujGM9`_~@}|6#{N8cinfig&#`^)ho6=#HStdIj@Q}$N|;e&&L#y>)xKi zX-YEdMXU*L5!NLbY3%xqms5RGu3#4olkcB4S~%<&b`OouL@f^Ncz$|eN7rk^3MpS{ z_>4AO)tws-jUS#2C%77d6XWTxIyZn(__K-j^cih2wtuKj{*EVMu!bzmN^4v1VO7%; zVaMh3iYj6C>gow{V~QxFH?HLHKP%xc%y&T#{SyIW;YYrT`dKCdveS+~jn6D4461O! zBhf_Z-Gi02XKRDDAWW+^i)vU~skP*SXqiB7-3Gf^8>WN%;`xds?H>Ar2Oxgh=eQ3Q zj=)RGKVA|L^2`+*(;rHzVA;QUsv+C4OaYwTGO!l{-PE2O`I(j_8Kj>TA2CL1V=(_@ zc}yoWx3+>b#NmucWywS!0(p*#?i4MW@fYLp(4bOZ-#d-FY$KU(=@5m(8&nnzh^f3L zZ@dBos-+M~DKi?bGWbA)4{Cj?f5zbM1!)OdjWgMy*QWo+o!CB(`qJ_KBlniTNi@m3UYtfn%xMVZvcb%X#-FW=G z`B^d4Iz?>5xjQ!&<%(0nDKC$8y-o2bfS9}mgo#;F#3BoEnQ(FX3&bUCE#L>%t*I*c z1!)Ody(D<7B+bCzbWV(VRtPQCMlk0f1%^gcWJ&H(w_S9?d*H_Jh^-;X?)Q|9tgIEF z5|GUH`=pi9^7#euYRWzJLwZ(7XMTc2i(aDZH)yVZ8|1_z2P8B26T?ruorOR3@I$a5*jI6Uq;2d8TdThrt3LK)63a&*fDs6)*us~`B zYp$Y|in|MmO<-~sSt%u2v|pfzKvMiuIC}==W$$D!%lHMr*qo)+4#>(Y4;46|BTpW5 z+6xqFF!!WFDVOKxqCsH4f<>d~P*xxJ*CLWm_`agE7Ml1vg;HE6sBl-Zr0AOdY*~S} z#^_p-f!CDs79dy@>c}fkQ8q8%J^OHsU8@k$>FSmeGna)vN^|BG*JyIP6JYWeX6Pj3%RuI$Kic*_Q7FE_XP4;o$iGu1OU1 z&J#J4HqE$U`Q)fLRkf1iG?ffFV=b2uR~j7);;p;!a2t-Y#FN0CqCJx&Be!0#?-vMk zBg!NWqjM{Q>*CFnp&S>DYwyqN3N+_-J|7Tztsd1Ph-Ov&)A3rlBRhKbkW>H0V^H8Z zD;G$pgs4!_HcB9B>@X8v88Ty0YF_pXUCIf`>s9=eE$Q&7>f?2rb6a(84_XRGW5qXr zzD#Po9VcV>>~VGXZpYA;odV?&E9~@dP0M%zsEp5@Ayl%3L+h37LIsxu!C;omW(k=9 zh#V;|mYklWXY=Qc!lZLR@A&2)3qQSAwBjEIYm&>*vMo~p2yT^4bDx0EPerRC+-1C= zD)XAS+7-@vJ}LpPGz053vwo;MnVVN{dIYWnsT9?bs{W1lxIAZT-uR4$+bDgJRXkaK zZ>vHr!80T1RqpyxCXuQe9SOh|-*_O}+*fMauC&H$k5qv;Jpy-fFif_~NqOUdRgyxG z!oUq>j%BAnxl}4;g&*AF=wyyd>Qb{yke&pg!MyPtS46N!stB1L0VEb+cs%&n%1Lp; z%EX5)Bjnsh7xJ~^d6`y9ZDGG(2=#3@9&RB&cdMQ1P!%reeKFikql-Us-N(6LGe~(jWAIvncGK)I($D z?YPhM&=B=SF3<+VOYbdOF{Q)D;0NZ*r>cGS@W11xrxRBZA7^tsB5ozrH$!q>UrZ)L zZ5;1j<@>@4mL54piS$dN52&y>YeYo2H^iBdo}RGul59z>A;DH@;O3iX3kK zbfZmJD{gI=e)kB;9y!;~4GM}Zyzs?6cqQIvTibLH?I-3dy*3Y3VB!G8&)kg<)Ob$_ zw0eO|5-I_(XjkwqQ2C8i6mfAU>k$_d{5YJPT+n{C`X6tuAaJcm&_@7JEB-0UY(qc+ zJ$ZrPaEJ^joblHJRrl9ub>r9cwH{A5o`w5s2WDBnEZ~)2^NgXSu1#2R7OYBg3qY?! zFfI68^gAArrXQs<4be)x#0~#BC;(|K+g8zr?`nPn`t>eRFmzmer5Hn-_2bn4+={t{ z+Ix=Haf*rEsvCa=M`U`NPB;?yPL!`jC7^Z{?T4mpOBVbnyzK8Uuy-7STRo$~4 zcP*cd=1S2sZFAqe*8a=RTE0_H8hA3-H^T{asIEt9wIfp)#7KN8NE>0ut5v#(d-sb#|?GT0V=&WxCqgq%T-&|7B+_->EeXJelj8b^>_osly(r z)y{;Am+qd#uOv~_)++bjc&$@Vum%l79r&>kL+b?4PocPen2CU)$jg|sB{$$5q?XUZ zMB{AEd)KP8_EjG6mhaS?2A<4~jbwN+OYeYGD*HRt#*S`=^sTRsym zVI6TG?#Pl1gCfh(Qy_`nLMo-wY3l@vBTH~XSD3WQ(rycCTBmSj28?|Fjn7C|wymz! z$A{4eK`%wL!~En=S?$F>aX2|>)F-!WL>;lX3kmCpBZs@40A9Dulrbws6g+rwXo43u z#lcCkRi3HikT{u;QM&QM?c^WXB!q58FszGH1V{&-v`wc%1;aGq4J$mw^`83-qfH-i zT|kISF;UrhjgVlFr zo!r;7AZHbC-_|5K6-IVHcjLW-aO#>jGMjJ)XUb=SkZo-d46CKL?9xOl==XfJ;~iG| zPTfB9-C2|5H2epsiL$Zp-V?o%jja>Rqu;#vn_~XP2P@qf#*N=u<9*oF^+j5MaS`EF z=_@2z(GVX#;~i<3YJ<_ zOW5*9f)@q}AL+qxdGGQ=+Wf+N7xd6S7jVY;nfrGDQF7b`=xGm&(Ii8)k33 zSB-6^Dx17d6ZT*a9+UUkKBJ9P{rxlAA32D@Px~(Rp?1VpU@P!?1^Bwd=f3f;pVlV$ zJ_XVZdLOSEqvbaqzdH7_75IPxpE-K;oAQg!{K^~8SRWYnF8Rk5;IsGq@0-VN{Gu;3 zRqFa*^^En$4_~-_(km2=k)gnE3gY9kq_w4BLn@}qLn)PD?xE&h^UrRs0-u)w6|2H| zn@p zV*|;Frfzd_mgQ7?}tPVJFM=to|c3qGk5AD1)`s@&nv8T3Vw#R~J|dp&}eK@4rB z-i6=sh(SdKY>2|=gIfcsvxBVuw|hN39rq+0MOA)@i>MWUqr$sg+E|At23A4{1ASqz z3ebpIYITxQ90o~-(+8T;k48CRomz3Xtuj?d0h)cq8GRtq^e|8zOy*+!)`mtKzdCKC!wBGcQUd$$%0H1aK(6mFu~A(VmYEq&i?iS^ZvBJUvr zuCLzs>^t9|uTbi!pW-k`GSUG%(bapI| z{_)f9HBX$}YY~-h2uc7_7OEb3RE={qu}JEi9e3_hJ*mnZijt{UHlKHFEj|H_01JmP zh=`teO{W3*wk7sT{Z^Vz{j|GBWjQ@~@>()Mv6DOBAC%VKcTak_CraCQ zoWrJs)=h&k%L{_5Srl|W6J6{&nn{(eJ@BT}fPCAs*_T}8W>gaQ>BjjOc*C+>h&mZ4 z<2YkB0@v0j&E`Q1_fn((i^#8P0sIKpYQLOqfWTSf^qObW0kEP&C~$+v$qB-pGZ78P z+IpnxN}Nxph4}40=~$(yX!CrhLMdEYGMpa3B(L<_CJZ|js9d_o(#N*|euQhqYEJuP zL%Zj5ftp5=vJa&_5E?P_vQk{LV(A1lzf}{O5ip)Vzf@_wiF}DXcmn9<@TD_~=l!An zte}%61;S~kYN8~^I|b(8BvV4Gx+{)qR^;*)m{l|rDP2#Fz(D=rlGXXa)}STs4!#l| zd9+^l7g3cDV^jkM9E`Q5{i!QjXS*0q3rW{&+E(D7Sg|@k(agu2U^U@$`p=4$-%fbs z`RBSgi?3)B>pCqD0kJ*%w&5Sd`&p(yI>huoedCvr0U+VC1T>*7f!ZKjfuB%7ZrJSa zcmPO3$O3O|p^e(asV0q9+v>1_r507fw)}7QV7iViMG{%4Tn52<53`yyPHlU>NfoQe zf-|a8*H%!FpSj=hkX@y2ib@!qm&2?kjZ@p|u!5x))gx@nGfj=N-p};_EJPdS7VW`g z#FkP_7Fx<6Snpw0lg6n{dQj7)u%DSP{hg~)@JeKog*yihuYW>5KXW&}4!u2#6_7zK z#@%p#NdZ1Z@VRgN%LgyBdO6SUrH8j**+b)6hPDhyPSL*c%QU?U$j-{GfdQ^#8)FJA z{jG;!FC2$GMRW8LLQK(q)LOKDJwM`^i$lHo1YE0~RS^JA{D%LuA}Z1HTvP=3X#536dj}?ub3#66 zs*(O2BdV;L&0bqPXwTnxAdkoXIQr|vWj&r43(Dr)-9~9<=VXD4ZajXje1f>lY)xru z0pE^ZYD$4yUaiWBT`R)(T@f>{NqfJ{qt?VLu{q=&te%LlQQ%M%Fe3kmk8>DRQZRNz1eV|epR({T>m#EL~5E{fmvSGUaZf( zo_9qs8k*#CXO*7|T{S9frQMVn46BwoI~2{JpF?OTIybq|ZR;Hn2c7a#QWoL!*NV4##(b zA`r2hcVZSoU};q?oU`-P%4yXss=>i9O*qh9mC__2;*hX5Uvx`s4sG_N#erqa*OyEn zfL-fhg2X`85?u$i>V%Xv$g&n5dX^W!_tmt077W2j7Y32!DWa@FqMT8;d=?{H*Lpa^ z;Vw%&w&ytN#19;_=D3ygU|62*x08d>K2L=`N{O7P{u=FKrb|u@KAahA2Q_yAhi>r__j~A9xG&G0@AXGvIdQ|*0iRs@QoIwzhbGK z2rbpd)3{23Nplc47#1Xf^Ef8l%7_+CaV?}tACI{`8UiN{lVvc?^dN1|z)AA58xM#c zkbkbvIyc1 zg>{KVZ#)2r-1bg%3oHuYF^`RUs<`Ob>#xu7Q#4C+U5EYY-;tw@I((Z>C*FLwm!l>N zzLhQys%sl2*G0T30EzaXQ!~A&Ie!ovI7Ys7>KsfqVO_oP0oi~IRz2k9s>im@)bk{j z=$-Mru4RJh|+#Bz>HJu(B%$gtHZLP71+Kkhh3tju(I6;1H6TFpE1cTTxj%yQv zfiS-%vx%{xQmHOAs=xA?OzcxB$LhvV-x*~ll@U}GGDm&ODfI~P)ayz}Yr&QDoVB7~ z?n}+wLo$ec`NpqiKYTvqM9eUe`M!qPTv`BArH(mk0sOyX>^nkwy29Ik7a&=2W&Vx|;=Q>E`MwXeaH;vE#8~3W=!>Tk(kCalqzQYD;(#^H6|IK_ zks3m2y46*#txeLXyH}r_kM^e8PVyPQ@i-(bt0X?XvLf*RZO#3hq-rXapUOc3sOj8y z_Glq@a?*_Jt(8bwrB~5-9B3)yGD}rL!lPhD&N97i3iUjbwrA$kBgYEB=iGTwfLgNTAtd2=$6oJK$T?aYC`!e>6XwB=*?@$X(BkIBlPx8cZVQIhsG?ie=GM@X z&MDm@+gxW2`rRai{g#VwEh0WRzfFnVVtd0<=qc^^?jmt&wW?xOzn}ms+_J7NN(^6GgyNqn*vSFW@;W)34c0{TrylKES7q&%&qQk&G$D@(m|hB4Uz-%# ztTceu#Cg)GoF@;69KpVX+?Up5i}%@#;;QL09c_mIhxN|B)LL=A%A&rsTGVUbvq^^2 zY93Fqx$(yR!3`?3SeDw)JH(n5e729`IK?K*kE_`g#XS&di9Ezd1y%p#5;;Me;*87@4 zzWh@KW3^HMhvVmK9~RIf2{w7P8vv9PAo0h(o!0fX*maZh00CRWEr+*zl3_&|^%Tpjh@$@C@T5<=IO**isTgN_YsWdQn5K^b_{i3RC9W2b?9wD`gDGvm zyS*)v;)|bM9B@DpEbxO^(7XWhjX#i7cMoC62`t7sN6101V92j@Su-hAqUuUjudtv zVy0MMXDlXnw&D({5?#IzuyN=!KuFWF%ADoknl&w3mWwirO206XPaq>_DC% zWI87+ZWe1(;6HERUgs+V!c6kST&N5a#a1ZgdtdcV%p$DeKZv5V^)KD=gE?3<7;d-)GyYE4 z)@IqPLCPsC9l_-rFAUmTf?@I(M;k31cF5C1V-?$RztF?*aS1jZ3=0qL;MpDs{)cRO zf?@c3q9K4d+hMQnq1GO>#E5#kHNUn2T`Sc354?S`bEH1p5*LISafm-d=Nf5b(u5yV(pkO5>LFaz{p~ zb^Ng*aoocl>ja%Kdp&yhOJ+vxnMIIsi_?$@k0D!|GEu`QT)0RD0=heIe6Yjn(m4w@ zgoswpK=6Yj|037~RPTrn**S|JW?Q>~q7;Iy?e9Txx+Z#Bv zT`JbVKE!XOl;_I^G~~^l*yRrcgSRL1B--9k17ktg0A?6jW+1hRM>j1t22?r_>{_(l zA0XMV&@KR;IQlK^ z2*jVoe`zWCU2P6ZMq{{GD4h+j5W}aBA2cNh7?EIUFHbVut3%Uxbxk`ANe7RoZ zC3nkqaXh`j{z*mjuiKS=yXs)QMs#~*BM?XgTOlQ|h8MwDIZDu_ zIa%}#9*-!quAE^05+tZHs7(oT|B0iR?pBfPdflMf!sl{;f_T*85v=~W<+n{aN}?Lb zf^MP-gwu;u7$g$Hv!kRmFHnF>ZmBE5J;UPLSf>}2ACIj`tY#dL=`y-PBzmx&N`Qwv&_%#|5Xzrlhb24*Sz!pOBWex}^ zyU?TnQPURF_Vakl6W>k|tVmqr-1R{;@w(K)eXXY+p@(oua|%GpzeINa^2FuEb&@`4 zbpg76Q_V@0flGK1_>jRVfzR9#4U$~Bb8FAZ%wb&!hgQx30Zk|@ede`@{}~(a&~FqS zdWciaNmDW5v^w$P=Kx0%*EKlNdDCGvCr!M|lnQSqO-|CMb<5TS&5Zbtnq>DD?#jY#$u33^Dm%P;3rV|4RG(m(Iv3EE?c&(m z;B9)z#P|6AMZ4;@{Dv@e-GJ}ziO0Wz{e~Jj%S@e%xn;XJ_BI6cj_>#$U$cKnfpGh_ zRphSlhFiRi&M;8xd%R^EN@Np1B$qkAS%J+TAdi>Yn(CHqi0IylPiR9F%-Iw$1-t@j ze57CFTDBqe2x#)>AqNl=;=&UzIM5jy>4?1PY`0s%l9Cc(p8lIPu&Q-Slmr(f7enCI zcuYs+M`zaGpbA%H!yP55Yb%#9J?8$z?xL&ISyFU!_az?F5&6;CZnuIZC8aCO(d9H?M|pt`c(zMV8>rAEnCT52bFq579ffJph=`y0Pu1jZvrzN({-fq8*W2xO^4 zg7N;NfeI$bz@^jl=(lfYOT_X1f?fzYsygM{rKv+4C2^bou9~I^lDu6A$5m90L9k^P zZkGegnU*Wi2_xPSXprWBD_IL!xq3B)Hv4oT81d4vg$=25u zKXrT^dClVKLkZP7kh#}Q@=B68M?iy@HC7|hn9ik-kCE`)7m9G09@aeqVp#K6IOhi& z*R`A*Q{+%lIQN!Yk_00K1BibS&`SLLgt*A}E%)#5mv9mtmnJPGl^BcU#*S`VLarfn z>`U34L_AsCUTh)y1vAKzVurZPIkO5FlQ}FG1w}Z_Sb^V4#njHH^QJ(oAssyO&O|Kp zAtgx&<_>;-4}J*AJXA|cCrhsaxi~q*%YJCX=+Lr>kldP1n~dZ4FtHOfK@*a5C%C2s z^TVa&M-f|BEGJt|r+{1L=*J3biq_7vyn#`6*xX_bO|RWX%qlgmX=F@L6Kw#CWlM^N zUNFm%C8I;jCKGa#)NF>g`yk;I+;^-H~r1l+;ZrfxCd|3rMQot=E zJpeKj#&2uqnJWMd6^u1xNU(LrblmhyPU?3=Lwpw6G;+W6^AJ?+KQkpX79cMv-R%?a zQ^_RwDI=7Y++3`alM1|u4hLDRQ$iL4a#b+o<3@9|#K{jjT%CAF0OVC@EOV7lc?C$N zbVXAqJ^Gap{KV_uLGtcEuJ4CQeP`o?XG?3A(}Sje$vpoKJOkSjBr$?I+@|z^li=o; zO|(ih9b>x6B`43d73Cq(#oEXbSMAK`Cc?F2qm^~qpZI$L?aqV!=~$L#bC7wS=dwpH zPb)nx9aUfy>&_2mHKmn}|~BK&TBBlDT~FY?R;&Kw3?|Zo=Fl zWf#lFRJ6$B{XWpgRoF|8oioZZdYL=GnVIs0s=c%*FZU%!Tyb-`MmG_z^cPOD(gR|} zZ!Alb92$jG{3!XwCOZZKiR6?%th|fh#&K0^j*IIu64EJhCYKI1$W&&@PyDBwqDq%6 zJsTxlz?D2NG1(@hg2R+ua!hO~i$=PM1Tlp2v_GQAkFA<% z$*<`)VDw68I<57qq#h^kNZ~UFWoT@fdoAr-xLz-~w;kcY&-*ShNx^|p+Ew2faki|w zdjtA4y_!aMJ#_adpy}#p@oP6Y$cLbDpbrR5=->R-6(4+bkV!UuBaxj<3`u%cG1Mr}Jb4)bwP+ zRxT2l?&_TvC*uMW5l8tuYB5QC2!ju)EL-mXwrI@bu-?<#8WqKS~+?*Asg(kE0C_P-4J~zQ8PDJ@6>IH zS13;}hk3RYc$uE=#=`TRH@7V%S)Hh~xqbEZiT79HNj-0G*ueyNR}ONxriO9vclc$W zQ`Z`vzMgM!(hopE{R)t#m48P=0h?u%@b z0`7g!paS3`seR$B*J@CG1zxoKA2T)F!U}}(FZ1XLqKlx*;+X)OHl>gia;gb*89wZg zh_9%BQNSzQmaS4DQIPfHS3MRig#6*f8L3X_GebY47Sc%8JFx+fu<8bT+n zX98^6lmINaAc1zour{{`Y$HxUI*p6UT-YVjZ{DM3D6b%0KOjBoK4@{yxV~?ZD=cTy zdC9TLQ`AQ3FSsE@PdVKsA~+!z9Rb+si3hI4X>_G5wGT@`I*kj&{HA%BC+46avgd&G z*n_tLC0AI6EWCO%CBw8%H)ul;ojc+jTU4UW`+#5DIb;nn$Up(V)n#!okT;g9lc=bH zfF=hu01z(hY5Qv{(xYUaY^7z#NiHN}Rt@Ok7vz;_;?=nLEo=~Y0eJ@omEeg089gH; zKq!N&gfJSm_Eng&Ql(1+3vhE^&5)O949f66_flEpm`r%kU!)L)K#M|B_(0Bn0auR% zeLWr7B?Q%1co%paZ9JkNkst~~b`bpV5Vp7*UZkvalibNLiVHv>7d{8z!=@r<=-wZL zf|RMyq?bBqqmq_Jw#=+Jq3{Ws3Vd)$&|QCUl)MsxUP1|F{%k{jf@oiR=smgk2GMS~ z0&UhVFbA6A69Bq-e#L?GC>fJYt(0?wry^icND7~zS=K6_sOPkLy@{Nh;qKE7`3a)k z)vWPVNpvM^vv$!qP`iLG)F&KBkFp%Hsg;7R>pt8UZv>}{MET*P>uNDbO)cSO-bi2? z7ZEz2f58vaz!rBC3(Pwa%R&&w(LdD$M(6mF^bn{tjefD=N z0jud!8tXww?cil|97@UqC(0UJL-)>9$~!<+a7`#)g*6=n@LxPaK?L$>>nP>L3=jVf z>vSNXCRdMMHxF2}G*ys}()i7225qZT#s3Z`6u1e#ed3$h)J7!#j)`}4)YNZttdbzH z+PP^&d^J$flVxdXwu^{XN1V!0MTC;agS6ns{gnC!x8rPc0>u6mEqVosTqE3*Sa>8Z zsESsEvx(8mhpnW;-~ntfY#``16$U8}xNp^WAnzVmVo%p}5AqbsD@atClUP+3Fwiab zGYxYs4EJrdYH7BMh*rn)HljYot>CH(hn=?SJDhzrfY`r6dYYBh!tRJCv9##TiNEVR z-jmY^8Ov$ca{I*JsY1K7S*uF7S@7HA=n}tT;%~2aHNoKhBzad8-w+SA^a=_U+>qTM z&nQ4wa&qDwKwypw@%x2aW2JONrPEo0-Gx@Lq@2YY zW@QoZqd!u-7}y5ka3SJyYmBBNf}=A`Oh-#@8?+%NyU8gP*-!&%SX;R&(_?PpYruB? zeg%Bzdo0uK=zI2%=Ikl|T(m_M3M4^&@4MYEqTg~5s(1+-lC(_-{eoMg24`YW*Bvtq zFCk?o%4de`oI)pNr#Z-S5nw0$B-XwjOcIz6OM?0m4)t`NJVrX^@1c+L(b3bJ(?3|s zEx-ggI_=xE5yGRe0;L9g0w*S3OO`k{C2x_?QDxO-u`K)JEk;okjtizl+4DE}XKK z{w-VS%}@4qj-w8)iT*qtk1?2bvwNswGw3WFN!mt0zu;D@0o{_u?w}2>!GZzfd9S}?(@5kL2GSUfY{e}_f zP_3HBAzBeF8>66fV-aD{?-rPN{q)2`8C<-T^&%Ka+9rX1!7Xgdkj$-gxc9_e!1VNY z5pE?cnrVS_)1zag&p)icpS6ZLKs-WVQUMZeZ9+;no?s$`hwIjquWP6k%5lwsr*wzXZIx_Hixl5;JF76T#9DF3 z>X4RI2_wZ;bY)2m%@q&$oU%laC`xjVTZSoFpwLI0{e82 z$T9%O1}6SxfD+ctfzsL=Z%mcma+1)!(T-2Pg32l+D zHEOzvsmI#{wFHB8_1P>BEaX~Z%Yp}pV-&{V30r9TfdCrkhz<8CTrHMu?~?QCH&V=v z5TaE+gsP-`4va_mPtsdI@p!Vh4)q8I{}G(nV9^)4QqZ9@AoyJFyl+1JbJ_P~+Dq9h z5FcuKaJ!stcS@H?x%cTj2}50&D+wM#K*qvtvlAiau_MOD_XPTKXLD8x^Rhk2?a^c_ zx-?=p*AVZxdu!ypnnK7(#5G>C!yF5fHBeHbRIMI;n5xv+TXR z4|01n*-c7rufg9J|K)ZQ-#OTDiQ)C(3!>5&oZh8a(3y0b)BWzK!0+xXgzxgv=0{iC zYw%5Yofe){d^|ewkh8DGdnev;hi<;FrJ$;#rTR$ix4=BUr9c{Y_x|L>Yefw7wB8T; z`9Zgde^78swMl`OPyD7T_rs8G`>}T*zV@SXM{$O+pa9+6oyB1`7@@K4hCgv^ za*LFDXo-2-?+Z0FS|pp4MJwH`zlF+4x%S=jfAbEHj$eU~Q7#Z3*n^6zeC$FIDSF@$ zi=eaQFa8npKDUCUM2@FVx{@Ea9w~K04OESCg+=4y;l?SI-k=SyP?>eF+lCtYU3FEa z$NI!4q;go;EfI`F(;=XUbXu(Tx)C|dUS#wKp*&cL_IS9LoDxl2LyNd*!~9)sQu!iG zG9n5tBzH{e%98A3Xj*MtR{wC|t)zsug$vSU$F#d)pTzr-1}{mHZijnczRcQ*$DD2E4i2Af z&%M?_Ps03>3z77dN*F)$H>3YZIaSVmxI-R4(%>a^s8bsz9>Lm_(j0egQgU1VBxPp_5Q^#z(m*!SplDeuv?aSdk zn{BD6Q@YN!%7J0etyPCz{clzv`E(6T9L0^27vOtqL3a_vhrgXDPi zKXsBOVi7Z0`!SgE!JK_o74(eWfuTAycT5Eau{2;9QR!z7Y@{A-G+i0t&I!iX^wJo^O@4R{ozbr%k*P;0kKzW-JTdF4=0J( z>*Uwr?ak?*U&&x8nSV#%4l<8fT~p2nlWIv5Ica+aJ?ZAg81`X?j7X>tkKYG53I2=LQ;*OW8xyf@n* zI*}U;?7BCQ(G)F6s+|4J*KARL_3!1z}HMkI|8>()g|JCX!qMgpdM6EnOtQz%kU0~t*rPV-SMWxzAU zTkI;~?3nn3hBR@JEM9cH<^p%-$Ce0_7L07mExE^caLvxv z4^fFk%LoPoyJe}A#D_tM1~4elyW4lLnJ0x(XkCC{VtYk6@SA@fsl4X%=9xOPk`qBX4;fnM!v=&bm}*mB`P z00>@6`{O4nO}&$fuoG*(0#bSbq?W$U4Aa&^;oJ#u(ZKI3G?^ifYNogtk<~Tr#b?Kc z=V{@QU(1Wz1|qz&qL>bYL-yC0RB}aQ8`N3xDZ#`u^y#2s)&|xFlUk~Z;ejXjFH*|R-O6WIC&_#@M5{6k`;h(&Cux7^wAr$ngCoM-SAAcxcAbFjHQj8xJZ1m<9xXnEk_^T zixm%`WGxwpRJqpRy^1P;eNO7egT1AUx_m&XUb8^wGU^9tz+JLL%kf z{Ws3X9=@uZ9w^*2J|b-4=s@r}P00dxDi9jnL(IIOiCb<&B{5yQ<%!4Xa+sBD{uqsI zH~jqBb<70 zaVcIJfgO4&zmv4~+}TGe0IEACY?UqEr(F~8KR4zJBRZ>vy(oU|i^z+a{=KNESt`)s z+2B0RCO%iBzo!w~qu%028GKf}_I?Mk`l6zT3-G`7LREM;xJYUYgDmD9qg0e*<&#K>CjgJUlIrNP<`+$n}kVqF|t|p4!BeF+XsEgP^Dt3?>W(`W8I*I7$;ZHDO;4y@xcuVXD1$h(2kPkgNIndVBCaz>e?3GJU;;XmnmdazzI49z^RO^E9v0a5ovQF)*)r} zf{idj-zplUMK{eNC(2?%k=;_g68L1{`4W8G%Z(2}_b>f4RS9pTr-H_YsJxe)oJ#QQ ztHsg;e(XS40-%2xLQX)az~d7ye`fEUc&g6V>vyT{%ey~kM!%FDFHxEeK*@M613p_& z=+veJZf%!VmZ~jO0Om$9dAMA1?zMT(s8*4&hN{Pqp#_&3H`Ek=5qp!2Nr#YAT@>9- zltONO2NprAMjm1voM12bC(*<2l{GJKg9HEuc^UhbdcdGf;10_R#&VhPS@xXb$y(Jt{7t7 z#zbM-0iIj*XyXlA8+nP_FzS;5Kr^;Z*PJKWU2^#*`ax(tupuocy{!f3nE;y(rAxK| zcsVLJYkfe@U9rl{BPU9Dq|?Mkug=Ga(>gwz5lv%*h>H`gm-EYpxl>Yh_`YSJ8F;PT zwA*XrRBOpl^_s)jx$l4amq+5cd`WWmuul@p)_9tnMt}(rB~XqeMUJf|;T=6850RFv z6u|swNoHI%jp$V@YBkl)uK=EU`ggUoc0je|u-WEn!E#{XD7Qu5Oh6Bp=(QqRop!cX zDB4&HQvamDRw~5G7n&NJ{w`m5=JAR><0a8nAn07Qa%N~yTy_Rq&)fefF4^$RTfxJ< z_yBmIl@`g=fun%1a$)%q>}%&&=R{7%xcbu03o;f#u`??#a!QCY%>dmW13Ww0U2n~FPYfeXNA9_zsX5MFZY@LtQV3(O?g!>pTe$YHHcUL3B4{Ahbp z0-r1W6F60lfNI5N#|i~fuNAHp7uEvTKYZriDUtGXBoEK`+dk+SFMs{M^}vC0a^XWL z5p7ROkjRbYWM42sA$HVN!1ucA)O27ia7g%U#;hBcX(tNovM&me5MT7}D}t(CIq^yQ zA;H*n8;UdMZ3?g!;JE+L?4)bwe=FXm$y+&8kGb!77aXv0VK*Ww;)GDP zTIp6O&AN*6<3j8Mi_k3*4i_RWw<@dI>$X9gFVvuDsw*=-9-DYK2q|Sw(4FH&z(qPD z=Lt8v5hUNp3Z=$HcjBFAd=!b?c|2o_bVT0VEn#6m#Vs6DB)QRTBP*@QkIpa`9X;W; z;f@l`>dMs=A9E951+E%gI_N>w;k5?O{H90h!A72k74V+jLo`{MhX#-` zK9q1H zy`JV0AC^XtbP2i^%u*IE!B!l9`=BKV|6+Fy!e_2X6C-!Nb>CvrvRz1lBb@kK2{#j7 zE>3Wc_N$5SFe^VS*XSl>D!uogJd~fMd8$yBvXISjxa()nrwU~$3z@tvwz{x= z>;Pq8LoJnver!?tk+HNreBv4iJrwSfnxzK_@$MC$c_9dw8?JkFYyUDqZrAXqb`Rxe zX@m~lQY=eZ;0IfAeDA@1Nt>m{tLz5^A3~vM3p3!mBy@^-2^iasAWa9&7Zj&7_v9>b zojbXGuz>-`T`JnU#(&@}zhI_UPP}6mXe(*THR?|(jdYLnI0K$6Wf2Aa6?$7Z?mWou zXW+Lb0bWO;Bq-I1?gs=PLRPc^&Dsq|F9BiW5zy`6`GP!6sb)r#>M!xY7UGke2MqkkAY>D)N9`V+K8XY?KHkhs8lF7 zWQ|IsjR2QSkrYB+UCB#Gb#-0YL7I7h$P)VRc*0ZS8~nO?5=6gS9gD>;()a+8jAw*IeWR>oi)3^@&Htaqim^DBbWkg*KYnEq*BV(9&mD+wTiCVBfS0WzkAE zb>kwvc0>FhQQ@I4E%fzT&xy4(GTKW#ZO|Lq`NmX9;!} zTEUW%QVO0v%|7{Y>#x>8)hJh3G%iRk2Enbrs?8T_U@fUEU6tuEH}MtRzi@r83pI4dmpgQS{t(=+dYD+$%H6Q7me@-wv!ga{8 z5Y-Z|gQg}GV&Can0x*!W2e&?4?om-NcdP8L*#F)~Z0SA0v)Mm znD@69RHw*WC|>Xb5aGOcGf(b|n@LRXE~5`ckN^W<+r{r+4DTzOsv zA>FTD`L0?-?x@?l68crBpswYpLYNZ?wTTYHe$$fb6j?d5cz)+0-KY+^Q4(u6Vl-Ru zzm^bo0ut3M7#m0Z{v5@_{0x8HLl?J@bxGdG)gUeivtovC_@mh0Z^EJ z^kDr)Ocb8_FcDgYEZv9_PI6Kp;46jV zP3{zw{5o!*HK+RBd-)x&soH&P;#1>qKBj;va8QBuiRTER5dY~>f1t7LhMzy1+#;nO zT4LVz`$7%%4pl75qLpse-$Lf3T>I|1|G*d{JAMUzIB-$vfjy|W%EvAmk)j74u?RX# z{^EZczt62;DUsvpldj~)tw&1TPy$ahWepg+U=}}I6 zdFe|)7ab1Md+Fj&(5=kE1DIPY`Cd07cT){Sm(@y#ZiNr#5SGpo&u~%r_yhEdZh$Cy zjveN>m1uPPi~SWfVTz;dYlfkFr-SrYdxE0tLE%B5^|FbCYI1r}b}> zn6of$v}QwZi%a9~xfSe+gWG7V0f!>iFy^9lE$w(ebgaNOl0X0E7Gx{}NCiOcMN29--$MqdQzD6K*V zX}jeGulET-yoAf!tx6@(f|KW@AOUSu_$@H0go^ZZHv*<2&INU$%+s=!-gu&v4*hG3 z@$@bt)~0BQcYg8#S0(;HHJcYOy0piIo8>bWN4$i~+pTIW*SNRx+J+(ocz>U~Q84W} z60vnRB7B$yYVhh`!uG<=^AC^9ac`rPt_Kr`!|@WR$-#-PdzT&fsc`Fp8KVh~$4Tqp zDOu<&6f#l{#ikWRcWcx0cDPtATcvcH--(x?eYdL3WC}Q7SDY3Hp@`x*2Bb}7 zjktT_H`NWr#JSgL7PSO|e#%ABqq%P<4Xwwj-3RN1^J|$ zGAc2a?PI4pw5As`LKRR`$-+&5Fa&}h;Q4a~Tyyg-rH!9lL24h|494N%lLP72lujtN zLlrU*fG**)Uc>BAN=@yL)Iue+Db=pZB1uZG2AT+IU_;gZrCEM6Co{ItoPxp-G^n*F zo<0y-5abm;8ES20%v=rlY(q;{5`#e|$EL>7i2zd6(z}COiqC?F*`btMX=JPhw^&3o zu69+LL4Bjw0eYer1o`Si8ax05STvm2>&!1a7`=UBMMWN;Za%nqg8$Dw33c!EVVhf9Xw?XWE%~>EnPp0~fECh1@+Qd|ZylHi)534SxPb z8*t%6>4ZVkAR@p8w%9ng;!HXSaFS)}4B6?jyByvz{hLZ@L3C{BhsGcI^Hh-K##I zo)zRVQ7G$7PKwsfWR`^kWG24VqB}pWD~I2=#(MTa8WL0BP zSNdks2B9D4JjI#=AZoeLYI1(!9rtN9%ew6(Q$TwWvar?dT_cItf0f`s{NagbKXwg} zuDS*VtLFO+Y1*eg{_dw|8D6G1%ZD~u$i zq~umA#r^k^Q=$>Mb`6}UYK{5Zxaju7+I*pgeotMM=~4EoHBEi!KkJbMHTnMR6?k#s zcst!kcyU9EgKi-KuIu(~2QORt_w7o5r@DC0e}{07sVk`GBHMRPJYvbIe*QGo*DEAV z`(#Uc@Ma%XAT@4wNJjGof0uPGP!%P7L962YuYc*W{2iZw4cHyh|8muySF3RD!!1O4;8w1uuSXi1OBjcEdX4p6 zD=BE4BJTMWWPbjDRBx8zD#vH;m_zjMUXH-`IU2X|^|c!i#m%^OY_6_oxhyn3mw?I! zh=iA$Cq5(4FJSxN^iyUW^DT)LEixjy>o!)GPL$JY`F6;{sK{FTNQaFQbyWmZ2 zJg!hg!EFlsg+ZWZ(6nI3v}n4aI0eGQ0fGr#sKYU;nUlkVJChyfTX4${C-y{NxJN@J zIo9}?AX0|-?CD==y_`Ve%SpaLYJ)Xha0(DXGZ)}eID>3scn*#2)Ju;m1Qy)XfG>9h zHG`%FQ>I0$ezFO|6`;KXLhE^QJ4EpVTn9Yd-n}6D!adUH;{ej{A%wMswa=dZbpToC zE1p_zO~1nE4upK}%xfaeCHL++aA#Zx4!Ffd!3`}xzGxK&woqT!EjVltqm`)+U%#Sc znM1DU6fKlQxPa1i&QTmdns|57<0E&P+9xn^s;-D6+Vz}vO|W1DXPDN?Hwu>5VT8FT zy|fYpm)x?m@NtqIx~(#48Tj@SxP<-%7FV{c?=82z>Y&mBp({PUV^sOETa(2B;xlvF zws&FV-)(`QZ2<-04+;AeaY_ z0$K>d;`=m6v|2u*DR>M{ynt}$(X}|P5YI{l_T}Pn<7?roVH`vsL4}nuOd6=#zuaAZ z`tb1YC8MPihlkH4pjI&HDKt`55h1klC~Z)1OpC?$X?X5!FJdY91tU-#NUAJp*{xrk zPth!JnF9xKnfUhZH6-&d?W{_-J#4+$cT7AsaSu%q1#|ckM33GfA@n>Ic1Hnu3;_3s zB7(_nV&MZx73FMkVeFcCNWJOxb4KMTWA$^lcuW?bOukBEIJq|#ksCJdF~>_L9{3E` zr;74Ua+~-}`3sZ&4fD7o0BTcRtDOhw!*jgp|U)e{$kkQM!RZ zL2eX%2wFe!L$JG@{RL7ylx_;7J6uk)OJ6lADJKQM3;vRc2R@Uv2w~$NV^4@K5Un4D z4%O{1u_%>Rx*aO&G`lA*-?oWA)T2&XS6;^>H$NeIp7RTP<*@>QsM67rTbz6ejR-p{Jt$p4csIEs0p)glaw{E? z+ogs)fVmAXF&Qo>6$5itRJf5Iy&TA1H_V=JujImxr!W0{4dHY*Ic2FEYG74YmQZDQ zv59AQIrW~)MrU8X5xioz#-edSN6eL5<1rnPAD!)%aKp6RM^358h8jrf+R9a#9&;04 z1GaNhz!Z3)0%_dcyG{HH`(u<$fxQaY#P8M9&`g1MD_|4!i&J@_IfKB{fJq^tic((#J@$c@Bu`>nsDqs`8S5HGT1>UWIP5it2W9&?U zy$aaG@72@LOo4YRU=#oD{unz`V6Or;@q6_&G*jT+3fRQIyFbRx6xgePP5fRx4b2pI zw*oft@9vMWGX?f4U=zPrPeU^W-mQR5{JZ;O>`Z~Z3fRQ&)zi>Sfp;rl6aVi17&}v7 zuL3sld-XIlQ{deS*u=lPKgP}!*sFj|{9Zi`%@laI0ygpQ?vJrE1@tQfKB|n`(x}(fxQaY#P8M9&`g1M zD_|4!i&J@_IfKB{fJ-4AjW456gkSP!qu!(=^ zRrt^G^8f5%3T#!tCVs1&`@n4C_i1TJOaYsCCdQkV|NV2Vc@rVy@(l%S;$PZFXv|Bw z8Lb~yz$X5!&GVgkD>viyy9(IEzqEJIn3r-hT0g9SP5e8X7yI%~avxyj^!-2;GbPx> z&!~2P3|z=Rdn+H%_s{;PPn-DrZ`Vu)`;MPcZ6wCN9?flR23JA5p+2{v(aFGE4!L0ygm~nas%)_=o~F@iUEds?CI1j45zj z0h{=l7TCPcsAe&5P@q5Y;PD2kFXCYTUE@W~Fe*a|C??+F*>~DiRHxeYz;>py@z>g^K?un1=end;7xkCZ#`3@E97;m3=$J~BruD9=l7HGHi z{IW;mcI=g*|nX_`Y8ph=l8ZV>g(U$_!oJgcdr75{xSuu=eITlYk z-|6K?D%m;l_I*)YikDA(!PgE@{fuXL$Yh^W8ey08sD0AmZanrsScKYgb}8Iwf}#|ND1n zsQOs>P|Rs(oA_NFzyX{5`rpjQWP5J9F>PWWBcc4IHkF8atc#hB&`tvWY+0E1URJ>L0!L zr;3KWM=O6r@;9h|l!H3CeaEY%db8sjhAwh=Sf(>suSLFr!{TYW74s0L?}AFcdM$=6$UhQpbxvGSumzpAg^ z1j_!%O+keRmU++{E`24 z`5W$By#5`TcAJk@?tXvszrr2mFe<1`{HS6zf17x?OEr1!IzKvXHu3JSR-x_j?XEVr zanB?_TKUoBu^UHaZ{LW!tZ$Hgw6?$*Uc(P3qX3!znYKp~Z)m_70jzRRAiIpWZ_hM) zt*pQ~$OS(jt-+oBmFDOV)={`KKV_yLFOS^&#VfNGjy-oUJ1+1Ln={QGqF6ZF#21pR z9h|uYHu1h|Ht~wP`^kwv`Av6d;zP-yn~Ju7$0h20NOrH|^`{tDT{r60Yn zXZ%CA;C0KNAkrU=eKk8iL55UGKjDRPhC;VFM-JQYbU>+cFS^OD(` zBZtX$%!aYbpa1caCI6$c|LKW`_W$vrd;Z@4-Jwh0@!{Uheqv6>y+QU->Vb2RYsb%` z=_fqTV+Y=7J&^ZI_ru@GqX09!7h;m^M=FoJ&s2Ui`O%UmuZvCmq;N%;>%RR3U=zQqBR26~N&Al9b)W9S**@_AXn(7}t6vq|(8j+gdlV!J zP?6`Z<1<;;stKIob?4{v87X4cGs%x$Jxd<0@Y*m8M<2au!)!x9;2h-I_!HR~DdHbe zj{ZF*{%}9pj>WJuvPaTqWe-<)QNPjfqa}}$O>GL7#J_&vJKh%+>wsoVin0=B6ThO~ zbZofB)8UhWHt{Ij(FPpt!(`|4=s+~w8JLZT#BUbdCVsP^IyjqnolfqjFna9#iR@4VAg%p3DsA)k`km={ ztl>=ZGg;T#7C6J}mWhuVoRPwkheobliP`8gJ%7W*AFX_}U4d-4C_8?SPrphB#8Ebj z{wcYK*-^|8omV73B72l63NR$vk2@dj`J{F>@sq+8VMn{4lH0@=eYx`>$?kK&`p#sV zux;Wu2^l(m>VFK7uAWW&u6C+$lI-5uCf;2>m1-NEjyB*G$!+2{UZ+0P(Vq9|i5~VH zFUm4;c(nT|xlQ~;=jKC_>>dHu^H7gPZlCyY=K}0wRm4Owt0jM0c*u7~_G{}OF8t_) zo2)f(hS!~+QfIpVh6eoM%Ki7;(In|XV6wW4o&kY(!v56noY+uBg{~MyzbNh~0b~1c62DwgsEh?$@-Qov* zeCzw);c+JUnXFg+Jcs9hw2GA<72s&{YZLmrKo(wicI%7=XR=;JKEokqJ(HXSaQSKT z=;}=6FOhvn>A)FYw@$n#@p$*+Pk2KD>~=oGA!a?3e7A%%c!wt5Q;k2^fc7)gAC9q! z-z@l;C0L2*57dT z{$Te__Hc!@*M5`V8)P3XC2$UM!4IUP0GWUMp*Bj986Vmn`oWrsxI@VyvEu*U?M&s7 z>6zqbvZj(_&%@hL$BsFN(PWo(kx6VofL+9z4L@@UV#$Y+yZ@Knr~kw7smk3ofz-3t zWg#y0Jj~Am(1W3Yb6~tfx7hY5#e;>gREMq}6LE)V2m2NbgMYGg(u~vFG8f z-=;TsgsT#XZy*#K5MVdp%|M*F1hM2p$?ZFS=BBL1q@IWQSw(v$K5!0<;j%8KwTbrv z?oe{55%zohkX*Z*ZQ|Wsj~^c4ZI`lRIFZ9x(6aZ@%8w?G(jAq3xqVXrZ;*YIc;F1L zJHMIEbbqw?t^R+oZ|IGzCxrn|PGM z7N)=-P{1bs4>Z|IGzCxrn|PGM7N)=-P{1bs4>Z|IGzCxrn|PGM7N)=-P{1bs4>Z|I zGzCxrn|PGM7N)=-P{1bs4>Z|IGzCxrn|PGM7N)=-P{1bs4>Z|IGzCxrn|PGM7N)=- zP{1bs4>Z|IGzCxrHu39AM5V<#s1|{8;z}F3?uRL9u@0(5pq#kUhOYZzN?NRgY7r!5~U3YY?>fGJ=Km;$DNDPRhi0;Yf|U<#N5rhqA63YY?aS%LonrkBSE literal 0 HcmV?d00001 diff --git a/easyfleet_easynav_deployment/maps/warehouse_a.yaml b/easyfleet_easynav_deployment/maps/warehouse_a.yaml new file mode 100644 index 0000000..1f6e391 --- /dev/null +++ b/easyfleet_easynav_deployment/maps/warehouse_a.yaml @@ -0,0 +1,10 @@ +# Generated by sdf_to_costmap.py from large_warehouse_world.sdf: +# visual geometry between z = 0.33 and 0.43 m, world (100.0, 300.0) at +# the origin of this map frame. +image: warehouse_a.pgm +mode: trinary +resolution: 0.2 +origin: [-101.0, -101.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/easyfleet_easynav_deployment/maps/warehouse_a_collision.pgm b/easyfleet_easynav_deployment/maps/warehouse_a_collision.pgm new file mode 100644 index 0000000000000000000000000000000000000000..1aa807e6f6054b7b981d9010edd7509be0e8d26b GIT binary patch literal 1020117 zcmeFVQIaJ)lbhMkTE#3u8Z+%<_P-d}WZkL)9r}Wv_m8BG$2mej^o0W;g9Lc8+WPKoUp-NgxR% zfh3Rwl0XvpzcGRT#sB%gF^1eZ3H)D@0DtiRYxw3DCkmAWlE5b=V1Hvi>0-G{5=a6U z3FH@l5lW~e@UaQx7yq%<=3+_UB7ywkFG2~G1U@!_{Ng{h+FUFNTqKZR{6#3ClEB9% zkYD`AR-20@fr|w4i@yjZR1)~u1oDgj*lKgJByf>He(@Kfgh~P*n?Qc?A6soMmIN*m z$S?jPlu$|FV-v_P{$s1n#gf290{O*Xgc2$Vd~5>w#eZzIxmXgoNFcxXi%>!(fsaif zzxa=>HWy0*7YXDSe-TQkB=E5b|OEf&Ah>w%S}Q30x$Q zU;ITVp_0JICXiqJ$5xw*C4q|s@{7L+B~%jl*aY&6|JZ7Cu_SPjKz{KTp@d2TADcja z@gG}lE|vr?638$9B9u@`;A0cWFaBez&BcQ3FH_5vDM~cN#G)Z{NgV{36%ssHi7)& zKepOjED2mBkYD^oD4~+T$0m?p{KrFfBVas^?I-kG{7sxz=$m`0TV4d(X$W;?01HgjFk(x;U$OsF0pkBfsGMKm((w&x8)(2HDF#Mtyr!K= z!^q+O*Z37MN`QN`YYs-s3I}M?(3}At&8b)mC z7qM4D<@^Fw__x?L(16ua42;-#O*@l@k;DDVkKVom_WzDh90st;R1G7x+u7*Y2Hxuq zI4y|&afB)4Km*KD42;4 z1I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#Ojo zU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGG zH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z z4K%fB)4Km*KD42;41I$tkjM(xb z5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&d zVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU z7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDL zih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D z*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQM zpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O z=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X z8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=% zQVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5 zz%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B! zMeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s> zLs(xLut17|5nK92?3L&Xs?H6z4K%fB)4 zKm*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMy zu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fy zwhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17| z5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}1 z7#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZx zEiVGGH-z=20SlxU7_p^a#BM}w$b^46)b$Hv@scVUvE@Y|_J*i+EsOPA`p8+)VdbNf+bZnVoSe>-H5gzb6yX1 z{lf4rsiF~EUIb!qh+5aeSg@puMr`R9u^Z7AWX|iMu3s45B~>(H%Zotl4N>b_7z>tE z(TFYmB6cI%g3Nh6)b$I)yQGRnY_)T&ne%$6>lcQ1NfnLQ@*)s>L)5w!#)2hPG-6A?h~0>`Aah<1 zb^XHdE~%msTV4cWZ-`pg!dS4Ribibd7qJ`B7G%!rp{`#T-X&EuV#|v_>eJ5Vfv_v0zCRjo8vJVmG2K$eh4st=@+pZ(H3OR>!Ge+7~UmSG-At(KslBK zmQ>M*E&U>PBie$@c|Fwi3&XplibiaC5s1AZYF!Is!ICN(v87+cZbVyLFT+3>iUJ@T~b9Ow!8?$-Vn8}g|T2s6^+=^FJd>MEy$eLLtVcxyi2NR#FiI< z*c+nOwJ;VesiF~E`bF$Uv;~>-dZ_CchIdI7jo9)c5PL(^x)#QQB~>(HOTUQSh_)be zUJrHs!tgGsq7hqO1Y&Q9TGzr@u%wDcZ0Q%VSEBaAsH$^=Z37K3OEEBF<2CI}8b%KH zFF$(w4%q)YLU9;HqZdG6ayo+ zya>eJ5Z0FlERbSg#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4Vd zIycxh&;YX(10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0Fl zERbSg#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX( z10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg#Fl;$ zdnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX(10%M)2*lnH z)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNjUs&j*F z0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX(10%M)2*lnH)|Un>kYZrO zmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNjUs&j*F0}U`sF)(7w zi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX(10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@ zbAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=9 z0x1SYZ0Q%VSE4VdIycxh&;YX(10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbF zFk;J#K;HqZdG z6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%V zSE4VdIycxh&;YX(10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ z5Z0FlERbSg#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh z&;YX(10%M)2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg z#Fl;$dnNjUs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX(10%M) z2*lnH)|Un>kYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#K;HqZdG6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNjU zs&j*F0}U`sF)(7wi$LrRVSQ=90x1SYZ0Q%VSE4VdIycxh&;YX(10%M)2*lnH)|Un> zkYZrOmVOa?CHjJ@bAxRI4KPbFFk;J#KfB)4Km*KD42;41I$tkjM(xb{>1*L3Y1`k^`*fqzXmA=Mr`R9u~(ums5&>;HqZdG6ayo+ zyof)s|EU5c7-4;BFw3t&ih&VZ`bF%O=nJaO4Ymz5z%0eUh%GPTPwaoHKnX@zUmDEv zYmj1K#Fl;$dnNjUs&j*F0}U`sF)(7wi}(}!pDIv-5!ROmv-}#Q7#OjoU&LOCzM$&d zVB0_g%u)=D*zzL&#QvuWlwgGQrNJz}1}O$cZ0Q%VSE4VdIycxh&;YX(10%M)h`+J_ z)5YIm^Iz*r1I$tkjM&mIVy{GBP<3vwZJ+^WDF#Mtc@ckO|EG(;!{)!%mj;-n7#Ojo zU&LOCzM$&dVB0_g%u)=D*zzL&#{N$ie}~O~tuGBQOEEBFOTUP{5`96{xxu!92AHK7 z7_sF={EhveF8&Ui|5{%fV3uNF#Fl;$dnNjUs&j*F0}U`sF)(7wi})M+KVAGCHvhH0 zG{7vyz=$pVBKAu31y$z;+Xfn7mSSMUmKX6i_J6wgJ8b@IeQAJMih&VZ`bF%O=nJaO z4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$ zfe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP z@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eU zh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR6 z3#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xL zut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD z42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVE zr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7E zEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92 z?3L&Xs?H6z4K%fB)4Km*KD42;4 z1I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#Ojo zU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGG zH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z z4K%fB)4Km*KD42;41I$tkjM(xb z5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&d zVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU z7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDL zih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D z*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQM zpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O z=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X z8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=% zQVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5 zz%0eUh%GMyu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B! zMeLR63#!fywhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s> zLs(xLut17|5nK92?3L&Xs?H6z4K%fB)4 zKm*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMy zu{VVEr2z}17#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fy zwhc7EEXBZxEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17| z5nK92?3L&Xs?H6z4K%fB)4Km*KD42;41I$tkjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}1 z7#OjoU&LOCzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZx zEiVGGH-z=20SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&X zs?H6z4K%fB)4Km*KD42;41I$tk zjM(xb5PL&dUmCDLih&VZ`bF%O=nJaO4Ymz5z%0eUh%GMyu{VVEr2z}17#OjoU&LOC zzM$&dVB0_g%u)=D*zzI}dqY@X8n8f$fe~B!MeLR63#!fywhc7EEXBZxEiVGGH-z=2 z0SlxU7_p^a#9oQMpz7RU+du=%QVfjP@*)s>Ls(xLut17|5nK92?3L&Xs?H6z4K%jas z{V2pI63Xuj;CxkF_^fT_s}H#YjsarDepL|cBy0l>@JTT+Vtd~gfw-PX)PixoDlUB1 z_P+hDL~h>!QGzHRx%iU|I|{kWBPQo_O0G|{CBewT_5s2%FL@gNStK!0EZSULfO62w(5G9E6k&8dcu#>P2G{7gt zz=-X3ns#&zFT4Xj^+ZC!d;y%ViVL5$&3yGCcfc_~tk|y#f}MnIpaDK921ab}`yvq6 z6Ny?d&R4~S&)VL%-<8PiJ0MCBP6&F5hd*6OnBDe2=C_$8uT>MFforG?CXh4e&`ZFk*Y(7lF8*NYsLH zzA7$!*7m;ru0(F%0a1b|AG!FG3_A(iKm&YI42;-rr)fvm@WMObQ%@um%oo7bvuHl7uz^9%_D3~vR^Hp)-v$mP9KI9HK28b2=RY9s?R{Sa;(8)c3XxbRuq`}VsMxqSyj38H-D;!iT{By0l>@JTT+V!NHD9bLl< z?|@G|kx(#S0OzaX!e?zWUwz0Oa10PD_N#(mCt(|CfKQ5n5!?H|2*mY7q85zvRdM07 zw)gFKC35=?h!RBk$i<&z*h$z18sL*+V8nJiO*^`V7v2G%dLp4X1@B6 zJKz`~R_s>=!A`<9&;Xwl10%NgeG!Q3i9{_J=d0qvXKnA>?@HwM9S|jm@{x-_$*_~K z4K%A$PzrK&;rW3WA-4ZJ+@@DF#Mt z@B1PU*At0aFwR%Sh0ogFx8Ie>?K>b!5alBmf0AJ*VH;?GPl|yN+wC;%=o(&l2Yl*@ zgo613IA0YPK5Lu#>O<~;V}Mw(Uljy93EMyed{PXI*xvU=Ag(78wP2jDiVL5$y>GuO zk=u7blpxASF8(CLPQo_O0G|{CBevUV+R-(<@DBLY6A1&c;OxJsV5Q&<_q9_Rb2S2ZRV>FxdV;?V#R({5bPvu0}b#=F)(6#-xq<~GT==Z*efwRB+`a>% z1W`V6@h2H}61IT`_@o#ZvE5G7j;`T_cfhBfNGO;ufb&&x;j^}xuRi1sI0lFn`&B`( zldugmz$eAPi0yq}1mb!kQ47ZTs<`l3+xzyr61jZ`L?CXh4e&`ZFk-u% zrX5|w3-5qWJ&{l_UjXN;;=*TbGhcnk9dHZ~EB32`U?*W4Xn;?Off3vLz6iwiM4}dq z^Hp)-v$pr`cO`QB4u}#&`N+keWY|gA1{&a#VqnB}J54*fh8NxepL!ypV7>s(SH*?T z+Gf7`kUQWQAXe;G1;I|jHqZc{6ayo+_k9tF>xo1y80V|v!e?#o+wV%`_8kxs?RJ`WbPX@O13vXcLcx3ioUe)tpS8_=^&xk_F+i-?uL^>lgl(V! zJ}Cx9Z14Lb5Z4olS}@L6#f8t>-nZYC$n85IN)Y8E7k`prCt(|CfKQ5n5!>xF?dTd_ zcn5syiG+gr0ytk47d~s7`RYUNfMbAIv0oJgI|yzmbA)DsB>^969eDlUB1HuKeo z+yTb`v0}d}2zC;-fd=@b7#Oj=?~6cOPb6x=IA0YPK5Kj5epe#5?|>*ll#g8eNrs(- zZJ+@@DF#Mtx6`zvYk1)u@Tn&f3g!#od{tcdtZnA254i)50b<2|RS@hXYy%DONii^D zd*2sf+!!k_>&Af3EMyed{PXI*lwq3N7wMeJK$4K zBoxdS!1=1U@LAi;S08c*90SCP{i-0?N!SJ&;FDrt#P+@~0&zW&s0HJERb2S2?S1=Q ziQK*eq6ASsa`7h_b`rLM2Kb~H7_r?>(~hp;g?GTGo=7N|FM#t^apAMJnXf+N4mbvg z75h~|u#>P2G{7gtz=-XAUj*WMB2f#*`Kq|^S=;;eyArv52Sf>?eB|O!GVCO50}b#= zF)(7gou(aK!wc_#Pd$-PFkb-YtK!0EZ8Kke$Q^JD5G(epf?y|M8)$$}ih&W^`@RUo z^+ci;jPq4-;j^~)?RO<|`woZ_MES_YpJdoc*ajNllVV`Rb~{Zwx`r3t0iSvzpcR|UaN!Zy$VpA-Wlw)cGzi0g?&Eg0vk;=*Tb@7wQ6WPGc`2sj!6&F5hoB8TP?to)}Sg~If1Um`aKm&YI42;;`_eCJCCla+_ zoUe)tpS8Vjzbld3cR-XN%118#B*RX^HqZc{6ayo++iBX-HN5Z+_|y{#1@i@PzA7$! z);9Cihui_j0I_1fDhPHGwt)utq!<{nz3+=aTu&rw!8l(P7d~ry-+osjx9@-`L6nbN z{7HtLgl(V!J}Cx9Y`4?2qicBK9q_3q5(?%E;CxkF_^fT_s}H#YjsarDepL|cBy0l> z@JTT+Vtd~gfw-PX)PixoDlUB1_P+hDL~h>!QGzHRx%iU|I|{kWBPQo_O0G|{CBewT_5s2%FL@gNStK!0EZSULf zO62w(5G9E6k&8dcu#>P2G{7gtz=-X3ns#&zFT4Xj^+ZC!d;y%ViVL5$&3yGCcfc_~ ztk|y#f}MnIpaDK921ab}`yvq66Ny?d&R4~S&)VL%-<8PiJ0MCBP6&F5hd*6OnBDe2= zC_$8uT>MFforG?CXh4e&`ZFk*Y(7lF8*NYsLHzA7$!*7m;ru0(F%0a1b|AG!FG3_A(iKm&YI42;-r zr)fvm@WMObQ%@um%oo7bvuHl7uz^9%_D3~vR^Hp)- zv$mP9KI9HK28b2=RY9s?R{Sa;(8)c3XxbRuq`}VsMxqSyj38H-D z;!iT{By0l>@JTT+V!NHD9bLlX1@B6JKz`~R_s>=!5$(BB!Rb1Aiwyxt}lmA0{>wG`NjW- zfyJH#-a3K&;@`Ty96kyBhY92t{~rbxdlGo-1oDf2>-uu|B=DamP=E6O>Du^{KoWSj z1oDf2x5jebB=D02@{9jTdRdzU-YtRr;@_>YoHq&lB!T?mf0AC-CV_WLAiwx`Yb@tY z0zXM0zxbb|m$gaY-4e(z{@ohOd6U3T638$9C+THv5_q=+@{51B#&X^y@RJ1ci~mV_ zS(^mjErI;v->tEnHwpYCf&Aisl3vy(fp<$Fzxa1+Eay!EKS?0J_@AVgwMpRJ638$9 z-5SezlfX|B$S?jU>1Ayac((-di+{Jqa^58HlLYdM|4DjTn*`o1f&Aj%t+AXp3H&61 z{NjI-Ue+docS|6@_;+h8=S>1XNg%)YpQM+yN#NZQ$S?lg8q0Z;z)up$Fa9U#Wo;68 zw*>Nwf49bR-X!po1oDgjNqSkE1l}!y{NmrOv79#v{3L<=;(wA})+T{BOQ8PX|Le`p zp5rEgB=DamkYD_Nnppfv;GGl5FaDj|%h{8_f0{sk@&9RJ@h5?IPTC+wD8x7$Eu#U?*W4Xn;?O zff3v7H0|gb9&!g9a<4;eW7)w8?Zz5|W{qR#+!61IT`_@o#ZvE5G7j;`S$cfc`TODGNlID@Gg z&e}e+Uxj4=$$d}!bbbD>dSq;oYB+1-tH%DXC3H^%ID@Gg&f1ubK4jqRSI_Qt`wln; zh&}_@N!SJ&;FDrt#CAJPJGzF4+yTdUEulCJ;0&f}IBWaNeifDhB=)Aji`9vNGt z8qV7Isa<4;eW7)w8?Zz5|W{qR#+!61IT`_@o#ZvE5G7j;`S$ zcfc`TODGNlID@Gg&e}e+Uxj4=$$d}!bbbDfN5&SZhO;)lYV7}7LiaR)GnlI3tc}^| zLk7-%_3Uo9?|@@~=re$wgl(V!J}Cx9Y`4?2qicA`9dL}-5{knB&S0vBv$oIdS78}I za^Dj_U7vsBk+DUp;jE3X8vDPN&^-;{45n&0YhyP0kb$#bJ-gfOJKz`~`V3$vVH;?G zPl|yN+wC;%=o%h!2OQ(IgyJxOGnlI3tnD-VRagd)-1o#!*XQ4OWNeXYIBVmp#{RD* zbWZ~~gQ*(M+L(<#WZ>*q&+c~n4mbvgJ_FcE*ajNllVV`Rb~{Zwx`v0`0mpbPp*RfS z45n&0Yx~T86_x=c_dW5`_4zj*8C#?p&f55@vHxoc-O~WhV5)|*HfEy_894jZv%B5C z1C9Zr&j5B3wt)utq!<{n-A>bvuHhkfz%gD+C=LTSgQ*(M+CH;ig=GNAeNX&!eg2I{ z#ullDvo^kJ?EhLq_cVYrn5yBdjoIi!2F`x<>~6R3fMbB@Gk~3hZJ+@@DF#Mtx6`zv zYk0^VaE#Xyio*cTV5)|*w$JQWVHrSj-xEJwpMT?#u|=xktc|Z4`@fdZJq_RtrfN8A zV>bGbfwNydyW8zM;20qK3}7c=8)$$}ih&W^?KJJ^8Xj^79OJcw;xK?Sn5yBd?KAsT zSO$>X_ry=v=ihi_Y>{d>YvZfN{;wr;PXjoEsT$7On2kPU;Otk=?soeQI0lG51K3H} z1{&a#VqnB}J54*fhKJk%$9OHFI1JznrfN8A`^&c*q@ajMoy1!vM};s)n<+&+J!W89;L16F*&_ zf8&v{MXKSfjjtN}zn0KF4d4u>YB+0SHu{i(vtK>C+wD8x7$Eu#U?*W4Xn;?Off3v7 zH0|gb9&!g9{rk3cKZ%E28ccb*h$z18sL*+V8nJiO*^`Vhui_jcrBqg4B!l=YB+2A z%zhP?0VMZ5@zeGBHy#;Vq#DlJ_^Pr0YYE-c0M1~lhO;(iqYoK4`_;3%-M#~k0iw?U zb`rLM2Kb~H7_r?>(~hp;A$PzrUP~wr12}`J8qV52vtNZ}0Lgt%{B(W(jYq~7sfM#Q zzH03MT0-|UfHRn?;jE3>=tBn1e)a5bx9@;sfao)TorG{nqKKyu#`KV6@H3q?K|KYAo>hoCt(|CfKQ5n5!>xF?dTdFat9pawS?j@fHRn?;jHa5`&C#5klgpg zPuJ()cw}smYB+1-tH%DXC3H^%ID@Gg&f1ubK4jqRSI_Qt`wln;h&}_@N!SJ&;FDrt z#CAJPJGzF4+yTdUEulCJ;0&f}IBWaNeifDhB=)Aji`9vNGt8qV7Isa<4;eW7)w8?Zz5|W{qR#+!61IT`_@o#ZvE5G7j;`S$cfc`TODGNlID@Gg z&e}e+Uxj4=$$d}!bbbDfN5&SZhO;)lYV7}7LiaR)GnlI3tc}^|Lk7-%_3Uo9?|@@~ z=re$wgl(V!J}Cx9Y`4?2qicA`9dL}-5{knB&S0vBv$oIdS78}Ia^Dj_U7vsBk+DUp z;jE3X8vDPN&^-;{45n&0YhyP0kb$#bJ-gfOJKz`~`V3$vVH;?GPl|yN+wC;%=o%h! z2OQ(IgyJxOGnlI3tnD-VRagd)-1o#!*XQ4OWNeXYIBVmp#{RD*bWZ~~gQ*(M+L(<# zWZ>*q&+c~n4mbvgJ_FcE*ajNllVV`Rb~{Zwx`v0`0mpbPp*RfS45n&0Yx~T86_x=c z_dW5`_4zj*8C#?p&f55@vHxoc-O~WhV5)|*HfEy_894jZv%B5C1C9Zr&j5B3wt)ut zq!<{n-A>bvuHhkfz%gD+C=LTSgQ*(M+CH;ig=GNAeNX&!eg2I{#ullDvo^kJ?EhLq z_cVYrn5yBdjoIi!2F`x<>~6R3fMbB@Gk~3hZJ+@@DF#Mtx6`zvYk0^VaE#Xyio*cT zV5)|*w$JQWVHrSj-xEJwpMT?#u|=xktc|Z4`@fdZJq_RtrfN8AV>bGbfwNydyW8zM z;20qK3}7c=8)$$}ih&W^?KJJ^8Xj^79OJcw;xK?Sn5yBd?KAsTSO$>X_ry=v=ihi_ zY>{d>YvZfN{;wr;PXjoEsT$7On2kPU;Otk=?soeQI0lG51K3H}1{&a#VqnB}J54*f zhKJk%$9OHFI1JznrfN8A`^&c*q@ajMoy1!vM};s)n<+&+J!W89;L16F*&_f8&v{MXKSfjjtN} zzn0KF4d4u>YB+0SHu{i(vtK>C+wD8x7$Eu#U?*W4Xn;?Off3v7H0|gb9&!g9{rk3 zcKZ%E28ccb*h$z18sL*+V8nJiO*^`Vhui_jcrBqg4B!l=YB+2A%zhP?0VMZ5@zeGB zHy#;Vq#DlJ_^Pr0YYE-c0M1~lhO;(iqYoK4`_;3%-M#~k0iw?Ub`rLM2Kb~H7_r?> z(~hp;A$PzrUP~wr12}`J8qV52vtNZ}0Lgt%{B(W(jYq~7sfM#QzH03MT0-|UfHRn? z;jE3>=tBn1e)a5bx9@;sfao)TorG{nqKKyu#`KV6@H3q?K|KYAo>ho zCt(|CfKQ5n5!>xF?dTdFat9pawS?j@fHRn?;jHa5`&C#5klgpgPuJ()cw}smYB+1- ztH%DXC3H^%ID@Gg&f1ubK4jqRSI_Qt`wln;h&}_@N!SJ&;FDrt#CAJPJGzF4+yTdU zEulCJ;0&f}IBWaNeifDhB=)Aji`9vNGt8qV7Isa<4;eW7 z)w8?Zz5|W{qR#+!61IT`_@o#ZvE5G7j;`S$cfc`TODGNlID@Gg&e}e+Uxj4=$$d}! zbbbDfN5&SZhO;)lYV7}7LiaR)GnlI3tc}^|Lk7-%_3Uo9?|@@~=re$wgl(V!J}Cx9 zY`4?2qicA`9dL}-5{knB&S0vBv$oIdS78}Ia^Dj_U7vsBk+DUp;jE3X8vDPN&^-;{ z45n&0YhyP0kb$#bJ-gfOJKz`~`V3$vVH;?GPl|yN+wC;%=o%h!2OQ(IgyJxOGnlI3 ztnD-VRagd)-1o#!*XQ4OWNeXYIBVmp#{RD*bWZ~~gQ*(M+L(<#WZ>*q&+c~n4mbvg zJ_FcE*ajNllVV`Rb~{Zwx`v0`0mpbPp*RfS45n&0Yx~T86_x=c_dW5`_4zj*8C#?p z&f55@vHxoc-O~WhV5)|*HfEy_894jZv%B5C1C9Zr&j5B3wt)utq!<{n-A>bvuHhkf zz%gD+C=LTSgQ*(M+CH;ig=GNAeNX&!eg2I{#ullDvo^kJ?EhLq_cVYrn5yBdjoIi! z2F`x<>~6R3fMbB@Gk~3hZJ+@@DF#Mtx6`zvYk0^VaE#Xyio*cTV5)|*w$JQWVHrSj z-xEJwpMT?#u|=xktc|Z4`@fdZJq_RtrfN8AV>bGbfwNydyW8zM;20qK3}7c=8)$$} zih&W^?KJJ^8Xj^79OJcw;xK?Sn5yBd?KAsTSO$>X_ry=v=ihi_Y>{d>YvZfN{;wr; zPXjoEsT$7On2kPU;Otk=?soeQI0lG51K3H}1{&a#VqnB}J54*fhKJk%$9OHFI1Jzn zrfN8A`^& zc&|I)wBHfBrva=oRl|ra{UY{CsGMJ*3jY?{1{$zhih&UuuW4t}Fmkwm`O({V!2aJ6 zio*a_nW|yLb~_s#+rWF>0jCAgUk-K>wt)utq!<{ny%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4 zffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK7 z7_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmO zeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv4 z0~SazFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhG zE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^ zzBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0 zn57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x; zh`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$ zXnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+ z^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y| z_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjz zwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m z8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g z10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5o zy%K#v z)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R` zNHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x z5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC z7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR z7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{ z5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lR zO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5R zS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQu zu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!9 z2AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6F zzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cW zZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHH zw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF= zAohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^ z!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~Saz zFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJ zof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg z6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sP zvE@Y|_J**&G+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbh zLDjjzwt)tir5G5oy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul z(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**& zG+==g10%Ndi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)ti zr5G5o zy%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeB zfLV%x5nEmaVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Nd zi`XmC7gU`aY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5oy%K#v)w#j8 zfd-hR7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJof~W$Xnq`R`NHH*C zOTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg6ayo+^o!Ul(HB&m8*CeBfLV%x5nEma zVs8lRO9K{2F)(6FzlgmOeL>Z^!M1@0n57sPvE@Y|_J**&G+==g10%Ndi`XmC7gU`a zY#V5RS&D%XTV4cWZwTv40~SazFk(x;h`kbhLDjjzwt)tir5G5o-(mA#>q`U7QVfjP(l27KL|;&KZm?~j0cI%%Mr?Tze`Eiri@(F> zzt)!qn57sPv87+cUWvY->fB)4Km*KD42; zy%K#v)w#j8fd-hR7#OkTMf{2VPZcP^2!ux+3LW+?_n zYq~=KehpF#jM&mIVy{GBP<3vwZJ+^WDF#Mtc@c=c zA*?SASRlo~h%Nmh_Db{xRp$oV1{z?NVqnCU7lGIt!uryH1yT%**wQazuS8!^b#Ab2 zpaEtn21aao5s1AZtS=2%AjQClE&U?)O7sO)=LXvb8eo=UV8oUef!G_u`qF>}QVfjP z(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy{UY{C^aWMt2HOT2V3uNF#FiI< z*c-z7(trh042;;)FJiAmUr=>!ux+3LW+?_nYz=$pVBKAu31y$z; z+Xfn7mSSMUmKTB88^Ze1fCW+vjM&mIVy{GBP<3vwZJ+^WDF#Mtc@c=cA*?SASRlo~ zh%Nmh_Db{xRp$oV1{z?NVqnCU7lGIt!uryH1yT%**wQazuS8!^b#Ab2paEtn21aao z5s1AZtS=2%AjQClE&U?)O7sO)=LXvb8eo=UV8oUef!G_u`qF>}QVfjP(l27KL|;&K zZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy{UY{C^aWMt2HOT2V3uNF#FiI<*c-z7(trh0 z42;;)FJiAmUr=>!ux+3LW+?_nYz=$pVBKAu31y$z;+Xfn7mSSMU zmKTB88^Ze1fCW+vjM&mIVy{GBP<3vwZJ+^WDF#Mtc@c=cA*?SASRlo~h%Nmh_Db{x zRp$oV1{z?NVqnCU7lGIt!uryH1yT%**wQazuS8!^b#Ab2paEtn21aao5s1AZtS=2% zAjQClE&U?)O7sO)=LXvb8eo=UV8oUef!G_u`qF>}QVfjP(l27KL|;&KZm?~j0cI%% zMr?Tzh`k}KFAZ2A#lVOy{UY{C^aWMt2HOT2V3uNF#FiI<*c-z7(trh042;;)FJiAm zUr=>!ux+3LW+?_nYz=$pVBKAu31y$z;+Xfn7mSSMUmKTB88^Ze1 zfCW+vjM&mIVy{GBP<3vwZJ+^WDF#Mtc@c=cA*?SASRlo~h%Nmh_Db{xRp$oV1{z?N zVqnCU7lGIt!uryH1yT%**wQazuS8!^b#Ab2paEtn21aao5s1AZtS=2%AjQClE&U?) zO7sO)=LXvb8eo=UV8oUef!G_u`qF>}QVfjP(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}K zFAZ2A#lVOy{UY{C^aWMt2HOT2V3uNF#FiI<*c-z7(trh042;;)FJiAmUr=>!ux+3L zW+?_nYz=$pVBKAu31y$z;+Xfn7mSSMUmKTB88^Ze1fCW+vjM&mI zVy{GBP<3vwZJ+^WDF#Mtc@c=cA*?SASRlo~h%Nmh_Db{xRp$oV1{z?NVqnCU7lGIt z!uryH1yT%**wQazuS8!^b#Ab2paEtn21aao5s1AZtS=2%AjQClE&U?)O7sO)=LXvb z8eo=UV8oUef!G_u`qF>}QVfjP(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy z{UY{C^aWMt2HOT2V3uNF#FiI<*c-z7(trh042;;)FJiAmUr=>!ux+3LW+?_nYz=$pVBKAu31y$z;+Xfn7mSSMUmKTB88^Ze1fCW+vjM&mIVy{GBP<3vw zZJ+^WDF#Mtc@c=cA*?SASRlo~h%Nmh_Db{xRp$oV1{z?NVqnCU7lGIt!uryH1yT%* z*wQazuS8!^b#Ab2paEtn21aao5s1AZtS=2%AjQClE&U?)O7sO)=LXvb8eo=UV8oUe zf!G_u`qF>}QVfjP(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy{UY{C^aWMt z2HOT2V3uNF#FiI<*c-z7(trh042;;)FJiAmUr=>!ux+3LW+?_nY zz=$pVBKAu31y$z;+Xfn7mSSMUmKTB88^Ze1fCW+vjM&mIVy{GBP<3vwZJ+^WDF#Mt zc@c=cA*?SASRlo~h%Nmh_Db{xRp$oV1{z?NVqnCU7lGIt!uryH1yT%**wQazuS8!^ zb#Ab2paEtn21aao5s1AZtS=2%AjQClE&U?)O7sO)=LXvb8eo=UV8oUef!G_u`qF>} zQVfjP(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy{UY{C^aWMt2HOT2V3uNF z#FiI<*c-z7(trh042;;)FJiAmUr=>!ux+3LW+?_nYz=$pVBKAu3 z1y$z;+Xfn7mSSMUmKTB88^Ze1fCW+vjM&mIVy{GBP<3vwZJ+^WDF#Mtc@c=cA*?SA zSRlo~h%Nmhb|Y#-Cj85xu3s37msHV+EiVGGH$<&#VJuiuMI*NKi`b253o_^RP}eUE z?~*DSvE@Y|_J*i+EsOP zA`p8+)VdbNf+bZnVoSe>-H5gzb6yX1{lf4rsiF~EUIb!qh+5aeSg@puMr`R9u^Z7A zWX|iMu3s45B~>(H%Zotl4N>b_7z>tE(TFYmB6cI%g3Nh6)b$I)yQGRnY_)T&ne%$6>lcQ1NfnLQ z@*)s>L)5w!#)2hPG-6A?h~0>`Aah<1b^XHdE~%msTV4cWZ-`pg!dS4Ribibd7qJ`B z7G%!rp{`#T-X&EuV#|v_>eJ5Vfv_v0zCR zjo8vJVmG2K$eh4st=@+pZ z(H3OR>!Ge+7~UmSG-At(KslBKmQ>M*E&U>PBie$@c|Fwi3&XplibiaC5s1AZ zYF!Is!ICN(v87+cZbVyLFT+3>iUJ@T~b9Ow!8?$-Vn8}g|T2s z6^+=^FJd>MEy$eLLtVcxyi2NR#FiI<*c+nOwJ;VesiF~E`bF$Uv;~>-dZ_CchIdI7 zjo9)c5PL(^x)#QQB~>(HOTUP{615*jRh=7b8)$%8ih&UuuW4t}Fmkwm`O({V!2aJ6 zio*a_nW|yLb~_s#+rWF>0jCAgUk-K>wt)utq!<{ny%K#v)w#j8fd-hR7#OkTMIiQuu)Z{4 zffNHHw)BhGE72EJof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK7 z7_sF=Aohl^zBFKg6ayo+^o!Ulu`lSiQoWbhHqZd86ayo+ya>eJ5Z0FlERbSg#Fl;$ zdnNV-{Z^{?659qEV3lHE#FiI<*c-z7(trh042;;)FJiC6zM$Vq^TTTV4cWZwTv40~SazFk(x;h`kc~ zf_^L2dx>oW4X{cvFk;J#KLs(xLut17|5nK92?3LIT^joRkOKclxfK`ft z5nEmaVs8lRO9K{2F)(6FzlgmO`+|Ne)q9C;0}ZfBF)(7wi$LrRVSQ=90x1SYZ0Q%V zS7Kk#Z>4%Kv2CCMRw)KXYz=$pVBKAt`3;L~8?}QVfjP(l27KL|;&KZm?~j0cI%%Mr?Tzh`k}KFAZ2A#lVOy z{UY{C>RPQCW4K%Yy_eWF&;Y9x10%M)2*lnH)|Un>kYZrOmVOa? zCH4jVR;u?B+Xfn7m11DTmKTB88^Ze1fCW+vjM&mIVz0!$px;XMUSiuo1FTXEjM(xb z5PL&dUmCDLih&VZ`bF%O*cbF$soqO$8)$%4ih&VZUIb!q2y%PI^ zek;{`iERT7uu3s7V#|v_>;HqZdG z6ayo+ya>eJ5Z0FlERbSg#Fl;$dnNV-{Z^{?659qEV3lHE#FiI<*c-z7(trh042;;) zFJiC6zM$Vq^TT zTV4cWZwTv40~SazFk(x;h`kc~f_^L2dx>oW4X{cvFk;J#Kb=Cafd*Km7#OkTMIiQuu)Z{4ffNHHw)BhGE72EJ zof~W$Xnq`R`NHH*COTUP{5`96{xxu!92AHK77_sF=Aohl^zBFKg z6ayo+^o!Ul(HB&m8*CeBfLV%x5nEmaVs8lRO9K{2F)(6FzlgmO`+|Ne)q9C;0}ZfB zF)(7wi$LrRVSQ=90x1SYZ0Q%VS7Kk#Z>4%Kv2CCMRw)KXYz=$pV zBKAt`3;L~8?Ls(xLut17|5nK92?3L&Xs?H6z4K%Yy_eWF&;Y9x z10%M)2*lnH)|Un>kYZrOmVOa?CH4jVR;u?B+Xfn7m11DTmKTB88^Ze1fCW+vjM&mI zVz0!$px;XMUSiuo1FTXEjM(xb5PL&dUmCDLih&VZ`bF%O*cbF$soqO$8)$%4ih&VZ zUIb!q2y%PI^ek;{`iERT7uu3s7V#|v_>}QVfjP(l27KL|;&K{{QWk(URjj2t@z?r?>9hsMl+kNXFQaS_$Vx;~)@l8mi{W z_u+x5W@WA$?bKY(a9~L(zqQm~eaL{Y}(FJTbhYyPz`gYj&hSvh^ zJ)e9Z9@u_#&fCr5!{Ww7bl6`Q4ti}rx`6HG@L_R7-wylU@LHg~=acWl1KW?zdAm7$ zSlpP14*LtkL9gvc7qHzNJ}hqN+hN}uUJJDMeDZyGVEfTIZ#RbziyITsVSiyb=(YXm z0=ApOhs6zjJM4SIYk~Hjd_w>NpHF~||9lPU00QR;u<_?v;R%7yC&0#kzJ}L3_)-6Q zFp(cgfQ|o1+1^w%8~-MdAM^|x|3PBCy;L^-?MRn@kd0qnqJagn@dM#5`ZOEAs4!E? zV&kV^UMdM2zmy!4i(%s@179E=8^3@Ewi4KQ8+uz}Hr|Gw#1JqLVB-zYNeKZP0XE)- zp2QF^5MbjC&`Aja8v!=nhMvR_Fc4tl4bVvm0UH4}-iDsU5HJv6;|}}cHr|Gw z#1JqLVB-zYNeKZP0XE)-p2QF^5MbjC&`Aja8v!=nhMvR_Fc4tl4bVvm0UH4}-iDsU z5HJv6;|}}cHr|Gw#1JqLVB-zYNeKZP0XE)-p2QF^5MbjC&`Aja8v!=nhMvR_ zFc4tl4bVvm0UH4}-iDsU5HJv6;|}}cHr|Gw#1JqLVB-zYNeKZP0XE)-p2QF^ z5MbjC&`Aja8v!=nhMvR_Fc4tl4bVvm0UH4}-iDsU5HJv6;|}}cHr|Gw#1JqL zVB-zYNeKZP0XE)-{$*n6^3sP(2;>Q{@p)!5BG~vDlq=?7ve7S;T-q5i90lhywGGcWSB>fHa-HJH$?YzQ3yGFN^-5OYv=g!n&QKx2pO-b%m_n zXmnzKV;lLWR@FS;$(j-3+x~DC(P&^y~64{v4?Xo%UD?RDTu|l{RuNV zM-TNwNxs^M=40g*-}Wa=^|%Ue#v4(8Jmb|ep!rxu#JBwkQ$4Q2oAE}}AJ2HT3}`-9 z5%Fz*#-`$R2tdG2fQ`3HKmib#NPvx>2%A*13H*%r?3AnLW8+s>i2802VBz{Yo4vhf`f4Btb5jqkK%<2xi6zJ~xC-)YIlcStaN4*@p5 z(~^zvkYIqo7ZYIPzgS#agFr`M?#6fNF+czUQwgx~Q*o0F0%ihiyjcL!LtrWaHhwB@ zl0m>sfQ>f`KzayFCBVi{#Z58@mfbs}f0#gaF@l$b=3<72XY`j?j(nDY>0XBXrZjwR3On{9y3qX1ZOeMg^ zPsL3#2$%`5@n!)?4}qx!zG~xJGgJAkmCOpjntUxCR<4GPf0sHMc+cF|P5v(VF1_Os z`%!^CfzSnZAS?9f{{Lxkcl-yH8-Q?D(^N4+~*&@whEx+ zo*L>Vw@#f$?0YS@nswfL${$^K?#p-X89poai!E!{Rr&ofqHOhglhr{6S!^$Bge4u+QkT&tdJn zFXxn{)vWVW#A_!j1Cl=oyjJM_w<}^S+!@mR7UQQxUJ7tPDv0 zAn;nD_usC7{rz2?dC}VWnF0f!RZ z`Y@~d<&*b%%&=IoUu>||tn=a<`!Fj5l0OLS^_cMub_@24(l)hpUVLL8hWZxu!Cnvh ze(e(9ZB%q7;-0xOdi6#R%`KWgTftxKO)Z_j_@22&eT({FuZR82ItD&%RCEUSSremI zZ}iaIqIq^T<=@oOdGU>X80uTp2YWs2`?X7aw^7lVh6>lpa7QPCOPXHAS=z0pH+i{{zYlz&r8=fyYnVW@9WAMEw8@7FHz z-9|-cBJPz+D8|fa$#W z#y+gB0ChW;{sOR{h6&&0v1b`KU^=tGfqz{A>UOR%5P)DPMw-fF&%$lMbY_DC|GEOy z?ObCZ0KrgMf}t2`Dvv!2w*k|c4G#S43Q)Imje!6J zLow1+9(xvU1Ew<@9QfB2pl;_H0|5wzVx*}&_AJ~6OlLMY@UJUC-Oe=z0uT(vNK<+2 zS-1_D&TMetUsr&-oofsPAQ*~~rt;Xca2qh4+2Fvxt^jpA*BA&uFcc$A<*{esHefol z!GV8W0qSC6TP{&fYY z+quR-0D_?yX)2FB3%3E&nGFv7>k3e}bB%!j1Vb^>R33X4ZUd$>8yxu86`*eC8Uq0c zhGL|tJoYTy225u*IPkA4K;6zY1_BTa#Yj_m>{+-Cn9gi);9pmOx}9qb1RxlSk*4z4 zvv3|P35s?;Wl78v%!IXT>eK{$DW1Tfa%Nz2mW;hsN1>5KmdZF7-=exJqx!1)0qtp{Obx( zw{wkw00cuZ(o`OM7H$KkGaDTE*A<{{=Nbb62!>*$sXX>9+y+c%HaPIFD?r`OH3k9@ z48=%OdF)xZ4VcbsaNu88fV!P)32tY6tBTeP8 zXW=$rIr!BC7emB*fi+kolJ z1_%Ci1*qG(#y|jqp%`f@k39>w0n?cc4*cs1P`7i9fdB+UG162Xdlqg3rZXEH_}3Mn zZs!^U0SJa-q^UgiEZhc6XEr$SuPZ>^&NT)C5DdjgQ+e!JxDA-jY;fRTSAe>mYYYS+ z7>bdm^4PO*8!(;O;K1K1fa*~VL3JZd*3PZT!T!KMa=703b{iF)Z+Mr{v#4Jl`x{30 z!V@cC&z9Y4)|m|s{8wP08iMLZnyj5$lZ&bq`$%P&13O5LR1pzkxD`cbz2%IFq#-9X*8w9?B02}`mGSUPDP7+|_PlCb?0$)LZ zjsFT6X#xT#39#`eLE#30uOM*P_`5#>B6puWA5Zcff%DkzdhU|`{k;3+`S`yXcYn_Z q-}T%j{rh?MN#Y>@0SG_<0uX=z1Rwwb2tWV=5P$##AOL~d1pWh<-JQ<> literal 0 HcmV?d00001 diff --git a/easyfleet_easynav_deployment/maps/warehouse_a_collision.yaml b/easyfleet_easynav_deployment/maps/warehouse_a_collision.yaml new file mode 100644 index 0000000..c5011af --- /dev/null +++ b/easyfleet_easynav_deployment/maps/warehouse_a_collision.yaml @@ -0,0 +1,10 @@ +# Generated by sdf_to_costmap.py from large_warehouse_world.sdf: +# collision geometry between z = 0.05 and 0.45 m, world (100.0, 300.0) at +# the origin of this map frame. +image: warehouse_a_collision.pgm +mode: trinary +resolution: 0.2 +origin: [-101.0, -101.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 diff --git a/easyfleet_easynav_deployment/maps/warehouse_a_routes.yaml b/easyfleet_easynav_deployment/maps/warehouse_a_routes.yaml new file mode 100644 index 0000000..dbb1047 --- /dev/null +++ b/easyfleet_easynav_deployment/maps/warehouse_a_routes.yaml @@ -0,0 +1,404 @@ +# Generated by aisle_routes.py from warehouse_a_collision.yaml: one route +# per rack aisle, two along the central aisle, the edges of the rack area, +# the cross corridors that join them and spurs to the docks (map frame). +routes: [aisle_0_0, aisle_0_1, aisle_0_2, aisle_0_3, aisle_0_4, aisle_0_5, aisle_0_6, + aisle_0_7, aisle_0_8, aisle_0_9, central_0_10, central_0_11, aisle_0_12, aisle_0_13, + aisle_0_14, aisle_0_15, aisle_0_16, aisle_0_17, aisle_0_18, aisle_0_19, aisle_0_20, + aisle_0_21, edge_0_22, edge_0_23, aisle_1_24, aisle_1_25, aisle_1_26, aisle_1_27, + aisle_1_28, aisle_1_29, aisle_1_30, aisle_1_31, aisle_1_32, central_1_33, central_1_34, + aisle_1_35, aisle_1_36, aisle_1_37, aisle_1_38, aisle_1_39, aisle_1_40, aisle_1_41, + aisle_1_42, aisle_1_43, aisle_1_44, edge_1_45, edge_1_46, dock_0, dock_1, dock_2, + dock_3, dock_4, dock_5, dock_6, corridor_0_0, corridor_0_1, corridor_0_2, corridor_0_3, + corridor_0_4, corridor_0_5, corridor_0_6, corridor_0_7, corridor_0_8, corridor_0_9, + corridor_0_10, corridor_0_11, corridor_0_12, corridor_0_13, corridor_0_14, corridor_0_15, + corridor_0_16, corridor_0_17, corridor_0_18, corridor_0_19, corridor_0_20, corridor_0_21, + corridor_0_22, corridor_1_0, corridor_1_1, corridor_1_2, corridor_1_3, corridor_1_4, + corridor_1_5, corridor_1_6, corridor_1_7, corridor_1_8, corridor_1_9, corridor_1_10, + corridor_1_11, corridor_1_12, corridor_1_13, corridor_1_14, corridor_1_15, corridor_1_16, + corridor_1_17, corridor_1_18, corridor_1_19, corridor_1_20, corridor_1_21, corridor_1_22, + corridor_2_0, corridor_2_1, corridor_2_3, corridor_2_4, corridor_2_5, corridor_2_6, + corridor_2_7, corridor_2_8, corridor_2_9, corridor_2_10, corridor_2_11, corridor_2_12, + corridor_2_13, corridor_2_14, corridor_2_15, corridor_2_16, corridor_2_17, corridor_2_18, + corridor_2_19, corridor_2_20, corridor_2_21, corridor_2_22, corridor_2_23, corridor_2_24, + corridor_2_25, corridor_2_26, corridor_2_27] +aisle_0_0: + start: {x: -83.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -83.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_1: + start: {x: -75.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -75.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_2: + start: {x: -67.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -67.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_3: + start: {x: -59.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -59.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_4: + start: {x: -51.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -51.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_5: + start: {x: -43.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -43.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_6: + start: {x: -36.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -36.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_7: + start: {x: -28.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -28.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_8: + start: {x: -20.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -20.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_9: + start: {x: -12.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -12.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +central_0_10: + start: {x: -3.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -3.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +central_0_11: + start: {x: 3.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 3.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_12: + start: {x: 12.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 12.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_13: + start: {x: 20.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 20.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_14: + start: {x: 28.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 28.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_15: + start: {x: 36.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 36.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_16: + start: {x: 44.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 44.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_17: + start: {x: 52.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 52.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_18: + start: {x: 59.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 59.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_19: + start: {x: 67.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 67.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_20: + start: {x: 75.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 75.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_0_21: + start: {x: 83.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 83.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +edge_0_22: + start: {x: -90.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -90.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +edge_0_23: + start: {x: 91.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 91.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_24: + start: {x: -83.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -83.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_25: + start: {x: -75.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -75.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_26: + start: {x: -67.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -67.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_27: + start: {x: -59.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -59.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_28: + start: {x: -51.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -51.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_29: + start: {x: -43.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -43.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_30: + start: {x: -36.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -36.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_31: + start: {x: -28.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -28.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_32: + start: {x: -12.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -12.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +central_1_33: + start: {x: -3.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -3.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +central_1_34: + start: {x: 3.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 3.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_35: + start: {x: 12.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 12.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_36: + start: {x: 20.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 20.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_37: + start: {x: 28.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 28.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_38: + start: {x: 36.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 36.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_39: + start: {x: 44.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 44.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_40: + start: {x: 52.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 52.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_41: + start: {x: 59.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 59.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_42: + start: {x: 67.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 67.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_43: + start: {x: 75.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 75.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +aisle_1_44: + start: {x: 83.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 83.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +edge_1_45: + start: {x: -90.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: -90.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +edge_1_46: + start: {x: 91.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} + end: {x: 91.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.707107, qw: 0.707107} +dock_0: + start: {x: -87.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: -87.1, y: -90.8, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_1: + start: {x: -62.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: -62.1, y: -89.3, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_2: + start: {x: -37.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: -37.1, y: -89.5, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_3: + start: {x: -12.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: -12.1, y: -89.3, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_4: + start: {x: 12.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: 12.9, y: -89.3, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_5: + start: {x: 37.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: 37.9, y: -90.0, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +dock_6: + start: {x: 62.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} + end: {x: 62.9, y: -89.5, z: 0.0, qx: 0.0, qy: 0.0, qz: -0.707107, qw: 0.707107} +corridor_0_0: + start: {x: -90.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -83.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_1: + start: {x: -83.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -75.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_2: + start: {x: -75.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -67.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_3: + start: {x: -67.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -59.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_4: + start: {x: -59.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -51.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_5: + start: {x: -51.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -43.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_6: + start: {x: -43.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -36.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_7: + start: {x: -36.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -28.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_8: + start: {x: -28.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -20.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_9: + start: {x: -20.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -12.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_10: + start: {x: -12.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -3.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_11: + start: {x: -3.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 3.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_12: + start: {x: 3.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 12.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_13: + start: {x: 12.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 20.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_14: + start: {x: 20.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 28.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_15: + start: {x: 28.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 36.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_16: + start: {x: 36.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 44.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_17: + start: {x: 44.3, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 52.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_18: + start: {x: 52.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 59.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_19: + start: {x: 59.9, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 67.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_20: + start: {x: 67.7, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 75.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_21: + start: {x: 75.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 83.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_0_22: + start: {x: 83.5, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 91.1, y: 94.65, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_0: + start: {x: -90.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -83.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_1: + start: {x: -83.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -75.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_2: + start: {x: -75.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -67.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_3: + start: {x: -67.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -59.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_4: + start: {x: -59.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -51.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_5: + start: {x: -51.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -43.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_6: + start: {x: -43.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -36.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_7: + start: {x: -36.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -28.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_8: + start: {x: -28.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -20.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_9: + start: {x: -20.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -12.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_10: + start: {x: -12.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -3.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_11: + start: {x: -3.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 3.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_12: + start: {x: 3.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 12.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_13: + start: {x: 12.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 20.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_14: + start: {x: 20.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 28.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_15: + start: {x: 28.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 36.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_16: + start: {x: 36.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 44.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_17: + start: {x: 44.3, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 52.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_18: + start: {x: 52.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 59.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_19: + start: {x: 59.9, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 67.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_20: + start: {x: 67.7, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 75.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_21: + start: {x: 75.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 83.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_1_22: + start: {x: 83.5, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 91.1, y: 13.9, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_0: + start: {x: -90.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -87.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_1: + start: {x: -87.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -83.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_3: + start: {x: -75.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -67.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_4: + start: {x: -67.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -62.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_5: + start: {x: -62.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -59.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_6: + start: {x: -59.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -51.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_7: + start: {x: -51.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -43.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_8: + start: {x: -43.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -37.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_9: + start: {x: -37.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -36.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_10: + start: {x: -36.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -28.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_11: + start: {x: -28.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -12.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_12: + start: {x: -12.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -12.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_13: + start: {x: -12.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: -3.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_14: + start: {x: -3.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 3.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_15: + start: {x: 3.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 12.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_16: + start: {x: 12.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 20.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_17: + start: {x: 20.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 28.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_18: + start: {x: 28.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 36.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_19: + start: {x: 36.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 37.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_20: + start: {x: 37.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 44.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_21: + start: {x: 44.3, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 52.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_22: + start: {x: 52.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 59.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_23: + start: {x: 59.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 62.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_24: + start: {x: 62.9, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 67.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_25: + start: {x: 67.7, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 75.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_26: + start: {x: 75.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 83.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} +corridor_2_27: + start: {x: 83.5, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} + end: {x: 91.1, y: -77.45, z: 0.0, qx: 0.0, qy: 0.0, qz: 0.0, qw: 1.0} diff --git a/easyfleet_easynav_deployment/scripts/aisle_routes.py b/easyfleet_easynav_deployment/scripts/aisle_routes.py new file mode 100755 index 0000000..0eddba5 --- /dev/null +++ b/easyfleet_easynav_deployment/scripts/aisle_routes.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Build an initial routes file (easynav_routes_maps_manager format) for a rack warehouse map. + +Racks are the tall, narrow occupied blocks of the map. One route runs along +each aisle (the middle of the gap between two neighbouring racks of a block), +two parallel ones along the widest (central) aisle, one along each edge of the +rack area, and the cross corridors above, between and below the rack blocks +join them all. Spurs leave the bottom corridor towards each group of objects +below it (e.g. packages at the docks). Every segment is checked to be free +with a clearance margin. +""" + +import argparse +import math + +import cv2 +import numpy as np +import yaml + + +def main(): + ap = argparse.ArgumentParser(description=__doc__.strip().splitlines()[0]) + ap.add_argument('map_yaml', help='map_server YAML of the map') + ap.add_argument('--output', required=True, help='routes YAML to write') + ap.add_argument('--also-free-in', action='append', default=[], + help='another map YAML (same size and origin) every route must ' + 'also be free in') + ap.add_argument('--min-rack-length', type=float, default=20.0, help='m') + ap.add_argument('--min-aisle-width', type=float, default=1.2, help='m') + ap.add_argument('--edge-offset', type=float, default=1.5, + help='distance (m) from the outermost racks to the edge routes') + ap.add_argument('--spur-group-gap', type=float, default=3.0, + help='objects closer than this (m) form one group for a dock spur') + ap.add_argument('--min-spur-group', type=float, default=3.0, + help='minimum length (m) of a group of objects to get a spur') + ap.add_argument('--clearance', type=float, default=0.3, + help='free space required around a route (m)') + args = ap.parse_args() + + with open(args.map_yaml) as f: + meta = yaml.safe_load(f) + image = cv2.imread( + str(__import__('pathlib').Path(args.map_yaml).parent / meta['image']), + cv2.IMREAD_GRAYSCALE) + res = float(meta['resolution']) + ox, oy = meta['origin'][:2] + rows = image.shape[0] + occupied = (image < 128).astype(np.uint8) + + def to_world(col, row): + return ox + (col + 0.5) * res, oy + (rows - row - 0.5) * res + + def to_cell(x, y): + return int((x - ox) / res), int(rows - (y - oy) / res) + + # Racks: tall, narrow connected occupied blocks. + count, _, stats, _ = cv2.connectedComponentsWithStats(occupied, connectivity=4) + racks = [] + for i in range(1, count): + cx, cy, w, h, _ = stats[i] + if h * res >= args.min_rack_length and w < h / 3: + x0, y_top = to_world(cx, cy) + x1, y_bottom = to_world(cx + w - 1, cy + h - 1) + racks.append((x0 - res / 2, x1 + res / 2, y_bottom - res / 2, y_top + res / 2)) + if not racks: + raise SystemExit('no racks found') + + # Two (or more) blocks of racks, split where their vertical extents don't overlap. + racks.sort(key=lambda r: -r[3]) + blocks = [] + for rack in racks: + for block in blocks: + if rack[2] < max(b[3] for b in block) and rack[3] > min(b[2] for b in block): + block.append(rack) + break + else: + blocks.append([rack]) + blocks.sort(key=lambda b: -max(r[3] for r in b)) + + blocked = occupied.copy() + for other in args.also_free_in: + with open(other) as f: + other_meta = yaml.safe_load(f) + other_image = cv2.imread( + str(__import__('pathlib').Path(other).parent / other_meta['image']), + cv2.IMREAD_GRAYSCALE) + if other_image.shape != image.shape: + raise SystemExit(f'{other}: not the same size as {args.map_yaml}') + blocked |= (other_image < 128).astype(np.uint8) + clearance = cv2.dilate( + blocked, np.ones((2 * int(math.ceil(args.clearance / res)) + 1,) * 2, np.uint8)) + + def free(a, b): + n = int(math.hypot(b[0] - a[0], b[1] - a[1]) / (res / 2)) + 1 + for t in np.linspace(0.0, 1.0, n): + c, r = to_cell(a[0] + t * (b[0] - a[0]), a[1] + t * (b[1] - a[1])) + if not (0 <= r < clearance.shape[0] and 0 <= c < clearance.shape[1]) or \ + clearance[r, c]: + return False + return True + + def free_y(x, y_from, y_to, step): + """Last y from y_from towards y_to (exclusive of obstacles) that is free.""" + y = y_from + while (y_to - y) * step > 0 and free((x, y), (x, y + step)): + y += step + return y + + def free_x(y, x_from, x_to, step): + """Last x from x_from towards x_to (exclusive of obstacles) that is free.""" + x = x_from + while (x_to - x) * step > 0 and free((x, y), (x + step, y)): + x += step + return x + + # Vertical routes, per block: the middle of the gap between neighbouring + # racks; two parallel ones in the widest gap (the central aisle), at a + # quarter and three quarters of its width; and the edges, beside the + # outermost racks (at most halfway to the wall). + aisles = [] # (block, x, kind) + for bi, block in enumerate(blocks): + block.sort(key=lambda r: r[0]) + gaps = [(right[0] - left[1], left, right) for left, right in zip(block, block[1:])] + widest = max(gaps, key=lambda g: g[0]) + for gap, left, right in gaps: + if gap < args.min_aisle_width: + continue + if gap is widest[0] and left is widest[1]: + for frac in (0.25, 0.75): + aisles.append((bi, round(left[1] + frac * gap, 2), 'central')) + else: + aisles.append((bi, round((left[1] + right[0]) / 2.0, 2), 'aisle')) + ys = np.linspace(min(r[2] for r in block) + 1, max(r[3] for r in block) - 1, 9) + x_min, x_max = block[0][0], max(r[1] for r in block) + wall_l = float(np.median([free_x(y, x_min - 0.5, x_min - 30, -0.2) for y in ys])) + wall_r = float(np.median([free_x(y, x_max + 0.5, x_max + 30, 0.2) for y in ys])) + # Near the racks: things are often stored along the walls. + left = x_min - min(args.edge_offset, (x_min - wall_l) / 2.0) + right = x_max + min(args.edge_offset, (wall_r - x_max) / 2.0) + aisles.append((bi, round(left, 2), 'edge')) + aisles.append((bi, round(right, 2), 'edge')) + + # Cross corridors: above the top block, between blocks, below the bottom + # one. Above and below, halfway to whatever is next, taking the median + # over every aisle so a single object doesn't decide it. + tops = [max(r[3] for r in b) for b in blocks] + bottoms = [min(r[2] for r in b) for b in blocks] + above = [free_y(x, tops[0] + 0.5, tops[0] + 30, 0.2) + for bi, x, kind in aisles if bi == 0 and kind == 'aisle'] + below = [free_y(x, bottoms[-1] - 0.5, bottoms[-1] - 30, -0.2) + for bi, x, kind in aisles if bi == len(blocks) - 1 and kind == 'aisle'] + corridor_y = [(tops[0] + float(np.median(above))) / 2.0] + for i in range(len(blocks) - 1): + corridor_y.append((bottoms[i] + tops[i + 1]) / 2.0) + corridor_y.append((bottoms[-1] + float(np.median(below))) / 2.0) + + routes = [] + corridor_stops = {i: set() for i in range(len(corridor_y))} + for bi, x, kind in aisles: + a, b = (x, corridor_y[bi + 1]), (x, corridor_y[bi]) + if free(a, b): + routes.append((f'{kind}_{bi}_{len(routes)}', a, b)) + corridor_stops[bi].add(x) + corridor_stops[bi + 1].add(x) + + # Spurs from the bottom corridor to each group of objects below it (e.g. + # the rows of packages at the loading docks), along its free side. + bottom_y = corridor_y[-1] + count, _, stats, _ = cv2.connectedComponentsWithStats(blocked, connectivity=8) + below_items = [] + for i in range(1, count): + cx, cy, w, h, _ = stats[i] + x0, y_top = to_world(cx, cy) + x1, y_bot = to_world(cx + w - 1, cy + h - 1) + if y_top < bottom_y - 1.5 and w * res < 30 and h * res < 30: + below_items.append([x0, x1, y_bot, y_top]) + below_items.sort() + groups = [] + for item in below_items: + if groups and item[0] - groups[-1][1] < args.spur_group_gap: + g = groups[-1] + g[1], g[2], g[3] = max(g[1], item[1]), min(g[2], item[2]), max(g[3], item[3]) + else: + groups.append(list(item)) + for gi, (x0, x1, y0, y1) in enumerate(groups): + if y1 - y0 < args.min_spur_group: + continue + for x in (x0 - 1.0, x1 + 1.0): + x = round(x, 2) + a, b = (x, bottom_y), (x, round((y0 + y1) / 2.0, 2)) + if free(a, b): + routes.append((f'dock_{gi}', a, b)) + corridor_stops[len(corridor_y) - 1].add(x) + break + + for ci, y in enumerate(corridor_y): + xs = sorted(corridor_stops[ci]) + for i, (xa, xb) in enumerate(zip(xs, xs[1:])): + if free((xa, y), (xb, y)): + routes.append((f'corridor_{ci}_{i}', (xa, y), (xb, y))) + + out = {'routes': [name for name, _, _ in routes]} + for name, a, b in routes: + yaw = math.atan2(b[1] - a[1], b[0] - a[0]) + q = {'qx': 0.0, 'qy': 0.0, 'qz': round(math.sin(yaw / 2), 6), + 'qw': round(math.cos(yaw / 2), 6)} + out[name] = { + 'start': {'x': round(float(a[0]), 2), 'y': round(float(a[1]), 2), 'z': 0.0, **q}, + 'end': {'x': round(float(b[0]), 2), 'y': round(float(b[1]), 2), 'z': 0.0, **q}, + } + with open(args.output, 'w') as f: + source = args.map_yaml.rsplit('/', 1)[-1] + f.write(f'# Generated by aisle_routes.py from {source}: one route\n' + '# per rack aisle, two along the central aisle, the edges of the rack area,\n' + '# the cross corridors that join them and spurs to the docks (map frame).\n') + yaml.safe_dump(out, f, sort_keys=False, default_flow_style=None) + print(f'{len(racks)} racks in {len(blocks)} blocks, corridors at y = ' + f'{[round(float(y), 2) for y in corridor_y]}, {len(routes)} routes') + + +if __name__ == '__main__': + main() diff --git a/easyfleet_easynav_deployment/scripts/sdf_to_costmap.py b/easyfleet_easynav_deployment/scripts/sdf_to_costmap.py new file mode 100755 index 0000000..83f7486 --- /dev/null +++ b/easyfleet_easynav_deployment/scripts/sdf_to_costmap.py @@ -0,0 +1,508 @@ +#!/usr/bin/env python3 +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Build a 2D occupancy map (PGM + map_server YAML) from a Gazebo SDF world. + +Every collision (or visual) geometry (box, cylinder, sphere, DAE/OBJ/GLB mesh) of the world +and of the models it includes is turned into triangles in the world frame, +clipped to the height band a ground robot can hit, and its footprint is +filled in the map. A wall leaves a line, a low shelf leaves an area. +""" + +import argparse +import math +import os +import xml.etree.ElementTree as ET + +import cv2 +import numpy as np + + +# -- poses ------------------------------------------------------------------- + +def pose_matrix(text): + """4x4 transform from an SDF '' text 'x y z roll pitch yaw'.""" + v = [float(x) for x in (text or '').split()] + [0.0] * 6 + x, y, z, roll, pitch, yaw = v[:6] + cr, sr = math.cos(roll), math.sin(roll) + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + m = np.eye(4) + m[:3, :3] = np.array([ + [cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr], + [sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], + [-sp, cp * sr, cp * cr]]) + m[:3, 3] = [x, y, z] + return m + + +def child_pose(elem): + p = elem.find('pose') + return pose_matrix(p.text if p is not None else None) + + +# -- primitive and mesh triangles (local frame) ------------------------------- + +def box_triangles(size): + sx, sy, sz = [s / 2.0 for s in size] + c = np.array([[x, y, z] for x in (-sx, sx) for y in (-sy, sy) for z in (-sz, sz)]) + faces = [(0, 1, 3, 2), (4, 6, 7, 5), (0, 4, 5, 1), (2, 3, 7, 6), (0, 2, 6, 4), (1, 5, 7, 3)] + tris = [] + for a, b, cc, d in faces: + tris.append(c[[a, b, cc]]) + tris.append(c[[a, cc, d]]) + return np.array(tris) + + +def cylinder_triangles(radius, length, n=24): + ang = np.linspace(0, 2 * math.pi, n, endpoint=False) + ring = np.stack([radius * np.cos(ang), radius * np.sin(ang)], axis=1) + lo, hi = -length / 2.0, length / 2.0 + tris = [] + for i in range(n): + p, q = ring[i], ring[(i + 1) % n] + a, b = [p[0], p[1], lo], [q[0], q[1], lo] + c, d = [q[0], q[1], hi], [p[0], p[1], hi] + tris += [[a, b, c], [a, c, d], [[0, 0, lo], b, a], [[0, 0, hi], d, c]] + return np.array(tris, dtype=float) + + +def sphere_triangles(radius): + # A box is a close enough footprint for the few spheres a world has. + return box_triangles([2 * radius] * 3) + + +def load_obj(path): + verts, tris = [], [] + with open(path, errors='ignore') as f: + for line in f: + if line.startswith('v '): + verts.append([float(x) for x in line.split()[1:4]]) + elif line.startswith('f '): + idx = [int(tok.split('/')[0]) for tok in line.split()[1:]] + idx = [i - 1 if i > 0 else len(verts) + i for i in idx] + for k in range(1, len(idx) - 1): + tris.append((idx[0], idx[k], idx[k + 1])) + if not tris: + return np.zeros((0, 3, 3)) + return np.array(verts)[np.array(tris)] + + +def _strip_ns(root): + for el in root.iter(): + if '}' in el.tag: + el.tag = el.tag.split('}', 1)[1] + return root + + +def _node_matrix(node): + m = np.eye(4) + for t in node: + if t.tag == 'matrix': + m = m @ np.array([float(x) for x in t.text.split()]).reshape(4, 4) + elif t.tag == 'translate': + tr = np.eye(4) + tr[:3, 3] = [float(x) for x in t.text.split()] + m = m @ tr + elif t.tag == 'rotate': + ax, ay, az, deg = [float(x) for x in t.text.split()] + a = math.radians(deg) + k = np.array([ax, ay, az]) / (np.linalg.norm([ax, ay, az]) or 1) + kx = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]]) + r = np.eye(4) + r[:3, :3] = np.eye(3) + math.sin(a) * kx + (1 - math.cos(a)) * kx @ kx + m = m @ r + elif t.tag == 'scale': + s = np.eye(4) + s[0, 0], s[1, 1], s[2, 2] = [float(x) for x in t.text.split()] + m = m @ s + return m + + +def load_dae(path): + root = _strip_ns(ET.parse(path).getroot()) + unit = root.find('asset/unit') + scale = float(unit.get('meter', '1')) if unit is not None else 1.0 + up = root.find('asset/up_axis') + fix = np.eye(4) + if up is not None and up.text.strip() == 'Y_UP': + fix[:3, :3] = [[1, 0, 0], [0, 0, -1], [0, 1, 0]] + + geometries = {} + for geom in root.iter('geometry'): + mesh = geom.find('mesh') + if mesh is None: + continue + sources = {s.get('id'): s for s in mesh.findall('source')} + verts_el = mesh.find('vertices') + pos_src = None + if verts_el is not None: + for inp in verts_el.findall('input'): + if inp.get('semantic') == 'POSITION': + pos_src = sources.get(inp.get('source').lstrip('#')) + if pos_src is None: + continue + pos = np.array([float(x) for x in pos_src.find('float_array').text.split()]).reshape(-1, 3) + tris = [] + for prim in list(mesh.findall('triangles')) + list(mesh.findall('polylist')) + \ + list(mesh.findall('polygons')): + inputs = prim.findall('input') + stride = max(int(i.get('offset', 0)) for i in inputs) + 1 + voff = next(int(i.get('offset', 0)) for i in inputs if i.get('semantic') == 'VERTEX') + ps = [p for p in prim.findall('p') if p.text and p.text.strip()] + if not ps: + continue + if prim.tag == 'polylist': + counts = [int(x) for x in prim.find('vcount').text.split()] + idx = np.array([int(x) for x in ps[0].text.split()])[voff::stride] + k = 0 + for c in counts: + poly = idx[k:k + c] + k += c + for j in range(1, c - 1): + tris.append((poly[0], poly[j], poly[j + 1])) + else: + for p in ps: + idx = np.array([int(x) for x in p.text.split()])[voff::stride] + if prim.tag == 'triangles': + tris += [tuple(idx[i:i + 3]) for i in range(0, len(idx) - 2, 3)] + else: + for j in range(1, len(idx) - 1): + tris.append((idx[0], idx[j], idx[j + 1])) + if tris: + geometries[geom.get('id')] = pos[np.array(tris)] + + out = [] + + def walk(node, m): + m = m @ _node_matrix(node) + for ig in node.findall('instance_geometry'): + g = geometries.get(ig.get('url', '').lstrip('#')) + if g is not None: + out.append(transform(g, m)) + for child in node.findall('node'): + walk(child, m) + + scene = root.find('scene/instance_visual_scene') + vs = None + if scene is not None: + vs_id = scene.get('url', '').lstrip('#') + vs = next((v for v in root.iter('visual_scene') if v.get('id') == vs_id), None) + if vs is None: + out = list(geometries.values()) + else: + for node in vs.findall('node'): + walk(node, np.eye(4)) + if not out: + return np.zeros((0, 3, 3)) + return transform(np.concatenate(out), fix @ np.diag([scale, scale, scale, 1])) + + +def load_glb(path): + import json + import struct + data = open(path, 'rb').read() + if data[:4] != b'glTF': + raise ValueError('not a GLB file') + offset, gltf, binary = 12, None, b'' + while offset < len(data): + length, kind = struct.unpack_from(' pose replaces the included model's own pose. + pose = inc.find('pose') + if pose is not None: + mp = m @ pose_matrix(pose.text) + own = model.find('pose') + if own is not None: + model.remove(own) + else: + mp = m + self.model(model, mp, os.path.dirname(sdf), out) + + +# -- rasterizing --------------------------------------------------------------- + +def clip_band(poly, zmin, zmax): + """Sutherland-Hodgman clip of a 3D polygon to zmin <= z <= zmax.""" + for keep_above, z0 in ((True, zmin), (False, zmax)): + def inside(p): + return p[2] >= z0 if keep_above else p[2] <= z0 + res = [] + for i in range(len(poly)): + a, b = poly[i], poly[(i + 1) % len(poly)] + if inside(a): + res.append(a) + if inside(a) != inside(b): + t = (z0 - a[2]) / (b[2] - a[2]) + res.append(a + t * (b - a)) + poly = res + if not poly: + return poly + return poly + + +def main(): + ap = argparse.ArgumentParser(description=__doc__.strip().splitlines()[0]) + ap.add_argument('world', help='SDF world file') + ap.add_argument('--model-path', action='append', default=[], + help='directory with models, for model:// URIs (repeatable)') + ap.add_argument('--center', type=float, nargs=2, required=True, + help='world x y that becomes the map frame origin') + ap.add_argument('--size', type=float, nargs=2, required=True, + help='map width and height in meters, centered on --center') + ap.add_argument('--resolution', type=float, default=0.2) + ap.add_argument('--zmin', type=float, default=0.05, help='lowest height that blocks (m)') + ap.add_argument('--zmax', type=float, default=0.5, help='highest height that blocks (m)') + ap.add_argument('--source', choices=['collision', 'visual'], default='collision', + help='geometry to use: collision (what stops the robot) or visual ' + '(what a GPU lidar sees)') + ap.add_argument('--inside', type=float, nargs=4, metavar=('XMIN', 'YMIN', 'XMAX', 'YMAX'), + help='world rectangle the robots must stay in: everything outside it is ' + 'marked occupied (closes doors to other zones)') + ap.add_argument('--output', required=True, help='output path without extension') + args = ap.parse_args() + + world = World(args.model_path, args.source) + root = _strip_ns(ET.parse(args.world).getroot()) + w = root.find('world') + tris = [] + base = os.path.dirname(os.path.abspath(args.world)) + for model in w.findall('model'): + world.model(model, np.eye(4), base, tris) + for inc in w.findall('include'): + world.include(inc, np.eye(4), base, tris) + objects = tris + + cx, cy = args.center + width, height = args.size + res = args.resolution + nx, ny = int(round(width / res)), int(round(height / res)) + x0, y0 = cx - width / 2.0, cy - height / 2.0 + + def to_pixels(poly): + pts = np.array([[(p[0] - x0) / res, (y0 + height - p[1]) / res] for p in poly]) + return np.round(pts * 16).astype(np.int32) # 4 bits of subpixel precision + + def draw(pts): + if len(pts) > 2 and cv2.contourArea(pts.astype(np.float32)) >= 16 * 16 * 0.25: + cv2.fillPoly(occupied, [pts], 255, cv2.LINE_8, shift=4) + cv2.polylines(occupied, [pts], True, 255, 1, cv2.LINE_8, shift=4) + + occupied = np.zeros((ny, nx), dtype=np.uint8) + used = total = 0 + for tris, convex in objects: + total += len(tris) + # Only triangles that can touch the map area and the height band. + lo, hi = tris.min(axis=1), tris.max(axis=1) + keep = (hi[:, 2] >= args.zmin) & (lo[:, 2] <= args.zmax) & \ + (hi[:, 0] >= x0) & (lo[:, 0] <= x0 + width) & \ + (hi[:, 1] >= y0) & (lo[:, 1] <= y0 + height) + if not keep.any(): + continue + used += int(keep.sum()) + polys = [clip_band(list(tri), args.zmin, args.zmax) for tri in tris[keep]] + polys = [p for p in polys if len(p) >= 2] + if convex: + # A solid convex shape (box, cylinder) blocks its whole slice, not + # just the faces that cross the band. + pts = to_pixels([p for poly in polys for p in poly]) + draw(cv2.convexHull(pts).reshape(-1, 2)) + else: + for poly in polys: + draw(to_pixels(poly)) + + if args.inside: + xmin, ymin, xmax, ymax = args.inside + c0, c1 = int(math.floor((xmin - x0) / res)), int(math.ceil((xmax - x0) / res)) + r0 = int(math.floor((y0 + height - ymax) / res)) + r1 = int(math.ceil((y0 + height - ymin) / res)) + outside = np.ones_like(occupied, dtype=bool) + outside[max(r0, 0):min(r1, ny), max(c0, 0):min(c1, nx)] = False + occupied[outside] = 255 + + image = np.where(occupied > 0, 0, 254).astype(np.uint8) + cv2.imwrite(args.output + '.pgm', image) + name = os.path.basename(args.output) + with open(args.output + '.yaml', 'w') as f: + f.write( + f'# Generated by sdf_to_costmap.py from {os.path.basename(args.world)}:\n' + f'# {args.source} geometry between z = {args.zmin} and {args.zmax} m, ' + f'world ({cx}, {cy}) at\n' + f'# the origin of this map frame.\n' + f'image: {name}.pgm\nmode: trinary\nresolution: {res}\n' + f'origin: [{-width / 2.0}, {-height / 2.0}, 0.0]\n' + 'negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.25\n') + print(f'{nx}x{ny} cells, {int((occupied > 0).sum())} occupied, ' + f'{used} of {total} triangles in the area and band') + if world.missing: + print('not found or not loaded:', *sorted(world.missing)[:20], sep='\n ') + + +if __name__ == '__main__': + main() diff --git a/easyfleet_plansys2_warehouse_deployment/CMakeLists.txt b/easyfleet_plansys2_warehouse_deployment/CMakeLists.txt new file mode 100644 index 0000000..877227d --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.20) +project(easyfleet_plansys2_warehouse_deployment) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(easyfleet_mission_manager REQUIRED) +find_package(plansys2_domain_expert REQUIRED) +find_package(plansys2_executor REQUIRED) +find_package(plansys2_msgs REQUIRED) +find_package(plansys2_planner REQUIRED) +find_package(plansys2_problem_expert REQUIRED) + +add_executable(warehouse_controller src/warehouse_controller.cpp) +target_link_libraries(warehouse_controller + rclcpp::rclcpp + easyfleet_mission_manager::easyfleet_mission_manager + plansys2_domain_expert::plansys2_domain_expert + plansys2_executor::plansys2_executor + ${plansys2_msgs_TARGETS} + plansys2_planner::plansys2_planner + plansys2_problem_expert::plansys2_problem_expert +) + +install( + TARGETS warehouse_controller + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY launch config pddl behavior_trees rviz + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/easyfleet_plansys2_warehouse_deployment/README.md b/easyfleet_plansys2_warehouse_deployment/README.md new file mode 100644 index 0000000..8abf455 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/README.md @@ -0,0 +1,77 @@ +# easyfleet_plansys2_warehouse_deployment + +Four Kobukis in Warehouse A of [large_warehouse_world](https://github.com/juanscelyg/large_warehouse_world), driven by [PlanSys2](https://plansys2.github.io/). Two scouts locate packages in the rack bays, and two carriers take them to the docks. The mission controller builds the PDDL problem from the capabilities the robots announce, and PlanSys2 plans each goal with Kairos. Each action of the plan runs a behavior tree, and the leaves of that tree call the robot's EasyFleet capabilities through the [easyfleet_bt_nodes](https://github.com/EasyNavigation/EasyFleet/tree/rolling/easyfleet_bt_nodes) plugins. Navigation is real EasyNav; perception and manipulation are mocks (a Kobuki has no camera or arm). + +See the [top-level README](../README.md) for the full list of examples. + +## What runs where + +| Robot | Role | Home (map) | Capabilities | +|---|---|---|---| +| `robot_1` | scout | `home_1` (0, 0) | `navigation` (EasyNav), `perception` (fake, detects `package`) | +| `robot_2` | scout | `home_2` (2, 1) | `navigation` (EasyNav), `perception` (fake, detects `package`) | +| `robot_3` | carrier | `home_3` (4, 0) | `navigation` (EasyNav), `manipulation` (fake, 8 s per task) | +| `robot_4` | carrier | `home_4` (6, 1) | `navigation` (EasyNav), `manipulation` (fake, 8 s per task) | + +Each robot (`launch/robot_launch.yaml`, under its namespace) runs: + +- `system_main`: EasyNav on easyfleet_easynav_deployment's `maps/warehouse_a.yaml` and `maps/warehouse_a_routes.yaml` (`config/easynav_system.params.yaml`). Its initial pose comes from `config/initial_poses.yaml`. +- `navigation`: `easynav_navigation_capability_node`, with the waypoints of `config/navigation_params.yaml`. +- `robot_node`: from easyfleet_easynav_collaboration_deployment, hosting the capability of its role (`config/scout.yaml` or `config/carrier.yaml`). + +Central processes (`launch/warehouse_launch.yaml`): + +- `navigation_manager_node`: the fleet's map and routes, plus the TF relay for the central RViz. +- `central_rviz`: `rviz/warehouse.rviz`, with each robot's `base_link` and planned path. +- PlanSys2 (`plansys2_bringup` monolithic, Kairos, `config/plansys2_params.yaml`). +- One `bt_action_node` per action a robot may run at once: `move_1`..`move_4`, `inspect_1`..`inspect_2`, `pick_1`..`pick_2` and `drop_1`..`drop_2`. +- `warehouse_controller`: started 10 s after the rest. + +## The PDDL model + +`pddl/warehouse.pddl` has four durative actions. Each one runs a tree from `behavior_trees/`: + +| Action | Behavior tree | Capability called | +|---|---|---| +| `move ?r ?from ?to` | `CallNavigation` to the waypoint `?to` | `//navigation` | +| `inspect ?r ?p ?l` | `CallPerception` until a `package` is detected (20 s limit) | `//perception` | +| `pick ?r ?p ?l` | `CallManipulation`, named task `pick` | `//manipulation` | +| `drop ?r ?p ?l` | `CallManipulation`, named task `place` | `//manipulation` | + +The locations are the waypoints of the navigation capability, with the same names: four homes, the bays `bay_1`..`bay_4` in the aisles of the bottom rack block, and the docks `dock_2`..`dock_5` at the end of the dock spurs. Only one robot can be at a location (`free`). A carrier can only pick a package that a scout has inspected (`located`). The duration of `move` comes from the `distance` between the two locations, which the controller computes along the grid of aisles and corridors. + +## The mission + +`warehouse_controller` (`src/warehouse_controller.cpp`, `config/controller.yaml`) works in four steps: + +1. It waits until every robot announces `navigation` plus `perception` or `manipulation` (`FleetSession`). +2. It writes the problem from what it found: `can_perceive` for a robot with perception, `can_manipulate` for a robot with manipulation. A robot that never announces navigation is left out of the problem. +3. It adds the packages, the docks and the distances. +4. It reaches each goal of `goals` in turn: plan, print the plan, run it and report each action as it starts and ends. + +The goals are: + +1. `pkg_1` (bay_2) to `dock_3` and `pkg_2` (bay_3) to `dock_4`. +2. `pkg_3` (bay_1) to `dock_2` and `pkg_4` (bay_4) to `dock_5`. +3. Every robot back home. + +## Running it + +```bash +# Terminal 1: Gazebo with the four Kobukis and the Zenoh router +ros2 launch easyfleet_plansys2_warehouse_deployment simulation_launch.yaml + +# Terminal 2: the fleet, PlanSys2 and the mission +ros2 launch easyfleet_plansys2_warehouse_deployment warehouse_launch.yaml +``` + +Launch arguments: + +- `simulation_launch.yaml`: + - `gui:=false` runs Gazebo headless. + - `world:=` chooses the world. By default it is `large_warehouse_world_a.sdf`, which only furnishes Warehouse A, so that the four lidars keep the simulation in real time. +- `warehouse_launch.yaml`: + - `mission:=false` starts everything but the controller; the goals can then be set from the PlanSys2 terminal (`ros2 run plansys2_terminal plansys2_terminal`). + - `central_rviz:=false` skips the central RViz. + +The first goal takes about 6 minutes of simulated time. diff --git a/easyfleet_plansys2_warehouse_deployment/behavior_trees/drop.xml b/easyfleet_plansys2_warehouse_deployment/behavior_trees/drop.xml new file mode 100644 index 0000000..3fc9e40 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/behavior_trees/drop.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/easyfleet_plansys2_warehouse_deployment/behavior_trees/inspect.xml b/easyfleet_plansys2_warehouse_deployment/behavior_trees/inspect.xml new file mode 100644 index 0000000..c74e274 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/behavior_trees/inspect.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/easyfleet_plansys2_warehouse_deployment/behavior_trees/move.xml b/easyfleet_plansys2_warehouse_deployment/behavior_trees/move.xml new file mode 100644 index 0000000..83d6aab --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/behavior_trees/move.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/easyfleet_plansys2_warehouse_deployment/behavior_trees/pick.xml b/easyfleet_plansys2_warehouse_deployment/behavior_trees/pick.xml new file mode 100644 index 0000000..817608b --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/behavior_trees/pick.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/easyfleet_plansys2_warehouse_deployment/config/capabilities/manipulation.json b/easyfleet_plansys2_warehouse_deployment/config/capabilities/manipulation.json new file mode 100644 index 0000000..a6c4a87 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/capabilities/manipulation.json @@ -0,0 +1,69 @@ +{ + "name": "manipulation", + "display_name": "Execute a manipulator trajectory", + "description": "Executes a pre-planned joint-space (or Cartesian) trajectory on the robot's manipulator arm. This capability does not plan motions itself: the caller (typically a MoveIt planning step running elsewhere) must supply a complete, collision-free trajectory to execute. Use this once a target end-effector pose or joint configuration has already been resolved into concrete waypoints.", + "action": { + "name": "//manipulation, being the robot that announced it", + "type": "easyfleet_interfaces/action/Manipulation", + "goal": { + "mode": "uint8 - 0 MODE_JOINT_TARGET, 1 MODE_POSE_TARGET, 2 MODE_NAMED_TASK; selects which of the fields below is used.", + "joint_target": "sensor_msgs/JointState - target joint configuration, used when mode == MODE_JOINT_TARGET.", + "pose_target": "geometry_msgs/PoseStamped - target end-effector pose, used when mode == MODE_POSE_TARGET.", + "end_effector_link": "string - end-effector link name, used when mode == MODE_POSE_TARGET.", + "named_task": "string - name of a pre-programmed task to run, used when mode == MODE_NAMED_TASK.", + "parameters_json": "string - optional JSON escape hatch for stack-specific manipulation parameters (e.g. planner id, velocity scaling); empty for defaults." + }, + "feedback": { + "state": "string - human-readable execution state (e.g. 'EXECUTING').", + "current_joint_state": "sensor_msgs/JointState - manipulator's current joint state while executing.", + "progress": "float32 - execution progress, from 0.0 to 1.0." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT, 10 PLANNING_FAILED, 11 COLLISION_DETECTED.", + "error_msg": "string - human-readable explanation, populated when error_code is non-zero.", + "final_joint_state": "sensor_msgs/JointState - manipulator's joint state when the goal ended." + } + }, + "requirements": [ + "The trajectory must have already been planned (e.g. by a MoveIt planning pipeline) and be free of collisions; this capability performs no planning or collision checking of its own.", + "The trajectory's joint names must match the robot's actual manipulator joints.", + "The start state of the trajectory must be reasonably close to the robot's current state." + ], + "effects": [ + "On success (error_code == SUCCESS), the manipulator's joints end up at the requested target.", + "A currently executing trajectory can be preempted by a newer goal (see 'preemption' below): the previous goal is aborted and the manipulator starts executing the new trajectory instead." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "manipulation.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the current trajectory execution and immediately starts executing the new one.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must wait for the current execution to finish, or explicitly cancel it, before sending another." + }, + "parameters": { + "manipulation.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new trajectory goal is allowed to preempt (replace) one that is currently executing." + }, + "manipulation.mock_execution_duration": { + "type": "double", + "unit": "seconds", + "default": 3.0, + "description": "Mock/simulated time it takes to execute a trajectory, regardless of its actual number of waypoints. This implementation does not drive a real manipulator: it simulates progress over this duration for integration testing of the wider system." + }, + "manipulation.mock_feedback_period": { + "type": "double", + "unit": "seconds", + "default": 0.3, + "description": "How often feedback is published while a goal is executing." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + } + }, + "notes": [ + "This is a mock implementation, part of the EasyFleet architecture, standing in for a real manipulator stack (MoveIt or otherwise) during integration testing of multi-capability systems. It does not perform real trajectory execution or controller interaction: 'state' feedback simply reports 'EXECUTING' while progress is simulated on a timer.", + "Deployment: easyfleet_plansys2_warehouse_deployment, four Kobukis in Warehouse A of large_warehouse_world; PlanSys2 runs the plan, the leaves of each action's behavior tree call this capability." + ] +} diff --git a/easyfleet_plansys2_warehouse_deployment/config/capabilities/navigation.json b/easyfleet_plansys2_warehouse_deployment/config/capabilities/navigation.json new file mode 100644 index 0000000..5875995 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/capabilities/navigation.json @@ -0,0 +1,90 @@ +{ + "name": "navigation", + "display_name": "Navigate to a named waypoint (EasyNav-backed, real navigation)", + "description": "Moves the robot to one of a fixed set of named waypoints (configured on this node as ROS parameters) by driving a real EasyNav navigation stack through easynav::GoalManagerClient. Internally structured as a BehaviorTree.CPP tree (StartOff -> Navigate -> Finish). Unlike a mock navigation capability, this is a real easyfleet_core::NavigationActionServerBase backend -- see easyfleet_easynav_navigation.", + "action": { + "name": "//navigation, being the robot that announced it", + "type": "easyfleet_interfaces/action/Navigation", + "goal": { + "target_pose": "geometry_msgs/PoseStamped - unused by this backend.", + "waypoints": "geometry_msgs/PoseStamped[] - unused by this backend.", + "parameters_json": "string - REQUIRED for this backend: a JSON object with a string field \"goal_id\" naming one of the waypoints configured on this node (see 'parameters' below for the current list). Goals without a valid goal_id are rejected." + }, + "feedback": { + "current_pose": "geometry_msgs/PoseStamped - robot's current pose, as reported by EasyNav.", + "navigation_time": "builtin_interfaces/Duration - time elapsed since this navigation goal started, as reported by EasyNav.", + "estimated_time_remaining": "builtin_interfaces/Duration - as reported by EasyNav.", + "distance_remaining": "float32 - straight-line distance left to the goal, as reported by EasyNav.", + "number_of_recoveries": "int16 - always 0: EasyNav has no equivalent concept.", + "current_waypoint_index": "uint32 - always 0: this backend navigates to one waypoint per goal, not a waypoint list." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT, 10 NO_PATH_FOUND, 11 LOCALIZATION_LOST.", + "error_msg": "string - human-readable explanation, populated when error_code is non-zero.", + "final_pose": "geometry_msgs/PoseStamped - robot's pose when the goal ended, as reported by EasyNav.", + "total_time": "builtin_interfaces/Duration - not populated by this backend (left at its default)." + } + }, + "requirements": [ + "parameters_json must contain a \"goal_id\" naming one of this node's configured waypoints.", + "EasyNav (easynav_system system_main) must already be running and reachable on the easynav_control topic, localized on the Warehouse A map, with this robot's Gazebo instance publishing scan_raw and the TF tree the costmap localizer/controller expect." + ], + "effects": [ + "On success (error_code == SUCCESS), the robot's physical location changes to be at (or very near) the requested waypoint.", + "A currently executing goal can be preempted by a newer navigation goal: EasyNav is redirected to the new waypoint without stopping first (see 'preemption' below).", + "A canceled or capability-shutdown goal sends EasyNav an explicit cancellation, stopping the robot." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "navigation.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the previous ROS goal, but EasyNav itself is redirected (not stopped) to the new waypoint -- the robot keeps moving, just towards the new target.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must wait for the current navigation to finish, or explicitly cancel it, before sending another." + }, + "parameters": { + "navigation.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new navigation goal is allowed to preempt (replace) one that is currently executing." + }, + "navigation.tick_rate_hz": { + "type": "double", + "unit": "hertz", + "default": 10.0, + "description": "Rate at which the internal behavior tree is ticked while a goal is executing." + }, + "navigation.waypoint_ids": { + "type": "string[]", + "default": [ + "home_1", + "home_2", + "home_3", + "home_4", + "bay_1", + "bay_2", + "bay_3", + "bay_4", + "dock_2", + "dock_3", + "dock_4", + "dock_5" + ], + "description": "The set of named waypoints this node accepts as parameters_json.goal_id, valid on the Warehouse A map (see navigation_params.yaml). Each id must have a matching navigation.waypoints..{frame_id,x,y,yaw} sub-parameter." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + }, + "behavior_tree_xml": { + "type": "string", + "description": "Path to the BehaviorTree.CPP XML tree to run per goal (StartOff -> Navigate -> Finish, reused from easyfleet_easynav_deployment)." + }, + "bt_plugins": { + "type": "string[]", + "description": "Paths to the BT.CPP plugin shared libraries (BT_REGISTER_NODES) to load before building the tree -- every node the tree XML references must come from one of these. Reused directly from easyfleet_easynav_deployment (libstart_off_bt_node.so/libfinish_bt_node.so) and easyfleet_easynav_navigation (libeasynav_navigate_bt_node.so); this package supplies no BT plugins of its own." + } + }, + "notes": [ + "The executable itself (easynav_navigation_capability_node) is not duplicated in this package: it is reused directly from easyfleet_easynav_deployment, launched twice (once per robot) with different namespaces/parameters.", + "Deployment: easyfleet_plansys2_warehouse_deployment, four Kobukis in Warehouse A of large_warehouse_world; PlanSys2 runs the plan, the leaves of each action's behavior tree call this capability." + ] +} diff --git a/easyfleet_plansys2_warehouse_deployment/config/capabilities/perception.json b/easyfleet_plansys2_warehouse_deployment/config/capabilities/perception.json new file mode 100644 index 0000000..e405607 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/capabilities/perception.json @@ -0,0 +1,70 @@ +{ + "name": "perception", + "display_name": "Detect objects of a known class", + "description": "Runs a continuous object-detection stream for a requested object class, publishing detections (both 2D image-plane and 3D spatial) as feedback until the caller cancels the goal. This mock implementation only knows how to detect one class: packages ('package'). Any goal that does not request that class is rejected outright.", + "action": { + "name": "//perception, being the robot that announced it", + "type": "easyfleet_interfaces/action/Perception", + "goal": { + "object_classes": "string[] - object classes to look for. Must include 'package' (case-insensitive substring match); any other request is rejected since this mock does not detect other classes.", + "continuous": "bool - accepted but not honored by this mock: every goal streams until canceled/preempted regardless of this flag.", + "max_rate_hz": "float32 - accepted but not honored by this mock: the fixed 'perception.detection_rate_hz' parameter is used instead.", + "parameters_json": "string - optional JSON escape hatch for stack-specific perception parameters; empty for defaults." + }, + "feedback": { + "detections_3d": "vision_msgs/Detection3DArray - 1 to 3 randomly placed 3D detections per message, published continuously while the goal is active.", + "detections_2d": "vision_msgs/Detection2DArray - the same detections' 2D image-plane bounding boxes, published alongside detections_3d." + }, + "result": { + "error_code": "uint16 - 0 SUCCESS, 1 REJECTED, 2 ABORTED, 3 CANCELED, 4 TIMEOUT. CANCELED is the expected way this capability's goals end (this mock never completes on its own); ABORTED means the goal was preempted or the capability shut down.", + "error_msg": "string - populated when error_code is not SUCCESS/CANCELED, explaining why.", + "detections_3d": "vision_msgs/Detection3DArray - final detection snapshot; empty in this mock implementation.", + "detections_2d": "vision_msgs/Detection2DArray - final detection snapshot; empty in this mock implementation." + } + }, + "requirements": [ + "The requested 'object_classes' list must include the target class this mock supports (default 'package'); otherwise the goal is rejected immediately.", + "This action never completes on its own: it streams feedback indefinitely until the caller cancels the goal (or it is preempted by a new one)." + ], + "effects": [ + "None on the physical world: this is a perception-only capability that reports detections, it does not act on them.", + "While a goal is active, detections_3d/detections_2d feedback is published at a steady rate (see 'perception.detection_rate_hz')." + ], + "preemption": { + "supported": true, + "controlled_by_parameter": "perception.allow_preemption", + "behavior_when_enabled": "Sending a new goal while one is active aborts the previous goal (error_code=ABORTED) and starts detecting for the new request instead.", + "behavior_when_disabled": "Sending a new goal while one is active is rejected outright; the caller must cancel the current goal first." + }, + "parameters": { + "perception.allow_preemption": { + "type": "bool", + "default": true, + "description": "Whether a new perception goal is allowed to preempt (replace) one that is currently executing." + }, + "perception.target_class": { + "type": "string", + "default": "package", + "description": "The only object class this mock can 'detect'. A goal is only accepted if its 'objects' list contains this string (case-insensitive substring match)." + }, + "perception.detection_rate_hz": { + "type": "double", + "unit": "hertz", + "default": 20.0, + "description": "Rate at which detections_3d/detections_2d feedback is published while a goal is active." + }, + "perception.frame_id": { + "type": "string", + "default": "camera_link", + "description": "Frame id stamped on the published detections." + }, + "capabilities_file": { + "type": "string", + "description": "Path to this very capability description file; published verbatim on /capabilities when the node is activated." + } + }, + "notes": [ + "This is a mock implementation, part of the EasyFleet architecture, standing in for a real perception stack (a classic detector, a VLM-based one, or otherwise) during integration testing of multi-capability systems. Detections are random placeholder positions, not derived from any sensor data.", + "Deployment: easyfleet_plansys2_warehouse_deployment, four Kobukis in Warehouse A of large_warehouse_world; PlanSys2 runs the plan, the leaves of each action's behavior tree call this capability." + ] +} diff --git a/easyfleet_plansys2_warehouse_deployment/config/carrier.yaml b/easyfleet_plansys2_warehouse_deployment/config/carrier.yaml new file mode 100644 index 0000000..ca1c733 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/carrier.yaml @@ -0,0 +1,25 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# A carrier: robot_node hosts a fake manipulation capability (pick, place). +/**: + ros__parameters: + capabilities: ["manipulation"] + config_package: "easyfleet_plansys2_warehouse_deployment" + config_subdir: "capabilities" + manipulation: + allow_preemption: true + mock_execution_duration: 8.0 + mock_feedback_period: 0.3 diff --git a/easyfleet_plansys2_warehouse_deployment/config/controller.yaml b/easyfleet_plansys2_warehouse_deployment/config/controller.yaml new file mode 100644 index 0000000..3dd79f7 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/controller.yaml @@ -0,0 +1,29 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# The mission of warehouse_controller: the robots (with their homes, where +# they start), the docks, where the packages are, and the goals reached one +# after another. The locations are the waypoints of config/navigation_params.yaml. +/**: + ros__parameters: + robots: ["robot_1", "robot_2", "robot_3", "robot_4"] + homes: ["home_1", "home_2", "home_3", "home_4"] + docks: ["dock_2", "dock_3", "dock_4", "dock_5"] + packages: ["pkg_1:bay_2", "pkg_2:bay_3", "pkg_3:bay_1", "pkg_4:bay_4"] + goals: + - "(and (package_at pkg_1 dock_3) (package_at pkg_2 dock_4))" + - "(and (package_at pkg_3 dock_2) (package_at pkg_4 dock_5))" + - "(and (robot_at robot_1 home_1) (robot_at robot_2 home_2) (robot_at robot_3 home_3) (robot_at robot_4 home_4))" + discovery_timeout: 60.0 diff --git a/easyfleet_plansys2_warehouse_deployment/config/easynav_system.params.yaml b/easyfleet_plansys2_warehouse_deployment/config/easynav_system.params.yaml new file mode 100644 index 0000000..b6be62b --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/easynav_system.params.yaml @@ -0,0 +1,151 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# EasyNav of every Kobuki in Warehouse A (launch/robot_launch.yaml), on +# easyfleet_easynav_deployment's maps/warehouse_a.yaml and maps/warehouse_a_routes.yaml. +# Each robot's initial pose comes from config/initial_poses.yaml. + +/**/controller_node: + ros__parameters: + use_sim_time: true + colision_checker: + active: true + debug_markers: true + downsample_leaf_size: 0.05 + robot_radius: 0.30 + brake_acc: 1.0 + safety_margin: 0.05 + controller_types: [rpp] + rpp: + rt_freq: 30.0 + plugin: easynav_regulated_pp_controller/RegulatedPurePursuitController + # Velocity / acceleration limits (TurtleBot 2 / Kobuki base) + max_linear_vel: 0.6 + min_linear_vel: -0.3 + max_angular_vel: 1.2 + min_angular_vel: -1.2 + max_linear_accel: 1.0 + max_linear_decel: 1.0 + max_angular_accel: 2.0 + max_angular_decel: 2.0 + use_dynamic_window: false + allow_reversing: false + # Lookahead + lookahead_dist: 0.5 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.2 + use_velocity_scaled_lookahead_dist: true + # Rotate to heading + use_rotate_to_heading: true + rotate_to_heading_angular_vel: 1.0 + rotate_to_heading_min_angle: 0.785 + # Curvature regulation + use_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.15 + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 1.0 + interpolate_curvature_after_goal: false + # Obstacle-proximity regulation (EasyNav's point-cloud based equivalent of Nav2's cost-based term) + use_obstacle_regulated_linear_velocity_scaling: true + obstacle_scaling_dist: 0.4 + obstacle_scaling_gain: 0.8 + # Approach to goal + min_approach_linear_velocity: 0.05 + approach_velocity_scaling_dist: 0.6 + # Goal tolerances (matching the serest config: 0.1 m / 6 deg) + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.105 + +/**/localizer_node: + ros__parameters: + use_sim_time: true + localizer_types: [costmap] + costmap: + rt_freq: 50.0 + freq: 5.0 + reseed_freq: 1.0 + plugin: easynav_costmap_localizer/AMCLLocalizer + num_particles: 100 + noise_translation: 0.05 + noise_rotation: 0.1 + noise_translation_to_rotation: 0.1 + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 + std_dev_xy: 0.02 + std_dev_yaw: 0.01 + +/**/maps_manager_node: + ros__parameters: + use_sim_time: true + map_types: [costmap, routes] + routes: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_routes_maps_manager/RoutesMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a_routes.yaml + filters: [routes_costmap] + routes_costmap: + plugin: easynav_routes_maps_manager/RoutesCostmapFilter + min_cost: 200 + route_width: 0.1 + cycle_time_nort: 0.001 + cycle_time_rt: 0.001 + costmap: + freq: 2.0 # 1010x1010 cells: 10 Hz is not kept + plugin: easynav_costmap_maps_manager/CostmapMapsManager + package: easyfleet_easynav_deployment + map_path_file: maps/warehouse_a.yaml + filters: [obstacles, inflation] + obstacles: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter + inflation: + plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter + inflation_radius: 1.3 + inscribed_radius: 0.25 + cost_scaling_factor: 3.0 + +/**/planner_node: + ros__parameters: + use_sim_time: true + planner_types: [simple] + simple: + freq: 0.5 + plugin: easynav_costmap_planner/CostmapPlanner + cost_factor: 10.0 + continuous_replan: true + +/**/sensors_node: + ros__parameters: + use_sim_time: true + forget_time: 0.5 + sensors: [laser1, fleet] + laser1: + topic: scan_raw + type: sensor_msgs/msg/LaserScan + # The other robots, from navigation_manager_node (robots.share_obstacles). + fleet: + topic: fleet_obstacles + type: sensor_msgs/msg/PointCloud2 + +/**/system_node: + ros__parameters: + use_sim_time: true + use_real_time: true + position_tolerance: 0.3 + angle_tolerance: 0.15 diff --git a/easyfleet_plansys2_warehouse_deployment/config/initial_poses.yaml b/easyfleet_plansys2_warehouse_deployment/config/initial_poses.yaml new file mode 100644 index 0000000..482c40e --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/initial_poses.yaml @@ -0,0 +1,45 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Initial pose of each robot's localizer: its spawn pose in launch/simulation_launch.yaml +# (map frame, origin at the center of Warehouse A). +/robot_1/localizer_node: + ros__parameters: + costmap: + initial_pose: + x: 0.0 + y: 0.0 + yaw: 0.0 +/robot_2/localizer_node: + ros__parameters: + costmap: + initial_pose: + x: 2.0 + y: 1.0 + yaw: 0.0 +/robot_3/localizer_node: + ros__parameters: + costmap: + initial_pose: + x: 4.0 + y: 0.0 + yaw: 0.0 +/robot_4/localizer_node: + ros__parameters: + costmap: + initial_pose: + x: 6.0 + y: 1.0 + yaw: 0.0 diff --git a/easyfleet_plansys2_warehouse_deployment/config/navigation_manager.params.yaml b/easyfleet_plansys2_warehouse_deployment/config/navigation_manager.params.yaml new file mode 100644 index 0000000..613e1d7 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/navigation_manager.params.yaml @@ -0,0 +1,33 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# The fleet's map and routes (Warehouse A), shared by the four robots. Only +# what places each base_link in "map" is relayed to /tf, for the central RViz. +# Each robot also gets the others as obstacles (//fleet_obstacles): a +# Kobuki's lidar barely sees another Kobuki (only its thin standoffs). +/**: + ros__parameters: + map_type: "costmap" + map.package: "easyfleet_easynav_deployment" + map.map_path_file: "maps/warehouse_a.yaml" + routes.package: "easyfleet_easynav_deployment" + routes.map_path_file: "maps/warehouse_a_routes.yaml" + # Each robot keeps its TF under its namespace; only what places its + # base_link in "map" reaches /tf, at most 10 times per second. + robots.relay_tf: true + robots.relay_tf_frames: ["odom", "base_footprint", "base_link"] + robots.relay_tf_rate_hz: 10.0 + robots.share_obstacles: true + robots.obstacle_radius_m: 0.25 diff --git a/easyfleet_plansys2_warehouse_deployment/config/navigation_params.yaml b/easyfleet_plansys2_warehouse_deployment/config/navigation_params.yaml new file mode 100644 index 0000000..dff85b1 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/navigation_params.yaml @@ -0,0 +1,87 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Waypoints of every robot's navigation capability, named as the locations of +# pddl/warehouse.pddl (map frame, origin at the center of Warehouse A): the +# robots' homes in the central plaza, rack bays in the aisles of the bottom +# block and the docks at the end of the dock spurs of maps/warehouse_a_routes.yaml. +# The controller reads the same file to give the planner the distances. +/**: + ros__parameters: + navigation: + allow_preemption: true + tick_rate_hz: 10.0 + waypoint_ids: ["home_1", "home_2", "home_3", "home_4", "bay_1", "bay_2", "bay_3", "bay_4", "dock_2", "dock_3", "dock_4", "dock_5"] + waypoints: + home_1: + frame_id: "map" + x: 0.0 + y: 0.0 + yaw: 0.0 + home_2: + frame_id: "map" + x: 2.0 + y: 1.0 + yaw: 0.0 + home_3: + frame_id: "map" + x: 4.0 + y: 0.0 + yaw: 0.0 + home_4: + frame_id: "map" + x: 6.0 + y: 1.0 + yaw: 0.0 + bay_1: + frame_id: "map" + x: -28.3 + y: -15.0 + yaw: -1.57 + bay_2: + frame_id: "map" + x: -12.5 + y: -15.0 + yaw: -1.57 + bay_3: + frame_id: "map" + x: 12.9 + y: -15.0 + yaw: -1.57 + bay_4: + frame_id: "map" + x: 28.5 + y: -15.0 + yaw: -1.57 + dock_2: + frame_id: "map" + x: -37.1 + y: -89.5 + yaw: -1.57 + dock_3: + frame_id: "map" + x: -12.1 + y: -89.3 + yaw: -1.57 + dock_4: + frame_id: "map" + x: 12.9 + y: -89.3 + yaw: -1.57 + dock_5: + frame_id: "map" + x: 37.9 + y: -90.0 + yaw: -1.57 diff --git a/easyfleet_plansys2_warehouse_deployment/config/plansys2_params.yaml b/easyfleet_plansys2_warehouse_deployment/config/plansys2_params.yaml new file mode 100644 index 0000000..eda01d9 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/plansys2_params.yaml @@ -0,0 +1,27 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +lifecycle_manager: + ros__parameters: + startup_timeout: 5 +planner: + ros__parameters: + plan_solver_timeout: 15.0 + plan_solver_plugins: ["KAIROS"] + KAIROS: + plugin: "plansys2/KairosPlanSolver" + # Keeps improving the plan until 2 s pass without a better one: the first + # plans of this domain send carriers on long detours. + no_improvement: 2.0 diff --git a/easyfleet_plansys2_warehouse_deployment/config/scout.yaml b/easyfleet_plansys2_warehouse_deployment/config/scout.yaml new file mode 100644 index 0000000..3e14363 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/config/scout.yaml @@ -0,0 +1,26 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# A scout: robot_node hosts a fake perception capability that detects packages. +/**: + ros__parameters: + capabilities: ["perception"] + config_package: "easyfleet_plansys2_warehouse_deployment" + config_subdir: "capabilities" + perception: + allow_preemption: true + target_class: "package" + detection_rate_hz: 5.0 + frame_id: "camera_link" diff --git a/easyfleet_plansys2_warehouse_deployment/launch/robot_launch.yaml b/easyfleet_plansys2_warehouse_deployment/launch/robot_launch.yaml new file mode 100644 index 0000000..0b87cbb --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/launch/robot_launch.yaml @@ -0,0 +1,99 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# One Kobuki of the fleet, under the namespace "robot": EasyNav (system_main) +# with its navigation capability, and robot_node with the capability of its +# role: "scout" (perception) or "carrier" (manipulation). + +launch: + - arg: {name: "robot", description: "Robot name and namespace, e.g. robot_1"} + - arg: {name: "role", description: "scout (perception) or carrier (manipulation)"} + - arg: {name: "rviz", default: "false", description: "Open this robot's own rviz2"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - group: + - push_ros_namespace: + namespace: "$(var robot)" + + - node: + pkg: "easynav_system" + exec: "system_main" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/easynav_system.params.yaml" + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/initial_poses.yaml" + - name: "tf_prefix" + value: "$(var robot)" + remap: + # Each robot has its own TF tree, under its namespace. + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + # Fleet-wide map and routes, from navigation_manager_node. + - from: "maps_manager_node/costmap/incoming_map" + to: "/global_map" + - from: "maps_manager_node/routes/incoming_routes" + to: "/global_routes" + + - node: + pkg: "easyfleet_easynav_deployment" + exec: "easynav_navigation_capability_node" + name: "navigation" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/navigation_params.yaml" + - name: "behavior_tree_xml" + value: "$(find-pkg-share easyfleet_easynav_deployment)/behavior_trees/navigate.xml" + - name: "bt_plugins" + value: + - "$(find-pkg-prefix easyfleet_easynav_navigation)/lib/libeasynav_navigate_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libstart_off_bt_node.so" + - "$(find-pkg-prefix easyfleet_easynav_deployment)/lib/libfinish_bt_node.so" + - name: "capabilities_file" + value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/capabilities/navigation.json" + + - node: + pkg: "easyfleet_easynav_collaboration_deployment" + exec: "robot_node" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/$(var role).yaml" + + - node: + if: "$(var rviz)" + pkg: "rviz2" + exec: "rviz2" + output: "screen" + args: "-d $(find-pkg-share easyfleet_easynav_deployment)/rviz/easynav_costmap.rviz -f $(var robot)/map" + param: + - name: "use_sim_time" + value: true + remap: + - from: "/tf" + to: "tf" + - from: "/tf_static" + to: "tf_static" + - from: "/maps_manager_node/costmap/dynamic_map" + to: "maps_manager_node/costmap/dynamic_map" + - from: "/maps_manager_node/costmap/dynamic_map_updates" + to: "maps_manager_node/costmap/dynamic_map_updates" + - from: "/scan_raw" + to: "scan_raw" + - from: "/localizer_node/costmap/particles" + to: "localizer_node/costmap/particles" + - from: "/planner_node/simple/path" + to: "planner_node/simple/path" + - from: "/goal_pose" + to: "goal_pose" diff --git a/easyfleet_plansys2_warehouse_deployment/launch/simulation_launch.yaml b/easyfleet_plansys2_warehouse_deployment/launch/simulation_launch.yaml new file mode 100644 index 0000000..36dc57e --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/launch/simulation_launch.yaml @@ -0,0 +1,85 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# Gazebo with Warehouse A of large_warehouse_world and four Kobukis in its +# central plaza, at world (100, 300), (102, 301), (104, 300) and (106, 301): +# (0, 0), (2, 1), (4, 0) and (6, 1) in the warehouse map, the robots' homes. +# Each robot's topics and TF tree are under its namespace (robot_1..robot_4). +# Also starts the Zenoh router. Run it before warehouse_launch.yaml. + +launch: + - arg: {name: "gui", default: "true", description: "Set to false to run Gazebo headless"} + # At 12 m the lidar sees almost nothing in the open areas of the warehouse; at 30 m, 200+ of 360 rays hit. + - arg: {name: "lidar_range", default: "30.0", description: "Maximum lidar range (m)"} + # Each Kobuki carries a cylinder at lidar height (lidar_target) so the others' lidars see it. + # Warehouse A furniture only: with the whole world's, the lidars slow the simulation down. + - arg: + name: "world" + default: "$(find-pkg-share large_warehouse_world)/worlds/large_warehouse_world_a.sdf" + description: "World file (large_warehouse_world.sdf for the whole furnished world)" + # Zenoh by default: the router every node of this example connects to. + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + - executable: + cmd: "ros2 run rmw_zenoh_cpp rmw_zenohd" + output: "screen" + # Gazebo finds the warehouse models here (the playground adds its own). + - set_env: + name: "GZ_SIM_RESOURCE_PATH" + value: "$(find-pkg-share large_warehouse_world)/models:$(env GZ_SIM_RESOURCE_PATH '')" + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/world.launch.yaml" + arg: + - {name: "world", value: "$(var world)"} + - {name: "gui", value: "$(var gui)"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_1"} + - {name: "namespace", value: "robot_1"} + - {name: "x", value: "100.0"} + - {name: "y", value: "300.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_2"} + - {name: "namespace", value: "robot_2"} + - {name: "x", value: "102.0"} + - {name: "y", value: "301.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_3"} + - {name: "namespace", value: "robot_3"} + - {name: "x", value: "104.0"} + - {name: "y", value: "300.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} + - include: + file: "$(find-pkg-share easynav_playground_kobuki_worlds)/launch/kobuki.launch.yaml" + arg: + - {name: "name", value: "kobuki_4"} + - {name: "namespace", value: "robot_4"} + - {name: "x", value: "106.0"} + - {name: "y", value: "301.0"} + - {name: "z", value: "0.05"} + - {name: "lidar_range", value: "$(var lidar_range)"} + - {name: "lidar_target", value: "true"} diff --git a/easyfleet_plansys2_warehouse_deployment/launch/warehouse_launch.yaml b/easyfleet_plansys2_warehouse_deployment/launch/warehouse_launch.yaml new file mode 100644 index 0000000..975d1a7 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/launch/warehouse_launch.yaml @@ -0,0 +1,176 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +# The fleet and its mission: the four robots (robot_1 and robot_2 scouts, +# robot_3 and robot_4 carriers), the navigation manager with the central +# RViz, PlanSys2 (Kairos) with one behavior-tree action performer per action +# a robot may run at once, and the mission controller. Each action's tree +# calls the robot's capabilities (easyfleet_bt_nodes). Run +# simulation_launch.yaml first. + +launch: + - arg: {name: "mission", default: "true", description: "Run the mission controller"} + - arg: {name: "central_rviz", default: "true", description: "Open the central RViz"} + - set_env: {name: "RMW_IMPLEMENTATION", value: "rmw_zenoh_cpp"} + + - node: + pkg: "easyfleet_navigation_manager" + exec: "navigation_manager_node" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/navigation_manager.params.yaml" + + - node: + if: "$(var central_rviz)" + pkg: "rviz2" + exec: "rviz2" + name: "central_rviz" + output: "screen" + args: "-d $(find-pkg-share easyfleet_plansys2_warehouse_deployment)/rviz/warehouse.rviz" + param: + - name: "use_sim_time" + value: true + + - include: + file: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/launch/robot_launch.yaml" + arg: + - {name: "robot", value: "robot_1"} + - {name: "role", value: "scout"} + - include: + file: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/launch/robot_launch.yaml" + arg: + - {name: "robot", value: "robot_2"} + - {name: "role", value: "scout"} + - include: + file: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/launch/robot_launch.yaml" + arg: + - {name: "robot", value: "robot_3"} + - {name: "role", value: "carrier"} + - include: + file: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/launch/robot_launch.yaml" + arg: + - {name: "robot", value: "robot_4"} + - {name: "role", value: "carrier"} + + - include: + file: "$(find-pkg-share plansys2_bringup)/launch/plansys2_bringup_launch_monolithic.py" + arg: + - {name: "model_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/pddl/warehouse.pddl"} + - {name: "params_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/plansys2_params.yaml"} + + # Action performers: as many of each action as robots may run it at once. + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "move_1" + output: "screen" + param: + - {name: "action_name", value: "move"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/move.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "move_2" + output: "screen" + param: + - {name: "action_name", value: "move"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/move.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "move_3" + output: "screen" + param: + - {name: "action_name", value: "move"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/move.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "move_4" + output: "screen" + param: + - {name: "action_name", value: "move"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/move.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "inspect_1" + output: "screen" + param: + - {name: "action_name", value: "inspect"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/inspect.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "inspect_2" + output: "screen" + param: + - {name: "action_name", value: "inspect"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/inspect.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "pick_1" + output: "screen" + param: + - {name: "action_name", value: "pick"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/pick.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "pick_2" + output: "screen" + param: + - {name: "action_name", value: "pick"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/pick.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "drop_1" + output: "screen" + param: + - {name: "action_name", value: "drop"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/drop.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + - node: + pkg: "plansys2_bt_actions" + exec: "bt_action_node" + name: "drop_2" + output: "screen" + param: + - {name: "action_name", value: "drop"} + - {name: "bt_xml_file", value: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/behavior_trees/drop.xml"} + - {name: "plugins", value: ["easyfleet_capability_bt_nodes"]} + + # Once every capability has had time to come up. + - timer: + if: "$(var mission)" + period: 10.0 + children: + - node: + pkg: "easyfleet_plansys2_warehouse_deployment" + exec: "warehouse_controller" + output: "screen" + param: + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/controller.yaml" + - from: "$(find-pkg-share easyfleet_plansys2_warehouse_deployment)/config/navigation_params.yaml" diff --git a/easyfleet_plansys2_warehouse_deployment/package.xml b/easyfleet_plansys2_warehouse_deployment/package.xml new file mode 100644 index 0000000..0705a4e --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/package.xml @@ -0,0 +1,49 @@ + + + + easyfleet_plansys2_warehouse_deployment + 0.1.0 + + Four Kobukis in Warehouse A of large_warehouse_world, driven by PlanSys2: + scouts locate packages in the rack bays (perception), carriers take them + to the docks (manipulation). The controller builds the PDDL problem from + the capabilities the robots announce, and the behavior tree of each + action calls them (easyfleet_bt_nodes). Navigation is real EasyNav; + perception and manipulation are mocks. + + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + rclcpp + easyfleet_mission_manager + plansys2_domain_expert + plansys2_executor + plansys2_msgs + plansys2_planner + plansys2_problem_expert + + launch + launch_ros + launch_yaml + easyfleet_bt_nodes + easyfleet_easynav_collaboration_deployment + easyfleet_easynav_deployment + easyfleet_navigation_manager + easynav_system + easynav_playground_kobuki_worlds + large_warehouse_world + plansys2_bringup + plansys2_bt_actions + plansys2_kairos_plan_solver + rmw_zenoh_cpp + rviz2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/easyfleet_plansys2_warehouse_deployment/pddl/warehouse.pddl b/easyfleet_plansys2_warehouse_deployment/pddl/warehouse.pddl new file mode 100644 index 0000000..d73f851 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/pddl/warehouse.pddl @@ -0,0 +1,122 @@ +;; Copyright 2026 Intelligent Robotics Lab +;; +;; This file is part of the project EasyFleet +;; 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. + +;; Warehouse A logistics: scouts (perception) locate packages in the rack +;; bays, carriers (manipulation) pick them and drop them at the docks. +;; A location holds one robot at a time. Each action runs a behavior tree +;; whose leaves call the robot's EasyFleet capabilities. +(define (domain warehouse) +(:requirements :strips :typing :fluents :durative-actions) + +(:types + robot + location + package +) + +(:predicates + (robot_at ?r - robot ?l - location) + (free ?l - location) + (idle ?r - robot) + (can_perceive ?r - robot) + (can_manipulate ?r - robot) + (hand_empty ?r - robot) + (is_dock ?l - location) + (package_at ?p - package ?l - location) + (located ?p - package) + (carrying ?r - robot ?p - package) +) + +(:functions + ;; meters along the routes; robots average about 0.4 m/s + (distance ?from ?to - location) +) + +;; navigation capability +(:durative-action move + :parameters (?r - robot ?from ?to - location) + :duration (= ?duration (/ (distance ?from ?to) 0.4)) + :condition (and + (at start (robot_at ?r ?from)) + (at start (free ?to)) + (at start (idle ?r)) + ) + :effect (and + (at start (not (robot_at ?r ?from))) + (at start (free ?from)) + (at start (not (free ?to))) + (at start (not (idle ?r))) + (at end (robot_at ?r ?to)) + (at end (idle ?r)) + ) +) + +;; perception capability +(:durative-action inspect + :parameters (?r - robot ?p - package ?l - location) + :duration (= ?duration 5) + :condition (and + (at start (can_perceive ?r)) + (at start (idle ?r)) + (over all (robot_at ?r ?l)) + (over all (package_at ?p ?l)) + ) + :effect (and + (at start (not (idle ?r))) + (at end (located ?p)) + (at end (idle ?r)) + ) +) + +;; manipulation capability +(:durative-action pick + :parameters (?r - robot ?p - package ?l - location) + :duration (= ?duration 10) + :condition (and + (at start (can_manipulate ?r)) + (at start (idle ?r)) + (at start (hand_empty ?r)) + (at start (located ?p)) + (at start (package_at ?p ?l)) + (over all (robot_at ?r ?l)) + ) + :effect (and + (at start (not (idle ?r))) + (at start (not (hand_empty ?r))) + (at start (not (package_at ?p ?l))) + (at end (carrying ?r ?p)) + (at end (idle ?r)) + ) +) + +;; manipulation capability +(:durative-action drop + :parameters (?r - robot ?p - package ?l - location) + :duration (= ?duration 10) + :condition (and + (at start (idle ?r)) + (at start (carrying ?r ?p)) + (at start (is_dock ?l)) + (over all (robot_at ?r ?l)) + ) + :effect (and + (at start (not (idle ?r))) + (at start (not (carrying ?r ?p))) + (at end (package_at ?p ?l)) + (at end (hand_empty ?r)) + (at end (idle ?r)) + ) +) +) diff --git a/easyfleet_plansys2_warehouse_deployment/rviz/warehouse.rviz b/easyfleet_plansys2_warehouse_deployment/rviz/warehouse.rviz new file mode 100644 index 0000000..4c04f58 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/rviz/warehouse.rviz @@ -0,0 +1,332 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Map1/Topic1 + - /MarkerArray1 + - /MarkerArray1/Topic1 + - /InteractiveMarkers1 + Splitter Ratio: 0.5 + Tree Height: 553 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + Value: true + routes_arrow: true + routes_line: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_routes_markers + Value: true + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: true + Enabled: true + Interactive Markers Namespace: /global_routes_imarkers + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: robot_1 base_link + Radius: 0.05 + Reference Frame: robot_1/base_link + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_1 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: robot_2 base_link + Radius: 0.05 + Reference Frame: robot_2/base_link + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 170; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_2 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_2/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: robot_3 base_link + Radius: 0.05 + Reference Frame: robot_3/base_link + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 170; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_3 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_3/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: robot_4 base_link + Radius: 0.05 + Reference Frame: robot_4/base_link + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 127 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: robot_4 path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_4/planner_node/simple/path + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MissionStatus + Namespaces: + {} + Topic: + Depth: 10 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /mission_status_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 7.371110439300537 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.17309048771858215 + Y: 3.5464937686920166 + Z: 3.0858142375946045 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.910398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.0103977918624878 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 850 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000187000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002b4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005130000003efc0100000002fb0000000800540069006d00650100000000000005130000026500fffffffb0000000800540069006d0065010000000000000450000000000000000000000513000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1299 + X: 1397 + Y: 17 diff --git a/easyfleet_plansys2_warehouse_deployment/src/warehouse_controller.cpp b/easyfleet_plansys2_warehouse_deployment/src/warehouse_controller.cpp new file mode 100644 index 0000000..b65dce7 --- /dev/null +++ b/easyfleet_plansys2_warehouse_deployment/src/warehouse_controller.cpp @@ -0,0 +1,362 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project EasyFleet +// 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. + +// Mission controller of the Warehouse A fleet: discovers what each robot can +// do (EasyFleet capabilities), turns it into the PlanSys2 problem +// (pddl/warehouse.pddl) and reaches each goal of the mission in turn by +// planning and running the plan. The actions' behavior trees call the +// robots' capabilities (easyfleet_bt_nodes). + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "easyfleet_mission_manager/fleet_session.hpp" +#include "easyfleet_mission_manager/robot_handle.hpp" + +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_executor/ExecutorClient.hpp" +#include "plansys2_msgs/action/execute_plan.hpp" +#include "plansys2_msgs/msg/action_execution_info.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" + +using namespace std::chrono_literals; + +namespace +{ + +struct Point +{ + double x{0.0}; + double y{0.0}; +}; + +/// Everything the controller is configured with. +struct Config +{ + std::vector robots; + std::vector homes; + std::map locations; + std::set docks; + std::vector> packages; // package, location + std::vector goals; + double discovery_timeout{60.0}; +}; + +/// "a:b" -> {a, b}; empty strings if there is no ':'. +std::pair split_pair(const std::string & item) +{ + const auto colon = item.find(':'); + if (colon == std::string::npos) { + return {"", ""}; + } + return {item.substr(0, colon), item.substr(colon + 1)}; +} + +bool read_config(rclcpp::Node & node, Config & config) +{ + config.robots = node.declare_parameter("robots", std::vector()); + config.homes = node.declare_parameter("homes", std::vector()); + const auto docks = node.declare_parameter("docks", std::vector()); + config.docks = std::set(docks.begin(), docks.end()); + config.goals = node.declare_parameter("goals", std::vector()); + config.discovery_timeout = node.declare_parameter("discovery_timeout", 60.0); + for (const auto & item : node.declare_parameter("packages", std::vector())) { + const auto [package, location] = split_pair(item); + if (package.empty() || location.empty()) { + RCLCPP_ERROR(node.get_logger(), "packages: '%s' is not :", item.c_str()); + return false; + } + config.packages.emplace_back(package, location); + } + // Waypoints of the navigation capability (config/navigation_params.yaml). + const auto ids = node.declare_parameter("navigation.waypoint_ids", std::vector()); + for (const auto & id : ids) { + config.locations[id] = Point{ + node.declare_parameter("navigation.waypoints." + id + ".x", 0.0), + node.declare_parameter("navigation.waypoints." + id + ".y", 0.0)}; + } + + bool ok = !config.robots.empty() && config.robots.size() == config.homes.size() && + !config.goals.empty(); + if (!ok) { + RCLCPP_ERROR(node.get_logger(), "needs 'robots', as many 'homes' and some 'goals'"); + } + auto known = [&](const std::string & what, const std::string & location) { + if (config.locations.count(location) == 0) { + RCLCPP_ERROR( + node.get_logger(), "%s '%s' is not a waypoint", what.c_str(), location.c_str()); + ok = false; + } + }; + for (const auto & home : config.homes) { + known("home", home); + } + for (const auto & dock : config.docks) { + known("dock", dock); + } + for (const auto & [package, location] : config.packages) { + known("location of " + package, location); + } + return ok; +} + +/// Waits until every robot announces navigation plus perception or +/// manipulation; the robots that never do are left out of the problem. +std::map> discover( + easyfleet::FleetSession & session, const Config & config) +{ + std::map> found; + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::duration_cast( + std::chrono::duration(config.discovery_timeout)); + std::cout << "Waiting for the capabilities of " << config.robots.size() << " robots..." << + std::endl; + while (rclcpp::ok()) { + found.clear(); + bool all = true; + for (const auto & robot : config.robots) { + for (const auto & info : session.capabilities_of(robot)) { + if (info.active) { + found[robot].insert(info.capability); + } + } + const auto & caps = found[robot]; + all = all && caps.count("navigation") && + (caps.count("perception") || caps.count("manipulation")); + } + if (all || std::chrono::steady_clock::now() > deadline) { + break; + } + session.spin_for(1s); + } + for (const auto & robot : config.robots) { + std::cout << " " << robot << ":"; + for (const auto & capability : found[robot]) { + std::cout << " " << capability; + } + std::cout << (found[robot].count("navigation") ? "" : " (no navigation: left out)") << + std::endl; + } + return found; +} + +/// Distance between waypoints along the warehouse's grid of aisles and corridors. +double route_distance(const Point & a, const Point & b) +{ + return std::abs(a.x - b.x) + std::abs(a.y - b.y); +} + +bool build_problem( + plansys2::ProblemExpertClient & problem, const Config & config, + const std::map> & capabilities) +{ + problem.clearKnowledge(); + bool ok = true; + auto add = [&](const std::string & predicate) { + ok = problem.addPredicate(plansys2::Predicate(predicate)) && ok; + }; + + for (const auto & [location, point] : config.locations) { + (void)point; + ok = problem.addInstance(plansys2::Instance{location, "location"}) && ok; + } + std::set occupied; + for (std::size_t i = 0; i < config.robots.size(); ++i) { + const auto & robot = config.robots[i]; + const auto it = capabilities.find(robot); + if (it == capabilities.end() || it->second.count("navigation") == 0) { + continue; + } + ok = problem.addInstance(plansys2::Instance{robot, "robot"}) && ok; + add("(robot_at " + robot + " " + config.homes[i] + ")"); + add("(idle " + robot + ")"); + occupied.insert(config.homes[i]); + if (it->second.count("perception")) { + add("(can_perceive " + robot + ")"); + } + if (it->second.count("manipulation")) { + add("(can_manipulate " + robot + ")"); + add("(hand_empty " + robot + ")"); + } + } + for (const auto & [location, point] : config.locations) { + (void)point; + if (occupied.count(location) == 0) { + add("(free " + location + ")"); + } + } + for (const auto & dock : config.docks) { + add("(is_dock " + dock + ")"); + } + for (const auto & [package, location] : config.packages) { + ok = problem.addInstance(plansys2::Instance{package, "package"}) && ok; + add("(package_at " + package + " " + location + ")"); + } + for (const auto & [from, a] : config.locations) { + for (const auto & [to, b] : config.locations) { + if (from != to) { + ok = problem.addFunction( + plansys2::Function( + "(= (distance " + from + " " + to + ") " + + std::to_string(route_distance(a, b)) + ")")) && ok; + } + } + } + return ok; +} + +/// Prints each action of the plan when it starts and ends. +void report( + const plansys2_msgs::action::ExecutePlan::Feedback & feedback, + std::map & last) +{ + using Info = plansys2_msgs::msg::ActionExecutionInfo; + // Endings first, so an action is reported done before the one it lets start. + for (const bool endings : {true, false}) { + for (const auto & action : feedback.action_execution_status) { + const bool ending = action.status == Info::SUCCEEDED || action.status == Info::FAILED; + if (ending != endings || last[action.action_full_name] == action.status) { + continue; + } + last[action.action_full_name] = action.status; + // action_full_name ends with ":" + const std::string name = action.action_full_name.substr( + 0, action.action_full_name.rfind(':')); + switch (action.status) { + case Info::EXECUTING: + std::cout << " > " << name << std::endl; + break; + case Info::SUCCEEDED: + std::cout << " ok " << name << std::endl; + break; + case Info::FAILED: + std::cout << " FAILED " << name << ": " << action.message_status << std::endl; + break; + default: + break; + } + } + } +} + +/// Plans for `goal` and runs the plan; true if it succeeds. +bool reach( + const std::string & goal, plansys2::DomainExpertClient & domain, + plansys2::ProblemExpertClient & problem, plansys2::PlannerClient & planner, + plansys2::ExecutorClient & executor) +{ + if (!problem.setGoal(plansys2::Goal(goal))) { + std::cout << "Invalid goal " << goal << std::endl; + return false; + } + const auto start = std::chrono::steady_clock::now(); + const auto plan = planner.getPlan(domain.getDomain(), problem.getProblem()); + if (!plan.has_value()) { + std::cout << "No plan reaches " << goal << std::endl; + return false; + } + std::cout << "Plan (" << plan->items.size() << " actions, found in " << + std::chrono::duration_cast( + std::chrono::steady_clock::now() - start).count() << " ms):" << std::endl; + for (const auto & item : plan->items) { + std::cout << " " << item.time << ": " << item.action << " [" << item.duration << "]" << + std::endl; + } + if (!executor.start_plan_execution(plan.value())) { + std::cout << "The executor did not start the plan" << std::endl; + return false; + } + + std::map last; + rclcpp::Rate rate(2); + while (rclcpp::ok() && executor.execute_and_check_plan()) { + report(executor.getFeedBack(), last); + rate.sleep(); + } + report(executor.getFeedBack(), last); + const auto result = executor.getResult(); + const bool succeeded = result.has_value() && + result->result == plansys2_msgs::action::ExecutePlan::Result::SUCCESS; + std::cout << (succeeded ? "Reached " : "Did not reach ") << goal << " in " << + std::chrono::duration_cast( + std::chrono::steady_clock::now() - start).count() << " s" << std::endl; + return succeeded; +} + +} // namespace + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("warehouse_controller"); + + Config config; + if (!read_config(*node, config)) { + rclcpp::shutdown(); + return 1; + } + + easyfleet::FleetSession session; + std::vector> robots; + for (const auto & name : config.robots) { + robots.push_back(std::make_unique(name)); + session.add_robot(*robots.back()); + } + session.discover_capabilities(); + const auto capabilities = discover(session, config); + + plansys2::DomainExpertClient domain(*node); + plansys2::ProblemExpertClient problem(*node); + plansys2::PlannerClient planner(*node); + plansys2::ExecutorClient executor(*node); + + if (!build_problem(problem, config, capabilities)) { + std::cout << "Could not build the problem" << std::endl; + session.shutdown(); + return 1; + } + + int reached = 0; + try { + for (std::size_t i = 0; i < config.goals.size() && rclcpp::ok(); ++i) { + std::cout << std::endl << "== Goal " << i + 1 << "/" << config.goals.size() << ": " << + config.goals[i] << std::endl; + if (!reach(config.goals[i], domain, problem, planner, executor)) { + break; + } + ++reached; + } + } catch (const rclcpp::exceptions::RCLError & e) { + // Shutting down: the middleware went away under a pending request. + std::cout << "Stopped: " << e.what() << std::endl; + } + std::cout << std::endl << "Mission: " << reached << " of " << config.goals.size() << + " goals reached" << std::endl; + + robots.clear(); + session.shutdown(); + return reached == static_cast(config.goals.size()) ? 0 : 1; +} From 5ee12206522bc3fe6387446641ff021c5f7c1fd8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 25 Oct 2026 21:03:28 +0100 Subject: [PATCH 3/3] large_warehouse_world from juanscelyg again: the warehouse changes are merged MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- easyfleet_example_deployments.repos | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/easyfleet_example_deployments.repos b/easyfleet_example_deployments.repos index c20cc35..bec72d6 100644 --- a/easyfleet_example_deployments.repos +++ b/easyfleet_example_deployments.repos @@ -23,12 +23,10 @@ repositories: type: git url: https://github.com/EasyNavigation/easynav_playgrounds.git version: rolling - # fmrico's branch until juanscelyg/large_warehouse_world#1 is merged: packages under - # the Warehouse A racks, ODE collisions and large_warehouse_world_a.sdf. large_warehouse_world: type: git - url: https://github.com/fmrico/large_warehouse_world.git - version: fill_racks + url: https://github.com/juanscelyg/large_warehouse_world.git + version: main # PlanSys2 with Kairos, for easyfleet_plansys2_warehouse_deployment. ros2_planning_system: type: git