fix(nav): survive controller_server crashes — respawn + lifecycle watchdog

ros2_ws/src/brain/brain_client/brain_client/nodes/brain_client_node.py

@@ -381,6 +381,7 @@ def _on_manual_skill_event(self, msg: String) -> None:
         primitive_name = payload.get("skill_name") or self._skill_name_for_id(skill_id)
         primitive_id = payload.get("primitive_id") or f"manual_{skill_id}_{int(time.time() * 1000)}"
         reason = payload.get("reason")
+        inputs = payload.get("inputs")
 
         self.get_logger().info(f"Manual skill event: {status} {primitive_name} ({skill_id})")
         if self.state.is_brain_active:
@@ -398,6 +399,7 @@ def _on_manual_skill_event(self, msg: String) -> None:
             status=status,
             skill_id=skill_id,
             reason=reason,
+            inputs=inputs,
         )
         self.chat.history.append(
             {
@@ -408,6 +410,7 @@ def _on_manual_skill_event(self, msg: String) -> None:
                 "primitiveId": primitive_id,
                 "skillId": skill_id,
                 **({"failureReason": reason} if reason else {}),
+                **({"inputs": inputs} if inputs else {}),
             }
         )
 

ros2_ws/src/brain/brain_client/brain_client/nodes/skills_server.py

@@ -322,7 +322,7 @@ def execute_callback(self, goal_handle):
 
         name = self._skill_display_name(skill_type)
         run_id = uuid.uuid4().hex
-        self._publish_skill_status(run_id, skill_type, name, "running")
+        self._publish_skill_status(run_id, skill_type, name, "running", inputs=inputs)
         try:
             if self.catalog.get_code_skill(skill_type) is not None:
                 result = self._execute_code_skill(goal_handle, skill_type, inputs)
@@ -341,12 +341,12 @@ def execute_callback(self, goal_handle):
         except Exception as e:
             # A 'running' broadcast went out above — a terminal status MUST
             # follow or every client shows this skill as active forever.
-            self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error")
+            self._publish_skill_status(run_id, skill_type, name, "failed", str(e) or "internal error", inputs=inputs)
             raise
         finally:
             self._release_skill_slot()
         status, reason = self._terminal_skill_status(result)
-        self._publish_skill_status(run_id, skill_type, name, status, reason)
+        self._publish_skill_status(run_id, skill_type, name, status, reason, inputs=inputs)
         return result
 
     @staticmethod
@@ -377,7 +377,7 @@ def _terminal_skill_status(result) -> tuple[str, str | None]:
         return "failed", result.message or None
 
     def _publish_skill_status(
-        self, run_id: str, skill_type: str, name: str, status: str, reason: str | None = None
+        self, run_id: str, skill_type: str, name: str, status: str, reason: str | None = None, inputs: dict | None = None
     ) -> None:
         payload = {
             "primitive_name": name,
@@ -389,6 +389,8 @@ def _publish_skill_status(
         }
         if reason:
             payload["reason"] = reason
+        if inputs:
+            payload["inputs"] = inputs
         self._skill_status_pub.publish(String(data=json.dumps(payload)))
 
     # ================= execution =================

ros2_ws/src/brain/brain_client/brain_client/transport/chat.py

@@ -54,6 +54,7 @@ def publish_task_status(
         status: str,
         skill_id: str | None = None,
         reason: str | None = None,
+        inputs: dict | None = None,
     ) -> None:
         """Publish a local task-status update for the controller-app UI."""
         payload = {
@@ -66,6 +67,8 @@ def publish_task_status(
         }
         if reason:
             payload["reason"] = reason
+        if inputs:
+            payload["inputs"] = inputs
         self._task_status_pub.publish(String(data=json.dumps(payload)))
 
     def history_json(self) -> str:

ros2_ws/src/brain/brain_client/launch/brain_client.sim.launch.py

@@ -108,6 +108,12 @@ def generate_launch_description():
         package="brain_client",
         executable="brain_client_node.py",
         name="brain_client_node",
+        # Sim runs on /clock (mars_sim_driver publishes it). A process-level
+        # --ros-args override, NOT a node parameter: these processes create
+        # extra nodes at runtime (BasicNavigator instances in skills and
+        # mobility) that a per-node parameters dict would never reach -- the
+        # global override applies to every node in the process.
+        arguments=["--ros-args", "-p", "use_sim_time:=true"],
         parameters=[
             {
                 "websocket_uri": LaunchConfiguration("websocket_uri"),
@@ -166,6 +172,9 @@ def generate_launch_description():
                 executable="skills_server.py",
                 name="skills_action_server",
                 output="screen",
+                # Same process-level sim-time override as brain_client_node:
+                # skills create BasicNavigator nodes at runtime.
+                arguments=["--ros-args", "-p", "use_sim_time:=true"],
                 parameters=[
                     {
                         "image_topic": LaunchConfiguration("image_topic"),

ros2_ws/src/brain/manipulation/launch/behavior.launch.py

@@ -7,6 +7,7 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
 from launch_ros.substitutions import FindPackageShare
 from mars_bringup.config_loader import settings_params, workspace_skills_dir
 
@@ -26,6 +27,11 @@ def generate_launch_description():
         "log_level", default_value="info", description="Log level for the behavior server"
     )
 
+    # ROS time source: false on the real robot (no /clock); the sim launcher
+    # passes true so this node follows the sim driver's /clock.
+    use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false")
+    use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)}
+
     # Resolved here because ROS YAML can't expand $INNATE_OS_ROOT. Mirrors recorder.launch.py.
     data_directory = str(workspace_skills_dir())
 
@@ -38,6 +44,7 @@ def generate_launch_description():
         parameters=[
             LaunchConfiguration("manipulation_config"),
             {"data_directory": data_directory},  # env-resolved; beats the YAML fallback
+            use_sim_time,
             *settings_params(),  # settings.yaml overrides, layered last
         ],
         arguments=["--ros-args", "--log-level", LaunchConfiguration("log_level")],
@@ -46,4 +53,4 @@ def generate_launch_description():
         respawn_delay=2.0,
     )
 
-    return LaunchDescription([manipulation_config_arg, log_level_arg, behavior_server_node])
+    return LaunchDescription([manipulation_config_arg, log_level_arg, use_sim_time_arg, behavior_server_node])

ros2_ws/src/cloud/innate_console/launch/console.launch.py

@@ -3,18 +3,26 @@
 """Launch file for the innate_console node."""
 
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
 
 
 def generate_launch_description() -> LaunchDescription:
+    # ROS time source: false on the real robot (no /clock); the sim launcher
+    # passes true so this node follows the sim driver's /clock.
+    use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false")
+    use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)}
     return LaunchDescription(
         [
+            use_sim_time_arg,
             Node(
                 package="innate_console",
                 executable="console_node",
                 name="innate_console",
                 output="screen",
-                parameters=[],
+                parameters=[use_sim_time],
             ),
         ]
     )

