Drive PID from odometry stamps instead of a wall-clock poll

.github/workflows/headless-simulation.yml

@@ -1,6 +1,7 @@
 name: Headless Simulation Test
 
 on:
+  pull_request:
   push:
     branches: [ main, docker, zed_node_update ]
 
@@ -26,10 +27,11 @@ jobs:
         context: .
         push: false
         tags: rumarino-headless:latest
-        cache-from: type=gha
-        cache-to: type=gha,mode=max
+        cache-from: type=gha,scope=rumarino-headless
+        cache-to: type=gha,mode=max,scope=rumarino-headless
         outputs: type=docker
 
+    # fast_fixed_step + sim stamps, RTF cap 5x. 15 s wall ≈ 75 s sim.
     - name: Run headless simulation test
       run: |
         docker run --rm \
@@ -38,9 +40,14 @@ jobs:
           bash -c "
             source /opt/ros/jazzy/setup.bash && \
             source /ros2_ws/install/setup.bash && \
-            ros2 launch bringup test_mission_executor_headless.launch.py \
+            ros2 launch bringup stonefish.launch.py \
               mission_name:=prequalify \
-              env_file_name:=hydrus_env_headless.scn &
+              auv_name:=hydrus \
+              env_file_name:=hydrus_env_headless.scn \
+              headless:=true \
+              fast_fixed_step:=true \
+              use_sim_time_stamps:=true \
+              realtime_factor_cap:=5.0 &
             LAUNCH_PID=\$! && \
             sleep 15 && \
             kill \$LAUNCH_PID 2>/dev/null || true

.gitmodules

@@ -3,7 +3,7 @@
 	url = https://github.com/patrykcieslak/stonefish.git
 [submodule "vendor/stonefish_ros2"]
 	path = vendor/stonefish_ros2
-	url = https://github.com/JuanDelPueblo/stonefish_ros2.git
+	url = https://github.com/Rumarino-Team/stonefish_ros2.git
 [submodule "vendor/zed-ros-interfaces"]
 	path = vendor/zed-ros-interfaces
 	url = https://github.com/stereolabs/zed-ros2-interfaces.git

Dockerfile

@@ -31,6 +31,8 @@ RUN apt-get update && apt-get install -y \
     libssl-dev \
     libboost-all-dev \
     libepoxy-dev \
