diff --git a/_meta/features/training/main b/_meta/features/training/main index 7193af2..58288c3 100644 --- a/_meta/features/training/main +++ b/_meta/features/training/main @@ -22,10 +22,6 @@ install(){ -e "./deps/rosnav_rl/rosnav_rl" popd > /dev/null - echo "Removing incompatible torch companion packages..." - uv pip uninstall --python "$ARENA_DIR/.venv/bin/python" torchvision torchaudio || true - - "$ARENA_DIR/_meta/tools/install_torch_companions" "$ARENA_DIR/.venv/bin/python" if ! grep -q "$name" "$INSTALLED" 2>/dev/null; then echo "$name" >> "$INSTALLED" diff --git a/_meta/tools/pull b/_meta/tools/pull index b5decb7..1fee0d6 100755 --- a/_meta/tools/pull +++ b/_meta/tools/pull @@ -49,9 +49,6 @@ if [ $_PYTHON -eq 1 ] ; then echo updating python deps... uv sync --inexact - echo removing incompatible torch companion packages... - uv pip uninstall --python "$ARENA_DIR/.venv/bin/python" torchvision torchaudio || true - if { [ -d "$ARENA_DIR/arena_training" ] && git submodule status arena_training 2>/dev/null | grep -qv '^-'; } || grep -qx 'training' "$INSTALLED" 2>/dev/null; then echo updating training python deps... pushd "$ARENA_DIR/arena_training" > /dev/null @@ -59,8 +56,6 @@ if [ $_PYTHON -eq 1 ] ; then -e "." \ -e "./deps/rosnav_rl/rosnav_rl" popd > /dev/null - - "$ARENA_DIR/_meta/tools/install_torch_companions" "$ARENA_DIR/.venv/bin/python" fi fi diff --git a/arena_bringup/arena_bringup/substitutions.py b/arena_bringup/arena_bringup/substitutions.py index d234db6..93a940f 100644 --- a/arena_bringup/arena_bringup/substitutions.py +++ b/arena_bringup/arena_bringup/substitutions.py @@ -420,6 +420,51 @@ def __init__(self, substitutions: dict): self._substitutions = substitutions +class VelSmootherSubstitution(launch.Substitution): + """Derives a ``[lin_x, lin_y, ang_z]`` velocity limit list from the + ``actions.continuous`` block in a robot's *model_params.yaml*. + + This removes the need for explicit ``vel_smoother_max_velocity`` / + ``vel_smoother_min_velocity`` parameters in every robot config file. + Holonomic robots may add a ``lateral_range`` key to ``actions.continuous``; + when absent, ``linear_range`` is used as the Y-axis fallback. + """ + + def __init__(self, model_params: YAMLFileSubstitution, *, use_max: bool): + launch.Substitution.__init__(self) + self._model_params = model_params + self._use_max = use_max # True → upper bound (index 1), False → lower (index 0) + + def perform(self, context: launch.LaunchContext): + params = self._model_params.perform_load(context) + continuous = (params.get("actions") or {}).get("continuous") or {} + idx = 1 if self._use_max else 0 + lin = continuous.get("linear_range", [-1.0, 1.0]) + lat = continuous.get("lateral_range", lin) # holonomic fallback: Y == X + ang = continuous.get("angular_range", [-1.0, 1.0]) + return [lin[idx], lat[idx], ang[idx]] + + +class VelSmootherAccelSubstitution(launch.Substitution): + """Derives a ``[lin_x, lin_y, ang_z]`` acceleration or deceleration list + from the ``actions`` block in a robot's *model_params.yaml*. + The ``acceleration`` key holds positive values; ``deceleration`` holds + the matching negative values. + """ + + def __init__(self, model_params: YAMLFileSubstitution, *, decel: bool): + launch.Substitution.__init__(self) + self._model_params = model_params + self._decel = decel + + def perform(self, context: launch.LaunchContext): + params = self._model_params.perform_load(context) + actions = params.get("actions") or {} + if self._decel: + return actions.get("deceleration", [-20.0, 0.0, -25.0]) + return actions.get("acceleration", [20.0, 0.0, 25.0]) + + class YAMLReplaceSubstitution(launch.Substitution): _substitutions: YAMLFileSubstitution diff --git a/arena_bringup/launch/arena.launch.py b/arena_bringup/launch/arena.launch.py index 19ab8ef..f90ed35 100644 --- a/arena_bringup/launch/arena.launch.py +++ b/arena_bringup/launch/arena.launch.py @@ -252,6 +252,45 @@ def create_task_generator( }, ) + def create_rviz_launcher( + context: launch.LaunchContext, + *, + n_substitution: launch.SomeSubstitutionsType, + ) -> typing.Optional[typing.List[launch.LaunchDescriptionEntity]]: + n = launch.utilities.type_utils.perform_typed_substitution( + context, + launch.utilities.normalize_to_list_of_substitutions(n_substitution), + int, + ) + n = typing.cast(int, n) + if n < 1: + return None + + base = 'task_generator_node' + if n == 1: + ns_args = [f'/{base}'] + else: + ns_args = [f'/{base}/env{i}' for i in range(n)] + + return [ + launch_ros.actions.Node( + package='rviz_utils', + executable='multi_env_rviz', + name='multi_env_rviz', + arguments=ns_args, + parameters=[{'use_sim_time': True}], + output='screen', + condition=launch.conditions.UnlessCondition( + PythonExpression([headless.substitution, '>1']) + ), + ) + ] + + launch_rviz = launch.actions.OpaqueFunction( + function=create_rviz_launcher, + kwargs={'n_substitution': env_n.substitution}, + ) + launch_simulator = launch.actions.IncludeLaunchDescription( launch.launch_description_sources.PythonLaunchDescriptionSource( os.path.join(bringup_dir, 'launch/simulator/sim/sim.launch.py') @@ -282,6 +321,7 @@ def create_task_generator( ), SetGlobalLogLevelAction(log_level.substitution), launch_task_generators, + launch_rviz, IsolatedGroupAction([launch_simulator]), world_generator_node, launch.actions.ExecuteProcess( diff --git a/arena_robots b/arena_robots index 8b0c9a8..fc8addd 160000 --- a/arena_robots +++ b/arena_robots @@ -1 +1 @@ -Subproject commit 8b0c9a89cc8e630dcb885cb68999e7cf6a373b11 +Subproject commit fc8addd11eee64eab6ae7f4cf87a92ea6ddea424 diff --git a/arena_simulation_setup/configs/nav2/model_params.yaml b/arena_simulation_setup/configs/nav2/model_params.yaml index b2c8a86..34092e1 100644 --- a/arena_simulation_setup/configs/nav2/model_params.yaml +++ b/arena_simulation_setup/configs/nav2/model_params.yaml @@ -33,4 +33,4 @@ planner_plugins_dict: footprint: "[ [0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1] ]" robot_base_frame: 'base_link' -robot_odom_frame: 'odom' \ No newline at end of file +robot_odom_frame: 'odom' diff --git a/arena_simulation_setup/configs/nav2/nav2.yaml b/arena_simulation_setup/configs/nav2/nav2.yaml index 10bae8e..760ef44 100644 --- a/arena_simulation_setup/configs/nav2/nav2.yaml +++ b/arena_simulation_setup/configs/nav2/nav2.yaml @@ -252,6 +252,21 @@ recoveries_server: min_rotational_vel: 0.4 rotational_acc_lim: 3.2 +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: false + feedback: "OPEN_LOOP" + max_velocity: ${vel_smoother_max_velocity} + min_velocity: ${vel_smoother_min_velocity} + max_accel: ${vel_smoother_max_accel} + max_decel: ${vel_smoother_max_decel} + odom_topic: "${robot_odom_frame:-odom}" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + collision_monitor: ros__parameters: observation_sources: ${observation_sources} diff --git a/arena_simulation_setup/launch/nav2.launch.py b/arena_simulation_setup/launch/nav2.launch.py index 1a68b34..d92c411 100644 --- a/arena_simulation_setup/launch/nav2.launch.py +++ b/arena_simulation_setup/launch/nav2.launch.py @@ -1,7 +1,8 @@ import os from arena_bringup.future import PythonExpression -from arena_bringup.substitutions import (LaunchArgument, YAMLFileSubstitution, +from arena_bringup.substitutions import (LaunchArgument, VelSmootherAccelSubstitution, VelSmootherSubstitution, + YAMLFileSubstitution, YAMLMergeSubstitution, YAMLReplaceSubstitution, YAMLRetrieveSubstitution) @@ -43,7 +44,7 @@ def generate_launch_description(): 'model_params.yaml' ]) ), - YAMLFileSubstitution( + robot_model_params_yaml := YAMLFileSubstitution( PathJoinSubstitution([ robots_root, 'robots', @@ -133,6 +134,11 @@ def generate_launch_description(): ), 'bt_navigator/ros__parameters/plugin_lib_names' ), + 'vel_smoother_max_velocity': VelSmootherSubstitution(robot_model_params_yaml, use_max=True), + 'vel_smoother_min_velocity': VelSmootherSubstitution(robot_model_params_yaml, use_max=False), + # Acceleration / deceleration derived from actions.continuous + 'vel_smoother_max_accel': VelSmootherAccelSubstitution(robot_model_params_yaml, decel=False), + 'vel_smoother_max_decel': VelSmootherAccelSubstitution(robot_model_params_yaml, decel=True), }, substitute=True ), @@ -192,13 +198,17 @@ def generate_launch_description(): name='goal_pose_relay', arguments=['/goal_pose', 'goal_pose'], ), - Node( - package='pose_to_tf', - executable='pose_to_tf', - name='pose_to_tf', - parameters=[{'odom_frame': 'odom', 'pose_topic': 'pose'}], - output='screen' - ), + # pose_to_tf disabled: map→odom TF is now published by the dynamic TF broadcaster in + # gazebo_simulator.py (_publish_map_to_odom_tfs). pose_to_tf incorrectly published + # map→odom = raw Gazebo pose without accounting for DiffDrive accumulated odom offset, + # which would double-count wheel motion and place the robot at the wrong map position. + # Node( + # package='pose_to_tf', + # executable='pose_to_tf', + # name='pose_to_tf', + # parameters=[{'odom_frame': 'odom', 'pose_topic': 'pose'}], + # output='screen' + # ), # Node( # package='tf2_ros', # executable='static_transform_publisher', diff --git a/task_generator/launch/task_generator.launch.py b/task_generator/launch/task_generator.launch.py index e0a0d82..0de45cb 100644 --- a/task_generator/launch/task_generator.launch.py +++ b/task_generator/launch/task_generator.launch.py @@ -6,7 +6,7 @@ import launch_ros.actions from ament_index_python.packages import get_package_share_directory from arena_bringup.future import PythonExpression -from arena_bringup.substitutions import CurrentNamespaceSubstitution, LaunchArgument +from arena_bringup.substitutions import LaunchArgument import launch @@ -104,23 +104,8 @@ def generate_launch_description(): PythonExpression(['"', human.substitution, '" == "hunav"']) ), ) - # Start the rviz config generator which launches also rviz2 with desired config file - rviz_node = launch_ros.actions.Node( - package="rviz_utils", - executable="rviz_config", - name="rviz_config_generator", - arguments=[ - CurrentNamespaceSubstitution(), - ], - parameters=[ - { - "use_sim_time": True, - "origin": reference.param_value(typing.List[float]), - } - ], - output="screen", - condition=launch.conditions.UnlessCondition(headless.substitution), - ) + # rviz is now launched centrally by arena.launch.py (multi_env_rviz) so + # each task_generator no longer starts its own rviz instance. task_generator_node = launch_ros.actions.Node( package="task_generator", @@ -151,7 +136,6 @@ def generate_launch_description(): **debug.param(bool), **train_mode.param(bool), }, - {"use_sim_time": False}, parameter_file.substitution, ], ) @@ -180,7 +164,6 @@ def generate_launch_description(): ), map_server_node, pedestrian_marker_node, - rviz_node, ] ), launch.actions.RegisterEventHandler( diff --git a/task_generator/task_generator/simulators/sim/gazebo_simulator/gazebo_simulator.py b/task_generator/task_generator/simulators/sim/gazebo_simulator/gazebo_simulator.py index b708424..78b8bd2 100644 --- a/task_generator/task_generator/simulators/sim/gazebo_simulator/gazebo_simulator.py +++ b/task_generator/task_generator/simulators/sim/gazebo_simulator/gazebo_simulator.py @@ -12,7 +12,7 @@ import rclpy.duration import tf2_ros from arena_simulation_setup.tree.assets.Object import ObjectIdentifier -from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, TransformStamped from ros_gz_interfaces.msg import Entity as EntityMsg from ros_gz_interfaces.msg import EntityFactory, WorldControl from ros_gz_interfaces.srv import ControlWorld, DeleteEntity, SetEntityPose, SpawnEntity @@ -59,6 +59,94 @@ def __init__(self, *args, namespace, **kwargs): self._tf_buffer = tf2_ros.Buffer() self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self.node) + # Regular TF broadcaster for map→odom transforms. + self._map_to_odom_tfs: dict[str, TransformStamped] = {} + self._tf_broadcaster = tf2_ros.TransformBroadcaster(self.node) + + # Periodically republish map→odom TFs so they don't expire in the TF buffer + self._tf_publish_timer = self.node.create_timer( + 0.1, + self._publish_map_to_odom_tfs, + ) + + # Gazebo ground-truth pose subscriptions for continuous map→odom correction. + # Keyed by odom_frame_name to avoid duplicates. + self._gz_pose_subs: dict[str, object] = {} + + def _setup_gazebo_pose_tracking(self, robot_name: str, odom_frame: str, base_frame: str): + """Subscribe to robot's Gazebo ground-truth pose for continuous map→odom correction. + + Instead of computing map→odom only at teleportation time (which is fragile + due to DiffDrive odom jumps after sim unpause), this continuously derives + map→odom from the actual Gazebo world pose and the current odom→base_link TF: + map→odom = gazebo_world_pose × inv(odom→base_link) + """ + if odom_frame in self._gz_pose_subs: + return + + pose_topic = self.node.service_namespace(robot_name, "pose") + + def pose_callback(msg: PoseStamped): + try: + odom_tf = self._tf_buffer.lookup_transform( + odom_frame, base_frame, rclpy.time.Time() + ) + + odom_x = odom_tf.transform.translation.x + odom_y = odom_tf.transform.translation.y + odom_yaw = self._quaternion_to_yaw( + odom_tf.transform.rotation.x, odom_tf.transform.rotation.y, + odom_tf.transform.rotation.z, odom_tf.transform.rotation.w, + ) + + gz_yaw = self._quaternion_to_yaw( + msg.pose.orientation.x, msg.pose.orientation.y, + msg.pose.orientation.z, msg.pose.orientation.w, + ) + + tf_yaw = gz_yaw - odom_yaw + cos_tf = math.cos(tf_yaw) + sin_tf = math.sin(tf_yaw) + tf_x = msg.pose.position.x - (odom_x * cos_tf - odom_y * sin_tf) + tf_y = msg.pose.position.y - (odom_x * sin_tf + odom_y * cos_tf) + + ts = TransformStamped() + ts.header.stamp = self.node.get_clock().now().to_msg() + ts.header.frame_id = "map" + ts.child_frame_id = odom_frame + ts.transform.translation.x = tf_x + ts.transform.translation.y = tf_y + ts.transform.translation.z = msg.pose.position.z + qx, qy, qz, qw = self._yaw_to_quaternion(tf_yaw) + ts.transform.rotation.x = qx + ts.transform.rotation.y = qy + ts.transform.rotation.z = qz + ts.transform.rotation.w = qw + + self._map_to_odom_tfs[odom_frame] = ts + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException): + pass # odom→base not available yet + + self._gz_pose_subs[odom_frame] = self.node.create_subscription( + PoseStamped, + pose_topic, + pose_callback, + 10, + ) + self._logger.info( + f"Tracking Gazebo ground-truth pose on {pose_topic} for map→{odom_frame}" + ) + + def _publish_map_to_odom_tfs(self): + """Periodically republish all map→odom TFs with current sim time.""" + if not self._map_to_odom_tfs: + return + now = self.node.get_clock().now().to_msg() + for ts in self._map_to_odom_tfs.values(): + ts.header.stamp = now + self._tf_broadcaster.sendTransform(list(self._map_to_odom_tfs.values())) + async def before_reset_task(self): self._logger.warn("Pausing simulation before reset") return bool(await self.pause_simulation()) @@ -271,7 +359,6 @@ async def _spawn_entity(self, entity: Entity) -> bool: return False async def _delete_entity(self, name: str): - return True async with self._semaphore: name = name @@ -481,7 +568,9 @@ async def _robot_bridge(self, robot: Robot, description: str): parameters=[ {"use_sim_time": True}, {"robot_description": description}, - {"frame_prefix": robot.frame + "/"}, # add trailing slash + # used throughout nav2, collision_monitor, and static TF publishers. + # e.g. "jackal_" - jackal_base_link, jackal_lidar_link + {"frame_prefix": str(robot.frame) + "_"}, ], ) ) @@ -637,12 +726,14 @@ async def _robot_move(self, robot: Robot) -> bool: odom_frame = robot_config.model_params.odom_frame base_frame = robot_config.model_params.base_frame - # Get frame names as raw strings (FrameNamespace.__str__ is sanitized - # by auto_sanitize, but TF frames in the tree use unsanitized '/' names) - odom_frame_name = str.__str__(robot.frame(odom_frame)) - base_frame_name = str.__str__(robot.frame(base_frame)) + # Get sanitized frame names (FrameNamespace.auto_sanitize replaces '/' with '_') + # These must match the frame_id/child_frame_id in the DiffDrive plugin + odom_frame_name = str(robot.frame(odom_frame)) + base_frame_name = str(robot.frame(base_frame)) - # Compute the correct map→odom TF accounting for DiffDrive odom offset + # Compute an initial map→odom estimate using the current odom reading. + # The Gazebo ground-truth pose subscription will continuously correct it + # once the sim unpauses (handles DiffDrive odom jumps after teleportation). tf_x, tf_y, tf_z, tf_qx, tf_qy, tf_qz, tf_qw = self._compute_map_to_odom_tf( desired_x=robot.pose.position.x, desired_y=robot.pose.position.y, @@ -655,24 +746,24 @@ async def _robot_move(self, robot: Robot) -> bool: base_frame_name=base_frame_name, ) - transform_pub_node = launch_ros.actions.Node( - package="tf2_ros", - executable="static_transform_publisher", - name="map_to_odomframe_publisher", - arguments=[ - str(tf_x), - str(tf_y), - str(tf_z), - str(tf_qx), - str(tf_qy), - str(tf_qz), - str(tf_qw), - "map", - robot.frame(odom_frame), - ], - parameters=[{"use_sim_time": True}], - ) - await self.node.do_launch(launch.LaunchDescription([transform_pub_node])) + ts = TransformStamped() + ts.header.stamp = self.node.get_clock().now().to_msg() + ts.header.frame_id = "map" + ts.child_frame_id = odom_frame_name + ts.transform.translation.x = tf_x + ts.transform.translation.y = tf_y + ts.transform.translation.z = tf_z + ts.transform.rotation.x = tf_qx + ts.transform.rotation.y = tf_qy + ts.transform.rotation.z = tf_qz + ts.transform.rotation.w = tf_qw + self._map_to_odom_tfs[odom_frame_name] = ts + self._tf_broadcaster.sendTransform(list(self._map_to_odom_tfs.values())) + + # Set up continuous ground-truth tracking from Gazebo pose topic. + # This corrects for DiffDrive odom jumps after teleportation and + # any accumulated odometry drift over time. + self._setup_gazebo_pose_tracking(robot.name, odom_frame_name, base_frame_name) return True diff --git a/utils/arena_rclpy_mixins/setup.py b/utils/arena_rclpy_mixins/setup.py index 705c3e6..83ebe03 100644 --- a/utils/arena_rclpy_mixins/setup.py +++ b/utils/arena_rclpy_mixins/setup.py @@ -45,7 +45,6 @@ # scripts here. entry_points={ 'console_scripts': [ - 'rviz_config = rviz_utils.scripts.rviz_config:main', 'visualize_robot_model = rviz_utils.scripts.visualize_robot_model:main' ], }, diff --git a/utils/rviz_utils/rviz_utils/scripts/multi_env_rviz.py b/utils/rviz_utils/rviz_utils/scripts/multi_env_rviz.py new file mode 100644 index 0000000..dd91cf3 --- /dev/null +++ b/utils/rviz_utils/rviz_utils/scripts/multi_env_rviz.py @@ -0,0 +1,441 @@ +#!/usr/bin/env python3 +"""Single-window multi-environment RViz launcher. + +Accepts N task_generator_node namespace paths as positional CLI arguments, +waits for every environment to finish initialising, then builds **one** +collapsible Group display per environment (Map, TF, Pedestrians, per-robot +sub-groups) and opens exactly **one** rviz2 window. + +Usage (called by arena.launch.py): + multi_env_rviz /task_generator_node + multi_env_rviz /task_generator_node/env0 /task_generator_node/env1 ... +""" + +import os +import subprocess +import sys +import tempfile +import time +import typing + +import rcl_interfaces.msg +import rcl_interfaces.srv +import rclpy +import yaml +from ament_index_python.packages import get_package_share_directory +from rclpy.node import Node + +from rviz_utils.utils import Utils + +# --------------------------------------------------------------------------- +# Snap / libpthread environment fix +# --------------------------------------------------------------------------- +_SYSTEM_LIB_DIRS = ["/lib/x86_64-linux-gnu", "/usr/lib/x86_64-linux-gnu"] +_PTHREAD_CANDIDATES = [ + "/lib/x86_64-linux-gnu/libpthread.so.0", + "/usr/lib/x86_64-linux-gnu/libpthread.so.0", +] + + +def _build_clean_env() -> dict: + """Return a sanitised copy of the current environment suitable for launching + rviz2 without snap-induced glibc PRIVATE symbol conflicts. + + Three-layer defence: + 1. Drop all SNAP* env vars and strip any /snap/ paths from path-list vars. + 2. Prepend system lib dirs to LD_LIBRARY_PATH so they win over any RPATH + that might still point into /snap/core20/. + 3. LD_PRELOAD the stub libpthread so it is already resident in memory before + any dlopen()'d GL/Qt plugin can load the snap version via its own RPATH. + """ + def _strip_snap_paths(val: str) -> str: + return ":".join(p for p in val.split(":") if "/snap/" not in p) + + env = { + k: (_strip_snap_paths(v) if k in ("LD_LIBRARY_PATH", "PYTHONPATH") else v) + for k, v in os.environ.items() + if not k.startswith("SNAP") + } + + existing = env.get("LD_LIBRARY_PATH", "") + env["LD_LIBRARY_PATH"] = ":".join( + _SYSTEM_LIB_DIRS + [p for p in existing.split(":") if p] + ) + + system_pthread = next((p for p in _PTHREAD_CANDIDATES if os.path.exists(p)), None) + if system_pthread: + existing_preload = env.get("LD_PRELOAD", "") + env["LD_PRELOAD"] = f"{system_pthread}:{existing_preload}".rstrip(":") + + return env + + +# Per-environment accent colour palette (cycles for > 8 envs) +_ENV_PALETTE = [ + "66; 134; 244", # blue + "244; 110; 66", # orange + "66; 214; 126", # green + "214; 66; 164", # pink + "214; 194; 66", # yellow + "66; 214; 214", # cyan + "164; 66; 214", # purple + "214; 66; 66", # red +] + + +class MultiEnvRvizGenerator(Node): + """Generate and write an rviz config covering all environments, then + launch a single rviz2 process.""" + + def __init__(self, taskgen_nodes: typing.List[str]) -> None: + Node.__init__(self, "multi_env_rviz_generator") + + self._nodes = taskgen_nodes + self._env_data: typing.List[typing.Dict[str, typing.Any]] = [] + + for taskgen_node in taskgen_nodes: + srv_name = os.path.join(taskgen_node, "get_parameters") + cli = self.create_client(rcl_interfaces.srv.GetParameters, srv_name) + self.get_logger().info(f"Waiting for service: {srv_name}") + while not cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info(f" still waiting for {srv_name} …") + + self._wait_for_param(cli, "initialized", test_fn=lambda x: x.bool_value) + robot_names = self._wait_for_param(cli, "robot_names").string_array_value + + self._env_data.append( + { + "node": taskgen_node, + "robot_names": list(robot_names), + } + ) + self.get_logger().info( + f"Env '{taskgen_node}' ready — robots: {list(robot_names)}" + ) + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + + def _wait_for_param( + self, + client: rclpy.client.Client, + param_name: str, + test_fn: typing.Optional[typing.Callable[[typing.Any], bool]] = None, + timeout: float = 1.0, + ) -> rcl_interfaces.msg.ParameterValue: + """Block until parameter exists and satisfies *test_fn* (if given).""" + while True: + self.get_logger().info(f" waiting for param '{param_name}' …") + for _ in range(5): + req = rcl_interfaces.srv.GetParameters.Request(names=[param_name]) + future = client.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=timeout) + result = future.result() + if result and result.values: + val = result.values[0] + if (not test_fn) or test_fn(val): + return val + time.sleep(timeout) + + @staticmethod + def _read_default_file(first_taskgen_node: str) -> dict: + pkg = get_package_share_directory("rviz_utils") + path = os.path.join(pkg, "config", "rviz_default.rviz") + with open(path) as fh: + raw = fh.read().format(task_generator_node=first_taskgen_node) + return yaml.safe_load(raw) + + @staticmethod + def _tmp_config_file(cfg: dict) -> str: + f = tempfile.NamedTemporaryFile("w", suffix=".rviz", delete=False) + yaml.dump(cfg, f) + f.close() + return f.name + + # ------------------------------------------------------------------ + # Display builders + # ------------------------------------------------------------------ + + def _create_pedestrian_group( + self, taskgen_node: str, all_topics: list + ) -> dict: + ped_group: dict = { + "Class": "rviz_common/Group", + "Name": "Pedestrians", + "Enabled": True, + "Displays": [], + } + for topic_name, topic_types in all_topics: + if not topic_name.startswith(taskgen_node): + continue + if "visualization_msgs/msg/MarkerArray" in topic_types and ( + topic_name.endswith("/pedestrian_markers") + or topic_name.endswith("/wall_markers") + ): + ped_group["Displays"].append( + Utils.Displays.pedestrians( + topic_name, + name=os.path.basename(topic_name), + enabled=True, + ) + ) + elif "people_msgs/msg/People" in topic_types and topic_name.endswith("/people"): + ped_group["Displays"].append( + Utils.Displays.pedestrians_raw(topic_name) + ) + # hunav_msgs/msg/Agents: no rviz plugin available, skip + + # Disabled TF display for quick pedestrian-frame debugging + ped_group["Displays"].append({ + "Class": "rviz_default_plugins/TF", + "Name": "Pedestrian TF Frames", + "Enabled": False, + "Frame Timeout": 15, + "Marker Scale": 0.3, + "Show Arrows": True, + "Show Axes": False, + "Show Names": True, + }) + return ped_group + + def _create_robot_group( + self, + taskgen_node: str, + robot_name: str, + env_color: str, + all_topics: list, + ) -> dict: + robot_group: dict = { + "Class": "rviz_common/Group", + "Name": f"Robot: {robot_name}", + "Enabled": True, + "Displays": [], + } + ns = taskgen_node + # Derive the TF prefix that rviz2 uses to locate robot link frames. + # rviz2 constructs per-link frame IDs as: tf_prefix + "/" + link_name + # robot_state_publisher is launched with frame_prefix = "