ros2_ws/src/cloud/innate_uninavid/launch/uninavid.launch.py

@@ -9,6 +9,7 @@
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
 from mars_bringup.config_loader import settings_params
 
 
@@ -27,15 +28,20 @@ def generate_launch_description() -> LaunchDescription:
         description="Velocity topic UniNavid commands should publish to",
     )
 
+    # ROS time source: false on the real robot (no /clock); the sim launcher
+    # passes true so this node follows the sim driver's /clock.
+    use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false")
+    use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)}
+
     node = Node(
         package="innate_uninavid",
         executable="uninavid_node",
         name="uninavid_node",
         output="screen",
-        parameters=[LaunchConfiguration("params_file"), *settings_params()],
+        parameters=[LaunchConfiguration("params_file"), use_sim_time, *settings_params()],
         remappings=[
             ("/cmd_vel", LaunchConfiguration("cmd_vel_topic")),
         ],
     )
 
-    return LaunchDescription([params_arg, cmd_vel_topic_arg, node])
+    return LaunchDescription([params_arg, cmd_vel_topic_arg, use_sim_time_arg, node])

ros2_ws/src/mars_bot/mars_control/launch/app.sim.launch.py

@@ -20,6 +20,8 @@ def generate_launch_description():
             {
                 "data_directory": data_directory,
                 "default_hardware_revision": default_hardware_revision,
+                # Sim runs on /clock (mars_sim_driver publishes it).
+                "use_sim_time": True,
             },
             *settings_params(),
         ],

ros2_ws/src/mars_bot/mars_nav/config/controller.yaml

@@ -63,7 +63,7 @@
       # Algorithm parameters
       temperature: 0.3                  # Trajectory selection temperature
       gamma: 0.015                      # Smoothness vs energy trade-off
-      visualize: false                    # Set to true for debugging (slows performance)
+      visualize: true                     # publishes /trajectories + /transformed_global_plan for the sim follower overlay (costs controller CPU)
       regenerate_noises: false
 
       # Goal tolerances

ros2_ws/src/mars_bot/mars_nav/launch/mode_manager.launch.py

@@ -6,9 +6,11 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
 from mars_bringup.config_loader import load_motion_limit_overrides, load_yaml_param_defaults
 
 
@@ -62,14 +64,24 @@ def generate_launch_description():
     nav_through_poses_bt_xml = os.path.join(share_dir, "config", "nav_through_poses.xml")
     nav_to_pose_mapfree_bt_xml = os.path.join(share_dir, "config", "nav_to_pose_mapfree.xml")  # noqa: F841
 
