Merge pull request #15 from IOES-Lab/sonar_ws_progress

Multibeam Sonar Migration Template

Author: hmoyen

.docker/jazzy.amd64.dockerfile

@@ -66,6 +66,15 @@ ADD https://raw.githubusercontent.com/IOES-Lab/dave/$BRANCH/\
 extras/repos/dave.$ROS_DISTRO.repos $DAVE_WS/dave.repos
 RUN vcs import --shallow --input $DAVE_WS/dave.repos
 
+# FIX ROS GPG KEY error (may be temporary)
+RUN rm /etc/apt/sources.list.d/ros2.list && apt update && apt install -y jq && rm -rf /var/lib/apt/lists/
+RUN UBUNTU_CODENAME=noble && \
+    ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | jq -r '.tag_name') && \
+    curl -L -o /tmp/ros2-apt-source.deb \
+    "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.${UBUNTU_CODENAME}_all.deb" && \
+    apt-get install -y /tmp/ros2-apt-source.deb && \
+    rm -f /tmp/ros2-apt-source.deb
+
 # Install dave dependencies
 RUN apt-get update && rosdep update && \
     rosdep install -iy --from-paths . && \

.pre-commit-config.yaml

@@ -28,7 +28,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
         args: ["--maxkb=2000"]
@@ -48,7 +48,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.16.0
+    rev: v3.19.1
     hooks:
       - id: pyupgrade
         args: [--py36-plus]
@@ -62,20 +62,23 @@ repos:
           ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.4.2
+    rev: 24.10.0
     hooks:
       - id: black
         args: ["--line-length=99", "--exclude=tools/code_check"]
 
   - repo: https://github.com/pycqa/flake8
-    rev: 7.1.0
+    rev: 7.1.1
     hooks:
       - id: flake8
-        args: ["--extend-ignore=E501", "--exclude=tools/code_check"]
+        args:
+          - "--ignore=E203,W503,E501"
+          - "--exclude=tools/code_check"
+
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v18.1.7
+    rev: v19.1.6
     hooks:
       - id: clang-format
         args: ["-fallback-style=none", "-i"]
@@ -126,7 +129,7 @@ repos:
 
   # Docs - RestructuredText hooks
   - repo: https://github.com/PyCQA/doc8
-    rev: v1.1.1
+    rev: v1.1.2
     hooks:
       - id: doc8
         args: ["--max-line-length=100", "--ignore=D001"]
@@ -151,7 +154,7 @@ repos:
 
   # Json lint
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.28.6
+    rev: 0.31.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]
@@ -162,7 +165,7 @@ repos:
 
   # XML lint for xacro, urdf, and sdf files
   - repo: https://github.com/pamoller/xmlformatter
-    rev: v0.2.6
+    rev: v0.2.8
     hooks:
       - id: xml-formatter
         name: Format XML

dave_interfaces/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
   <name>dave_interfaces</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Topic and Service Message Formats</description>
   <maintainer email="helenamoyen@usp.br">lena</maintainer>
   <maintainer email="gaurav.og.9920@gmail.com">Gaurav Kumar</maintainer>
   <license>Apache 2.0</license>

examples/dave_demos/launch/dave_sensor.launch.py

@@ -12,7 +12,7 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration("use_sim_time")
     debug = LaunchConfiguration("debug")
     headless = LaunchConfiguration("headless")
-    verbose = LaunchConfiguration("verbose")
+    verbosity_level = LaunchConfiguration("verbosity_level")
     namespace = LaunchConfiguration("namespace")
     world_name = LaunchConfiguration("world_name")
     x = LaunchConfiguration("x")
@@ -38,8 +38,7 @@ def launch_setup(context, *args, **kwargs):
     if paused.perform(context) == "false":
         gz_args.append(" -r")
     if debug.perform(context) == "true":