+    libtinyxml2-dev \
+    ros-jazzy-visualization-msgs \
     && rm -rf /var/lib/apt/lists/*
 
 # Install Rust
@@ -55,6 +57,7 @@ COPY src/interfaces/package.xml ./src/interfaces/package.xml
 COPY src/bringup/package.xml ./src/bringup/package.xml
 COPY src/bridge_stonefish/package.xml ./src/bridge_stonefish/package.xml
 COPY src/mission_executor/package.xml ./src/mission_executor/package.xml
+COPY src/detection_mocker/package.xml ./src/detection_mocker/package.xml
 COPY vendor/stonefish_ros2/package.xml ./src/stonefish_ros2/package.xml
 
 RUN rosdep init || true && \
@@ -84,12 +87,13 @@ RUN bash -lc "source /opt/ros/jazzy/setup.bash && \
 
 COPY src/bringup ./src/bringup
 COPY src/bridge_stonefish ./src/bridge_stonefish
+COPY src/detection_mocker ./src/detection_mocker
 COPY vendor/stonefish_ros2 ./src/stonefish_ros2
 
 # build ROS 2 workspace except interfaces and mission_executor
 RUN bash -c "source /opt/ros/jazzy/setup.bash && \
     colcon build \
-    --packages-select stonefish_ros2 bridge_stonefish bringup \
+    --packages-select stonefish_ros2 bridge_stonefish bringup detection_mocker \
     --cmake-args -DCMAKE_BUILD_TYPE=Release"
 
 # build mission_executor with actual source code

README.md

@@ -13,17 +13,21 @@ cd ./autonomy-stack
 # Build the Docker image
 docker build -t rumarino-headless:latest .
 
-# Run headless simulation test
+# Run headless simulation test (fast_fixed_step, sim-time stamps, 5x RTF cap)
 docker run --rm \
   --name headless-test \
   rumarino-headless:latest \
   bash -c "
     source /opt/ros/jazzy/setup.bash && \
     source /ros2_ws/install/setup.bash && \
-    ros2 launch bringup test_mission_executor_headless.launch.py \
+    ros2 launch bringup stonefish.launch.py \
       mission_name:=prequalify \
-      controller_name:=stonefish_hydrus \
-      env_file_name:=hydrus_env_headless.scn &
+      auv_name:=hydrus \
+      env_file_name:=hydrus_env_headless.scn \
+      headless:=true \
+      fast_fixed_step:=true \
+      use_sim_time_stamps:=true \
+      realtime_factor_cap:=5.0 &
     LAUNCH_PID=\$! && \
     sleep 15 && \
     kill \$LAUNCH_PID 2>/dev/null || true
@@ -258,6 +262,29 @@ ros2 launch bringup stonefish.launch.py \
     env_file_name:=hydrus_env.scn \
     headless:=false
 
+# Faster-than-realtime graphical sim. Odometry is stamped with sim time so
+# the PID dt stays correct; realtime_factor_cap keeps plant delay bounded.
+# 0.0 disables the cap. Requires vendor/stonefish_ros2 from
+# https://github.com/Rumarino-Team/stonefish_ros2.git
+ros2 launch bringup stonefish.launch.py \
+    mission_name:=prequalify \
+    auv_name:=hydrus \
+    env_file_name:=hydrus_env.scn \
+    headless:=false \
+    fast_fixed_step:=true \
+    use_sim_time_stamps:=true \
+    realtime_factor_cap:=5.0
+
+# Headless CI-style run (nogpu, 5x cap)
+ros2 launch bringup stonefish.launch.py \
+    mission_name:=prequalify \
+    auv_name:=hydrus \
+    env_file_name:=hydrus_env_headless.scn \
+    headless:=true \
+    fast_fixed_step:=true \
+    use_sim_time_stamps:=true \
+    realtime_factor_cap:=5.0
+
 # proteus, teleop mission
 # if you don't have xterm, set TERMINAL to your terminal or install xterm.
 # sudo apt install xterm

src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn

@@ -47,7 +47,7 @@
 			</external_part>
 		</base_link>
 
-		<ros_subscriber thrusters="/hydrus/thrusters" />
+		<ros_subscriber thrusters="/bridge/thrusters" />
 
 		<!-- Corner Thrusters -->
 		<actuator name="thruster_front_left" type="thruster">
@@ -222,7 +222,7 @@
 		<sensor name="Odometry" type="odometry" rate="30.0">
 			<link name="Hydrus" />
 			<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-			<ros_publisher topic="/hydrus/odometry" />
+			<ros_publisher topic="/bridge/odometry" />
 		</sensor>
 
 		<!-- Camera sensor removed for headless mode compatibility -->

src/bringup/launch/stonefish.launch.py

@@ -49,17 +49,31 @@ def _launch_setup(context, *args, **kwargs):
         detection_env_file_name = 'pool_env.scn'
     detection_scn_path = os.path.join(bridge_share, 'data', 'scenarios', detection_env_file_name)
 
+    # Evaluate in this launch context. Passing unevaluated LaunchConfiguration
+    # into IncludeLaunchDescription can resolve against stonefish_ros2's own
+    # defaults (use_sim_time_stamps=false).
+    simulation_rate = LaunchConfiguration('simulation_rate').perform(context)
+    fast_fixed_step_s = LaunchConfiguration('fast_fixed_step').perform(context)
+    use_sim_time_stamps = LaunchConfiguration('use_sim_time_stamps').perform(context)
+    realtime_factor_cap = LaunchConfiguration('realtime_factor_cap').perform(context)
+    fast_fixed_step = fast_fixed_step_s.lower() in ('true', '1', 'yes')
+
     simulator_launch = 'stonefish_simulator_nogpu.launch.py' if headless else 'stonefish_simulator.launch.py'
     simulator_arguments = {
         'simulation_data': os.path.join(bridge_share, 'data'),
         'scenario_desc': scenario_desc_path,
-        'simulation_rate': '300.0',
+        'simulation_rate': simulation_rate,
+        'fast_fixed_step': fast_fixed_step_s,
+        'use_sim_time_stamps': use_sim_time_stamps,
+        'realtime_factor_cap': realtime_factor_cap,
     }
     if not headless:
+        # High-quality 1080p plus camera/IMU publish at 100x realtime adds
+        # wall-clock delay that becomes seconds of plant delay in sim time.
         simulator_arguments.update({
-            'window_res_x': '1920',
-            'window_res_y': '1080',
-            'rendering_quality': 'high',
+            'window_res_x': '1280' if fast_fixed_step else '1920',
+            'window_res_y': '720' if fast_fixed_step else '1080',
+            'rendering_quality': 'low' if fast_fixed_step else 'high',
         })
 
     # Setup ROS2 workspace based on current working directory
@@ -133,6 +147,15 @@ def generate_launch_description():
     auv_file_name_arg = DeclareLaunchArgument('auv_file_name', default_value='')
     headless_arg = DeclareLaunchArgument('headless', default_value='false')
     stonefish_only_arg = DeclareLaunchArgument('stonefish_only', default_value='false')
+    simulation_rate_arg = DeclareLaunchArgument('simulation_rate', default_value='300.0')
+    fast_fixed_step_arg = DeclareLaunchArgument('fast_fixed_step', default_value='false')
+    # Stamp odometry with simulation time so PID dt stays correct when the
+    # sim runs faster than wall clock (fast_fixed_step:=true).
+    use_sim_time_stamps_arg = DeclareLaunchArgument('use_sim_time_stamps', default_value='true')
+    # Only applies when fast_fixed_step is on; real-time stepping is already 1x.
+    # Keeps thruster command delay a fraction of a control period instead of
+    # seconds of plant time. 0.0 disables the cap.
+    realtime_factor_cap_arg = DeclareLaunchArgument('realtime_factor_cap', default_value='5.0')
 
     return LaunchDescription([
         mission_name_arg,
@@ -141,5 +164,9 @@ def generate_launch_description():
         auv_file_name_arg,
         headless_arg,
         stonefish_only_arg,
+        simulation_rate_arg,
+        fast_fixed_step_arg,
+        use_sim_time_stamps_arg,
+        realtime_factor_cap_arg,
         OpaqueFunction(function=_launch_setup),
     ])

src/mission_executor/src/main.rs

@@ -12,7 +12,7 @@ mod inotify;
 use std::ops::Bound;
 use std::sync::Arc;
 use std::sync::atomic::{AtomicBool, AtomicUsize, Ordering};
-use std::time::{Duration, Instant};
+use std::time::Duration;
 use arc_swap::ArcSwap;
 use parry3d_f64::shape::Segment;
 use tokio::sync::{Mutex, Notify};
@@ -73,6 +73,10 @@ fn wrap_angle(angle: f64) -> f64 {
     (angle + std::f64::consts::PI).rem_euclid(2.0 * std::f64::consts::PI) - std::f64::consts::PI
 }
 
+fn pose_stamp_ns(stamp: &r2r::builtin_interfaces::msg::Time) -> i64 {
+    i64::from(stamp.sec) * 1_000_000_000 + i64::from(stamp.nanosec)
+}
+
 impl MissionExecutor {
     pub fn new(node: r2r::Node) -> Self {
         // hardcoded so it doesn't freak out while it waits for first odometry
@@ -360,12 +364,6 @@ async fn main() {
         }
     };
 
-    let consume_odometry_sub = |td: Arc<MissionExecutor>| async move {
-        while let Some(msg) = odometry_sub.next().await {
-            td.pose.store(Arc::new(Pose::from(&msg.pose.pose)));
-        }
-    };
-
     let cfg = Arc::new(ArcSwap::from_pointee(load_live_config(&live_config_path, &auv_name).unwrap()));
 
     let mut inotify_stream = inotify::InotifyStream::new();
@@ -388,19 +386,35 @@ async fn main() {
     let go_to_goal = |td: Arc<MissionExecutor>| async move {
         let mut sum_err = Vector6::zeros();
         let mut prev_pose_err = Vector6::zeros();
-        let mut prev_now = Instant::now();
+        let mut previous_timestamp_ns: Option<i64> = None;
         let mut count = 1.0; //Technically can be an integer but since we are multiplying by float...
-        let log_interval = Duration::from_millis(500);
-        let mut last_log = Instant::now();
-        while !td.stop.load(Ordering::Relaxed) {
+        while let Some(msg) = odometry_sub.next().await {
+            if td.stop.load(Ordering::Relaxed) {
+                break;
+            }
+
+            let pose = Pose::from(&msg.pose.pose);
+            td.pose.store(Arc::new(pose));
+
             let current_cfg = cfg.load();
             let PidConfig { kp, ki, kd } = current_cfg.pid[&bridge_name];
             let tam_x_y_z_roll_pitch_yaw = &current_cfg.tam;
 
-            let now = Instant::now();
-            let dt = now.duration_since(prev_now).as_secs_f64();
+            let timestamp_ns = pose_stamp_ns(&msg.header.stamp);
+            let dt = previous_timestamp_ns.and_then(|previous| {
+                let elapsed_ns = timestamp_ns - previous;
+                if elapsed_ns > 0 {
+                    Some(elapsed_ns as f64 * 1e-9)
+                } else {
+                    r2r::log_warn!(
+                        "go_to_goal",
+                        "odometry stamp not increasing (prev={previous} ns, now={timestamp_ns} ns); skipping I/D"
+                    );
+                    None
+                }
+            });
+            previous_timestamp_ns = Some(timestamp_ns);
 
-            let pose = **td.pose.load();
             let goal = **td.goal.load();
             let current_pose = Vector6::<f64>::from(pose);
 
@@ -429,8 +443,12 @@ async fn main() {
 
             pose_err[5] = yaw_error;
 
-            let vel_err = (pose_err - prev_pose_err) / dt;
-            sum_err += pose_err * dt;
+            let vel_err = if let Some(dt) = dt {
+                sum_err += pose_err * dt;
+                (pose_err - prev_pose_err) / dt
+            } else {
+                Vector6::zeros()
+            };
 
             let wrench = kp.component_mul(&pose_err)
                 + ki.component_mul(&sum_err)
@@ -473,30 +491,24 @@ async fn main() {
             let mut avg_curr = td.avg_current.lock().await;
             *avg_curr = (*avg_curr * (count - 1.0) + sum_curr) / count;
             count += 1.0;
-            if now.duration_since(last_log) >= log_interval {
-                 r2r::log_info!(
-                     "thruster_report",
-                     "Average thruster usage in runtime: {:.2}",
-                     *avg_curr
-                 );
-                 r2r::log_info!(
-                     "thruster_report",
-                     "Current sum of thrusters: {:.2}",
-                     sum_curr
-                );
-                r2r::log_info!(
-                    "thruster_report",
-                    "Estimated battery life remaining: {:.2}",
-                    BATTERY_CAPACITY / *avg_curr
-                );
-                last_log = now;
-            }
+            r2r::log_info!(
+                "thruster_report",
+                "Average thruster usage in runtime: {:.2}",
+                *avg_curr
+            );
+            r2r::log_info!(
+                "thruster_report",
+                "Current sum of thrusters: {:.2}",
+                sum_curr
+            );
+            r2r::log_info!(
+                "thruster_report",
+                "Estimated battery life remaining: {:.2}",
+                BATTERY_CAPACITY / *avg_curr
+            );
             drop(avg_curr);
 
             prev_pose_err = pose_err;
-            prev_now = now;
-
-            tokio::time::sleep(Duration::from_millis(100)).await;
         }
     };
 
@@ -533,7 +545,6 @@ async fn main() {
 
     tokio::spawn(consume_inotify_stream());
     tokio::spawn(consume_map_sub(Arc::clone(&td)));
-    tokio::spawn(consume_odometry_sub(Arc::clone(&td)));
     tokio::spawn(consume_new_objects(Arc::clone(&td)));
     tokio::spawn(go_to_goal(Arc::clone(&td)));