From 6e1f9f57c5ae87a622e099b60053300222cb8c49 Mon Sep 17 00:00:00 2001 From: brendon-albacore Date: Mon, 27 Apr 2026 10:26:45 -0400 Subject: [PATCH 01/12] fix: add conditional in ardusub env file for zsh --- extras/ros-jazzy-gz-harmonic-install.sh | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/extras/ros-jazzy-gz-harmonic-install.sh b/extras/ros-jazzy-gz-harmonic-install.sh index a9263312..e2979095 100644 --- a/extras/ros-jazzy-gz-harmonic-install.sh +++ b/extras/ros-jazzy-gz-harmonic-install.sh @@ -131,7 +131,11 @@ ENV_FILE="$ENV_DIR/env" mkdir -p "$ENV_DIR" cat > "$ENV_FILE" <<'EOF' -source /opt/ros/jazzy/setup.bash +if [ -n "$ZSH_VERSION" ]; then + source /opt/ros/jazzy/setup.zsh +else + source /opt/ros/jazzy/setup.bash +fi export PATH=/opt/ardusub_ws/ardupilot/Tools/autotest:$PATH export PATH=/opt/ardusub_ws/ardupilot/build/sitl/bin:$PATH export GEOGRAPHICLIB_GEOID_PATH=/usr/share/GeographicLib/geoids From 845aafcd61160266ae901c6bc74583bd668ca3b9 Mon Sep 17 00:00:00 2001 From: ppakr Date: Mon, 18 May 2026 13:15:57 +0200 Subject: [PATCH 02/12] feat(multibeam_sonar): add retroImage for per-ray laser retro data and update reflectivityImage computation --- .../multibeam_sonar/MultibeamSonarSensor.cc | 15 ++++++++++++--- .../multibeam_sonar/MultibeamSonarSensor.hh | 1 + 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc index 1db557a4..e8696a27 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc @@ -699,7 +699,7 @@ bool MultibeamSonarSensor::Implementation::InitializeBeamArrangement(MultibeamSo } this->beamCorrectorSum = 0.0; - this->constMu = true; + this->constMu = false; this->mu = 1e-3; return true; @@ -926,6 +926,7 @@ void MultibeamSonarSensor::Implementation::FillPointCloudMsg(const float * _rayB // After filling pointMsg, compute pointCloudImage as well this->lock_.lock(); this->pointCloudImage.create(height, width, CV_32FC1); + this->retroImage.create(height, width, CV_32FC1); cv::MatIterator_ iter_image = this->pointCloudImage.begin(); bool angles_calculation_flag = false; @@ -955,9 +956,11 @@ void MultibeamSonarSensor::Implementation::FillPointCloudMsg(const float * _rayB // Index in _rayBuffer auto index = j * width * channels + i * channels; float depth = _rayBuffer[index]; + float retro = _rayBuffer[index + 1]; float range = std::isfinite(depth) ? depth : 100000.0f; *iter_image = range; + this->retroImage.at(j, i) = std::isfinite(retro) ? retro : 0.0f; // Store azimuth angles on the first row only if (angles_calculation_flag && j == 0) @@ -1066,10 +1069,16 @@ void MultibeamSonarSensor::Implementation::ComputeSonarImage() ComputeCorrector(); } - if (this->reflectivityImage.rows == 0) + // reflectivityImage is indexed (row=ray, col=beam) by the CUDA kernel — + // i.e. (height, width), matching pointCloudImage. + if (this->constMu || this->retroImage.rows == 0) { this->reflectivityImage = - cv::Mat(this->pointMsg.width(), this->pointMsg.height(), CV_32FC1, cv::Scalar(this->mu)); + cv::Mat(this->pointMsg.height(), this->pointMsg.width(), CV_32FC1, cv::Scalar(this->mu)); + } + else + { + cv::max(this->retroImage, this->mu, this->reflectivityImage); } auto start = std::chrono::high_resolution_clock::now(); diff --git a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh index 3138bd10..aeccd801 100644 --- a/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh +++ b/gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.hh @@ -176,6 +176,7 @@ private: // OpenCV images cv::Mat pointCloudImage; cv::Mat reflectivityImage; + cv::Mat retroImage; // Per-ray laser_retro from GpuRays, fed into reflectivityImage. cv::Mat randImage; // Angles From 3895b650de4fe88daffcd7c35dee2e7c30c4a48f Mon Sep 17 00:00:00 2001 From: ppakr Date: Mon, 18 May 2026 13:26:14 +0200 Subject: [PATCH 03/12] feat(laser_retro): implement script to add tags to model.sdf files --- .../sonar_3d_demo/scripts/add_laser_retro.py | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100755 gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/add_laser_retro.py diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/add_laser_retro.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/add_laser_retro.py new file mode 100755 index 00000000..3bfa1c09 --- /dev/null +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/add_laser_retro.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 +"""Add tags to every model.sdf in ~/blender_models/. + +Phase B of the material reflectivity plan. Idempotent: skips files +that already have a tag. Inserts the tag inside each + block (must have exactly one visual per file — fails loudly +otherwise). + +Reflectivity values are starting placeholders based on acoustic-impedance +ratios; they get calibrated against wetlab data in Phase C. +""" + +import re +import sys +from pathlib import Path + +# Starting reflectivity values per model. +# Phase C will refit these against wl_wetlab_apr22 data. +RETRO_VALUES = { + # Metal targets — reference material + "circle_metal": 1.0, + "square_metal": 1.0, + "triangle_metal": 1.0, + "metal_board": 1.0, + # Wood targets + "circle_wood": 0.25, + "square_wood": 0.25, + "triangle_wood": 0.25, + # PETG (3D-printed) targets + "circle_petg": 0.10, + "square_petg": 0.10, + "triangle_petg": 0.10, + # Other + "brick": 0.40, + "float": 0.05, + # NOTE: ~/blender_models/wetlab_tank is NOT the tank used by + # sonar_3d_demo. The actual tank model is WL_wetlab_tank, which + # lives in src/dave/models/dave_object_models/description/. + # Per the material_reflectivity_plan §5 ("tank walls flood noise floor" + # mitigation), keep the live tank UNTAGGED so walls floor at mu. + # Revisit if Phase C calibration needs wall returns. +} + +VISUAL_CLOSE_PATTERN = re.compile(r"(\s*)") + + +def patch_sdf(sdf_path: Path, retro: float) -> str: + """Return one of: 'added', 'skipped', 'multi-visual', 'no-visual'.""" + text = sdf_path.read_text() + + if "" in text: + return "skipped" + + visual_count = text.count(" 1: + return "multi-visual" + + # Insert just before , indented to match + # the existing block. + def insert(match: re.Match) -> str: + indent = match.group(1) + # Two-space deeper indent for the new child element + inner_indent = indent + " " + return f"\n{inner_indent}{retro}{match.group(0)}" + + new_text, n = VISUAL_CLOSE_PATTERN.subn(insert, text, count=1) + if n != 1: + return "no-visual" + + sdf_path.write_text(new_text) + return "added" + + +def main() -> int: + root = Path.home() / "blender_models" + if not root.is_dir(): + print(f"ERROR: {root} does not exist", file=sys.stderr) + return 1 + + results: dict[str, list[str]] = { + "added": [], "skipped": [], "multi-visual": [], "no-visual": [], "missing": [], + } + for model_name, retro in RETRO_VALUES.items(): + sdf = root / model_name / "model.sdf" + if not sdf.is_file(): + results["missing"].append(model_name) + continue + status = patch_sdf(sdf, retro) + results[status].append(f"{model_name} (retro={retro})") + + for status, items in results.items(): + if not items: + continue + marker = "OK " if status in ("added", "skipped") else "!! " + print(f"{marker}{status} ({len(items)}):") + for it in items: + print(f" {it}") + + if results["multi-visual"] or results["no-visual"] or results["missing"]: + print("\nNon-trivial cases above need manual review.", file=sys.stderr) + return 1 + return 0 + + +if __name__ == "__main__": + sys.exit(main()) From cc98a539fff009db4b4e63314219cfdc676c12a1 Mon Sep 17 00:00:00 2001 From: woensug-choi Date: Wed, 20 May 2026 09:44:49 +0900 Subject: [PATCH 04/12] upgrade precommit pyupgrade --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 747efca6..5ea9a24c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -48,7 +48,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.19.1 + rev: v3.21.2 hooks: - id: pyupgrade args: [--py36-plus] @@ -194,4 +194,4 @@ repos: }" language: python types: [file, yaml] - files: '\.(yaml|yml)$' \ No newline at end of file + files: '\.(yaml|yml)$' From 65199df6bc122c36a2263c169a64a99a334b8c0f Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 27 May 2026 09:52:15 +0200 Subject: [PATCH 05/12] fix(lidar_3d): adjust lidar parameters for improved simulation accuracy --- .../sonar_3d_demo/config/targets.yaml | 2 +- .../description/lidar_3d/model.sdf | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml index d638cc48..53376af2 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml @@ -75,7 +75,7 @@ targets: square_metal: x: 0.65 y: 0.0 - z: 0.27 + z: 0.2650 roll: 0.0 pitch: 0.0 yaw: 1.5708 diff --git a/models/dave_sensor_models/description/lidar_3d/model.sdf b/models/dave_sensor_models/description/lidar_3d/model.sdf index 6dc6d964..d70b9cb4 100644 --- a/models/dave_sensor_models/description/lidar_3d/model.sdf +++ b/models/dave_sensor_models/description/lidar_3d/model.sdf @@ -55,20 +55,24 @@ - + 0 0 0 0 0 0 5 - 105 + 256 1.0 -0.785398 0.785398 - 25 + 64 1.0 -0.349066 0.349066 @@ -76,7 +80,7 @@ 0.1 - 15.0 + 10.0 0.01 From 14127d7ba6876ea840a7bd8d59278d0a21a4240e Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 27 May 2026 09:52:37 +0200 Subject: [PATCH 06/12] feat(sonar_3d_demo): add playback script for object trajectory and enhance launch configuration --- .../sonar_3d_demo/CMakeLists.txt | 5 + .../sonar_3d_demo/launch/tank_scene.launch.py | 153 +++++++++++++++- .../scripts/play_object_trajectory.py | 172 ++++++++++++++++++ 3 files changed, 326 insertions(+), 4 deletions(-) create mode 100755 gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/play_object_trajectory.py diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/CMakeLists.txt b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/CMakeLists.txt index 3c3b9818..e2340da8 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/CMakeLists.txt +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/CMakeLists.txt @@ -10,6 +10,11 @@ if(CUDAToolkit_FOUND) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + install( + PROGRAMS scripts/play_object_trajectory.py + DESTINATION lib/${PROJECT_NAME} + ) + ament_environment_hooks( "${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") else() diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py index 7facd2a8..e5e84b3a 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py @@ -9,11 +9,27 @@ # The world file (tank_with_float.world) only carries the tank, lights, and # GUI plugins; everything pose-related lives in the YAML. # +# pose_source argument +# -------------------- +# yaml Use the position+rotation from targets.yaml (legacy behaviour). +# real_median (default) Look up the target's median position in +# /summary_static.csv (extracted by +# sonar_camera_logger/scripts/extract_object_poses.py), and +# spawn at sim_world_T_sonar ⊕ sonar_T_object. Rotation still +# comes from the YAML — the CSV's quaternion mostly reflects +# floater spin, not the intended target orientation. +# Falls back silently to YAML if no row matches the target. +# real_trajectory Same as real_median for spawn, but also exposes the per-bag +# timeseries CSV to a downstream playback node (task wired in +# a separate launch helper). +# # Usage: # ros2 launch sonar_3d_demo tank_scene.launch.py target:=brick -# ros2 launch sonar_3d_demo tank_scene.launch.py target:=brick sensor:=lidar +# ros2 launch sonar_3d_demo tank_scene.launch.py target:=square_petg sensor:=lidar # ros2 launch sonar_3d_demo tank_scene.launch.py target:=square_metal z:=0.25 +# ros2 launch sonar_3d_demo tank_scene.launch.py target:=circle_petg pose_source:=yaml +import csv import os import yaml @@ -24,6 +40,7 @@ DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, + TimerAction, ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration @@ -34,6 +51,11 @@ WORLD_NAME = "tank_with_float" POSE_KEYS = ("x", "y", "z", "roll", "pitch", "yaw") +DEFAULT_POSES_DIR = ( + "/media/aki/2C76C6780AEDB4DB1/wl_wetlab_apr22_processed/object_poses" +) +POSE_SOURCES = ("yaml", "real_median", "real_trajectory") + # Map the user-facing `sensor` arg to (namespace, YAML key). # The namespace is what dave_sensor.launch.py uses to look up the model # under dave_sensor_models/description//model.sdf, and what gets @@ -50,6 +72,30 @@ def _resolve(arg_value, fallback): return float(s) if s else float(fallback) +def _lookup_real_median(target: str, poses_dir: str): + """Return (x, y, z) of `target` in the sonar frame from summary_static.csv. + + Returns None if the CSV is missing, has no row for this target, or any + row value is unparseable. The caller is expected to fall back to the YAML. + """ + summary = os.path.join(poses_dir, "summary_static.csv") + if not os.path.isfile(summary): + return None + with open(summary) as f: + for row in csv.DictReader(f): + if row.get("target") != target: + continue + try: + return ( + float(row["med_x"]), + float(row["med_y"]), + float(row["med_z"]), + ) + except (KeyError, ValueError): + return None + return None + + def _spawn_node(name, sdf_path, pose): """Build a ros_gz_sim create Node for the given model + pose dict.""" return Node( @@ -112,11 +158,17 @@ def _lidar_nodes(pose): def launch_setup(context, *args, **kwargs): target = LaunchConfiguration("target").perform(context) sensor = LaunchConfiguration("sensor").perform(context) + pose_source = LaunchConfiguration("pose_source").perform(context) + poses_dir = LaunchConfiguration("poses_dir").perform(context) if sensor not in SENSOR_PROFILES: raise RuntimeError( f"Unknown sensor '{sensor}'. Options: {sorted(SENSOR_PROFILES)}" ) + if pose_source not in POSE_SOURCES: + raise RuntimeError( + f"Unknown pose_source '{pose_source}'. Options: {list(POSE_SOURCES)}" + ) profile = SENSOR_PROFILES[sensor] pose_overrides = { @@ -144,10 +196,32 @@ def launch_setup(context, *args, **kwargs): float_pose = scene["float"] target_yaml = manifest["targets"][target] + # YAML baseline pose (rotations always come from here — see module docstring). target_pose = { k: _resolve(pose_overrides[k], target_yaml[k]) for k in POSE_KEYS } + # Optionally overwrite XYZ with the real median pose. We compose + # sim_world_T_object = sim_world_T_sonar (translation) + sonar_T_object (CSV), + # which assumes sensor_pose has identity rotation — true for the current + # targets.yaml. CLI overrides on x/y/z (non-empty pose_overrides) still win. + real_xyz_used = False + if pose_source in ("real_median", "real_trajectory"): + real_xyz = _lookup_real_median(target, poses_dir) + if real_xyz is not None: + sx, sy, sz = sensor_pose["x"], sensor_pose["y"], sensor_pose["z"] + world_xyz = (sx + real_xyz[0], sy + real_xyz[1], sz + real_xyz[2]) + for k, val in zip(("x", "y", "z"), world_xyz): + if not str(pose_overrides[k]).strip(): + target_pose[k] = float(val) + real_xyz_used = True + else: + print( + f"[tank_scene] pose_source={pose_source} but no row for " + f"target='{target}' in {poses_dir}/summary_static.csv — " + f"falling back to YAML." + ) + float_sdf = os.path.join(BLENDER_MODELS_DIR, "float", "model.sdf") target_sdf = os.path.join(BLENDER_MODELS_DIR, target, "model.sdf") for path in (float_sdf, target_sdf): @@ -156,9 +230,12 @@ def launch_setup(context, *args, **kwargs): print( f"[tank_scene] sensor={sensor} (namespace={profile['namespace']}) " - f"target={target} " - f"pose=({target_pose['x']:.3f}, {target_pose['y']:.3f}, {target_pose['z']:.3f}) " - f"rpy=({target_pose['roll']:.4f}, {target_pose['pitch']:.4f}, {target_pose['yaw']:.4f})" + f"target={target} pose_source={pose_source}" + f"{' (CSV)' if real_xyz_used else ' (YAML)'}\n" + f" pose=({target_pose['x']:.3f}, {target_pose['y']:.3f}, " + f"{target_pose['z']:.3f}) " + f"rpy=({target_pose['roll']:.4f}, {target_pose['pitch']:.4f}, " + f"{target_pose['yaw']:.4f})" ) tank_sim = IncludeLaunchDescription( @@ -187,9 +264,49 @@ def launch_setup(context, *args, **kwargs): float_spawner = _spawn_node("float", float_sdf, float_pose) target_spawner = _spawn_node(target, target_sdf, target_pose) + # Stagger the spawns and push them past Gazebo startup. The GUI and the + # server-side rendering sensors (gpu_lidar / cameras) use *separate* + # gz-rendering scenes; a model created in the same step the Sensors system + # is still building its scene can land in the GUI scene but be missed by the + # sensor scene -> visible in Gazebo but invisible to the lidar. Spawning + # after the sensor scene is up, one model per timer, avoids that race. + # Bump the periods if Gazebo is slow to initialize on your machine. + float_spawner = TimerAction(period=5.0, actions=[float_spawner]) + target_spawner = TimerAction(period=7.0, actions=[target_spawner]) + nodes = [tank_sim, float_spawner, target_spawner] if sensor == "lidar": nodes += _lidar_nodes(sensor_pose) + + if pose_source == "real_trajectory": + bag = LaunchConfiguration("bag").perform(context).strip() + if not bag: + raise RuntimeError( + "pose_source=real_trajectory requires bag:= " + "(e.g. apr22_petg_circle_moving)." + ) + csv_path = os.path.join(poses_dir, f"{bag}.csv") + if not os.path.isfile(csv_path): + raise RuntimeError(f"Trajectory CSV not found: {csv_path}") + player = Node( + package="sonar_3d_demo", + executable="play_object_trajectory.py", + name="play_object_trajectory", + arguments=[ + "--csv", csv_path, + "--model_name", target, + "--world", "default", + "--sensor_x", str(sensor_pose["x"]), + "--sensor_y", str(sensor_pose["y"]), + "--sensor_z", str(sensor_pose["z"]), + "--rate", LaunchConfiguration("rate").perform(context), + "--startup_delay_s", "8.0", + ], + output="screen", + ) + # Start after the spawner timer (period=7s). + nodes.append(TimerAction(period=9.0, actions=[player])) + return nodes @@ -217,5 +334,33 @@ def generate_launch_description(): DeclareLaunchArgument("paused", default_value="false"), DeclareLaunchArgument("debug", default_value="true"), DeclareLaunchArgument("verbosity_level", default_value="4"), + DeclareLaunchArgument( + "pose_source", + default_value="real_median", + description=( + "Where the target pose comes from: 'yaml' (config/targets.yaml), " + "'real_median' (median XYZ from summary_static.csv, YAML rotation), " + "or 'real_trajectory' (median XYZ at spawn, playback via " + "trajectory_player.launch.py)." + ), + ), + DeclareLaunchArgument( + "poses_dir", + default_value=DEFAULT_POSES_DIR, + description="Directory containing summary_static.csv and per-bag CSVs.", + ), + DeclareLaunchArgument( + "bag", + default_value="", + description=( + "When pose_source=real_trajectory, which per-bag CSV (basename " + "without .csv) to play back, e.g. apr22_petg_circle_moving." + ), + ), + DeclareLaunchArgument( + "rate", + default_value="10.0", + description="Trajectory playback rate (Hz). Higher = more gz service calls.", + ), ] return LaunchDescription(args + [OpaqueFunction(function=launch_setup)]) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/play_object_trajectory.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/play_object_trajectory.py new file mode 100755 index 00000000..99f68c65 --- /dev/null +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/scripts/play_object_trajectory.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +""" +Play back an object_top trajectory CSV by calling Gazebo's /world//set_pose. + +This is the runtime counterpart to extract_object_poses.py in sonar_camera_logger. +It takes a per-bag CSV (t_ns, t_rel_s, x, y, z, qx, qy, qz, qw, in sonar frame), +adds the sim-world offset for the sonar, drops obvious ArUco pose-flip glitches +with an 8·MAD filter, and republishes each pose as a Gazebo set_pose request at +the recorded inter-frame intervals. + +Why a script instead of a ROS node: + - No ROS-side state is needed; the only outputs are gz service calls. + - `gz service` already speaks the Gazebo Sim protocol fluently; reusing it + avoids depending on gz-transport python bindings being installed. + +Usage: + python3 play_object_trajectory.py \ + --csv /media/aki/2C76C6780AEDB4DB1/wl_wetlab_apr22_processed/object_poses/apr22_petg_circle_moving.csv \ + --model_name circle_petg \ + --world default \ + --sensor_x -0.95 --sensor_y 0.0 --sensor_z 0.5 \ + --rate 10 +""" + +import argparse +import csv +import shutil +import subprocess +import sys +import time +from pathlib import Path + +import numpy as np + + +def reject_outliers(xyz: np.ndarray, mad_scale: float = 8.0) -> np.ndarray: + med = np.median(xyz, axis=0) + mad = np.median(np.abs(xyz - med), axis=0) + 1e-6 + return np.all(np.abs(xyz - med) < mad_scale * mad, axis=1) + + +def load_csv(path: Path): + t_rel = [] + xyz = [] + quat = [] + with path.open() as f: + for row in csv.DictReader(f): + t_rel.append(float(row["t_rel_s"])) + xyz.append([float(row["x"]), float(row["y"]), float(row["z"])]) + quat.append([float(row["qx"]), float(row["qy"]), float(row["qz"]), float(row["qw"])]) + return np.array(t_rel), np.array(xyz), np.array(quat) + + +def gz_set_pose(world: str, name: str, x, y, z, qx, qy, qz, qw, timeout_ms: int = 200): + """Call /world//set_pose synchronously via the gz CLI. + + Returns True on success, False on failure (timeout, service missing, etc). + """ + req = ( + f'name: "{name}", ' + f'position: {{x: {x}, y: {y}, z: {z}}}, ' + f'orientation: {{x: {qx}, y: {qy}, z: {qz}, w: {qw}}}' + ) + cmd = [ + "gz", "service", + "-s", f"/world/{world}/set_pose", + "--reqtype", "gz.msgs.Pose", + "--reptype", "gz.msgs.Boolean", + "--timeout", str(timeout_ms), + "--req", req, + ] + try: + out = subprocess.run(cmd, capture_output=True, text=True, check=False) + except FileNotFoundError: + print("ERROR: 'gz' CLI not found in PATH.", file=sys.stderr) + return False + return out.returncode == 0 and "true" in (out.stdout or "").lower() + + +def main(): + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("--csv", required=True, help="Per-bag CSV from extract_object_poses.py") + ap.add_argument("--model_name", required=True, + help="Gazebo entity name to drive (same name passed to ros_gz_sim create).") + ap.add_argument("--world", default="default") + ap.add_argument("--sensor_x", type=float, default=0.0) + ap.add_argument("--sensor_y", type=float, default=0.0) + ap.add_argument("--sensor_z", type=float, default=0.0) + ap.add_argument("--rate", type=float, default=10.0, + help="Target set_pose rate in Hz. CSV is downsampled to this.") + ap.add_argument("--use_csv_orientation", action="store_true", + help="Use rotation from CSV. Default keeps spawn rotation (identity here).") + ap.add_argument("--qx", type=float, default=0.0, + help="Fixed quaternion x when --use_csv_orientation is off.") + ap.add_argument("--qy", type=float, default=0.0) + ap.add_argument("--qz", type=float, default=0.0) + ap.add_argument("--qw", type=float, default=1.0) + ap.add_argument("--time_rate", type=float, default=1.0, + help="Playback speed multiplier (>1 = faster than real time).") + ap.add_argument("--startup_delay_s", type=float, default=2.0, + help="Wait this long before the first set_pose (lets the spawn settle).") + args = ap.parse_args() + + if shutil.which("gz") is None: + print("ERROR: 'gz' CLI not on PATH. Source the Gazebo env first.", file=sys.stderr) + sys.exit(2) + + csv_path = Path(args.csv).expanduser().resolve() + if not csv_path.is_file(): + print(f"ERROR: CSV not found: {csv_path}", file=sys.stderr) + sys.exit(2) + + t_rel, xyz, quat = load_csv(csv_path) + if len(t_rel) == 0: + print("ERROR: CSV is empty.", file=sys.stderr) + sys.exit(2) + + keep = reject_outliers(xyz) + t_rel, xyz, quat = t_rel[keep], xyz[keep], quat[keep] + print(f"Loaded {len(t_rel)} inlier frames " + f"(from {len(keep)} total, dropped {(~keep).sum()} outliers).") + + # Downsample to target rate. + if args.rate > 0: + keep_idx = [0] + last_t = t_rel[0] + period = 1.0 / args.rate + for i in range(1, len(t_rel)): + if t_rel[i] - last_t >= period: + keep_idx.append(i) + last_t = t_rel[i] + t_rel = t_rel[keep_idx] + xyz = xyz[keep_idx] + quat = quat[keep_idx] + print(f"Downsampled to {len(t_rel)} frames at ~{args.rate} Hz.") + + # Apply sim_world translation. + xyz = xyz + np.array([args.sensor_x, args.sensor_y, args.sensor_z]) + + if args.startup_delay_s > 0: + print(f"Waiting {args.startup_delay_s}s before first set_pose ...") + time.sleep(args.startup_delay_s) + + t0_wall = time.monotonic() + t0_csv = t_rel[0] + failures = 0 + for i in range(len(t_rel)): + target_wall = t0_wall + (t_rel[i] - t0_csv) / max(args.time_rate, 1e-6) + sleep_for = target_wall - time.monotonic() + if sleep_for > 0: + time.sleep(sleep_for) + + if args.use_csv_orientation: + qx, qy, qz, qw = quat[i] + else: + qx, qy, qz, qw = args.qx, args.qy, args.qz, args.qw + + ok = gz_set_pose( + args.world, args.model_name, + xyz[i, 0], xyz[i, 1], xyz[i, 2], + qx, qy, qz, qw, + ) + if not ok: + failures += 1 + if failures == 1: + print("WARN: first set_pose failed — is the model spawned yet?", file=sys.stderr) + + print(f"Playback done. {len(t_rel) - failures}/{len(t_rel)} set_pose calls succeeded.") + + +if __name__ == "__main__": + main() From 9ae4509dede078851f5b0f1f3d26a1395b7d6818 Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 27 May 2026 10:26:38 +0200 Subject: [PATCH 07/12] feat(tank_scene): enhance float pose handling with CLI overrides and update launch arguments --- .../sonar_3d_demo/config/targets.yaml | 114 ++++++++++-------- .../sonar_3d_demo/launch/tank_scene.launch.py | 36 +++++- 2 files changed, 97 insertions(+), 53 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml index 53376af2..ee2f442d 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml @@ -10,11 +10,22 @@ # CLI overrides on the target (x, y, z, roll, pitch, yaw, target) take # precedence over the value here. The sonar and float poses are not # overridable from the CLI — change them in this YAML. +# +# Float ↔ target alignment: by default tank_scene.launch.py sets the float's +# x and y to the chosen target's x and y at spawn time, so the float sits +# directly above the target (centered on the same Z column). Each target may +# override this by setting its own `float_x` and `float_y` keys (see below). +# CLI args `float_x:=` and `float_y:=` win over both. +# +# Target positions below are the medians extracted from the Apr 22 wet-lab +# bags via sonar_camera_logger/scripts/extract_object_poses.py and converted +# from the sonar frame to the sim world by adding scene.sonar_3d.{x,y,z}. +# See docs/msc_docs_md/object_poses_report.md. scene: float: - x: 0.65 - y: 0.0 + x: 0.65 # Overridden at launch to match target.x (see header). + y: 0.0 # Overridden at launch to match target.y. z: 0.95 roll: 0.0 pitch: 0.0 @@ -37,80 +48,83 @@ scene: yaw: 0.0 targets: - brick: - x: 0.6 - y: 0.0 - z: 0.3 + # ── Targets with real-data medians (Apr 22 static bags) ───────────────── + brick: # bag: 1_apr22_static_brick + x: 0.738 # sonar + 1.688 + y: 0.012 # sonar + 0.012 + z: 0.414 # sonar + (-0.086) roll: 1.5708 pitch: 0.0 yaw: 1.5708 - circle_metal: - x: 0.65 - y: 0.0 - z: 0.24 + circle_petg: # bag: apr22_petg_circle_static + x: 0.629 # sonar + 1.579 + y: 0.044 # sonar + 0.044 + z: 0.339 # sonar + (-0.161) roll: 0.0 pitch: 0.0 yaw: 1.5708 - circle_petg: - x: 0.65 - y: 0.0 - z: 0.26 + circle_wood: # bag: apr22_wood_circle_static + x: 0.615 # sonar + 1.565 + y: -0.034 # sonar + (-0.034) + z: 0.355 # sonar + (-0.145) roll: 0.0 pitch: 0.0 yaw: 1.5708 - circle_wood: - x: 0.65 - y: 0.0 - z: 0.26 + metal_board: # bag: 3_apr22_metal_sheet_static (median; bag has pose-flips) + x: 0.752 # sonar + 1.702 + y: 0.149 # sonar + 0.149 + z: 0.318 # sonar + (-0.182) roll: 0.0 pitch: 0.0 yaw: 1.5708 - metal_board: - x: 0.65 - y: 0.0 - z: 0.285 + square_petg: # bag: apr22_petg_square_static (median; bag has pose-flips) + x: 0.519 # sonar + 1.469 + y: 0.100 # sonar + 0.100 + z: 0.349 # sonar + (-0.151) roll: 0.0 pitch: 0.0 yaw: 1.5708 - square_metal: - x: 0.65 - y: 0.0 - z: 0.2650 + square_wood: # bag: apr22_wood_square_static (median; bag has pose-flips) + x: 0.663 # sonar + 1.613 + y: 0.107 # sonar + 0.107 + z: 0.337 # sonar + (-0.163) roll: 0.0 pitch: 0.0 yaw: 1.5708 - square_petg: - x: 0.65 - y: 0.0 - z: 0.26 - roll: 0.0 + triangle_petg: # bag: apr22_petg_triangle_static + x: 0.569 # sonar + 1.519 + y: 0.054 # sonar + 0.054 + z: 0.336 # sonar + (-0.164) + roll: 3.14159 pitch: 0.0 yaw: 1.5708 - square_wood: - x: 0.65 - y: 0.0 - z: 0.23 - roll: 0.0 + triangle_wood: # bag: apr22_wood_triangle_static + x: 0.624 # sonar + 1.574 + y: 0.007 # sonar + 0.007 + z: 0.333 # sonar + (-0.167) + roll: 3.14159 pitch: 0.0 yaw: 1.5708 - triangle_metal: - x: 0.61 - y: -0.15 - z: 0.15 - roll: 3.14159 + + # ── Metal targets — real-data medians from chain-suspended-sheet bags ─── + circle_metal: # bag: apr22_chain_circle_static + x: 0.655 # sonar + 1.605 + y: -0.032 # sonar + (-0.032) + z: 0.376 # sonar + (-0.124) + roll: 0.0 pitch: 0.0 yaw: 1.5708 - triangle_petg: - x: 0.6 - y: 0.0 - z: 0.17 - roll: 3.14159 + square_metal: # bag: apr22_chain_square_static + x: 0.677 # sonar + 1.627 + y: -0.048 # sonar + (-0.048) + z: 0.388 # sonar + (-0.112) + roll: 0.0 pitch: 0.0 yaw: 1.5708 - triangle_wood: - x: 0.6 - y: 0.0 - z: 0.1 + triangle_metal: # bag: apr22_chain_triangle_static + x: 0.581 # sonar + 1.531 + y: 0.029 # sonar + 0.029 + z: 0.382 # sonar + (-0.118) roll: 3.14159 pitch: 0.0 yaw: 1.5708 diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py index e5e84b3a..cde5da05 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py @@ -193,7 +193,7 @@ def launch_setup(context, *args, **kwargs): f"(needed for sensor:={sensor})" ) sensor_pose = scene[profile["yaml_key"]] - float_pose = scene["float"] + float_pose = dict(scene["float"]) # mutable copy — x/y are overridden below target_yaml = manifest["targets"][target] # YAML baseline pose (rotations always come from here — see module docstring). @@ -228,14 +228,31 @@ def launch_setup(context, *args, **kwargs): if not os.path.isfile(path): raise RuntimeError(f"SDF not found: {path}") + # Float x/y resolution, in order of precedence: + # 1. CLI float_x:= / float_y:= (non-empty wins) + # 2. Per-target `float_x` / `float_y` keys in targets.yaml + # 3. Default to the target's own x / y (center-aligned on the Z axis) + cli_fx = LaunchConfiguration("float_x").perform(context).strip() + cli_fy = LaunchConfiguration("float_y").perform(context).strip() + yaml_fx = target_yaml.get("float_x") + yaml_fy = target_yaml.get("float_y") + float_pose["x"] = float(cli_fx) if cli_fx else ( + float(yaml_fx) if yaml_fx is not None else target_pose["x"] + ) + float_pose["y"] = float(cli_fy) if cli_fy else ( + float(yaml_fy) if yaml_fy is not None else target_pose["y"] + ) + print( f"[tank_scene] sensor={sensor} (namespace={profile['namespace']}) " f"target={target} pose_source={pose_source}" f"{' (CSV)' if real_xyz_used else ' (YAML)'}\n" - f" pose=({target_pose['x']:.3f}, {target_pose['y']:.3f}, " + f" target=({target_pose['x']:.3f}, {target_pose['y']:.3f}, " f"{target_pose['z']:.3f}) " f"rpy=({target_pose['roll']:.4f}, {target_pose['pitch']:.4f}, " - f"{target_pose['yaw']:.4f})" + f"{target_pose['yaw']:.4f})\n" + f" float =({float_pose['x']:.3f}, {float_pose['y']:.3f}, " + f"{float_pose['z']:.3f}) (x,y track target)" ) tank_sim = IncludeLaunchDescription( @@ -362,5 +379,18 @@ def generate_launch_description(): default_value="10.0", description="Trajectory playback rate (Hz). Higher = more gz service calls.", ), + DeclareLaunchArgument( + "float_x", + default_value="", + description=( + "Override the float's x (m). Empty = use target.float_x from " + "targets.yaml if set, otherwise the target's own x." + ), + ), + DeclareLaunchArgument( + "float_y", + default_value="", + description="Override the float's y (m). Same precedence as float_x.", + ), ] return LaunchDescription(args + [OpaqueFunction(function=launch_setup)]) From cc73261d132a5502c6ab2f88f120c314fa6b2d4c Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 27 May 2026 12:01:50 +0200 Subject: [PATCH 08/12] feat(tank_scene): add float_z handling for improved target alignment and CLI overrides --- .../sonar_3d_demo/config/targets.yaml | 20 +++++++++++++-- .../sonar_3d_demo/launch/tank_scene.launch.py | 25 ++++++++++++++++--- 2 files changed, 39 insertions(+), 6 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml index ee2f442d..cfa4251c 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml @@ -14,8 +14,13 @@ # Float ↔ target alignment: by default tank_scene.launch.py sets the float's # x and y to the chosen target's x and y at spawn time, so the float sits # directly above the target (centered on the same Z column). Each target may -# override this by setting its own `float_x` and `float_y` keys (see below). -# CLI args `float_x:=` and `float_y:=` win over both. +# override this by setting its own `float_x`, `float_y`, and `float_z` keys. +# CLI args `float_x:=`, `float_y:=`, `float_z:=` win over both. +# +# Per-target `float_z` values below are chosen so the float-model's bottom +# (its origin is ~55 cm above the model's lowest vertex) clears the target's +# top vertex by ~5 cm. Compute new values with the procedure in +# docs/msc_docs_md/object_poses_report.md (Float-clearance section). # # Target positions below are the medians extracted from the Apr 22 wet-lab # bags via sonar_camera_logger/scripts/extract_object_poses.py and converted @@ -56,6 +61,7 @@ targets: roll: 1.5708 pitch: 0.0 yaw: 1.5708 + float_z: 1.119 # TODO circle_petg: # bag: apr22_petg_circle_static x: 0.629 # sonar + 1.579 y: 0.044 # sonar + 0.044 @@ -63,6 +69,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0313 circle_wood: # bag: apr22_wood_circle_static x: 0.615 # sonar + 1.565 y: -0.034 # sonar + (-0.034) @@ -70,6 +77,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0447 metal_board: # bag: 3_apr22_metal_sheet_static (median; bag has pose-flips) x: 0.752 # sonar + 1.702 y: 0.149 # sonar + 0.149 @@ -77,6 +85,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 0.9864 square_petg: # bag: apr22_petg_square_static (median; bag has pose-flips) x: 0.519 # sonar + 1.469 y: 0.100 # sonar + 0.100 @@ -84,6 +93,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0450 square_wood: # bag: apr22_wood_square_static (median; bag has pose-flips) x: 0.663 # sonar + 1.613 y: 0.107 # sonar + 0.107 @@ -91,6 +101,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0644 triangle_petg: # bag: apr22_petg_triangle_static x: 0.569 # sonar + 1.519 y: 0.054 # sonar + 0.054 @@ -98,6 +109,7 @@ targets: roll: 3.14159 pitch: 0.0 yaw: 1.5708 + float_z: 1.193 # TODO triangle_wood: # bag: apr22_wood_triangle_static x: 0.624 # sonar + 1.574 y: 0.007 # sonar + 0.007 @@ -105,6 +117,7 @@ targets: roll: 3.14159 pitch: 0.0 yaw: 1.5708 + float_z: 1.301 # TODO # ── Metal targets — real-data medians from chain-suspended-sheet bags ─── circle_metal: # bag: apr22_chain_circle_static @@ -114,6 +127,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0998 square_metal: # bag: apr22_chain_square_static x: 0.677 # sonar + 1.627 y: -0.048 # sonar + (-0.048) @@ -121,6 +135,7 @@ targets: roll: 0.0 pitch: 0.0 yaw: 1.5708 + float_z: 1.0674 triangle_metal: # bag: apr22_chain_triangle_static x: 0.581 # sonar + 1.531 y: 0.029 # sonar + 0.029 @@ -128,3 +143,4 @@ targets: roll: 3.14159 pitch: 0.0 yaw: 1.5708 + float_z: 1.274 # TODO diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py index cde5da05..b0af7e04 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py @@ -228,20 +228,28 @@ def launch_setup(context, *args, **kwargs): if not os.path.isfile(path): raise RuntimeError(f"SDF not found: {path}") - # Float x/y resolution, in order of precedence: - # 1. CLI float_x:= / float_y:= (non-empty wins) - # 2. Per-target `float_x` / `float_y` keys in targets.yaml - # 3. Default to the target's own x / y (center-aligned on the Z axis) + # Float x/y/z resolution, in order of precedence: + # 1. CLI float_x:= / float_y:= / float_z:= (non-empty wins) + # 2. Per-target `float_x` / `float_y` / `float_z` keys in targets.yaml + # 3. For x/y: default to the target's own x/y (center-aligned). + # For z: default to scene.float.z (kept from float_pose dict). cli_fx = LaunchConfiguration("float_x").perform(context).strip() cli_fy = LaunchConfiguration("float_y").perform(context).strip() + cli_fz = LaunchConfiguration("float_z").perform(context).strip() yaml_fx = target_yaml.get("float_x") yaml_fy = target_yaml.get("float_y") + yaml_fz = target_yaml.get("float_z") float_pose["x"] = float(cli_fx) if cli_fx else ( float(yaml_fx) if yaml_fx is not None else target_pose["x"] ) float_pose["y"] = float(cli_fy) if cli_fy else ( float(yaml_fy) if yaml_fy is not None else target_pose["y"] ) + if cli_fz: + float_pose["z"] = float(cli_fz) + elif yaml_fz is not None: + float_pose["z"] = float(yaml_fz) + # else: float_pose["z"] stays at scene.float.z (the dict's original value). print( f"[tank_scene] sensor={sensor} (namespace={profile['namespace']}) " @@ -392,5 +400,14 @@ def generate_launch_description(): default_value="", description="Override the float's y (m). Same precedence as float_x.", ), + DeclareLaunchArgument( + "float_z", + default_value="", + description=( + "Override the float's z (m). Precedence: CLI > target.float_z > " + "scene.float.z. Per-target float_z is set so the float bottom " + "clears the target top by 5 cm." + ), + ), ] return LaunchDescription(args + [OpaqueFunction(function=launch_setup)]) From 550647d8bd79dc2a9153a7f48b4d985deaffcfa4 Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 27 May 2026 13:29:06 +0200 Subject: [PATCH 09/12] fix(targets): update float_z values for improved target alignment and add csv_overlay handling --- .../sonar_3d_demo/config/targets.yaml | 17 +++++++++---- .../sonar_3d_demo/launch/tank_scene.launch.py | 24 +++++++++++++++---- 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml index cfa4251c..38324465 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml @@ -61,7 +61,7 @@ targets: roll: 1.5708 pitch: 0.0 yaw: 1.5708 - float_z: 1.119 # TODO + float_z: 1.0664 circle_petg: # bag: apr22_petg_circle_static x: 0.629 # sonar + 1.579 y: 0.044 # sonar + 0.044 @@ -109,7 +109,7 @@ targets: roll: 3.14159 pitch: 0.0 yaw: 1.5708 - float_z: 1.193 # TODO + float_z: 1.1425 triangle_wood: # bag: apr22_wood_triangle_static x: 0.624 # sonar + 1.574 y: 0.007 # sonar + 0.007 @@ -117,7 +117,7 @@ targets: roll: 3.14159 pitch: 0.0 yaw: 1.5708 - float_z: 1.301 # TODO + float_z: 1.2549 # ── Metal targets — real-data medians from chain-suspended-sheet bags ─── circle_metal: # bag: apr22_chain_circle_static @@ -137,10 +137,17 @@ targets: yaw: 1.5708 float_z: 1.0674 triangle_metal: # bag: apr22_chain_triangle_static + # Mesh origin is offset +0.150 m in Y from the visible centroid. We + # spawn the target 0.150 m below in Y so the mesh centre lands at the + # bag-derived pose (0.581, 0.029). `csv_overlay: false` prevents + # tank_scene.launch.py from re-overwriting target.y from the CSV. x: 0.581 # sonar + 1.531 - y: 0.029 # sonar + 0.029 + y: -0.1214 # bag y (0.029) − mesh Y offset (0.150) = −0.121 z: 0.382 # sonar + (-0.118) roll: 3.14159 pitch: 0.0 yaw: 1.5708 - float_z: 1.274 # TODO + csv_overlay: false # YAML pose hand-tuned for mesh offset — keep it + float_x: 0.581 # = bag x + float_y: 0.0286 # = bag y (where the mesh visible centre lands) + float_z: 1.224 diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py index b0af7e04..55e52fdc 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/launch/tank_scene.launch.py @@ -205,8 +205,14 @@ def launch_setup(context, *args, **kwargs): # sim_world_T_object = sim_world_T_sonar (translation) + sonar_T_object (CSV), # which assumes sensor_pose has identity rotation — true for the current # targets.yaml. CLI overrides on x/y/z (non-empty pose_overrides) still win. + # + # `csv_overlay: false` on a target opts that target out of the CSV overlay + # entirely. Use this when the YAML x/y/z have been hand-tuned (e.g. to + # compensate for an off-centre mesh origin) and must not be replaced by the + # raw bag medians. real_xyz_used = False - if pose_source in ("real_median", "real_trajectory"): + csv_overlay = bool(target_yaml.get("csv_overlay", True)) + if pose_source in ("real_median", "real_trajectory") and csv_overlay: real_xyz = _lookup_real_median(target, poses_dir) if real_xyz is not None: sx, sy, sz = sensor_pose["x"], sensor_pose["y"], sensor_pose["z"] @@ -221,6 +227,11 @@ def launch_setup(context, *args, **kwargs): f"target='{target}' in {poses_dir}/summary_static.csv — " f"falling back to YAML." ) + elif not csv_overlay: + print( + f"[tank_scene] csv_overlay=false for target='{target}' — using " + f"hand-tuned YAML position verbatim." + ) float_sdf = os.path.join(BLENDER_MODELS_DIR, "float", "model.sdf") target_sdf = os.path.join(BLENDER_MODELS_DIR, target, "model.sdf") @@ -251,16 +262,21 @@ def launch_setup(context, *args, **kwargs): float_pose["z"] = float(yaml_fz) # else: float_pose["z"] stays at scene.float.z (the dict's original value). + if real_xyz_used: + pose_tag = " (CSV)" + elif not csv_overlay: + pose_tag = " (YAML, csv_overlay=false)" + else: + pose_tag = " (YAML)" print( f"[tank_scene] sensor={sensor} (namespace={profile['namespace']}) " - f"target={target} pose_source={pose_source}" - f"{' (CSV)' if real_xyz_used else ' (YAML)'}\n" + f"target={target} pose_source={pose_source}{pose_tag}\n" f" target=({target_pose['x']:.3f}, {target_pose['y']:.3f}, " f"{target_pose['z']:.3f}) " f"rpy=({target_pose['roll']:.4f}, {target_pose['pitch']:.4f}, " f"{target_pose['yaw']:.4f})\n" f" float =({float_pose['x']:.3f}, {float_pose['y']:.3f}, " - f"{float_pose['z']:.3f}) (x,y track target)" + f"{float_pose['z']:.3f})" ) tank_sim = IncludeLaunchDescription( From 7166058c658e43c1b9c7090f931a2edd5ed01e5e Mon Sep 17 00:00:00 2001 From: ppakr Date: Fri, 29 May 2026 19:45:13 +0200 Subject: [PATCH 10/12] fix(sonar): update sonar frequency to 1.2 MHz per WaterLinked specifications --- .../sonar_3d/generate_3d_sonar.py | 3 +- models/dave_sensor_models/3d_sonar/model.sdf | 192 ++++++++++++------ 2 files changed, 130 insertions(+), 65 deletions(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d/generate_3d_sonar.py b/gazebo/dave_gz_multibeam_sonar/sonar_3d/generate_3d_sonar.py index 0bc1624b..02290804 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d/generate_3d_sonar.py +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d/generate_3d_sonar.py @@ -134,7 +134,8 @@ {multibeam_vertical_fov_deg} - 900e3 + + 1.2e6 29.9e3 1500 220 diff --git a/models/dave_sensor_models/3d_sonar/model.sdf b/models/dave_sensor_models/3d_sonar/model.sdf index 56d6eca0..2a08e58a 100644 --- a/models/dave_sensor_models/3d_sonar/model.sdf +++ b/models/dave_sensor_models/3d_sonar/model.sdf @@ -61,7 +61,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -106,7 +107,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -151,7 +153,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -196,7 +199,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -241,7 +245,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -286,7 +291,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -331,7 +337,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -376,7 +383,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -421,7 +429,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -466,7 +475,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -511,7 +521,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -556,7 +567,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -601,7 +613,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -646,7 +659,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -691,7 +705,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -736,7 +751,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -781,7 +797,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -826,7 +843,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -871,7 +889,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -916,7 +935,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -961,7 +981,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1006,7 +1027,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1051,7 +1073,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1096,7 +1119,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1141,7 +1165,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1186,7 +1211,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1231,7 +1257,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1276,7 +1303,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1321,7 +1349,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1366,7 +1395,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1411,7 +1441,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1456,7 +1487,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1501,7 +1533,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1546,7 +1579,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1591,7 +1625,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1636,7 +1671,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1681,7 +1717,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1726,7 +1763,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1771,7 +1809,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1816,7 +1855,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1861,7 +1901,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1906,7 +1947,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1951,7 +1993,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -1996,7 +2039,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2041,7 +2085,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2086,7 +2131,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2131,7 +2177,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2176,7 +2223,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2221,7 +2269,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2266,7 +2315,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2311,7 +2361,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2356,7 +2407,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2401,7 +2453,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2446,7 +2499,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2491,7 +2545,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2536,7 +2591,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2581,7 +2637,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2626,7 +2683,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2671,7 +2729,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2716,7 +2775,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2761,7 +2821,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2806,7 +2867,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2851,7 +2913,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 @@ -2896,7 +2959,8 @@ 1.6 - 900e3 + + 1.2e6 29.9e3 1500 220 From 5913331fbae9ef52ef45f05d54126f5a1a66424f Mon Sep 17 00:00:00 2001 From: ppakr Date: Fri, 29 May 2026 19:45:50 +0200 Subject: [PATCH 11/12] feat(sonar): add per-beam intensity-cull threshold for noise reduction --- .../sonar_3d/src/Sonar3D.cc | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc b/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc index b7029c9a..8007784e 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc @@ -34,6 +34,33 @@ class SonarAggregator : public rclcpp::Node { received_.fill(false); + // Per-beam intensity-cull threshold (sim dB, NOT WaterLinked's 0–51 dB scale). + // + // Beams whose peak intensity falls below this value are dropped from the + // output cloud — i.e. this is a noise-FLOOR cull, not a saturation ceiling. + // + // The kernel emits intensity as 10·log10(power) (== 20·log10(amplitude); the + // two formulas agree numerically). The slope vs `` is calibrated + // to +10 dB/decade (Phases A+B). However the *zero point* of this dB scale + // is the sim's raw scatter level — NOT WL's receiver noise floor — so the + // numeric range here (currently ~70–95 dB on the tank scene) is offset from + // WL's strength image's 0–51 dB scale by an unknown constant tied to + // constMu + the absent TVG model. + // + // Calibration recipe (Phase C/D, see docs/msc_docs_md/material_reflectivity_phase_c_notes.md): + // 1. Launch sonar_3d_demo in the empty-tank world. + // 2. Watch the per-msg log line `Sonar %d first msg: beam dB range [..., ...]` + // across all 64 sub-sonars; take the global max as `reverb_floor_sim_dB`. + // 3. Set this parameter to `reverb_floor_sim_dB + ~3 dB` so reverb-only + // beams are culled but real targets pass. + // Override at runtime with: `--ros-args -p intensity_threshold_db:=`. + this->declare_parameter("intensity_threshold_db", 70.0); + intensity_threshold_db_ = + static_cast(this->get_parameter("intensity_threshold_db").as_double()); + RCLCPP_INFO( + this->get_logger(), "Per-beam intensity-cull threshold: %.1f dB (sim scale)", + intensity_threshold_db_); + pc_pub_ = this->create_publisher("/sensor/sonar_3d/pointcloud", 10); @@ -253,7 +280,7 @@ class SonarAggregator : public rclcpp::Node // Find the range index of maximum intensity for each beam (column). std::vector max_indices(beam_count, 0); std::vector max_values(beam_count, 0.0f); - float threshold{85.0f}; + const float threshold = intensity_threshold_db_; for (uint32_t beam = 0; beam < beam_count; ++beam) { double max_val = -1.0; @@ -348,6 +375,7 @@ class SonarAggregator : public rclcpp::Node std::array received_; size_t received_count_; std_msgs::msg::Header latest_header_; + float intensity_threshold_db_; }; int main(int argc, char * argv[]) From 1c7a0514f4b83e09dbde38d702ef04cd87d1cbf1 Mon Sep 17 00:00:00 2001 From: ppakr Date: Wed, 3 Jun 2026 16:26:15 +0200 Subject: [PATCH 12/12] fix(sonar): update intensity threshold to 85 dB for improved target detection --- gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc b/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc index 8007784e..74665713 100644 --- a/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc +++ b/gazebo/dave_gz_multibeam_sonar/sonar_3d/src/Sonar3D.cc @@ -54,7 +54,7 @@ class SonarAggregator : public rclcpp::Node // 3. Set this parameter to `reverb_floor_sim_dB + ~3 dB` so reverb-only // beams are culled but real targets pass. // Override at runtime with: `--ros-args -p intensity_threshold_db:=`. - this->declare_parameter("intensity_threshold_db", 70.0); + this->declare_parameter("intensity_threshold_db", 85.0); intensity_threshold_db_ = static_cast(this->get_parameter("intensity_threshold_db").as_double()); RCLCPP_INFO(