auv-sim-bench 0.1.0__tar.gz

auv_sim_bench-0.1.0/PKG-INFO

@@ -0,0 +1,203 @@
+Metadata-Version: 2.4
+Name: auv_sim_bench
+Version: 0.1.0
+Summary: Lightweight 6-DOF AUV physics sim for dry-testing state estimation.
+Maintainer: wuisabel-gif
+Maintainer-email: 231155141+wuisabel-gif@users.noreply.github.com
+License: MIT
+Description-Content-Type: text/markdown
+Requires-Dist: setuptools
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
+
+# AUVSimBench
+
+A lightweight **physics twin** for autonomous underwater vehicles — a 6-DOF AUV
+simulator as a single ROS 2 node, in pure Python/numpy. It runs the vehicle's
+physics and publishes the **sensor topics a state estimator consumes, plus
+ground-truth pose**, so you can develop and stress-test an underwater localization
+or control stack **on the bench** — no water, no GPU, no Isaac Sim. It runs happily
+on a Jetson.
+
+## Why this is hard (and why the twin helps)
+
+Validating underwater state estimation is uniquely painful:
+
+- **Wet tests are expensive and rare.** Pool/ocean time is slow to schedule, and you
+  get only a handful of runs before everyone's tired and the battery's flat.
+- **There is no ground truth underwater.** No GPS below the surface — so even when you
+  *do* get a dive, you often can't measure *how wrong* the estimate was.
+- **Most failures aren't about the water.** In practice the things that break a dive are
+  wiring (a node subscribed to the wrong topic), frame conventions (axes flipped),
+  startup ordering (a sensor not powered on), or an estimator diverging — and you
+  usually discover them *mid-dive*, burning a wet test to debug a typo.
+- **Recorded bags only go so far.** A bag replays exactly one motion; you can't steer
+  it, you can't make a sensor fail on cue, and replayed DVL/depth won't agree with a
+  real IMU sitting still on the bench.
+
+A **physics twin** answers all four. It simulates the vehicle's dynamics, so every
+sensor stream is derived from *one consistent truth*; it *knows* the true pose, so
+estimator accuracy is measurable; and you can script scenarios, disturbances, and
+on-demand sensor failures — all from a laptop or the robot's own compute.
+
+```
+wrench in  ──►  6-DOF dynamics (thrust + buoyancy + drag + current)  ──►  IMU / DVL / depth + ground truth
+```
+
+## What it gives you
+
+- **Consistent sensors** — IMU, DVL and depth all generated from the same simulated
+  body, so they physically agree (the thing bag-replay can't do).
+- **Ground truth** — `/auv_sim/ground_truth` is the exact pose, so you can plot
+  estimator error directly.
+- **Scenarios & disturbances** — water current, and on-demand sensor failures (DVL
+  dropout, noise, NaN) to probe estimator robustness — the exact failure modes that
+  bite in real wet tests.
+- **Driven by your real controller** — it takes a `geometry_msgs/Wrench` with a deadman,
+  the same contract a PID/thruster-allocation stack already produces.
+
+## Physics model
+
+An independent **6-DOF Newton-Euler** rigid body (FLU body frame, ENU world,
+quaternion orientation):
+
+- **Buoyancy** `ρ·V·g`, applied at a **center of buoyancy above the center of mass**, so
+  the vehicle self-rights — giving realistic roll/pitch, and therefore realistic IMU
+  output. Default displacement is exactly neutral.
+- **Linear + quadratic drag** on both linear and angular velocity.
+- **Water current** — drag acts on velocity *relative to the water*, so a current
+  actually pushes a passive vehicle (modeled properly, not faked through gravity).
+- **Wrench input** in the body frame, with a 0.5 s command-timeout deadman.
+
+The accelerometer output is the true specific force `(ΣF − gravity)/m` in the body
+frame, so a level, neutrally-buoyant vehicle reads `az ≈ +9.81` — matching a real
+ENU/FLU IMU.
+
+## Vehicle-agnostic
+
+It is **not tied to any one vehicle.** Every topic name, frame, rate and physics
+constant is a parameter, and the defaults are generic:
+
+| published topic (default) | type | content |
+|---------------------------|------|---------|
+| `imu/data` | `sensor_msgs/Imu` | orientation, body angular velocity, specific-force accel |
+| `dvl/odometry` | `nav_msgs/Odometry` | body-frame velocity |
+| `dvl/altitude`, `ping/range` | `sensor_msgs/Range` | height above the floor |
+| `pressure` | `sensor_msgs/FluidPressure` | from depth |
+| `ground_truth` | `geometry_msgs/PoseStamped` | exact sim pose |
+
+Adapt it to a specific vehicle with a **profile** — a small YAML that remaps the
+topics/frames and sets the mass/buoyancy. `config/barracuda.yaml` is one example
+(Barracuda's real `/barracuda/...` topics); write your own for any other AUV.
+
+Profiles can also avoid hardcoded robot names by using placeholders in topic and
+frame strings:
+
+| placeholder | expands to |
+|-------------|------------|
+| `{topic_prefix}` | the normalized `topic_prefix` parameter, such as `/barracuda` |
+| `{frame_prefix}` | the normalized `frame_prefix` parameter, such as `barracuda` |
+
+For example, `wrench_topic: "{topic_prefix}/wrench"` becomes
+`/barracuda/wrench` when `topic_prefix: /barracuda`. Leave both prefixes empty
+for generic relative names.
+
+Profiles can also tune the vehicle dynamics and scenario start state:
+
+| parameter | meaning |
+|-----------|---------|
+| `inertia` | diagonal body inertia `[Ixx, Iyy, Izz]` |
+| `cob_offset` | center of buoyancy relative to center of mass, body frame |
+| `linear_drag`, `quadratic_drag` | per-axis translational drag coefficients |
+| `angular_drag`, `angular_quadratic_drag` | per-axis rotational drag coefficients |
+| `initial_position` | starting world position |
+| `initial_orientation_wxyz` | starting orientation quaternion |
+| `initial_linear_velocity`, `initial_angular_velocity` | starting body velocities |
+
+## See it in 3D
+
+There's no built-in render, but it broadcasts **TF** and a **vehicle marker**, so it
+shows up live in **RViz2** — which runs on a Jetson (plain OpenGL, no RTX/Isaac):
+
+```bash
+ros2 launch auv_sim_bench sim.launch.py rviz:=true
+```
+
+You get a 3D box for the vehicle flying around under thrust/current/buoyancy, the TF
+frames, and — if you set `estimate_topic` to your estimator's pose — a second marker
+for the **estimate**, so you can watch estimate-vs-truth drift in real time. It's
+schematic (boxes + axes), not photoreal; photoreal water is the one thing that needs
+Isaac Sim on an x86 box.
+
+## Failure & disturbance injection
+
+For the real reason to simulate — breaking the estimator on purpose:
+
+```yaml
+current: [0.3, 0.0, 0.0]        # steady cross-current
+dvl_dropout_start_sec: 10.0     # DVL goes silent at t=10s ...
+dvl_dropout_duration_sec: 5.0   # ... for 5s
+dvl_noise_stddev: 0.02          # velocity noise
+dvl_emit_nan: false             # or feed the estimator NaNs
+```
+
+## Build & run
+
+```bash
+cd ~/ros2_ws/src && git clone <this repo> auv_sim_bench
+cd ~/ros2_ws && colcon build --packages-select auv_sim_bench
+source install/setup.bash
+
+ros2 launch auv_sim_bench sim.launch.py                       # generic defaults
+ros2 launch auv_sim_bench sim.launch.py rviz:=true            # + 3D view
+ros2 launch auv_sim_bench sim.launch.py config:=barracuda.yaml   # Barracuda profile
+# drive it by publishing geometry_msgs/Wrench on the wrench topic (default cmd_wrench)
+```
+
+### On a Jetson (AGX Orin)
+
+Validated on a Jetson AGX Orin (JetPack 6 / ROS 2 Humble) — pure Python + numpy, so
+no GPU or Isaac needed:
+
+```bash
+mkdir -p ~/auv_ws/src
+cp -r AUVSimBench ~/auv_ws/src/auv_sim_bench
+cd ~/auv_ws && colcon build --packages-select auv_sim_bench
+source /opt/ros/humble/setup.bash
+source ~/auv_ws/install/setup.bash        # both source lines, in every new terminal
+ros2 launch auv_sim_bench sim.launch.py
+```
+
+Headless over SSH there's no display for RViz, so drop `rviz:=true` and watch the
+data instead (`ros2 topic echo /ground_truth`); driving it with a `/cmd_wrench`
+publisher moves the body as expected. For the 3D view, run RViz on a monitor
+attached to the Jetson.
+
+Test your estimator: run the sim + your estimator, then compare
+`/barracuda/estimated_pose` against `/auv_sim/ground_truth` — an accuracy check you
+cannot get from a real dive (no underwater ground truth) or from bag replay.
+
+## Tests
+
+The physics core (`dynamics.py`) is plain numpy with no ROS dependency, so it is
+unit-tested directly — buoyancy, gravity, drag, thrust response, and current:
+
+```bash
+pytest test/
+```
+
+## Credits & scope
+
+Inspired by Leonardo Lima's [`isaac_underwater`](https://github.com/leonlime/isaac_underwater)
+examples (MIT), which show buoyancy and drag in Isaac Sim. AUVSimBench takes that
+idea in a different direction — its own 6-DOF dynamics in a standalone ROS 2 node, so
+it runs anywhere ROS 2 does, including a Jetson.
+
+It's a controls/estimation test rig, not a hydrodynamics research tool: drag and
+inertia are simple tunable coefficients, not CFD. Added-mass and richer current
+fields are natural next steps.

auv_sim_bench-0.1.0/README.md

@@ -0,0 +1,186 @@
+# AUVSimBench
+
+A lightweight **physics twin** for autonomous underwater vehicles — a 6-DOF AUV
+simulator as a single ROS 2 node, in pure Python/numpy. It runs the vehicle's
+physics and publishes the **sensor topics a state estimator consumes, plus
+ground-truth pose**, so you can develop and stress-test an underwater localization
+or control stack **on the bench** — no water, no GPU, no Isaac Sim. It runs happily
+on a Jetson.
+
+## Why this is hard (and why the twin helps)
+
+Validating underwater state estimation is uniquely painful:
+
+- **Wet tests are expensive and rare.** Pool/ocean time is slow to schedule, and you
+  get only a handful of runs before everyone's tired and the battery's flat.
+- **There is no ground truth underwater.** No GPS below the surface — so even when you
+  *do* get a dive, you often can't measure *how wrong* the estimate was.
+- **Most failures aren't about the water.** In practice the things that break a dive are
+  wiring (a node subscribed to the wrong topic), frame conventions (axes flipped),
+  startup ordering (a sensor not powered on), or an estimator diverging — and you
+  usually discover them *mid-dive*, burning a wet test to debug a typo.
+- **Recorded bags only go so far.** A bag replays exactly one motion; you can't steer
+  it, you can't make a sensor fail on cue, and replayed DVL/depth won't agree with a
+  real IMU sitting still on the bench.
+
+A **physics twin** answers all four. It simulates the vehicle's dynamics, so every
+sensor stream is derived from *one consistent truth*; it *knows* the true pose, so
+estimator accuracy is measurable; and you can script scenarios, disturbances, and
+on-demand sensor failures — all from a laptop or the robot's own compute.
+
+```
+wrench in  ──►  6-DOF dynamics (thrust + buoyancy + drag + current)  ──►  IMU / DVL / depth + ground truth
+```
+
+## What it gives you
+
+- **Consistent sensors** — IMU, DVL and depth all generated from the same simulated
+  body, so they physically agree (the thing bag-replay can't do).
+- **Ground truth** — `/auv_sim/ground_truth` is the exact pose, so you can plot
+  estimator error directly.
+- **Scenarios & disturbances** — water current, and on-demand sensor failures (DVL
+  dropout, noise, NaN) to probe estimator robustness — the exact failure modes that
+  bite in real wet tests.
+- **Driven by your real controller** — it takes a `geometry_msgs/Wrench` with a deadman,
+  the same contract a PID/thruster-allocation stack already produces.
+
+## Physics model
+
+An independent **6-DOF Newton-Euler** rigid body (FLU body frame, ENU world,
+quaternion orientation):
+
+- **Buoyancy** `ρ·V·g`, applied at a **center of buoyancy above the center of mass**, so
+  the vehicle self-rights — giving realistic roll/pitch, and therefore realistic IMU
+  output. Default displacement is exactly neutral.
+- **Linear + quadratic drag** on both linear and angular velocity.
+- **Water current** — drag acts on velocity *relative to the water*, so a current
+  actually pushes a passive vehicle (modeled properly, not faked through gravity).
+- **Wrench input** in the body frame, with a 0.5 s command-timeout deadman.
+
+The accelerometer output is the true specific force `(ΣF − gravity)/m` in the body
+frame, so a level, neutrally-buoyant vehicle reads `az ≈ +9.81` — matching a real
+ENU/FLU IMU.
+
+## Vehicle-agnostic
+
+It is **not tied to any one vehicle.** Every topic name, frame, rate and physics
+constant is a parameter, and the defaults are generic:
+
+| published topic (default) | type | content |
+|---------------------------|------|---------|
+| `imu/data` | `sensor_msgs/Imu` | orientation, body angular velocity, specific-force accel |
+| `dvl/odometry` | `nav_msgs/Odometry` | body-frame velocity |
+| `dvl/altitude`, `ping/range` | `sensor_msgs/Range` | height above the floor |
+| `pressure` | `sensor_msgs/FluidPressure` | from depth |
+| `ground_truth` | `geometry_msgs/PoseStamped` | exact sim pose |
+
+Adapt it to a specific vehicle with a **profile** — a small YAML that remaps the
+topics/frames and sets the mass/buoyancy. `config/barracuda.yaml` is one example
+(Barracuda's real `/barracuda/...` topics); write your own for any other AUV.
+
+Profiles can also avoid hardcoded robot names by using placeholders in topic and
+frame strings:
+
+| placeholder | expands to |
+|-------------|------------|
+| `{topic_prefix}` | the normalized `topic_prefix` parameter, such as `/barracuda` |
+| `{frame_prefix}` | the normalized `frame_prefix` parameter, such as `barracuda` |
+
+For example, `wrench_topic: "{topic_prefix}/wrench"` becomes
+`/barracuda/wrench` when `topic_prefix: /barracuda`. Leave both prefixes empty
+for generic relative names.
+
+Profiles can also tune the vehicle dynamics and scenario start state:
+
+| parameter | meaning |
+|-----------|---------|
+| `inertia` | diagonal body inertia `[Ixx, Iyy, Izz]` |
+| `cob_offset` | center of buoyancy relative to center of mass, body frame |
+| `linear_drag`, `quadratic_drag` | per-axis translational drag coefficients |
+| `angular_drag`, `angular_quadratic_drag` | per-axis rotational drag coefficients |
+| `initial_position` | starting world position |
+| `initial_orientation_wxyz` | starting orientation quaternion |
+| `initial_linear_velocity`, `initial_angular_velocity` | starting body velocities |
+
+## See it in 3D
+
+There's no built-in render, but it broadcasts **TF** and a **vehicle marker**, so it
+shows up live in **RViz2** — which runs on a Jetson (plain OpenGL, no RTX/Isaac):
+
+```bash
+ros2 launch auv_sim_bench sim.launch.py rviz:=true
+```
+
+You get a 3D box for the vehicle flying around under thrust/current/buoyancy, the TF
+frames, and — if you set `estimate_topic` to your estimator's pose — a second marker
+for the **estimate**, so you can watch estimate-vs-truth drift in real time. It's
+schematic (boxes + axes), not photoreal; photoreal water is the one thing that needs
+Isaac Sim on an x86 box.
+
+## Failure & disturbance injection
+
+For the real reason to simulate — breaking the estimator on purpose:
+
+```yaml
+current: [0.3, 0.0, 0.0]        # steady cross-current
+dvl_dropout_start_sec: 10.0     # DVL goes silent at t=10s ...
+dvl_dropout_duration_sec: 5.0   # ... for 5s
+dvl_noise_stddev: 0.02          # velocity noise
+dvl_emit_nan: false             # or feed the estimator NaNs
+```
+
+## Build & run
+
+```bash
+cd ~/ros2_ws/src && git clone <this repo> auv_sim_bench
+cd ~/ros2_ws && colcon build --packages-select auv_sim_bench
+source install/setup.bash
+
+ros2 launch auv_sim_bench sim.launch.py                       # generic defaults
+ros2 launch auv_sim_bench sim.launch.py rviz:=true            # + 3D view
+ros2 launch auv_sim_bench sim.launch.py config:=barracuda.yaml   # Barracuda profile
+# drive it by publishing geometry_msgs/Wrench on the wrench topic (default cmd_wrench)
+```
+
+### On a Jetson (AGX Orin)
+
+Validated on a Jetson AGX Orin (JetPack 6 / ROS 2 Humble) — pure Python + numpy, so
+no GPU or Isaac needed:
+
+```bash
+mkdir -p ~/auv_ws/src
+cp -r AUVSimBench ~/auv_ws/src/auv_sim_bench
+cd ~/auv_ws && colcon build --packages-select auv_sim_bench
+source /opt/ros/humble/setup.bash
+source ~/auv_ws/install/setup.bash        # both source lines, in every new terminal
+ros2 launch auv_sim_bench sim.launch.py
+```
+
+Headless over SSH there's no display for RViz, so drop `rviz:=true` and watch the
+data instead (`ros2 topic echo /ground_truth`); driving it with a `/cmd_wrench`
+publisher moves the body as expected. For the 3D view, run RViz on a monitor
+attached to the Jetson.
+
+Test your estimator: run the sim + your estimator, then compare
+`/barracuda/estimated_pose` against `/auv_sim/ground_truth` — an accuracy check you
+cannot get from a real dive (no underwater ground truth) or from bag replay.
+
+## Tests
+
+The physics core (`dynamics.py`) is plain numpy with no ROS dependency, so it is
+unit-tested directly — buoyancy, gravity, drag, thrust response, and current:
+
+```bash
+pytest test/
+```
+
+## Credits & scope
+
+Inspired by Leonardo Lima's [`isaac_underwater`](https://github.com/leonlime/isaac_underwater)
+examples (MIT), which show buoyancy and drag in Isaac Sim. AUVSimBench takes that
+idea in a different direction — its own 6-DOF dynamics in a standalone ROS 2 node, so
+it runs anywhere ROS 2 does, including a Jetson.
+
+It's a controls/estimation test rig, not a hydrodynamics research tool: drag and
+inertia are simple tunable coefficients, not CFD. Added-mass and richer current
+fields are natural next steps.

auv_sim_bench-0.1.0/auv_sim_bench/dynamics.py

@@ -0,0 +1,94 @@
+"""6-DOF rigid-body AUV dynamics (pure numpy, no ROS).
+
+Body frame is FLU, world frame is ENU (z up). State is integrated in the body
+frame with Newton-Euler; orientation as a wxyz quaternion.
+"""
+import numpy as np
+
+
+def quat_mul(a, b):
+    aw, ax, ay, az = a
+    bw, bx, by, bz = b
+    return np.array([
+        aw * bw - ax * bx - ay * by - az * bz,
+        aw * bx + ax * bw + ay * bz - az * by,
+        aw * by - ax * bz + ay * bw + az * bx,
+        aw * bz + ax * by - ay * bx + az * bw,
+    ])
+
+
+def quat_normalize(q):
+    n = np.linalg.norm(q)
+    return q / n if n > 0 else np.array([1.0, 0.0, 0.0, 0.0])
+
+
+def quat_to_rot(q):
+    """Body -> world rotation matrix for a wxyz quaternion."""
+    w, x, y, z = q
+    return np.array([
+        [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
+        [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
+        [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
+    ])
+
+
+class AUVParams:
+    def __init__(self):
+        self.mass = 15.0                 # kg
+        self.water_density = 1025.0      # kg/m^3
+        self.gravity = 9.81              # m/s^2
+        # displaced volume; rho*V*g should be ~ weight for neutral buoyancy
+        self.volume = (self.mass) / self.water_density   # exactly neutral by default
+        self.inertia = np.array([0.5, 0.8, 0.8])         # diag Ixx,Iyy,Izz
+        # center of buoyancy above the center of mass (body frame) -> righting moment
+        self.cob_offset = np.array([0.0, 0.0, 0.02])
+        # drag: F = -(lin * v + quad * v*|v|), per body axis
+        self.lin_drag = np.array([20.0, 40.0, 40.0])
+        self.quad_drag = np.array([30.0, 80.0, 80.0])
+        self.ang_drag = np.array([2.0, 4.0, 4.0])
+        self.ang_quad_drag = np.array([1.0, 2.0, 2.0])
+        # water current in the world frame (m/s); drag acts on velocity
+        # relative to the water, so a current pushes a passive vehicle.
+        self.current_world = np.zeros(3)
+
+
+class AUVState:
+    def __init__(self):
+        self.p = np.zeros(3)                    # world position (ENU)
+        self.q = np.array([1.0, 0.0, 0.0, 0.0]) # world orientation (wxyz)
+        self.v = np.zeros(3)                    # body-frame linear velocity
+        self.w = np.zeros(3)                    # body-frame angular velocity
+
+
+class AUVDynamics:
+    def __init__(self, params=None, state=None):
+        self.p = params or AUVParams()
+        self.s = state or AUVState()
+        self.specific_force = np.zeros(3)  # what the accelerometer would read (body)
+
+    def step(self, dt, force_body, torque_body):
+        p, s = self.p, self.s
+        R = quat_to_rot(s.q)
+        Rt = R.T
+
+        g_body = Rt @ np.array([0.0, 0.0, -p.mass * p.gravity])
+        b_body = Rt @ np.array([0.0, 0.0, p.water_density * p.volume * p.gravity])
+
+        v_rel = s.v - Rt @ p.current_world  # velocity relative to the water
+        drag_f = -(p.lin_drag * v_rel + p.quad_drag * v_rel * np.abs(v_rel))
+        drag_t = -(p.ang_drag * s.w + p.ang_quad_drag * s.w * np.abs(s.w))
+
+        F = np.asarray(force_body, float) + g_body + b_body + drag_f
+        T = np.asarray(torque_body, float) + np.cross(p.cob_offset, b_body) + drag_t
+
+        v_dot = F / p.mass - np.cross(s.w, s.v)
+        w_dot = (T - np.cross(s.w, p.inertia * s.w)) / p.inertia
+
+        s.v = s.v + v_dot * dt
+        s.w = s.w + w_dot * dt
+        s.q = quat_normalize(s.q + 0.5 * quat_mul(s.q, np.array([0.0, *s.w])) * dt)
+        s.p = s.p + R @ s.v * dt
+
+        # accelerometer specific force = (total - gravity) / m, in body frame
+        self.specific_force = (F - g_body) / p.mass
+        return s

auv_sim_bench-0.1.0/auv_sim_bench/sim_node.py

@@ -0,0 +1,322 @@
+import random
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, TransformStamped, Wrench
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import FluidPressure, Imu, Range
+from visualization_msgs.msg import Marker
+from tf2_ros import StaticTransformBroadcaster, TransformBroadcaster
+
+from .dynamics import AUVDynamics, AUVParams
+
+
+class AUVSimNode(Node):
+    def __init__(self):
+        super().__init__("auv_sim_bench")
+
+        self.declare_parameter("sim_rate_hz", 200.0)
+        self.declare_parameter("imu_rate_hz", 100.0)
+        self.declare_parameter("dvl_rate_hz", 8.0)
+        self.declare_parameter("depth_rate_hz", 5.0)
+
+        # generic defaults; remap or load a profile (e.g. config/barracuda.yaml)
+        self.declare_parameter("topic_prefix", "")
+        self.declare_parameter("frame_prefix", "")
+        self.declare_parameter("wrench_topic", "cmd_wrench")
+        self.declare_parameter("imu_topic", "imu/data")
+        self.declare_parameter("dvl_odom_topic", "dvl/odometry")
+        self.declare_parameter("dvl_alt_topic", "dvl/altitude")
+        self.declare_parameter("ping_range_topic", "ping/range")
+        self.declare_parameter("pressure_topic", "pressure")
+        self.declare_parameter("truth_topic", "ground_truth")
+        self.declare_parameter("estimate_topic", "")  # "" disables the estimate marker
+
+        self.declare_parameter("odom_frame", "odom")
+        self.declare_parameter("base_frame", "base_link")
+        self.declare_parameter("imu_frame", "imu_link")
+        self.declare_parameter("dvl_frame", "dvl_link")
+        self.declare_parameter("ping_frame", "ping_link")
+
+        self.declare_parameter("wrench_timeout_sec", 0.5)
+        self.declare_parameter("floor_z", -10.0)
+        self.declare_parameter("surface_z", 0.0)
+        self.declare_parameter("atm_pressure", 101325.0)
+
+        self.declare_parameter("mass", 15.0)
+        self.declare_parameter("volume", 0.0)
+        self.declare_parameter("water_density", 1025.0)
+        self.declare_parameter("current", [0.0, 0.0, 0.0])
+        self.declare_parameter("inertia", [0.5, 0.8, 0.8])
+        self.declare_parameter("cob_offset", [0.0, 0.0, 0.02])
+        self.declare_parameter("linear_drag", [20.0, 40.0, 40.0])
+        self.declare_parameter("quadratic_drag", [30.0, 80.0, 80.0])
+        self.declare_parameter("angular_drag", [2.0, 4.0, 4.0])
+        self.declare_parameter("angular_quadratic_drag", [1.0, 2.0, 2.0])
+        self.declare_parameter("initial_position", [0.0, 0.0, 0.0])
+        self.declare_parameter("initial_orientation_wxyz", [1.0, 0.0, 0.0, 0.0])
+        self.declare_parameter("initial_linear_velocity", [0.0, 0.0, 0.0])
+        self.declare_parameter("initial_angular_velocity", [0.0, 0.0, 0.0])
+
+        self.declare_parameter("publish_tf", True)
+        self.declare_parameter("publish_markers", True)
+        self.declare_parameter("body_size", [0.6, 0.35, 0.25])
+
+        self.declare_parameter("dvl_noise_stddev", 0.0)
+        self.declare_parameter("dvl_dropout_start_sec", -1.0)
+        self.declare_parameter("dvl_dropout_duration_sec", 0.0)
+        self.declare_parameter("dvl_emit_nan", False)
+        self.declare_parameter("seed", 0)
+
+        def param_value(name):
+            return self.get_parameter(name).value
+
+        def vec_param(name, size=3):
+            value = [float(x) for x in param_value(name)]
+            if len(value) != size:
+                raise ValueError(f"{name} must contain {size} values")
+            return value
+
+        def normalize_prefix(prefix):
+            prefix = str(prefix).strip().strip("/")
+            return f"/{prefix}" if prefix else ""
+
+        topic_prefix = normalize_prefix(param_value("topic_prefix"))
+        frame_prefix = str(param_value("frame_prefix")).strip().strip("/")
+
+        def expand_name(value):
+            return str(value).format(
+                topic_prefix=topic_prefix,
+                frame_prefix=frame_prefix,
+            )
+
+        self.odom_frame = expand_name(param_value("odom_frame"))
+        self.base_frame = expand_name(param_value("base_frame"))
+        self.imu_frame = expand_name(param_value("imu_frame"))
+        self.dvl_frame = expand_name(param_value("dvl_frame"))
+        self.ping_frame = expand_name(param_value("ping_frame"))
+        self.wrench_timeout = float(param_value("wrench_timeout_sec"))
+        self.floor_z = float(param_value("floor_z"))
+        self.surface_z = float(param_value("surface_z"))
+        self.atm = float(param_value("atm_pressure"))
+        self.publish_tf = bool(param_value("publish_tf"))
+        self.publish_markers = bool(param_value("publish_markers"))
+        self.body_size = [float(x) for x in param_value("body_size")]
+        self.dvl_noise = float(param_value("dvl_noise_stddev"))
+        self.dvl_drop_start = float(param_value("dvl_dropout_start_sec"))
+        self.dvl_drop_dur = float(param_value("dvl_dropout_duration_sec"))
+        self.dvl_nan = bool(param_value("dvl_emit_nan"))
+        self.rng = random.Random(int(param_value("seed")))
+
+        params = AUVParams()
+        params.mass = float(param_value("mass"))
+        params.water_density = float(param_value("water_density"))
+        vol = float(param_value("volume"))
+        params.volume = vol if vol > 0.0 else params.mass / params.water_density
+        params.current_world = np.array(vec_param("current"))
+        params.inertia = np.array(vec_param("inertia"))
+        params.cob_offset = np.array(vec_param("cob_offset"))
+        params.lin_drag = np.array(vec_param("linear_drag"))
+        params.quad_drag = np.array(vec_param("quadratic_drag"))
+        params.ang_drag = np.array(vec_param("angular_drag"))
+        params.ang_quad_drag = np.array(vec_param("angular_quadratic_drag"))
+        self.sim = AUVDynamics(params)
+        self.sim.s.p = np.array(vec_param("initial_position"))
+        self.sim.s.q = np.array(vec_param("initial_orientation_wxyz", 4))
+        self.sim.s.v = np.array(vec_param("initial_linear_velocity"))
+        self.sim.s.w = np.array(vec_param("initial_angular_velocity"))
+
+        self.force = np.zeros(3)
+        self.torque = np.zeros(3)
+        self.last_wrench_t = self.now()
+        self.estimate = None
+
+        self.create_subscription(Wrench, expand_name(param_value("wrench_topic")), self.on_wrench, 10)
+        self.imu_pub = self.create_publisher(Imu, expand_name(param_value("imu_topic")), 10)
+        self.dvl_pub = self.create_publisher(Odometry, expand_name(param_value("dvl_odom_topic")), 10)
+        self.alt_pub = self.create_publisher(Range, expand_name(param_value("dvl_alt_topic")), 10)
+        self.ping_pub = self.create_publisher(Range, expand_name(param_value("ping_range_topic")), 10)
+        self.press_pub = self.create_publisher(FluidPressure, expand_name(param_value("pressure_topic")), 10)
+        self.truth_pub = self.create_publisher(PoseStamped, expand_name(param_value("truth_topic")), 10)
+
+        if self.publish_tf:
+            self.tf_bc = TransformBroadcaster(self)
+            self.static_bc = StaticTransformBroadcaster(self)
+            self._publish_static_frames()
+        if self.publish_markers:
+            self.marker_pub = self.create_publisher(Marker, "auv_sim/markers", 10)
+        est = expand_name(param_value("estimate_topic"))
+        if est:
+            self.create_subscription(PoseStamped, est, self.on_estimate, 10)
+
+        self.sim_dt = 1.0 / float(param_value("sim_rate_hz"))
+        self.acc = {"imu": 0.0, "dvl": 0.0, "depth": 0.0}
+        self.rates = {
+            "imu": 1.0 / float(param_value("imu_rate_hz")),
+            "dvl": 1.0 / float(param_value("dvl_rate_hz")),
+            "depth": 1.0 / float(param_value("depth_rate_hz")),
+        }
+        self.elapsed = 0.0
+        self.create_timer(self.sim_dt, self.step)
+        self.get_logger().info(
+            f"AUV sim bench @ {param_value('sim_rate_hz')} Hz, neutral V={params.volume:.4f} m^3; "
+            f"rviz fixed frame = {self.odom_frame}"
+        )
+
+    def now(self):
+        return self.get_clock().now().nanoseconds * 1e-9
+
+    def on_wrench(self, msg):
+        self.force = np.array([msg.force.x, msg.force.y, msg.force.z])
+        self.torque = np.array([msg.torque.x, msg.torque.y, msg.torque.z])
+        self.last_wrench_t = self.now()
+
+    def on_estimate(self, msg):
+        self.estimate = msg
+
+    def step(self):
+        if self.now() - self.last_wrench_t > self.wrench_timeout:
+            self.force = np.zeros(3)
+            self.torque = np.zeros(3)
+
+        s = self.sim.step(self.sim_dt, self.force, self.torque)
+        self.elapsed += self.sim_dt
+        for k in self.acc:
+            self.acc[k] += self.sim_dt
+        stamp = self.get_clock().now().to_msg()
+
+        if self.acc["imu"] >= self.rates["imu"]:
+            self.acc["imu"] = 0.0
+            self.publish_imu(stamp, s)
+            self.publish_truth(stamp, s)
+        if self.acc["dvl"] >= self.rates["dvl"]:
+            self.acc["dvl"] = 0.0
+            self.publish_dvl(stamp, s)
+        if self.acc["depth"] >= self.rates["depth"]:
+            self.acc["depth"] = 0.0
+            self.publish_depth(stamp, s)
+
+    def publish_imu(self, stamp, s):
+        f = self.sim.specific_force
+        msg = Imu()
+        msg.header.stamp = stamp
+        msg.header.frame_id = self.imu_frame
+        msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z = s.q
+        msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z = s.w
+        msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z = f
+        self.imu_pub.publish(msg)
+
+    def publish_dvl(self, stamp, s):
+        if self.dvl_drop_start >= 0.0 and (
+            self.dvl_drop_start <= self.elapsed < self.dvl_drop_start + self.dvl_drop_dur
+        ):
+            return
+        msg = Odometry()
+        msg.header.stamp = stamp
+        msg.header.frame_id = self.odom_frame
+        msg.child_frame_id = self.dvl_frame
+        if self.dvl_nan:
+            vx = vy = vz = float("nan")
+        else:
+            def noise():
+                return self.rng.gauss(0.0, self.dvl_noise) if self.dvl_noise > 0 else 0.0
+
+            vx, vy, vz = s.v[0] + noise(), s.v[1] + noise(), s.v[2] + noise()
+        msg.twist.twist.linear.x = vx
+        msg.twist.twist.linear.y = vy
+        msg.twist.twist.linear.z = vz
+        self.dvl_pub.publish(msg)
+
+    def publish_depth(self, stamp, s):
+        altitude = float(s.p[2] - self.floor_z)
+        for pub, frame in ((self.alt_pub, self.dvl_frame), (self.ping_pub, self.ping_frame)):
+            r = Range()
+            r.header.stamp = stamp
+            r.header.frame_id = frame
+            r.radiation_type = Range.ULTRASOUND
+            r.min_range = 0.1
+            r.max_range = 100.0
+            r.range = altitude
+            pub.publish(r)
+
+        depth = max(0.0, self.surface_z - float(s.p[2]))
+        pmsg = FluidPressure()
+        pmsg.header.stamp = stamp
+        pmsg.header.frame_id = self.imu_frame
+        pmsg.fluid_pressure = self.atm + self.sim.p.water_density * self.sim.p.gravity * depth
+        self.press_pub.publish(pmsg)
+
+    def publish_truth(self, stamp, s):
+        msg = PoseStamped()
+        msg.header.stamp = stamp
+        msg.header.frame_id = self.odom_frame
+        msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = s.p
+        msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z = s.q
+        self.truth_pub.publish(msg)
+
+        if self.publish_tf:
+            tf = TransformStamped()
+            tf.header.stamp = stamp
+            tf.header.frame_id = self.odom_frame
+            tf.child_frame_id = self.base_frame
+            tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z = s.p
+            tf.transform.rotation.w, tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z = s.q
+            self.tf_bc.sendTransform(tf)
+
+        if self.publish_markers:
+            self._publish_body_marker(stamp)
+            if self.estimate is not None:
+                self._publish_estimate_marker(stamp)
+
+    def _publish_static_frames(self):
+        tfs = []
+        for child in (self.imu_frame, self.dvl_frame, self.ping_frame):
+            tf = TransformStamped()
+            tf.header.stamp = self.get_clock().now().to_msg()
+            tf.header.frame_id = self.base_frame
+            tf.child_frame_id = child
+            tf.transform.rotation.w = 1.0
+            tfs.append(tf)
+        self.static_bc.sendTransform(tfs)
+
+    def _publish_body_marker(self, stamp):
+        m = Marker()
+        m.header.stamp = stamp
+        m.header.frame_id = self.base_frame
+        m.ns = "auv"
+        m.id = 0
+        m.type = Marker.CUBE
+        m.action = Marker.ADD
+        m.pose.orientation.w = 1.0
+        m.scale.x, m.scale.y, m.scale.z = self.body_size
+        m.color.r, m.color.g, m.color.b, m.color.a = 0.2, 0.5, 1.0, 0.85
+        self.marker_pub.publish(m)
+
+    def _publish_estimate_marker(self, stamp):
+        m = Marker()
+        m.header.stamp = stamp
+        m.header.frame_id = self.odom_frame
+        m.ns = "estimate"
+        m.id = 1
+        m.type = Marker.SPHERE
+        m.action = Marker.ADD
+        m.pose = self.estimate.pose
+        m.scale.x = m.scale.y = m.scale.z = 0.25
+        m.color.r, m.color.g, m.color.b, m.color.a = 0.2, 1.0, 0.3, 0.9
+        self.marker_pub.publish(m)
+
+
+def main():
+    rclpy.init()
+    node = AUVSimNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/PKG-INFO

@@ -0,0 +1,203 @@
+Metadata-Version: 2.4
+Name: auv_sim_bench
+Version: 0.1.0
+Summary: Lightweight 6-DOF AUV physics sim for dry-testing state estimation.
+Maintainer: wuisabel-gif
+Maintainer-email: 231155141+wuisabel-gif@users.noreply.github.com
+License: MIT
+Description-Content-Type: text/markdown
+Requires-Dist: setuptools
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: license
+Dynamic: maintainer
+Dynamic: maintainer-email
+Dynamic: requires-dist
+Dynamic: summary
+
+# AUVSimBench
+
+A lightweight **physics twin** for autonomous underwater vehicles — a 6-DOF AUV
+simulator as a single ROS 2 node, in pure Python/numpy. It runs the vehicle's
+physics and publishes the **sensor topics a state estimator consumes, plus
+ground-truth pose**, so you can develop and stress-test an underwater localization
+or control stack **on the bench** — no water, no GPU, no Isaac Sim. It runs happily
+on a Jetson.
+
+## Why this is hard (and why the twin helps)
+
+Validating underwater state estimation is uniquely painful:
+
+- **Wet tests are expensive and rare.** Pool/ocean time is slow to schedule, and you
+  get only a handful of runs before everyone's tired and the battery's flat.
+- **There is no ground truth underwater.** No GPS below the surface — so even when you
+  *do* get a dive, you often can't measure *how wrong* the estimate was.
+- **Most failures aren't about the water.** In practice the things that break a dive are
+  wiring (a node subscribed to the wrong topic), frame conventions (axes flipped),
+  startup ordering (a sensor not powered on), or an estimator diverging — and you
+  usually discover them *mid-dive*, burning a wet test to debug a typo.
+- **Recorded bags only go so far.** A bag replays exactly one motion; you can't steer
+  it, you can't make a sensor fail on cue, and replayed DVL/depth won't agree with a
+  real IMU sitting still on the bench.
+
+A **physics twin** answers all four. It simulates the vehicle's dynamics, so every
+sensor stream is derived from *one consistent truth*; it *knows* the true pose, so
+estimator accuracy is measurable; and you can script scenarios, disturbances, and
+on-demand sensor failures — all from a laptop or the robot's own compute.
+
+```
+wrench in  ──►  6-DOF dynamics (thrust + buoyancy + drag + current)  ──►  IMU / DVL / depth + ground truth
+```
+
+## What it gives you
+
+- **Consistent sensors** — IMU, DVL and depth all generated from the same simulated
+  body, so they physically agree (the thing bag-replay can't do).
+- **Ground truth** — `/auv_sim/ground_truth` is the exact pose, so you can plot
+  estimator error directly.
+- **Scenarios & disturbances** — water current, and on-demand sensor failures (DVL
+  dropout, noise, NaN) to probe estimator robustness — the exact failure modes that
+  bite in real wet tests.
+- **Driven by your real controller** — it takes a `geometry_msgs/Wrench` with a deadman,
+  the same contract a PID/thruster-allocation stack already produces.
+
+## Physics model
+
+An independent **6-DOF Newton-Euler** rigid body (FLU body frame, ENU world,
+quaternion orientation):
+
+- **Buoyancy** `ρ·V·g`, applied at a **center of buoyancy above the center of mass**, so
+  the vehicle self-rights — giving realistic roll/pitch, and therefore realistic IMU
+  output. Default displacement is exactly neutral.
+- **Linear + quadratic drag** on both linear and angular velocity.
+- **Water current** — drag acts on velocity *relative to the water*, so a current
+  actually pushes a passive vehicle (modeled properly, not faked through gravity).
+- **Wrench input** in the body frame, with a 0.5 s command-timeout deadman.
+
+The accelerometer output is the true specific force `(ΣF − gravity)/m` in the body
+frame, so a level, neutrally-buoyant vehicle reads `az ≈ +9.81` — matching a real
+ENU/FLU IMU.
+
+## Vehicle-agnostic
+
+It is **not tied to any one vehicle.** Every topic name, frame, rate and physics
+constant is a parameter, and the defaults are generic:
+
+| published topic (default) | type | content |
+|---------------------------|------|---------|
+| `imu/data` | `sensor_msgs/Imu` | orientation, body angular velocity, specific-force accel |
+| `dvl/odometry` | `nav_msgs/Odometry` | body-frame velocity |
+| `dvl/altitude`, `ping/range` | `sensor_msgs/Range` | height above the floor |
+| `pressure` | `sensor_msgs/FluidPressure` | from depth |
+| `ground_truth` | `geometry_msgs/PoseStamped` | exact sim pose |
+
+Adapt it to a specific vehicle with a **profile** — a small YAML that remaps the
+topics/frames and sets the mass/buoyancy. `config/barracuda.yaml` is one example
+(Barracuda's real `/barracuda/...` topics); write your own for any other AUV.
+
+Profiles can also avoid hardcoded robot names by using placeholders in topic and
+frame strings:
+
+| placeholder | expands to |
+|-------------|------------|
+| `{topic_prefix}` | the normalized `topic_prefix` parameter, such as `/barracuda` |
+| `{frame_prefix}` | the normalized `frame_prefix` parameter, such as `barracuda` |
+
+For example, `wrench_topic: "{topic_prefix}/wrench"` becomes
+`/barracuda/wrench` when `topic_prefix: /barracuda`. Leave both prefixes empty
+for generic relative names.
+
+Profiles can also tune the vehicle dynamics and scenario start state:
+
+| parameter | meaning |
+|-----------|---------|
+| `inertia` | diagonal body inertia `[Ixx, Iyy, Izz]` |
+| `cob_offset` | center of buoyancy relative to center of mass, body frame |
+| `linear_drag`, `quadratic_drag` | per-axis translational drag coefficients |
+| `angular_drag`, `angular_quadratic_drag` | per-axis rotational drag coefficients |
+| `initial_position` | starting world position |
+| `initial_orientation_wxyz` | starting orientation quaternion |
+| `initial_linear_velocity`, `initial_angular_velocity` | starting body velocities |
+
+## See it in 3D
+
+There's no built-in render, but it broadcasts **TF** and a **vehicle marker**, so it
+shows up live in **RViz2** — which runs on a Jetson (plain OpenGL, no RTX/Isaac):
+
+```bash
+ros2 launch auv_sim_bench sim.launch.py rviz:=true
+```
+
+You get a 3D box for the vehicle flying around under thrust/current/buoyancy, the TF
+frames, and — if you set `estimate_topic` to your estimator's pose — a second marker
+for the **estimate**, so you can watch estimate-vs-truth drift in real time. It's
+schematic (boxes + axes), not photoreal; photoreal water is the one thing that needs
+Isaac Sim on an x86 box.
+
+## Failure & disturbance injection
+
+For the real reason to simulate — breaking the estimator on purpose:
+
+```yaml
+current: [0.3, 0.0, 0.0]        # steady cross-current
+dvl_dropout_start_sec: 10.0     # DVL goes silent at t=10s ...
+dvl_dropout_duration_sec: 5.0   # ... for 5s
+dvl_noise_stddev: 0.02          # velocity noise
+dvl_emit_nan: false             # or feed the estimator NaNs
+```
+
+## Build & run
+
+```bash
+cd ~/ros2_ws/src && git clone <this repo> auv_sim_bench
+cd ~/ros2_ws && colcon build --packages-select auv_sim_bench
+source install/setup.bash
+
+ros2 launch auv_sim_bench sim.launch.py                       # generic defaults
+ros2 launch auv_sim_bench sim.launch.py rviz:=true            # + 3D view
+ros2 launch auv_sim_bench sim.launch.py config:=barracuda.yaml   # Barracuda profile
+# drive it by publishing geometry_msgs/Wrench on the wrench topic (default cmd_wrench)
+```
+
+### On a Jetson (AGX Orin)
+
+Validated on a Jetson AGX Orin (JetPack 6 / ROS 2 Humble) — pure Python + numpy, so
+no GPU or Isaac needed:
+
+```bash
+mkdir -p ~/auv_ws/src
+cp -r AUVSimBench ~/auv_ws/src/auv_sim_bench
+cd ~/auv_ws && colcon build --packages-select auv_sim_bench
+source /opt/ros/humble/setup.bash
+source ~/auv_ws/install/setup.bash        # both source lines, in every new terminal
+ros2 launch auv_sim_bench sim.launch.py
+```
+
+Headless over SSH there's no display for RViz, so drop `rviz:=true` and watch the
+data instead (`ros2 topic echo /ground_truth`); driving it with a `/cmd_wrench`
+publisher moves the body as expected. For the 3D view, run RViz on a monitor
+attached to the Jetson.
+
+Test your estimator: run the sim + your estimator, then compare
+`/barracuda/estimated_pose` against `/auv_sim/ground_truth` — an accuracy check you
+cannot get from a real dive (no underwater ground truth) or from bag replay.
+
+## Tests
+
+The physics core (`dynamics.py`) is plain numpy with no ROS dependency, so it is
+unit-tested directly — buoyancy, gravity, drag, thrust response, and current:
+
+```bash
+pytest test/
+```
+
+## Credits & scope
+
+Inspired by Leonardo Lima's [`isaac_underwater`](https://github.com/leonlime/isaac_underwater)
+examples (MIT), which show buoyancy and drag in Isaac Sim. AUVSimBench takes that
+idea in a different direction — its own 6-DOF dynamics in a standalone ROS 2 node, so
+it runs anywhere ROS 2 does, including a Jetson.
+
+It's a controls/estimation test rig, not a hydrodynamics research tool: drag and
+inertia are simple tunable coefficients, not CFD. Added-mass and richer current
+fields are natural next steps.

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/SOURCES.txt

@@ -0,0 +1,20 @@
+README.md
+package.xml
+setup.cfg
+setup.py
+auv_sim_bench/__init__.py
+auv_sim_bench/dynamics.py
+auv_sim_bench/sim_node.py
+auv_sim_bench.egg-info/PKG-INFO
+auv_sim_bench.egg-info/SOURCES.txt
+auv_sim_bench.egg-info/dependency_links.txt
+auv_sim_bench.egg-info/entry_points.txt
+auv_sim_bench.egg-info/requires.txt
+auv_sim_bench.egg-info/top_level.txt
+auv_sim_bench.egg-info/zip-safe
+config/barracuda.yaml
+config/sim.yaml
+launch/sim.launch.py
+resource/auv_sim_bench
+rviz/auv_sim.rviz
+test/test_dynamics.py

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/entry_points.txt

@@ -0,0 +1,2 @@
+[console_scripts]
+sim = auv_sim_bench.sim_node:main

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/requires.txt

@@ -0,0 +1 @@
+setuptools

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/top_level.txt

@@ -0,0 +1 @@
+auv_sim_bench

auv_sim_bench-0.1.0/auv_sim_bench.egg-info/zip-safe

@@ -0,0 +1 @@
+

auv_sim_bench-0.1.0/config/barracuda.yaml

@@ -0,0 +1,21 @@
+# Barracuda profile: maps the generic sim onto Barracuda's real topic + frame names.
+# Run with:  ros2 launch auv_sim_bench sim.launch.py config:=barracuda.yaml
+auv_sim_bench:
+  ros__parameters:
+    wrench_topic: /barracuda/wrench
+    imu_topic: /barracuda/vectornav/imu
+    dvl_odom_topic: /barracuda/dvl/odometry
+    dvl_alt_topic: /barracuda/dvl/altitude
+    ping_range_topic: /barracuda/ping1d/range
+    pressure_topic: /barracuda/vectornav/pressure
+    truth_topic: /auv_sim/ground_truth
+    estimate_topic: /barracuda/estimated_pose
+
+    odom_frame: odom
+    base_frame: barracuda/base_link
+    imu_frame: barracuda/imu_link
+    dvl_frame: barracuda/dvl_link
+    ping_frame: barracuda/ping1d_link
+
+    mass: 15.0
+    water_density: 1025.0

auv_sim_bench-0.1.0/config/sim.yaml

@@ -0,0 +1,56 @@
+auv_sim_bench:
+  ros__parameters:
+    sim_rate_hz: 200.0
+    imu_rate_hz: 100.0
+    dvl_rate_hz: 8.0
+    depth_rate_hz: 5.0
+
+    # generic topic names (load config/barracuda.yaml for the Barracuda profile)
+    topic_prefix: ""
+    frame_prefix: ""
+    wrench_topic: cmd_wrench
+    imu_topic: imu/data
+    dvl_odom_topic: dvl/odometry
+    dvl_alt_topic: dvl/altitude
+    ping_range_topic: ping/range
+    pressure_topic: pressure
+    truth_topic: ground_truth
+    estimate_topic: ""          # set to your estimator's pose topic to show it in rviz
+
+    odom_frame: odom
+    base_frame: base_link
+    imu_frame: imu_link
+    dvl_frame: dvl_link
+    ping_frame: ping_link
+
+    wrench_timeout_sec: 0.5
+    floor_z: -10.0
+    surface_z: 0.0
+    atm_pressure: 101325.0
+
+    mass: 15.0
+    volume: 0.0                 # 0 -> neutral buoyancy (mass / water_density)
+    water_density: 1025.0
+    current: [0.0, 0.0, 0.0]    # world-frame water current (m/s)
+    inertia: [0.5, 0.8, 0.8]    # diagonal Ixx,Iyy,Izz (kg*m^2)
+    cob_offset: [0.0, 0.0, 0.02] # center of buoyancy relative to COM, body frame (m)
+    linear_drag: [20.0, 40.0, 40.0]
+    quadratic_drag: [30.0, 80.0, 80.0]
+    angular_drag: [2.0, 4.0, 4.0]
+    angular_quadratic_drag: [1.0, 2.0, 2.0]
+
+    initial_position: [0.0, 0.0, 0.0]
+    initial_orientation_wxyz: [1.0, 0.0, 0.0, 0.0]
+    initial_linear_velocity: [0.0, 0.0, 0.0]
+    initial_angular_velocity: [0.0, 0.0, 0.0]
+
+    publish_tf: true
+    publish_markers: true
+    body_size: [0.6, 0.35, 0.25]   # rviz box dimensions (m)
+
+    # failure injection (off by default)
+    dvl_noise_stddev: 0.0
+    dvl_dropout_start_sec: -1.0
+    dvl_dropout_duration_sec: 0.0
+    dvl_emit_nan: false
+    seed: 0

auv_sim_bench-0.1.0/launch/sim.launch.py

@@ -0,0 +1,40 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    share = get_package_share_directory("auv_sim_bench")
+    config_arg = DeclareLaunchArgument("config", default_value="sim.yaml")
+    rviz_arg = DeclareLaunchArgument("rviz", default_value="false")
+
+    config = PathJoinSubstitution([share, "config", LaunchConfiguration("config")])
+    rviz_cfg = PathJoinSubstitution(
+        [FindPackageShare("auv_sim_bench"), "rviz", "auv_sim.rviz"]
+    )
+
+    return LaunchDescription([
+        config_arg,
+        rviz_arg,
+        Node(
+            package="auv_sim_bench",
+            executable="sim",
+            name="auv_sim_bench",
+            parameters=[config],
+            output="screen",
+        ),
+        Node(
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            arguments=["-d", rviz_cfg],
+            condition=IfCondition(LaunchConfiguration("rviz")),
+            output="screen",
+        ),
+    ])

auv_sim_bench-0.1.0/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>auv_sim_bench</name>
+  <version>0.1.0</version>
+  <description>Lightweight 6-DOF AUV physics simulator that publishes consistent IMU, DVL, depth and ground-truth topics for dry-testing underwater state estimation. Pure ROS 2 / numpy, runs on a Jetson.</description>
+  <maintainer email="231155141+wuisabel-gif@users.noreply.github.com">wuisabel-gif</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

auv_sim_bench-0.1.0/rviz/auv_sim.rviz

@@ -0,0 +1,46 @@
+Panels:
+  - Class: rviz_common/Displays
+    Name: Displays
+Visualization Manager:
+  Global Options:
+    Fixed Frame: odom
+    Background Color: 20; 30; 40
+  Displays:
+    - Class: rviz_default_plugins/Grid
+      Name: Grid
+      Enabled: true
+      Plane: XY
+      Cell Size: 1
+      Plane Cell Count: 20
+    - Class: rviz_default_plugins/TF
+      Name: TF
+      Enabled: true
+      Show Names: true
+      Show Axes: true
+      Marker Scale: 1
+    - Class: rviz_default_plugins/Marker
+      Name: Vehicle
+      Enabled: true
+      Topic:
+        Value: /auv_sim/markers
+        Depth: 5
+        Reliability Policy: Reliable
+    - Class: rviz_default_plugins/Odometry
+      Name: DVL odometry
+      Enabled: false
+      Topic:
+        Value: /dvl/odometry
+  Tools:
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/FocusCamera
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Name: Current View
+      Distance: 12
+      Pitch: 0.4
+      Yaw: 0.8
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0

auv_sim_bench-0.1.0/setup.cfg

@@ -0,0 +1,10 @@
+[develop]
+script_dir = $base/lib/auv_sim_bench
+
+[install]
+install_scripts = $base/lib/auv_sim_bench
+
+[egg_info]
+tag_build = 
+tag_date = 0
+

auv_sim_bench-0.1.0/setup.py

@@ -0,0 +1,33 @@
+import os
+from glob import glob
+from pathlib import Path
+
+from setuptools import setup
+
+package_name = "auv_sim_bench"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
+        (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")),
+        (os.path.join("share", package_name, "config"), glob("config/*.yaml")),
+        (os.path.join("share", package_name, "rviz"), glob("rviz/*.rviz")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="wuisabel-gif",
+    maintainer_email="231155141+wuisabel-gif@users.noreply.github.com",
+    description="Lightweight 6-DOF AUV physics sim for dry-testing state estimation.",
+    long_description=Path("README.md").read_text(encoding="utf-8"),
+    long_description_content_type="text/markdown",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            "sim = auv_sim_bench.sim_node:main",
+        ],
+    },
+)

auv_sim_bench-0.1.0/test/test_dynamics.py

@@ -0,0 +1,46 @@
+import numpy as np
+
+from auv_sim_bench.dynamics import AUVDynamics, AUVParams
+
+
+def test_neutral_buoyancy_hovers():
+    sim = AUVDynamics(AUVParams())  # default volume = neutral
+    for _ in range(2000):
+        sim.step(0.005, [0, 0, 0], [0, 0, 0])
+    assert abs(sim.s.p[2]) < 0.05  # stays near its starting depth
+
+
+def test_accelerometer_reads_gravity_at_rest():
+    sim = AUVDynamics(AUVParams())
+    sim.step(0.005, [0, 0, 0], [0, 0, 0])
+    # level, neutrally buoyant: specific force is +g on body z
+    assert sim.specific_force[2] > 9.0
+    assert abs(sim.specific_force[0]) < 0.1
+    assert abs(sim.specific_force[1]) < 0.1
+
+
+def test_negative_buoyancy_sinks():
+    p = AUVParams()
+    p.volume = p.mass / p.water_density * 0.9  # under-displaced -> heavy
+    sim = AUVDynamics(p)
+    for _ in range(2000):
+        sim.step(0.005, [0, 0, 0], [0, 0, 0])
+    assert sim.s.p[2] < -0.1  # sinks
+
+
+def test_forward_thrust_moves_and_drag_limits_speed():
+    sim = AUVDynamics(AUVParams())
+    for _ in range(4000):
+        sim.step(0.005, [40, 0, 0], [0, 0, 0])  # steady forward force
+    assert sim.s.p[0] > 0.5      # moved forward
+    assert sim.s.v[0] < 5.0      # drag bounded the speed
+
+
+def test_current_pushes_passive_vehicle():
+    p = AUVParams()
+    p.current_world = np.array([0.5, 0.0, 0.0])  # 0.5 m/s current along +x
+    sim = AUVDynamics(p)
+    for _ in range(4000):
+        sim.step(0.005, [0, 0, 0], [0, 0, 0])    # no thrust
+    assert sim.s.p[0] > 0.2      # carried downstream
+    assert sim.s.v[0] > 0.1      # drifting toward the water velocity