From 077e807698646e393e497d66ac1170a08b862880 Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Sat, 6 Jun 2026 07:36:41 +0300 Subject: [PATCH 1/6] Fixed where the data collected is being stored and added a new file to keep track of notes related to data collection. This will help us stay organized and ensure that we have a clear record of our data collection process. Also, made some modificatoins to ensure running scenarios in sim dowsn't shift the map between episodes to ensure consistency in our data collection. This will help us maintain the integrity of our data and ensure that we can accurately analyze it later on. --- DATA_COLLECTION_NOTES.md | 103 ++++++++++++++++++ arena_bringup/launch/arena_runtime.launch.py | 14 ++- arena_evaluation | 2 +- arena_simulation_setup/launch/robot.launch.py | 5 +- 4 files changed, 120 insertions(+), 4 deletions(-) create mode 100644 DATA_COLLECTION_NOTES.md diff --git a/DATA_COLLECTION_NOTES.md b/DATA_COLLECTION_NOTES.md new file mode 100644 index 000000000..18206d3b0 --- /dev/null +++ b/DATA_COLLECTION_NOTES.md @@ -0,0 +1,103 @@ +# Data Collection Notes + +## Workspace State + +- The workspace was corrected to use `https://github.com/ivaROS/Arena.git`. +- Current top-level branch: `jazzy`. +- Current top-level commit checked during this work: `02d9024`. +- Important submodule revision: `arena_evaluation` at `3e0e19e`, which includes + the ROS 2 `TopicMetadata.offered_qos_profiles` fix for rosbag writing. + +## What Was Observed + +- The earlier smoke command used `--scale-episodes 0.1`; for one-episode stages + this rounded to `0` planned episodes, so those runs were not useful as data + collection tests. +- A real one-episode run using `map_empty` scenario `1.json` launched Gazebo and + started the episode, but DWB timed out after 60 seconds. +- A simpler one-episode run using `scenario_empty` succeeded and wrote a row to + `progress.csv`. +- GPU was not meaningfully used because these tests ran Gazebo in headless mode. + This path is mostly CPU-bound unless running a GPU-backed simulator/rendering + path such as Isaac or a GPU-enabled Gazebo rendering workflow. + +## Fixes Made + +- `arena_simulation_setup/launch/robot.launch.py` + - Changed recorder launch arguments from one nested list to normal argv tokens: + `--dir`, then `record_data_dir`. + - Reason: the recorder process was starting, but the requested benchmark + output directory was not being honored. + +- `arena_evaluation/arena_evaluation/data_recorder_node.py` + - Preserved absolute recorder paths instead of always rewriting them under the + package share directory. + - Reason: benchmark runs pass an absolute directory like + `/opt/arena_ws/data/benchmarks///`, and rosbag + output should live there. + +- `arena_evaluation/benchmark/runner.py` + - Included each step's `record_dir` in the environment grouping key. + - Reason: the recorder opens its rosbag writer when the environment is + spawned. Reusing one environment across multiple stages meant later stages + could be written into the first stage's recording folder. + +## Useful Commands + +From the host: + +```bash +cd /home/hussam-arena/arena_ws +source ./arena +``` + +Build only the packages needed for Gazebo/DWB benchmark collection: + +```bash +colcon build --symlink-install --continue-on-error \ + --cmake-args -DPython3_ROOT_DIR=/opt/venv/bin/python3 -DBUILD_TESTING=OFF \ + --base-paths /opt/arena_ws/src \ + --build-base /opt/arena_ws/build \ + --install-base /opt/arena_ws/install \ + --packages-select arena_simulation_setup arena_evaluation arena_bringup arena_runtime arena_robots task_generator task_generator_msgs arena_rclpy_mixins arena_models arena_planners +``` + +Run a quick passing data-collection smoke: + +```bash +ros2 run arena_evaluation benchmark sim:=gazebo headless:=true \ + --run-id smoke-empty-dwb \ + --suite '{"stages":[{"name":"scenario_empty","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"empty"}}}]}' \ + --contest '[{"name":"dwb","mobile.local_planner":"dwb","mobile.inter_planner":"navigate_w_replanning_time"}]' +``` + +Inspect outputs: + +```bash +arena evaluation status smoke-empty-dwb +arena evaluation tail smoke-empty-dwb +find /opt/arena_ws/data/benchmarks/smoke-empty-dwb -maxdepth 5 -type f | sort +``` + +Run the built-in map-empty suite: + +```bash +arena benchmark sim:=gazebo headless:=true --suite map_empty --contest basic +``` + +Avoid `--scale-episodes 0.1` for one-episode suites because it can round to +zero episodes. + +Run a small standard Nav2 planner comparison: + +```bash +ros2 run arena_evaluation benchmark sim:=gazebo headless:=true \ + --run-id planner-compare-nav2-smoke \ + --suite '{"stages":[{"name":"empty","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"empty"}}},{"name":"map_empty_1","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"1.json"}}}]}' \ + --contest '[{"name":"dwb","mobile.local_planner":"dwb","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"rpp","mobile.local_planner":"regulated_pure_pursuit","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"graceful","mobile.local_planner":"graceful","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"mppi","mobile.local_planner":"mppi","mobile.inter_planner":"navigate_w_replanning_time"}]' +``` + +In the first comparison run, all four planners completed the `empty` stage and +timed out on `map_empty_1`. The non-empty scenario logs showed unresolved +scenario models such as `shelf1`, `shelf2`, and `shelf3`, which is the next +area to investigate before treating the planner scores as meaningful. diff --git a/arena_bringup/launch/arena_runtime.launch.py b/arena_bringup/launch/arena_runtime.launch.py index 8932929d2..a8fd31344 100644 --- a/arena_bringup/launch/arena_runtime.launch.py +++ b/arena_bringup/launch/arena_runtime.launch.py @@ -6,7 +6,7 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -_RUNTIME_OWNED = frozenset({'log_level', 'sim', 'use_sim_time', 'world', 'headless'}) +_RUNTIME_OWNED = frozenset({'log_level', 'sim', 'use_sim_time', 'world', 'headless', 'slot_buffer'}) def generate_launch_description(): @@ -39,6 +39,12 @@ def generate_launch_description(): default_value='False', ) + slot_buffer = LaunchArgument( + name='slot_buffer', + default_value='5.0', + description='Runtime env packing buffer in metres. Use 0 for literal world coordinates.', + ) + launch_sim = launch.actions.IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare('arena_bringup'), @@ -72,7 +78,11 @@ def _arena_node(context: launch.LaunchContext) -> list[launch.Action]: name='arena', namespace='', output='screen', - parameters=[{'sim': sim.substitution, 'env_args': env_args}], + parameters=[{ + 'sim': sim.substitution, + 'env_args': env_args, + 'slot_buffer': slot_buffer.substitution, + }], ), ] diff --git a/arena_evaluation b/arena_evaluation index 3e0e19ea7..cc90d1c24 160000 --- a/arena_evaluation +++ b/arena_evaluation @@ -1 +1 @@ -Subproject commit 3e0e19ea7f7d3a557562997b4ec92c990ec42561 +Subproject commit cc90d1c24ab80fb04d587d32e43fe83e0e37225c diff --git a/arena_simulation_setup/launch/robot.launch.py b/arena_simulation_setup/launch/robot.launch.py index c0b1ea0c2..89820154b 100644 --- a/arena_simulation_setup/launch/robot.launch.py +++ b/arena_simulation_setup/launch/robot.launch.py @@ -48,8 +48,11 @@ def generate_launch_description(): package='arena_evaluation', executable='record', name=PythonExpression(['"data_recorder" + "', namespace.substitution, '".replace("/","_")']), + # Pass --dir as two argv tokens; a nested list was not reliably + # reaching argparse, so benchmark recorder output fell back to auto:/. arguments=[ - ['--dir', ' ', record_data_dir.substitution], + '--dir', + record_data_dir.substitution, ], condition=launch.conditions.IfCondition(PythonExpression(['bool("', record_data_dir.substitution, '")'])), ) From 5d6b5632c3039aebb668d001d5874eca4cca9d5d Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Sat, 6 Jun 2026 07:36:41 +0300 Subject: [PATCH 2/6] Fixed where the data collected is being stored and added a new file to keep track of notes related to data collection. This will help us stay organized and ensure that we have a clear record of our data collection process. Also, made some modificatoins to ensure running scenarios in sim dowsn't shift the map between episodes to ensure consistency in our data collection. This will help us maintain the integrity of our data and ensure that we can accurately analyze it later on. --- DATA_COLLECTION_NOTES.md | 103 ++++++++++++++++++ arena_bringup/launch/arena_runtime.launch.py | 14 ++- arena_evaluation | 2 +- arena_simulation_setup/launch/robot.launch.py | 5 +- 4 files changed, 120 insertions(+), 4 deletions(-) create mode 100644 DATA_COLLECTION_NOTES.md diff --git a/DATA_COLLECTION_NOTES.md b/DATA_COLLECTION_NOTES.md new file mode 100644 index 000000000..18206d3b0 --- /dev/null +++ b/DATA_COLLECTION_NOTES.md @@ -0,0 +1,103 @@ +# Data Collection Notes + +## Workspace State + +- The workspace was corrected to use `https://github.com/ivaROS/Arena.git`. +- Current top-level branch: `jazzy`. +- Current top-level commit checked during this work: `02d9024`. +- Important submodule revision: `arena_evaluation` at `3e0e19e`, which includes + the ROS 2 `TopicMetadata.offered_qos_profiles` fix for rosbag writing. + +## What Was Observed + +- The earlier smoke command used `--scale-episodes 0.1`; for one-episode stages + this rounded to `0` planned episodes, so those runs were not useful as data + collection tests. +- A real one-episode run using `map_empty` scenario `1.json` launched Gazebo and + started the episode, but DWB timed out after 60 seconds. +- A simpler one-episode run using `scenario_empty` succeeded and wrote a row to + `progress.csv`. +- GPU was not meaningfully used because these tests ran Gazebo in headless mode. + This path is mostly CPU-bound unless running a GPU-backed simulator/rendering + path such as Isaac or a GPU-enabled Gazebo rendering workflow. + +## Fixes Made + +- `arena_simulation_setup/launch/robot.launch.py` + - Changed recorder launch arguments from one nested list to normal argv tokens: + `--dir`, then `record_data_dir`. + - Reason: the recorder process was starting, but the requested benchmark + output directory was not being honored. + +- `arena_evaluation/arena_evaluation/data_recorder_node.py` + - Preserved absolute recorder paths instead of always rewriting them under the + package share directory. + - Reason: benchmark runs pass an absolute directory like + `/opt/arena_ws/data/benchmarks///`, and rosbag + output should live there. + +- `arena_evaluation/benchmark/runner.py` + - Included each step's `record_dir` in the environment grouping key. + - Reason: the recorder opens its rosbag writer when the environment is + spawned. Reusing one environment across multiple stages meant later stages + could be written into the first stage's recording folder. + +## Useful Commands + +From the host: + +```bash +cd /home/hussam-arena/arena_ws +source ./arena +``` + +Build only the packages needed for Gazebo/DWB benchmark collection: + +```bash +colcon build --symlink-install --continue-on-error \ + --cmake-args -DPython3_ROOT_DIR=/opt/venv/bin/python3 -DBUILD_TESTING=OFF \ + --base-paths /opt/arena_ws/src \ + --build-base /opt/arena_ws/build \ + --install-base /opt/arena_ws/install \ + --packages-select arena_simulation_setup arena_evaluation arena_bringup arena_runtime arena_robots task_generator task_generator_msgs arena_rclpy_mixins arena_models arena_planners +``` + +Run a quick passing data-collection smoke: + +```bash +ros2 run arena_evaluation benchmark sim:=gazebo headless:=true \ + --run-id smoke-empty-dwb \ + --suite '{"stages":[{"name":"scenario_empty","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"empty"}}}]}' \ + --contest '[{"name":"dwb","mobile.local_planner":"dwb","mobile.inter_planner":"navigate_w_replanning_time"}]' +``` + +Inspect outputs: + +```bash +arena evaluation status smoke-empty-dwb +arena evaluation tail smoke-empty-dwb +find /opt/arena_ws/data/benchmarks/smoke-empty-dwb -maxdepth 5 -type f | sort +``` + +Run the built-in map-empty suite: + +```bash +arena benchmark sim:=gazebo headless:=true --suite map_empty --contest basic +``` + +Avoid `--scale-episodes 0.1` for one-episode suites because it can round to +zero episodes. + +Run a small standard Nav2 planner comparison: + +```bash +ros2 run arena_evaluation benchmark sim:=gazebo headless:=true \ + --run-id planner-compare-nav2-smoke \ + --suite '{"stages":[{"name":"empty","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"empty"}}},{"name":"map_empty_1","map":"map_empty","robot":"jackal","episodes":1,"tm_robots":"scenario","tm_obstacles":"scenario","timeout":"120s","config":{"scenario":{"file":"1.json"}}}]}' \ + --contest '[{"name":"dwb","mobile.local_planner":"dwb","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"rpp","mobile.local_planner":"regulated_pure_pursuit","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"graceful","mobile.local_planner":"graceful","mobile.inter_planner":"navigate_w_replanning_time"},{"name":"mppi","mobile.local_planner":"mppi","mobile.inter_planner":"navigate_w_replanning_time"}]' +``` + +In the first comparison run, all four planners completed the `empty` stage and +timed out on `map_empty_1`. The non-empty scenario logs showed unresolved +scenario models such as `shelf1`, `shelf2`, and `shelf3`, which is the next +area to investigate before treating the planner scores as meaningful. diff --git a/arena_bringup/launch/arena_runtime.launch.py b/arena_bringup/launch/arena_runtime.launch.py index 8932929d2..a8fd31344 100644 --- a/arena_bringup/launch/arena_runtime.launch.py +++ b/arena_bringup/launch/arena_runtime.launch.py @@ -6,7 +6,7 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare -_RUNTIME_OWNED = frozenset({'log_level', 'sim', 'use_sim_time', 'world', 'headless'}) +_RUNTIME_OWNED = frozenset({'log_level', 'sim', 'use_sim_time', 'world', 'headless', 'slot_buffer'}) def generate_launch_description(): @@ -39,6 +39,12 @@ def generate_launch_description(): default_value='False', ) + slot_buffer = LaunchArgument( + name='slot_buffer', + default_value='5.0', + description='Runtime env packing buffer in metres. Use 0 for literal world coordinates.', + ) + launch_sim = launch.actions.IncludeLaunchDescription( PathJoinSubstitution([ FindPackageShare('arena_bringup'), @@ -72,7 +78,11 @@ def _arena_node(context: launch.LaunchContext) -> list[launch.Action]: name='arena', namespace='', output='screen', - parameters=[{'sim': sim.substitution, 'env_args': env_args}], + parameters=[{ + 'sim': sim.substitution, + 'env_args': env_args, + 'slot_buffer': slot_buffer.substitution, + }], ), ] diff --git a/arena_evaluation b/arena_evaluation index 3e0e19ea7..cc90d1c24 160000 --- a/arena_evaluation +++ b/arena_evaluation @@ -1 +1 @@ -Subproject commit 3e0e19ea7f7d3a557562997b4ec92c990ec42561 +Subproject commit cc90d1c24ab80fb04d587d32e43fe83e0e37225c diff --git a/arena_simulation_setup/launch/robot.launch.py b/arena_simulation_setup/launch/robot.launch.py index c0b1ea0c2..89820154b 100644 --- a/arena_simulation_setup/launch/robot.launch.py +++ b/arena_simulation_setup/launch/robot.launch.py @@ -48,8 +48,11 @@ def generate_launch_description(): package='arena_evaluation', executable='record', name=PythonExpression(['"data_recorder" + "', namespace.substitution, '".replace("/","_")']), + # Pass --dir as two argv tokens; a nested list was not reliably + # reaching argparse, so benchmark recorder output fell back to auto:/. arguments=[ - ['--dir', ' ', record_data_dir.substitution], + '--dir', + record_data_dir.substitution, ], condition=launch.conditions.IfCondition(PythonExpression(['bool("', record_data_dir.substitution, '")'])), ) From 6d41e50454c5e91522b0834b47413cc55c662492 Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Sat, 13 Jun 2026 23:06:05 +0300 Subject: [PATCH 3/6] benchmark: collect collision data --- arena_evaluation | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/arena_evaluation b/arena_evaluation index cc90d1c24..75dd43d9c 160000 --- a/arena_evaluation +++ b/arena_evaluation @@ -1 +1 @@ -Subproject commit cc90d1c24ab80fb04d587d32e43fe83e0e37225c +Subproject commit 75dd43d9c949dc274d236c83fc82eea2a197bd11 From 7efdf40649641602a6bf513755bf6d2b4ac38846 Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Sun, 14 Jun 2026 03:41:03 +0300 Subject: [PATCH 4/6] benchmark: add collision recorder scenarios --- arena_evaluation | 2 +- arena_robots | 2 +- .../collision_pedestrian/scenario.json | 29 ++++++++ .../scenarios/collision_robot/scenario.json | 19 ++++++ .../scenarios/collision_wall/scenario.json | 13 ++++ .../robot_manager/collision_tracker.py | 31 +++++++++ .../tasks/robots/adapters/mobile/__init__.py | 7 ++ .../adapters/mobile/collision_benchmark.py | 68 +++++++++++++++++++ 8 files changed, 169 insertions(+), 2 deletions(-) create mode 100644 arena_simulation_setup/worlds/map_empty/scenarios/collision_pedestrian/scenario.json create mode 100644 arena_simulation_setup/worlds/map_empty/scenarios/collision_robot/scenario.json create mode 100644 arena_simulation_setup/worlds/map_empty/scenarios/collision_wall/scenario.json create mode 100644 task_generator/task_generator/tasks/robots/adapters/mobile/collision_benchmark.py diff --git a/arena_evaluation b/arena_evaluation index 75dd43d9c..ab5f66e27 160000 --- a/arena_evaluation +++ b/arena_evaluation @@ -1 +1 @@ -Subproject commit 75dd43d9c949dc274d236c83fc82eea2a197bd11 +Subproject commit ab5f66e27af51d64594f4f75c765565fab8adcb9 diff --git a/arena_robots b/arena_robots index cf24d1044..288f12c54 160000 --- a/arena_robots +++ b/arena_robots @@ -1 +1 @@ -Subproject commit cf24d10446ea5b417d7c213df10378daed1b2004 +Subproject commit 288f12c540fd39cbb18f6fd0c599f79b0d781be6 diff --git a/arena_simulation_setup/worlds/map_empty/scenarios/collision_pedestrian/scenario.json b/arena_simulation_setup/worlds/map_empty/scenarios/collision_pedestrian/scenario.json new file mode 100644 index 000000000..3c18c9e7c --- /dev/null +++ b/arena_simulation_setup/worlds/map_empty/scenarios/collision_pedestrian/scenario.json @@ -0,0 +1,29 @@ +{ + "robots": [ + { + "start": [5.0, 10.0, 0.0], + "phases": [ + {"goto": [12.0, 10.0, 0.0]} + ] + } + ], + "static": [], + "dynamic": [ + { + "name": "ped_blocker", + "pose": [8.0, 10.0, 0.0], + "type": "adult", + "model": "arenian", + "velocity": 0.0, + "desired_velocity": 0.0, + "linear_vel": 0.0, + "angular_vel": 0.0, + "cyclic_goals": false, + "goal_radius": 0.0, + "waypoints": [ + [8.0, 10.0, 0.0] + ] + } + ], + "interactive": [] +} diff --git a/arena_simulation_setup/worlds/map_empty/scenarios/collision_robot/scenario.json b/arena_simulation_setup/worlds/map_empty/scenarios/collision_robot/scenario.json new file mode 100644 index 000000000..8ca1de687 --- /dev/null +++ b/arena_simulation_setup/worlds/map_empty/scenarios/collision_robot/scenario.json @@ -0,0 +1,19 @@ +{ + "robots": [ + { + "start": [5.0, 10.0, 0.0], + "phases": [ + {"goto": [12.0, 10.0, 0.0]} + ] + }, + { + "start": [9.0, 10.0, 3.1416], + "phases": [ + {"goto": [2.0, 10.0, 3.1416]} + ] + } + ], + "static": [], + "dynamic": [], + "interactive": [] +} diff --git a/arena_simulation_setup/worlds/map_empty/scenarios/collision_wall/scenario.json b/arena_simulation_setup/worlds/map_empty/scenarios/collision_wall/scenario.json new file mode 100644 index 000000000..94f8f921d --- /dev/null +++ b/arena_simulation_setup/worlds/map_empty/scenarios/collision_wall/scenario.json @@ -0,0 +1,13 @@ +{ + "robots": [ + { + "start": [20.0, 10.0, 0.0], + "phases": [ + {"goto": [35.0, 10.0, 0.0]} + ] + } + ], + "static": [], + "dynamic": [], + "interactive": [] +} diff --git a/task_generator/task_generator/manager/robot_manager/collision_tracker.py b/task_generator/task_generator/manager/robot_manager/collision_tracker.py index 24c6e5e61..e1002de9b 100644 --- a/task_generator/task_generator/manager/robot_manager/collision_tracker.py +++ b/task_generator/task_generator/manager/robot_manager/collision_tracker.py @@ -84,6 +84,15 @@ def __init__( self._on_peds, 10, ) + env_peds_topic = str(robot_manager._namespace('arena_peds')) # pylint: disable=protected-access + self._sub_env_peds = None + if env_peds_topic != str(robot_manager.namespace('arena_peds')): + self._sub_env_peds = self.create_subscription( + arena_people_msgs.msg.Pedestrians, + env_peds_topic, + self._on_peds, + 10, + ) self._timer = self.create_timer(1.0 / rate_hz, self._tick) def _on_peds(self, msg: arena_people_msgs.msg.Pedestrians): @@ -148,6 +157,28 @@ def _tick(self): events.append(ev) polygons_hit[name] = entry['action_code'] + robots_manager = getattr(self._rm.node, 'robots_manager', None) + if robots_manager is not None: + for other_name, other in robots_manager.managers.items(): + if other is self._rm: + continue + other_pose = other.pose + if other_pose is None: + continue + ox, oy, _ = other_pose.to_2d() + if math.isnan(ox) or math.isnan(oy): + continue + other_disc = shapely.Point(ox, oy).buffer(max(other.radius, 0.01)) + if not robot_poly.intersects(other_disc): + continue + ev = arena_robots_msgs.msg.CollisionEvent() + ev.obstacle_id = f'' + ev.polygon_name = name + ev.distance = 0.0 + ev.obstacle_position = geometry_msgs.msg.Point(x=ox, y=oy, z=0.0) + events.append(ev) + polygons_hit[name] = entry['action_code'] + state = nav2_msgs.msg.CollisionMonitorState() if polygons_hit: name, code = min(polygons_hit.items(), key=lambda kv: (1 if kv[1] == 0 else 0, kv[1])) diff --git a/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py b/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py index fb7d7735c..2283ac092 100644 --- a/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py +++ b/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py @@ -95,3 +95,10 @@ def _load_test_collision() -> type[Adapter]: from .test_collision import TestCollisionAdapter return TestCollisionAdapter + + +@ADAPTERS["mobile"].register("collision-benchmark") +def _load_collision_benchmark() -> type[Adapter]: + from .collision_benchmark import CollisionBenchmarkAdapter + + return CollisionBenchmarkAdapter diff --git a/task_generator/task_generator/tasks/robots/adapters/mobile/collision_benchmark.py b/task_generator/task_generator/tasks/robots/adapters/mobile/collision_benchmark.py new file mode 100644 index 000000000..5ff82e80f --- /dev/null +++ b/task_generator/task_generator/tasks/robots/adapters/mobile/collision_benchmark.py @@ -0,0 +1,68 @@ +"""Timed constant-velocity adapter for benchmark collision-recorder checks.""" + +from __future__ import annotations + +import asyncio +from typing import TYPE_CHECKING, ClassVar + +from arena_robots.bringup.mobile.collision_benchmark import CollisionBenchmarkBringup +from arena_robots.clients.goto_pose import GotoPoseClient +from arena_robots.task_kinds import TaskKind + +from task_generator.manager.robot_manager.collision_tracker import CollisionTrackerNode +from task_generator.manager.world_manager.shims import requires_map_server +from task_generator.tasks.robots.adapters import AdapterMeta, ResetContext +from task_generator.tasks.robots.adapters.mobile import MobileAdapter + +if TYPE_CHECKING: + from task_generator.manager.robot_manager.robot_manager import RobotManager + from task_generator.tasks.robots.request import TaskPhase + + +@AdapterMeta.attach( + accepts={TaskKind.GOTO_POSE}, + bringup=CollisionBenchmarkBringup, + client=GotoPoseClient, + cap="mobile", +) +@requires_map_server +class CollisionBenchmarkAdapter(MobileAdapter): + kind: ClassVar[str] = "collision-benchmark" + + def __init__( + self, + *args: object, + duration_s: float = 8.0, + linear_x: float = 1.0, + rate_hz: float = 10.0, + **kwargs: object, + ): + super().__init__(*args, duration_s=duration_s, linear_x=linear_x, rate_hz=rate_hz, **kwargs) + self._duration_s = float(duration_s) + self._linear_x = float(linear_x) + self._rate_hz = float(rate_hz) + self._episode_start_s: float | None = None + self._drive_task: asyncio.Task | None = None + self._collision: CollisionTrackerNode | None = None + polys = self.rm.robot.model.resolve_sync().caps.mobile.polygons_dict + if polys: + self._collision = CollisionTrackerNode(self.rm, polys) + self.rm.node.executor.add_node(self._collision) + + async def on_reset(self, robot: RobotManager, ctx: ResetContext) -> None: + if self._drive_task is not None: + self._drive_task.cancel() + self._drive_task = None + await super().on_reset(robot, ctx) + self._episode_start_s = robot.node.sim_time.to_seconds() + + def is_phase_done(self, phase: TaskPhase, robot: RobotManager) -> bool | None: + del phase + start = self._episode_start_s + if start is None: + self._episode_start_s = robot.node.sim_time.to_seconds() + return False + return (robot.node.sim_time.to_seconds() - start) >= self._duration_s + + async def dispatch_phase(self, phase: TaskPhase, robot: RobotManager) -> None: + del phase, robot From a5b607faa80c94759ad25d3d2c94c9d0f1308e64 Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Tue, 16 Jun 2026 16:28:53 +0300 Subject: [PATCH 5/6] Improve collision benchmark reliability and multi-robot collision detection ## Summary Enhanced benchmark collision data reliability for multi-robot scenarios. Key improvements include: - Benchmark timeout episodes now write rows to progress.csv instead of just headers - Launch arguments now preserve explicit 'false' values (e.g., mobile.safety_monitor: false) - Nav2 can now run in debug 'safety off' mode bypassing collision monitor and velocity smoother - Passive collision tracker is now attached to all mobile adapters, not just debug adapters - Collision tracker now detects robot-vs-wall, robot-vs-obstacle, robot-vs-pedestrian, and robot-vs-robot collisions - collision_events now represents real stop-polygon contacts, not slow-polygon near misses - Added crowded two-Jackal benchmark scenario and configs for visual collision stress testing - Hunav is explicitly enabled in benchmark contests to prevent fallback to dummy humans ## Changes Made 1. **arena_evaluation/benchmark/runner.py** - Fixed launch arg handling to preserve False values - Added timeout episode record generation for progress.csv - Tests: test_build_launch_args_false_value_forwarded, test_timeout_episode_record_can_be_written_to_progress 2. **arena_robots/bringup/mobile/nav2.py & launch/adapters/mobile/nav2.launch.py** - Added optional safety_monitor parameter (default: true) - When false, bypasses both collision_monitor and velocity_smoother 3. **task_generator/tasks/robots/adapters/mobile/__init__.py** - Mounted passive CollisionTrackerNode at base MobileAdapter level - Now available for all mobile adapters by default 4. **task_generator/tasks/robots/adapters/mobile/rosnav_rl.py & test_collision.py** - Removed duplicate collision tracker setup (moved to base adapter) 5. **task_generator/manager/robot_manager/collision_tracker.py** - Emit collision_events only from stop-action polygons (not slow polygons) - Added robot-vs-robot collision detection with deduplication - Subscribe to both robot-local and shared Hunav pedestrian topics - Added info logs for dynamic obstacle registration 6. **task_generator/simulators/human/hunav/hunav.py** - Added debug logs for Hunav pedestrian registration 7. **arena_evaluation/configs/benchmark/suites/crowded_collision_check.yaml** (NEW) - New benchmark suite for two-Jackal collision stress testing 8. **arena_evaluation/configs/benchmark/contests/crowded_collision_check.yaml** (NEW) 9. **arena_evaluation/configs/benchmark/contests/crowded_collision_check_dwb.yaml** (NEW) - Contest configs with multiple planners (DWB, MPPI, regulated-pure-pursuit, graceful) - Explicit human: hunav configuration 10. **arena_simulation_setup/worlds/map_empty/scenarios/collision_stress/** (NEW) - New scenario with two Jackals and dynamic pedestrians - Designed for visual collision stress testing ## Verification - 80 unit tests passed - arena_robots, task_generator, and arena_evaluation built successfully - Timeout benchmark runs now write real progress.csv rows - Live collision topics publish correctly as ROS topics --- .../tasks/robots/adapters/mobile/__init__.py | 9 +++++++++ .../tasks/robots/adapters/mobile/rosnav_rl.py | 10 ---------- .../tasks/robots/adapters/mobile/test_collision.py | 10 ---------- 3 files changed, 9 insertions(+), 20 deletions(-) diff --git a/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py b/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py index fb7d7735c..de2399b72 100644 --- a/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py +++ b/task_generator/task_generator/tasks/robots/adapters/mobile/__init__.py @@ -4,6 +4,7 @@ import geometry_msgs.msg +from task_generator.manager.robot_manager.collision_tracker import CollisionTrackerNode from task_generator.tasks.robots.adapters import ADAPTERS, Adapter, AdapterDisplayHint if TYPE_CHECKING: @@ -28,6 +29,14 @@ class MobileAdapter(Adapter): ), ) + def __init__(self, *args: object, **kwargs: object) -> None: + super().__init__(*args, **kwargs) + self._collision_tracker: CollisionTrackerNode | None = None + polys = self.rm.robot.model.resolve_sync().caps.mobile.polygons_dict + if polys: + self._collision_tracker = CollisionTrackerNode(self.rm, polys) + self.rm.node.executor.add_node(self._collision_tracker) + async def on_reset(self, robot: RobotManager, ctx: ResetContext) -> None: if ctx.start_pose is not None: await robot.move(ctx.start_pose) diff --git a/task_generator/task_generator/tasks/robots/adapters/mobile/rosnav_rl.py b/task_generator/task_generator/tasks/robots/adapters/mobile/rosnav_rl.py index a12b386af..4447d4ec3 100644 --- a/task_generator/task_generator/tasks/robots/adapters/mobile/rosnav_rl.py +++ b/task_generator/task_generator/tasks/robots/adapters/mobile/rosnav_rl.py @@ -10,7 +10,6 @@ from arena_robots.task_kinds import TaskKind from arena_robots_msgs.action import GotoPose -from task_generator.manager.robot_manager.collision_tracker import CollisionTrackerNode from task_generator.manager.world_manager.shims import requires_map_server from task_generator.tasks.robots.adapters import AdapterDisplayHint, AdapterMeta from task_generator.tasks.robots.adapters.mobile import MobileAdapter @@ -41,15 +40,6 @@ class RosnavRlAdapter(MobileAdapter): kind: ClassVar[str] = "rosnav_rl" - def __init__(self, *args: object, **kwargs: object): - super().__init__(*args, **kwargs) - self._collision: CollisionTrackerNode | None = None - polys = self.rm.robot.model.resolve_sync().caps.mobile.polygons_dict - if not polys: - return - self._collision = CollisionTrackerNode(self.rm, polys) - self.rm.node.executor.add_node(self._collision) - def is_phase_done(self, phase: TaskPhase, robot: RobotManager) -> bool | None: return None diff --git a/task_generator/task_generator/tasks/robots/adapters/mobile/test_collision.py b/task_generator/task_generator/tasks/robots/adapters/mobile/test_collision.py index de8513181..44681ba80 100644 --- a/task_generator/task_generator/tasks/robots/adapters/mobile/test_collision.py +++ b/task_generator/task_generator/tasks/robots/adapters/mobile/test_collision.py @@ -13,7 +13,6 @@ from arena_robots.clients.goto_pose import GotoPoseClient from arena_robots.task_kinds import TaskKind -from task_generator.manager.robot_manager.collision_tracker import CollisionTrackerNode from task_generator.manager.world_manager.shims import requires_map_server from task_generator.tasks.robots.adapters import AdapterMeta from task_generator.tasks.robots.adapters.mobile import MobileAdapter @@ -33,15 +32,6 @@ class TestCollisionAdapter(MobileAdapter): kind: ClassVar[str] = "test-collision" - def __init__(self, *args: object, **kwargs: object): - super().__init__(*args, **kwargs) - self._collision: CollisionTrackerNode | None = None - polys = self.rm.robot.model.resolve_sync().caps.mobile.polygons_dict - if not polys: - return - self._collision = CollisionTrackerNode(self.rm, polys) - self.rm.node.executor.add_node(self._collision) - def is_phase_done(self, phase: TaskPhase, robot: RobotManager) -> bool | None: return True From 20a979ab8e196110d109a1c39433a23612754dbc Mon Sep 17 00:00:00 2001 From: H-M-ashour Date: Tue, 16 Jun 2026 20:09:07 +0300 Subject: [PATCH 6/6] benchmark: sync collision tracking with upstream --- arena_evaluation | 2 +- .../src/arena_simulation_setup/__init__.py | 4 + .../scenarios/collision_stress/scenario.json | 80 +++++++++++++++++++ .../robot_manager/collision_tracker.py | 76 +++++++++++++----- .../simulators/human/hunav/hunav.py | 10 +++ 5 files changed, 150 insertions(+), 22 deletions(-) create mode 100644 arena_simulation_setup/worlds/map_empty/scenarios/collision_stress/scenario.json diff --git a/arena_evaluation b/arena_evaluation index cc90d1c24..d7c017c49 160000 --- a/arena_evaluation +++ b/arena_evaluation @@ -1 +1 @@ -Subproject commit cc90d1c24ab80fb04d587d32e43fe83e0e37225c +Subproject commit d7c017c49b297234ad8571db00c1e82b0cf15643 diff --git a/arena_simulation_setup/src/arena_simulation_setup/__init__.py b/arena_simulation_setup/src/arena_simulation_setup/__init__.py index 42c7ec10c..0d1f76e6f 100644 --- a/arena_simulation_setup/src/arena_simulation_setup/__init__.py +++ b/arena_simulation_setup/src/arena_simulation_setup/__init__.py @@ -12,6 +12,10 @@ ASS_DIR = ament_index_python.packages.get_package_share_path('arena_simulation_setup') AB_DIR = ament_index_python.packages.get_package_share_path('arena_bringup') + if not (ASS_DIR / 'worlds').exists(): + source_root = Path(__file__).resolve().parents[2] + if (source_root / 'worlds').exists(): + ASS_DIR = source_root except ImportError: ASS_DIR = Path(os.environ.get('ASS_DIR', 'arena_simulation_setup')) AB_DIR = Path(os.environ.get('AB_DIR', 'arena_bringup')) diff --git a/arena_simulation_setup/worlds/map_empty/scenarios/collision_stress/scenario.json b/arena_simulation_setup/worlds/map_empty/scenarios/collision_stress/scenario.json new file mode 100644 index 000000000..bfa2218ae --- /dev/null +++ b/arena_simulation_setup/worlds/map_empty/scenarios/collision_stress/scenario.json @@ -0,0 +1,80 @@ +{ + "robots": [ + { + "start": [5.0, 10.0, 0.0], + "phases": [ + {"goto": [12.0, 10.0, 0.0]} + ] + }, + { + "start": [9.0, 10.0, 3.1416], + "phases": [ + {"goto": [2.0, 10.0, 3.1416]} + ] + } + ], + "static": [], + "dynamic": [ + { + "name": "stress_ped_cross_1", + "id": 103, + "pos": [8.4, 2.0, 0.0], + "type": "adult", + "model": "arenian", + "velocity": 2.5, + "desired_velocity": 2.5, + "cyclic_goals": true, + "goal_radius": 0.4, + "waypoints": [ + [8.4, 21.0, 0.0], + [8.4, 2.0, 0.0] + ] + }, + { + "name": "stress_ped_cross_2", + "id": 104, + "pos": [10.2, 21.0, 0.0], + "type": "adult", + "model": "arenian", + "velocity": 2.5, + "desired_velocity": 2.5, + "cyclic_goals": true, + "goal_radius": 0.4, + "waypoints": [ + [10.2, 2.0, 0.0], + [10.2, 21.0, 0.0] + ] + }, + { + "name": "stress_ped_diag_1", + "id": 105, + "pos": [2.0, 4.0, 0.0], + "type": "adult", + "model": "arenian", + "velocity": 2.5, + "desired_velocity": 2.5, + "cyclic_goals": true, + "goal_radius": 0.4, + "waypoints": [ + [14.0, 16.0, 0.0], + [2.0, 4.0, 0.0] + ] + }, + { + "name": "stress_ped_diag_2", + "id": 106, + "pos": [14.0, 4.0, 0.0], + "type": "adult", + "model": "arenian", + "velocity": 2.5, + "desired_velocity": 2.5, + "cyclic_goals": true, + "goal_radius": 0.4, + "waypoints": [ + [2.0, 16.0, 0.0], + [14.0, 4.0, 0.0] + ] + } + ], + "interactive": [] +} diff --git a/task_generator/task_generator/manager/robot_manager/collision_tracker.py b/task_generator/task_generator/manager/robot_manager/collision_tracker.py index 24c6e5e61..afb59fef2 100644 --- a/task_generator/task_generator/manager/robot_manager/collision_tracker.py +++ b/task_generator/task_generator/manager/robot_manager/collision_tracker.py @@ -25,6 +25,7 @@ 'approach': 3, 'limit': 4, } +_STOP_ACTION = _ACTION_CODES['stop'] class CollisionTrackerNode(rclpy.node.Node): @@ -78,12 +79,21 @@ def __init__( self._pub_state = self.create_publisher(nav2_msgs.msg.CollisionMonitorState, 'collision_monitor_state', 10) self._pub_events = self.create_publisher(arena_robots_msgs.msg.CollisionEvents, 'collision_events', 10) - self._sub_peds = self.create_subscription( - arena_people_msgs.msg.Pedestrians, - str(robot_manager.namespace('arena_peds')), - self._on_peds, - 10, - ) + peds_topics = [str(robot_manager.namespace('arena_peds'))] + robot_ns = str(robot_manager.namespace).rstrip('/') + if '/' in robot_ns: + shared_peds_topic = f"{robot_ns.rsplit('/', 1)[0]}/arena_peds" + if shared_peds_topic not in peds_topics: + peds_topics.append(shared_peds_topic) + self._sub_peds = [ + self.create_subscription( + arena_people_msgs.msg.Pedestrians, + topic, + self._on_peds, + 10, + ) + for topic in peds_topics + ] self._timer = self.create_timer(1.0 / rate_hz, self._tick) def _on_peds(self, msg: arena_people_msgs.msg.Pedestrians): @@ -110,26 +120,29 @@ def _tick(self): for name, entry in self._poly_cache.items(): robot_poly = self._robot_polygon(entry, rx, ry, rth) + emits_collision_event = entry['action_code'] == _STOP_ACTION if not walls.is_empty and robot_poly.intersects(walls): - ev = arena_robots_msgs.msg.CollisionEvent() - ev.obstacle_id = '' - ev.polygon_name = name - ev.distance = 0.0 - ev.obstacle_position = geometry_msgs.msg.Point(x=rx, y=ry, z=0.0) - events.append(ev) + if emits_collision_event: + ev = arena_robots_msgs.msg.CollisionEvent() + ev.obstacle_id = '' + ev.polygon_name = name + ev.distance = 0.0 + ev.obstacle_position = geometry_msgs.msg.Point(x=rx, y=ry, z=0.0) + events.append(ev) polygons_hit[name] = entry['action_code'] for obs_name, poly in statics.items(): if not robot_poly.intersects(poly): continue centroid = poly.centroid - ev = arena_robots_msgs.msg.CollisionEvent() - ev.obstacle_id = obs_name - ev.polygon_name = name - ev.distance = 0.0 - ev.obstacle_position = geometry_msgs.msg.Point(x=float(centroid.x), y=float(centroid.y), z=0.0) - events.append(ev) + if emits_collision_event: + ev = arena_robots_msgs.msg.CollisionEvent() + ev.obstacle_id = obs_name + ev.polygon_name = name + ev.distance = 0.0 + ev.obstacle_position = geometry_msgs.msg.Point(x=float(centroid.x), y=float(centroid.y), z=0.0) + events.append(ev) polygons_hit[name] = entry['action_code'] if self._peds_msg is not None: @@ -140,13 +153,34 @@ def _tick(self): ped_disc = shapely.Point(px, py).buffer(self._default_peds_radius) if not robot_poly.intersects(ped_disc): continue + if emits_collision_event: + ev = arena_robots_msgs.msg.CollisionEvent() + ev.obstacle_id = p.name + ev.polygon_name = name + ev.distance = 0.0 + ev.obstacle_position = geometry_msgs.msg.Point(x=px, y=py, z=0.0) + events.append(ev) + polygons_hit[name] = entry['action_code'] + + managers = getattr(getattr(self._rm.node, '_robots_manager', None), 'managers', {}) + for other_name, other in managers.items(): + if other is self._rm: + continue + other_pose = other.pose + if other_pose is None: + continue + ox, oy, _ = other_pose.to_2d() + other_disc = shapely.Point(ox, oy).buffer(other.radius) + if not robot_poly.intersects(other_disc): + continue + if emits_collision_event: ev = arena_robots_msgs.msg.CollisionEvent() - ev.obstacle_id = p.name + ev.obstacle_id = f'' ev.polygon_name = name ev.distance = 0.0 - ev.obstacle_position = geometry_msgs.msg.Point(x=px, y=py, z=0.0) + ev.obstacle_position = geometry_msgs.msg.Point(x=ox, y=oy, z=0.0) events.append(ev) - polygons_hit[name] = entry['action_code'] + polygons_hit[name] = entry['action_code'] state = nav2_msgs.msg.CollisionMonitorState() if polygons_hit: diff --git a/task_generator/task_generator/simulators/human/hunav/hunav.py b/task_generator/task_generator/simulators/human/hunav/hunav.py index f95694a4f..bea17fbe4 100644 --- a/task_generator/task_generator/simulators/human/hunav/hunav.py +++ b/task_generator/task_generator/simulators/human/hunav/hunav.py @@ -292,6 +292,10 @@ async def obstacle_to_walls(obstacle: Obstacle) -> list[Wall]: async def _spawn_dynamic_obstacles_impl(self, obstacles: Sequence[DynamicObstacle]) -> Sequence[DynamicObstacle | None]: async with self._agents_lock: + self._logger.info( + f"registering {len(obstacles)} dynamic obstacle(s) with Hunav: " + f"{[obstacle.name for obstacle in obstacles]}" + ) results: list[DynamicObstacle | None] = [] new_agent_msgs: list = [] @@ -310,6 +314,12 @@ async def _spawn_dynamic_obstacles_impl(self, obstacles: Sequence[DynamicObstacl arena_pedestrian = self._create_arena_pedestrian(hunav_obstacle, unique_id) self._arena_pedestrians_container.pedestrians.append(arena_pedestrian) # type: ignore + self._logger.info( + f"registered Hunav pedestrian {arena_pedestrian.name} " + f"id={arena_pedestrian.id} at " + f"({arena_pedestrian.pose.position.x:.2f}, " + f"{arena_pedestrian.pose.position.y:.2f})" + ) self._pedestrians[agent_msg.id] = { "last_update": time.time(),