-        gz_args.append(" -v ")
-        gz_args.append(verbose.perform(context))
+        gz_args.append(f"-v {verbosity_level.perform(context)}")
 
     gz_sim_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -117,9 +116,9 @@ def generate_launch_description():
             description="Flag to enable the gazebo headless mode",
         ),
         DeclareLaunchArgument(
-            "verbose",
-            default_value="0",
-            description="Adjust level of console verbosity",
+            "verbosity_level",
+            default_value="1",
+            description="Verbosity level for Gazebo if debug is enabled (0-4)",
         ),
         DeclareLaunchArgument(
             "world_name",

gazebo/dave_gz_multibeam_sonar/README.md

@@ -0,0 +1,10 @@
+# DAVE Multibeam Sonar Plugin
+
+The DAVE multibeam sonar plugin, adapted for ROS 2 Jazzy and Gazebo Harmonic, uses a ray-based multibeam model that simulates phase, reverberation, and speckle noise through a point scattering approach. It generates realistic intensity-range (A-plot) data while accounting for time and angular ambiguities as well as speckle noise.
+
+> **System Requirement**:
+> An **NVIDIA graphics card with CUDA compatibility** is required to use the sonar plugin. This enables GPU-accelerated simulation for real-time performance.
+
+**Documentation**
+
+The documentation for the plugin and tutorial can be found here: [Wiki](https://dave-ros2.notion.site/Multibeam-Sonar-Plugin-223661362ab2803b873bda4878fc55a8)

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/AcousticBeam.hh

@@ -0,0 +1,76 @@
+#ifndef GZ_SENSORS_ACOUSTICBEAM_HH_
+#define GZ_SENSORS_ACOUSTICBEAM_HH_
+
+#include <cmath>
+#include <gz/math/Angle.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Quaternion.hh>
+#include <gz/math/Vector2.hh>
+#include <gz/math/Vector3.hh>
+#include <string>
+#include "AxisAlignedPatch2.hh"
+
+namespace gz::sensors
+{
+
+class AcousticBeam
+{
+public:
+  AcousticBeam(
+    int _id, const gz::math::Angle & _apertureAngle, const gz::math::Angle & _rotationAngle,
+    const gz::math::Angle & _tiltAngle)
+  : id(_id),
+    apertureAngle(_apertureAngle),
+    normalizedRadius(std::atan(_apertureAngle.Radian() / 2.))
+  {
+    using Quaterniond = gz::math::Quaterniond;
+
+    // Rotação extrínseca XY (rot_x * rot_y)
+    this->transform.Rot() = Quaterniond::EulerToQuaternion(_rotationAngle.Radian(), 0., 0.) *
+                            Quaterniond::EulerToQuaternion(0., _tiltAngle.Radian(), 0.);
+
+    this->axis = this->transform.Rot() * gz::math::Vector3d::UnitX;
+
+    const gz::math::Angle azimuthAngle = std::atan2(this->axis.Y(), this->axis.X());
+    const gz::math::Angle inclinationAngle = std::atan2(
+      this->axis.Z(), std::sqrt(this->axis.X() * this->axis.X() + this->axis.Y() * this->axis.Y()));
+
+    const gz::math::Vector2d topLeft{
+      (azimuthAngle + _apertureAngle / 2.).Radian(),
+      (inclinationAngle + _apertureAngle / 2.).Radian()};
+
+    const gz::math::Vector2d bottomRight{
+      (azimuthAngle - _apertureAngle / 2.).Radian(),
+      (inclinationAngle - _apertureAngle / 2.).Radian()};
+
+    this->sphericalFootprint = AxisAlignedPatch2d{topLeft, bottomRight};
+  }
+
+  int Id() const { return this->id; }
+  const gz::math::Pose3d & Transform() const { return this->transform; }
+  const gz::math::Vector3d & Axis() const { return this->axis; }
+  double NormalizedRadius() const { return this->normalizedRadius; }
+  const gz::math::Angle & ApertureAngle() const { return this->apertureAngle; }
+  const AxisAlignedPatch2d & SphericalFootprint() const { return this->sphericalFootprint; }
+
+private:
+  int id;
+  gz::math::Angle apertureAngle;
+  double normalizedRadius;
+  gz::math::Pose3d transform;
+  gz::math::Vector3d axis;
+  AxisAlignedPatch2d sphericalFootprint;
+};
+
+/// \brief Acoustic beam reflecting target.
+/// Pose is defined w.r.t. the beams frame.
+struct ObjectTarget
+{
+  gz::math::Pose3d pose;
+  uint64_t entity;
+  std::string name;
+};
+
+}  // namespace gz::sensors
+
+#endif  // GZ_SENSORS_ACOUSTIC_BEAM_HH_

gazebo/dave_gz_multibeam_sonar/multibeam_sonar/AxisAlignedPatch2.hh

@@ -0,0 +1,86 @@
+#ifndef GZ_SENSORS_AXISALIGNEDPATCH2_HH_
+#define GZ_SENSORS_AXISALIGNEDPATCH2_HH_
+
+#include <algorithm>
+#include <gz/math/Vector2.hh>
+
+namespace gz::sensors
+{
+
+/// \brief Axis-aligned patch on a plane, using image frame conventions.
+template <typename T>
+class AxisAlignedPatch2
+{
+public:
+  AxisAlignedPatch2() = default;
+
+  AxisAlignedPatch2(
+    const gz::math::Vector2<T> & _topLeft, const gz::math::Vector2<T> & _bottomRight)
+  : topLeft(_topLeft), bottomRight(_bottomRight)
+  {
+  }
+
+  /// Scalar converting copy constructor
+  template <typename U>
+  AxisAlignedPatch2(const AxisAlignedPatch2<U> & _other)
+  {
+    this->topLeft.X(static_cast<T>(_other.XMax()));
+    this->topLeft.Y(static_cast<T>(_other.YMax()));
+    this->bottomRight.X(static_cast<T>(_other.XMin()));
+    this->bottomRight.Y(static_cast<T>(_other.YMin()));
+  }
+
+  T XMax() const { return this->topLeft.X(); }
+  T XMin() const { return this->bottomRight.X(); }
+  T XSize() const { return this->XMax() - this->XMin(); }
+
+  T YMax() const { return this->topLeft.Y(); }
+  T YMin() const { return this->bottomRight.Y(); }
+  T YSize() const { return this->YMax() - this->YMin(); }
+
+  /// Merge patch with `_other`. Returns *this
+  AxisAlignedPatch2<T> & Merge(const AxisAlignedPatch2<T> & _other)
+  {
+    this->topLeft.Set(
+      std::max(this->topLeft.X(), _other.topLeft.X()),
+      std::max(this->topLeft.Y(), _other.topLeft.Y()));
+    this->bottomRight.Set(
+      std::min(this->bottomRight.X(), _other.bottomRight.X()),
+      std::min(this->bottomRight.Y(), _other.bottomRight.Y()));
+    return *this;
+  }
+
+  AxisAlignedPatch2<T> Flip() const { return {-this->bottomRight, -this->topLeft}; }
+
+  const AxisAlignedPatch2<T> operator*(gz::math::Vector2<T> _vector) const
+  {
+    return {this->topLeft * _vector, this->bottomRight * _vector};
+  }
+
+  const AxisAlignedPatch2<T> operator/(gz::math::Vector2<T> _vector) const
+  {
+    return {this->topLeft / _vector, this->bottomRight / _vector};
+  }
+
+  const AxisAlignedPatch2<T> operator+(gz::math::Vector2<T> _vector) const
+  {
+    return {this->topLeft + _vector, this->bottomRight + _vector};
+  }
+
+  const AxisAlignedPatch2<T> operator-(gz::math::Vector2<T> _vector) const
+  {
+    return {this->topLeft - _vector, this->bottomRight - _vector};
+  }
+
+private:
+  gz::math::Vector2<T> topLeft;
+  gz::math::Vector2<T> bottomRight;
+};
+
+// Handy typedefs
+using AxisAlignedPatch2d = AxisAlignedPatch2<double>;
+using AxisAlignedPatch2i = AxisAlignedPatch2<int>;
+
+}  // namespace gz::sensors
+
+#endif  // GZ_SENSORS_AXISALIGNEDPATCH2_HH_