diff --git a/.github/workflows/headless-simulation.yml b/.github/workflows/headless-simulation.yml
index 4d04e73..d9092fd 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 \
@@ -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
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..3bb397f 100644
--- a/README.md
+++ b/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
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..eee0824 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
@@ -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),
])
diff --git a/src/mission_executor/src/main.rs b/src/mission_executor/src/main.rs
index ec7eb95..2f46e77 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,35 @@ 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) {
+ 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;
+ 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::::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)));
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