Warehouse A deployments, a PlanSys2 fleet, playgrounds and Zenoh

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

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`).

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

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:

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.

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.

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

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

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 "/**/<node_name>" 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