+    # ROS time source: false on the real robot (no /clock); the sim launcher
+    # passes true so the whole nav stack follows the sim driver's /clock and
+    # freezes with the world. Layered last in each node's parameters so it
+    # overrides the yamls' hardcoded use_sim_time: false.
+    use_sim_time_arg = DeclareLaunchArgument("use_sim_time", default_value="false")
+    use_sim_time = {"use_sim_time": ParameterValue(LaunchConfiguration("use_sim_time"), value_type=bool)}
+
     navigation_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([mars_nav_launch_dir, "/navigation.launch.py"])
+        PythonLaunchDescriptionSource([mars_nav_launch_dir, "/navigation.launch.py"]),
+        launch_arguments={"use_sim_time": LaunchConfiguration("use_sim_time")}.items(),
     )
 
     # mapfree_launch = IncludeLaunchDescription(
     #     PythonLaunchDescriptionSource([mars_nav_launch_dir, '/mapfree_local_nav.launch.py'])
     # )
 
+    # mapping.launch.py keeps its own use_sim_time default (true) -- threading
+    # ours through would change slam_toolbox's existing real-robot behavior.
     mapping_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([mars_nav_launch_dir, "/mapping.launch.py"])
     )
@@ -84,7 +96,8 @@ def generate_launch_description():
             {
                 "slowdown_radius": 1.0,
                 "min_speed_fraction": 0.3,
-            }
+            },
+            use_sim_time,
         ],
         remappings=[
             ("cmd_vel_in", "/cmd_vel_raw"),
@@ -101,7 +114,7 @@ def generate_launch_description():
         executable="velocity_smoother",
         name="velocity_smoother",
         output="screen",
-        parameters=[smoother_params_file, smoother_limit_overrides],
+        parameters=[smoother_params_file, smoother_limit_overrides, use_sim_time],
         # Nav2 output feeds the priority mux, not the base directly.
         remappings=[("cmd_vel", "/cmd_vel_scaled"), ("cmd_vel_smoothed", "/cmd_vel_nav")],
         arguments=["--ros-args", "--log-level", "warn"],
@@ -114,6 +127,7 @@ def generate_launch_description():
         executable="cmd_vel_mux.py",
         name="cmd_vel_mux",
         output="screen",
+        parameters=[use_sim_time],
     )
 
     # Shared BT navigator node
@@ -126,6 +140,7 @@ def generate_launch_description():
             bt_navigator_params_file,
             {"default_nav_to_pose_bt_xml": nav_to_pose_bt_xml},
             {"default_nav_through_poses_bt_xml": nav_through_poses_bt_xml},
+            use_sim_time,
         ],
         remappings=expand_action_remappings(
             [
@@ -143,7 +158,7 @@ def generate_launch_description():
         executable="behavior_server",
         name="behavior_server",
         output="screen",
-        parameters=[behavior_params_file],
+        parameters=[behavior_params_file, use_sim_time],
         arguments=["--ros-args", "--log-level", "warn"],
         remappings=[("cmd_vel", "cmd_vel_raw")],
     )
@@ -153,6 +168,7 @@ def generate_launch_description():
         executable="dynamic_footprint",
         name="dynamic_footprint",
         output="screen",
+        parameters=[use_sim_time],
     )
 
     # Mapfree planner node (runs in mapfree namespace)
@@ -162,7 +178,7 @@ def generate_launch_description():
         name="planner_server",
         namespace="mapfree",
         output="screen",
-        parameters=[planner_params_file, costmap_params_file],
+        parameters=[planner_params_file, costmap_params_file, use_sim_time],
         remappings=[
             # TF remappings - critical for namespaced nodes
             ("tf", "/tf"),
@@ -174,10 +190,13 @@ def generate_launch_description():
     )
 
     # Null map node for identity map->odom transform
-    null_map_node = Node(package="mars_nav", executable="null_map_node", name="null_map_node", output="screen")
+    null_map_node = Node(
+        package="mars_nav", executable="null_map_node", name="null_map_node", output="screen", parameters=[use_sim_time]
+    )
 
     return LaunchDescription(
         [
+            use_sim_time_arg,
             null_map_node,
             navigation_launch,
             # mapfree_launch,
@@ -194,7 +213,7 @@ def generate_launch_description():
                 executable="mode_manager.py",
                 name="mode_manager",
                 output="screen",
-                parameters=[],
+                parameters=[use_sim_time],
                 # nav2's BasicNavigator creates internal nodes that can share a
                 # name; mute the benign "Publisher already registered" warning.
                 arguments=["--ros-args", "--log-level", "rcl.logging_rosout:=ERROR"],