From a219aac4d3d05b84240b2adcbbca2676f22ad745 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Thu, 20 Aug 2026 22:13:38 -0700 Subject: [PATCH 1/5] Drive the PID from odometry messages and pose stamps. Fold pose updates into the controller callback and take dt from the odometry header so I/D terms follow sensor/sim time instead of a 100 ms Instant poll. Co-authored-by: Cursor --- src/mission_executor/src/main.rs | 55 +++++++++++++++++++------------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index ec7eb95..42392f4 100644 --- a/src/mission_executor/src/main.rs +++ b/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| 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,30 @@ async fn main() { let go_to_goal = |td: Arc| 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 = 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) { + let log_interval_s = 0.5; + let mut last_log_s: Option = None; + 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 = ¤t_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; + (elapsed_ns > 0).then_some(elapsed_ns as f64 * 1e-9) + }); + previous_timestamp_ns = Some(timestamp_ns); + let now_s = timestamp_ns as f64 * 1e-9; - let pose = **td.pose.load(); let goal = **td.goal.load(); let current_pose = Vector6::::from(pose); @@ -429,8 +438,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,7 +486,7 @@ 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 { + if last_log_s.map(|prev| now_s - prev >= log_interval_s).unwrap_or(false) { r2r::log_info!( "thruster_report", "Average thruster usage in runtime: {:.2}", @@ -489,14 +502,13 @@ async fn main() { "Estimated battery life remaining: {:.2}", BATTERY_CAPACITY / *avg_curr ); - last_log = now; + last_log_s = Some(now_s); + } else if last_log_s.is_none() { + last_log_s = Some(now_s); } 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))); From 450508b25c9f97faecdd48ca60b435b82cbce78d Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Thu, 20 Aug 2026 22:49:22 -0700 Subject: [PATCH 2/5] Wire fast Stonefish sim into launch, CI, and the Rumarino stonefish_ros2 fork. Point the submodule at Rumarino-Team/stonefish_ros2, pass fast_fixed_step / sim stamps / RTF cap from bringup, and run headless CI at 5x so PID dt follows pose time. Co-authored-by: Cursor --- .github/workflows/headless-simulation.yml | 11 ++- .gitmodules | 2 +- Dockerfile | 6 +- README.md | 26 ++++++- .../data/scenarios/hydrus_auv_headless.scn | 4 +- src/bringup/launch/stonefish.launch.py | 74 +++++++++++++------ .../test_mission_executor_headless.launch.py | 40 ++++++++++ vendor/stonefish_ros2 | 2 +- 8 files changed, 132 insertions(+), 33 deletions(-) create mode 100644 src/bringup/launch/test_mission_executor_headless.launch.py diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index 4d04e73..91228cb 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.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 \ @@ -40,7 +42,10 @@ jobs: source /ros2_ws/install/setup.bash && \ ros2 launch bringup test_mission_executor_headless.launch.py \ mission_name:=prequalify \ - env_file_name:=hydrus_env_headless.scn & + env_file_name:=hydrus_env_headless.scn \ + 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 diff --git a/.gitmodules b/.gitmodules index 2fa1635..eb66213 100644 --- a/.gitmodules +++ b/.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 diff --git a/Dockerfile b/Dockerfile index 18154c2..b92a547 100644 --- a/Dockerfile +++ b/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 diff --git a/README.md b/README.md index d527e85..667992f 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ 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 \ @@ -22,8 +22,10 @@ docker run --rm \ source /ros2_ws/install/setup.bash && \ ros2 launch bringup test_mission_executor_headless.launch.py \ mission_name:=prequalify \ - controller_name:=stonefish_hydrus \ - env_file_name:=hydrus_env_headless.scn & + env_file_name:=hydrus_env_headless.scn \ + 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 +260,24 @@ 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 test_mission_executor_headless.launch.py \ + mission_name:=prequalify \ + env_file_name:=hydrus_env_headless.scn + # proteus, teleop mission # if you don't have xterm, set TERMINAL to your terminal or install xterm. # sudo apt install xterm diff --git a/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn b/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn index 48075a0..52dccef 100644 --- a/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn +++ b/src/bridge_stonefish/data/scenarios/hydrus_auv_headless.scn @@ -47,7 +47,7 @@ - + @@ -222,7 +222,7 @@ - + diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index a30bc1c..8782cef 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/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 @@ -101,27 +115,30 @@ def _launch_setup(context, *args, **kwargs): 'publish_all_objects': True, }], ), - Node( + ] + if not headless: + ret.append(Node( package='joy', executable='joy_node', - ), - ] + )) if not stonefish_only: - ret += [ - Node( - package='mission_executor', - executable='mission_executor', - emulate_tty=True, - output='screen', - prefix=terminal_prefix, - parameters=[{ - 'mission_name': mission_name, - 'bridge_name': 'stonefish', - 'auv_name': auv_name, - 'live_config_path': os.path.join(cwd, 'src', 'bringup', 'config', 'mission_executor.toml'), - }], - ), - ] + # xterm/tmux startup is ~1s of wall time. With fast_fixed_step that is + # minutes of uncommanded sim before the PID starts. + me_kwargs = { + 'package': 'mission_executor', + 'executable': 'mission_executor', + 'emulate_tty': True, + 'output': 'screen', + 'parameters': [{ + 'mission_name': mission_name, + 'bridge_name': 'stonefish', + 'auv_name': auv_name, + 'live_config_path': os.path.join(cwd, 'src', 'bringup', 'config', 'mission_executor.toml'), + }], + } + if mission_name == 'teleop': + me_kwargs['prefix'] = terminal_prefix + ret += [Node(**me_kwargs)] return ret @@ -133,6 +150,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 +167,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), ]) diff --git a/src/bringup/launch/test_mission_executor_headless.launch.py b/src/bringup/launch/test_mission_executor_headless.launch.py new file mode 100644 index 0000000..636a585 --- /dev/null +++ b/src/bringup/launch/test_mission_executor_headless.launch.py @@ -0,0 +1,40 @@ +"""Headless Hydrus prequalify launch used by CI (fast_fixed_step).""" + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +import os + + +def generate_launch_description(): + """Wrap stonefish.launch.py with headless CI defaults.""" + bringup_share = get_package_share_directory('bringup') + + return LaunchDescription([ + DeclareLaunchArgument('mission_name', default_value='prequalify'), + DeclareLaunchArgument('auv_name', default_value='hydrus'), + DeclareLaunchArgument( + 'env_file_name', default_value='hydrus_env_headless.scn'), + DeclareLaunchArgument('use_sim_time_stamps', default_value='true'), + DeclareLaunchArgument('fast_fixed_step', default_value='true'), + DeclareLaunchArgument('realtime_factor_cap', default_value='5.0'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(bringup_share, 'launch', 'stonefish.launch.py') + ]), + launch_arguments={ + 'headless': 'true', + 'auv_name': LaunchConfiguration('auv_name'), + 'env_file_name': LaunchConfiguration('env_file_name'), + 'mission_name': LaunchConfiguration('mission_name'), + 'use_sim_time_stamps': LaunchConfiguration( + 'use_sim_time_stamps'), + 'fast_fixed_step': LaunchConfiguration('fast_fixed_step'), + 'realtime_factor_cap': LaunchConfiguration( + 'realtime_factor_cap'), + }.items(), + ), + ]) diff --git a/vendor/stonefish_ros2 b/vendor/stonefish_ros2 index 29e56f6..f7538da 160000 --- a/vendor/stonefish_ros2 +++ b/vendor/stonefish_ros2 @@ -1 +1 @@ -Subproject commit 29e56f6b62412e3c64b09792616e099363257a59 +Subproject commit f7538da3907b1c1e2722bb9534c78c1e08751feb From bc16427623f18ce816e53ddba5825bde798e32b6 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 22 Aug 2026 00:44:28 -0700 Subject: [PATCH 3/5] remove last_log var --- src/mission_executor/src/main.rs | 38 +++++++++++++------------------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index 42392f4..62673a8 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -388,8 +388,6 @@ async fn main() { let mut prev_pose_err = Vector6::zeros(); let mut previous_timestamp_ns: Option = None; let mut count = 1.0; //Technically can be an integer but since we are multiplying by float... - let log_interval_s = 0.5; - let mut last_log_s: Option = None; while let Some(msg) = odometry_sub.next().await { if td.stop.load(Ordering::Relaxed) { break; @@ -408,7 +406,6 @@ async fn main() { (elapsed_ns > 0).then_some(elapsed_ns as f64 * 1e-9) }); previous_timestamp_ns = Some(timestamp_ns); - let now_s = timestamp_ns as f64 * 1e-9; let goal = **td.goal.load(); let current_pose = Vector6::::from(pose); @@ -486,26 +483,21 @@ 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 last_log_s.map(|prev| now_s - prev >= log_interval_s).unwrap_or(false) { - 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_s = Some(now_s); - } else if last_log_s.is_none() { - last_log_s = Some(now_s); - } + 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; From b8374a212163dfeb6bece4f003ed92a1a6966549 Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 22 Aug 2026 01:02:38 -0700 Subject: [PATCH 4/5] deleted useless launchfiles --- .github/workflows/headless-simulation.yml | 4 +- README.md | 13 ++++-- src/bringup/launch/stonefish.launch.py | 39 +++++++++--------- .../test_mission_executor_headless.launch.py | 40 ------------------- 4 files changed, 31 insertions(+), 65 deletions(-) delete mode 100644 src/bringup/launch/test_mission_executor_headless.launch.py diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml index 91228cb..d9092fd 100644 --- a/.github/workflows/headless-simulation.yml +++ b/.github/workflows/headless-simulation.yml @@ -40,9 +40,11 @@ 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 \ + 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 & diff --git a/README.md b/README.md index 667992f..3bb397f 100644 --- a/README.md +++ b/README.md @@ -20,9 +20,11 @@ docker run --rm \ 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 \ + 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 & @@ -274,9 +276,14 @@ ros2 launch bringup stonefish.launch.py \ realtime_factor_cap:=5.0 # Headless CI-style run (nogpu, 5x cap) -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 # proteus, teleop mission # if you don't have xterm, set TERMINAL to your terminal or install xterm. diff --git a/src/bringup/launch/stonefish.launch.py b/src/bringup/launch/stonefish.launch.py index 8782cef..eee0824 100644 --- a/src/bringup/launch/stonefish.launch.py +++ b/src/bringup/launch/stonefish.launch.py @@ -115,30 +115,27 @@ def _launch_setup(context, *args, **kwargs): 'publish_all_objects': True, }], ), - ] - if not headless: - ret.append(Node( + Node( package='joy', executable='joy_node', - )) + ), + ] if not stonefish_only: - # xterm/tmux startup is ~1s of wall time. With fast_fixed_step that is - # minutes of uncommanded sim before the PID starts. - me_kwargs = { - 'package': 'mission_executor', - 'executable': 'mission_executor', - 'emulate_tty': True, - 'output': 'screen', - 'parameters': [{ - 'mission_name': mission_name, - 'bridge_name': 'stonefish', - 'auv_name': auv_name, - 'live_config_path': os.path.join(cwd, 'src', 'bringup', 'config', 'mission_executor.toml'), - }], - } - if mission_name == 'teleop': - me_kwargs['prefix'] = terminal_prefix - ret += [Node(**me_kwargs)] + ret += [ + Node( + package='mission_executor', + executable='mission_executor', + emulate_tty=True, + output='screen', + prefix=terminal_prefix, + parameters=[{ + 'mission_name': mission_name, + 'bridge_name': 'stonefish', + 'auv_name': auv_name, + 'live_config_path': os.path.join(cwd, 'src', 'bringup', 'config', 'mission_executor.toml'), + }], + ), + ] return ret diff --git a/src/bringup/launch/test_mission_executor_headless.launch.py b/src/bringup/launch/test_mission_executor_headless.launch.py deleted file mode 100644 index 636a585..0000000 --- a/src/bringup/launch/test_mission_executor_headless.launch.py +++ /dev/null @@ -1,40 +0,0 @@ -"""Headless Hydrus prequalify launch used by CI (fast_fixed_step).""" - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -import os - - -def generate_launch_description(): - """Wrap stonefish.launch.py with headless CI defaults.""" - bringup_share = get_package_share_directory('bringup') - - return LaunchDescription([ - DeclareLaunchArgument('mission_name', default_value='prequalify'), - DeclareLaunchArgument('auv_name', default_value='hydrus'), - DeclareLaunchArgument( - 'env_file_name', default_value='hydrus_env_headless.scn'), - DeclareLaunchArgument('use_sim_time_stamps', default_value='true'), - DeclareLaunchArgument('fast_fixed_step', default_value='true'), - DeclareLaunchArgument('realtime_factor_cap', default_value='5.0'), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - os.path.join(bringup_share, 'launch', 'stonefish.launch.py') - ]), - launch_arguments={ - 'headless': 'true', - 'auv_name': LaunchConfiguration('auv_name'), - 'env_file_name': LaunchConfiguration('env_file_name'), - 'mission_name': LaunchConfiguration('mission_name'), - 'use_sim_time_stamps': LaunchConfiguration( - 'use_sim_time_stamps'), - 'fast_fixed_step': LaunchConfiguration('fast_fixed_step'), - 'realtime_factor_cap': LaunchConfiguration( - 'realtime_factor_cap'), - }.items(), - ), - ]) From b7c48e6079d310050b5205729ccf17cc05fa5fee Mon Sep 17 00:00:00 2001 From: Cruiz102 Date: Sat, 22 Aug 2026 01:20:20 -0700 Subject: [PATCH 5/5] add a warning if odometry is not increasing. --- src/mission_executor/src/main.rs | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs index 62673a8..2f46e77 100644 --- a/src/mission_executor/src/main.rs +++ b/src/mission_executor/src/main.rs @@ -403,7 +403,15 @@ async fn main() { let timestamp_ns = pose_stamp_ns(&msg.header.stamp); let dt = previous_timestamp_ns.and_then(|previous| { let elapsed_ns = timestamp_ns - previous; - (elapsed_ns > 0).then_some(elapsed_ns as f64 * 1e-9) + 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);