Dev/material reflectivity

.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)$'
+        files: '\.(yaml|yml)$'

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

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/MultibeamSonarSensor.cc

@@ -200,15 +200,15 @@
   // Check if this sensor is of the right type
   if (_sdf.Type() != sdf::SensorType::CUSTOM)
   {
     gzerr << "Expected [" << this->Name() << "] sensor to be "
           << "a Multibeam Sonar but found a " << _sdf.TypeStr() << "." << std::endl;
     return false;
   }
 
   sdf::ElementPtr elem = _sdf.Element();
   if (!elem->HasAttribute("gz:type"))
   {
     gzerr << "Missing 'gz:type' attribute "
           << "for sensor [" << this->Name() << "]. "
           << "Aborting load." << std::endl;
     return false;
@@ -216,14 +216,14 @@
   const auto type = elem->Get<std::string>("gz:type");
   if (type != "multibeam_sonar")
   {
     gzerr << "Expected sensor [" << this->Name() << "] to be a "
           << "multibeam_sonar but it is of '" << type << "' type. Aborting load." << std::endl;
     return false;
   }
   if (!elem->HasElement("gz:multibeam_sonar"))
   {
     gzerr << "Missing 'gz:multibeam_sonar' configuration for "
           << "sensor [" << this->Name() << "]. "
           << "Aborting load." << std::endl;
     return false;
   }
@@ -276,8 +276,8 @@
 
   if (!this->InitializeBeamArrangement(_sensor))
   {
     gzerr << "Failed to initialize beam arrangement for "
           << "[" << _sensor->Name() << "] sensor." << std::endl;
     return false;
   }
 
@@ -300,8 +300,8 @@
   this->raySensor = _sensor->Scene()->CreateGpuRays(_sensor->Name() + "_ray_sensor");
   if (!this->raySensor)
   {
     gzerr << "Failed to create Ray (GPU Ray) sensor for "
           << "for [" << _sensor->Name() << "] sensor." << std::endl;
     return false;
   }
 
@@ -311,15 +311,15 @@
 
   if (!rayElement)
   {
     gzerr << "No beam properties(format of GPU Ray) specified for "
           << "[" << _sensor->Name() << "] sensor" << std::endl;
     return false;
   }
 
   if (!sensorElement)
   {
     gzerr << "No sonar properties(format of GPU Ray) specified for "
           << "[" << _sensor->Name() << "] sensor" << std::endl;
     return false;
   }
 
@@ -436,8 +436,8 @@
     this->writeInterval = sensorElement->Get<int>("writeFrameInterval", 10).first;
 
     RCLCPP_INFO_STREAM(
       this->ros_node_->get_logger(), "Raw data at " << "/SonarRawData_{numbers}.csv"
                                                     << " every " << this->writeInterval
                                                     << " frames");
     RCLCPP_INFO_STREAM(
       this->ros_node_->get_logger(), "Beam angles at SonarRawData_beam_angles.csv");
@@ -616,9 +616,9 @@
   this->pointMsg.set_height(this->raySensor->VerticalRangeCount());
   this->pointMsg.set_row_step(this->pointMsg.point_step() * this->pointMsg.width());
 
   this->rayConnection = this->raySensor->ConnectNewGpuRaysFrame(
     std::bind(
       &MultibeamSonarSensor::Implementation::OnNewFrame, this, std::placeholders::_1,
       std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
 
   // Transmission path properties (typical model used here)
@@ -699,7 +699,7 @@
   }
   this->beamCorrectorSum = 0.0;
 
-  this->constMu = true;
+  this->constMu = false;
   this->mu = 1e-3;
 
   return true;
@@ -761,8 +761,8 @@
     {
       if (!this->dataPtr->Initialize(this))
       {
         gzerr << "Failed to initialize "
               << "[" << this->Name() << "]"
               << " sensor." << std::endl;
       }
     }
@@ -814,7 +814,7 @@
 
   if (!this->dataPtr->worldState)
   {
     gzwarn << "No world state available, "
            << "cannot estimate velocities." << std::endl;
     return;
   }
@@ -926,6 +926,7 @@
   // 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_<float> iter_image = this->pointCloudImage.begin<float>();
 
   bool angles_calculation_flag = false;
@@ -955,9 +956,11 @@
       // 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<float>(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 @@
     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();

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

gazebo/dave_gz_multibeam_sonar/sonar_3d/generate_3d_sonar.py

@@ -52,7 +52,7 @@
   <version>1.0</version>
   <sdf version="1.9">model.sdf</sdf>
   <description>
     {multibeam_vertical_num_sensors} multibeam sonars stacked with {sonar_3d_vertical_seperation_deg} degree elevation difference.
     Horizontal FOV: 90 deg, Vertical FOV: 40 deg.
   </description>
 </model>
@@ -134,7 +134,8 @@
         </ray>
           <spec>
             <verticalFOV>{multibeam_vertical_fov_deg}</verticalFOV>
-            <sonarFreq>900e3</sonarFreq>
+            <!-- WaterLinked Sonar 3D-15 Navigation mode = 1.2 MHz (per WL spec). -->
+            <sonarFreq>1.2e6</sonarFreq>
             <bandwidth>29.9e3</bandwidth>
             <soundSpeed>1500</soundSpeed>
             <sourceLevel>220</sourceLevel>

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 `<laser_retro>` 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:=<value>`.
+    this->declare_parameter<double>("intensity_threshold_db", 85.0);
+    intensity_threshold_db_ =
+      static_cast<float>(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_msgs::msg::PointCloud2>("/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<uint32_t> max_indices(beam_count, 0);
     std::vector<float> 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<bool, NUM_SONARS> received_;
   size_t received_count_;
   std_msgs::msg::Header latest_header_;
+  float intensity_threshold_db_;
 };
 
 int main(int argc, char * argv[])

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()

gazebo/dave_gz_multibeam_sonar/sonar_3d_demo/config/targets.yaml

@@ -10,11 +10,27 @@
 # 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`, `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
+# 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 +53,101 @@ 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
+    float_z: 1.0664      
+  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
+    float_z: 1.0313       
+  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
+    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
+    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
+    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
+    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.27
+    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
+    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
+    float_z: 1.0644      
+  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
+    float_z: 1.1425      
+  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
+    float_z: 1.2549       
+
+  # ── 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
+    float_z: 1.0998      
+  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
+    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.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
+    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