Fix Gazebo pose, multi env rviz, add velocity smoother, remove torch companions

_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"

_meta/tools/pull

@@ -49,18 +49,13 @@ 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
         uv pip install --python "$ARENA_DIR/.venv/bin/python" \
             -e "." \
             -e "./deps/rosnav_rl/rosnav_rl"
         popd > /dev/null
-
-        "$ARENA_DIR/_meta/tools/install_torch_companions" "$ARENA_DIR/.venv/bin/python"
     fi
 fi
 

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

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(

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'
+robot_odom_frame: 'odom'

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}

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',

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(