Merge pull request #514 from gazebosim/scpeters/merge_9_main

Merge gz-sensors9 ➡️  main

Author: iche033

Changelog.md

@@ -4,6 +4,17 @@
 
 ## Gazebo Sensors 9
 
+### Gazebo Sensors 9.1.0 (2025-02-12)
+
+1. Enable packing w/o padding for pointclouds
+    * [Pull request #493](https://github.com/gazebosim/gz-sensors/pull/493)
+
+1. Add support for L8 and L16 image formats in WideAngleCameraSensor
+    * [Pull request #494](https://github.com/gazebosim/gz-sensors/pull/494)
+
+1. Check camera resolution
+    * [Pull request #480](https://github.com/gazebosim/gz-sensors/pull/480)
+
 ### Gazebo Sensors 9.0.0 (2024-09-25)
 
 1. **Baseline:** this includes all changes from 8.2.0 and earlier.

README.md

@@ -43,6 +43,28 @@ configured at run time to mimic specific real-world sensors. A noise model
 is also provided that can be used to introduce Gaussian or custom noise
 models into sensor streams.
 
+## Supported Sensors
+
+| **Sensor Name**           | **API Link**                                                                                                 | **SDF Element**                                                                                      | **Notes**                                          |
+|---------------------------|-------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------|----------------------------------------------------|
+| **Air Pressure Sensor**   | [AirPressureSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1AirPressureSensor.html)        | [`<air_pressure>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_air_pressure)                | Measures atmospheric pressure                      |
+| **Air Speed Sensor**      | [AirSpeedSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1AirSpeedSensor.html)              | N/A                                                                                    | Measures the speed of air relative to the sensor    |
+| **Altimeter**             | [AltimeterSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1AltimeterSensor.html)            | [`<altimeter>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_altimeter)                      | Measures altitude above a reference point          |
+| **Bounding Box Camera**   | [BoundingBoxCameraSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1BoundingBoxCameraSensor.html) | [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera)                         | Captures images with bounding box annotations      |
+| **Camera**                | [CameraSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1CameraSensor.html)                  | [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera)                           | Captures standard RGB images                       |
+| **Depth Camera**          | [DepthCameraSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1DepthCameraSensor.html)        | [`<depth_camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#camera_depth_camera)              | Captures depth information                         |
+| **Doppler Velocity Log**  | [DopplerVelocityLog](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1DopplerVelocityLog.html)      | N/A                                | Measures relative velocity of an underwater vehicle |
+| **Force-Torque**          | [ForceTorqueSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1ForceTorqueSensor.html)        | [`<force_torque>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_force_torque)              | Measures forces and torques on a joint             |
+| **GPU Lidar**             | [GpuLidarSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1GpuLidarSensor.html)              | [`<lidar>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_lidar)                            | Simulates a 3D laser scanner using GPU acceleration|
+| **IMU**                   | [ImuSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1ImuSensor.html)                        | [`<imu>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_imu)                                | Measures linear acceleration and angular velocity  |
+| **Logical Camera**        | [LogicalCameraSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1LogicalCameraSensor.html)    | [`<logical_camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_logical_camera)          | Detects models within a specified volume           |
+| **Magnetometer**          | [MagnetometerSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1MagnetometerSensor.html)      | [`<magnetometer>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_magnetometer)              | Measures magnetic field strength and direction     |
+| **NavSat (GPS)**          | [NavSatSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1NavSatSensor.html)                  | [`<navsat>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_navsat)                          | Simulates GPS-like positioning sensor              |
+| **RGBD Camera**           | [RGBDCameraSensor](https://gazebosim.org/api/sensors/7/classgz_1_1sensors_1_1RgbdCameraSensor.html)          | [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera) | Captures RGB + Depth streams                       |
+| **Segmentation Camera**   | [SegmentationCameraSensor](https://gazebosim.org/api/sensors/9/classgz_1_1sensors_1_1SegmentationCameraSensor.html) | [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera)              | Captures labeled segmentation images               |
+| **Thermal Camera**        | [ThermalCameraSensor](https://gazebosim.org/api/sensors/7/classgz_1_1sensors_1_1ThermalCameraSensor.html)    | [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera)                         | Detects heat signature                             |
+| **Wide Angle Camera**     | [WideAngleCameraSensor](https://gazebosim.org/api/sensors/8/classgz_1_1sensors_1_1WideAngleCameraSensor.html)| [`<camera>`](http://sdformat.org/spec?ver=1.12&elem=sensor#sensor_camera)                         | Captures wide field of view                        |
+
 # Install
 
 See the [installation tutorial](https://gazebosim.org/api/sensors/9/installation.html).
@@ -51,6 +73,8 @@ See the [installation tutorial](https://gazebosim.org/api/sensors/9/installation
 
 Please refer to the [examples directory](https://github.com/gazebosim/gz-sensors/tree/main/examples).
 
+A list of sensors and SDF examples can be found in the [SDF specification](http://sdformat.org/spec?ver=1.12&elem=sensor)
+
 # Folder Structure
 
 Refer to the following table for information about important directories and files in this repository.

include/gz/sensors/LogicalCameraSensor.hh

@@ -104,6 +104,14 @@ namespace gz
       /// \return True if there are subscribers, false otherwise
       public: virtual bool HasConnections() const override;
 
+      /// \brief Check if there are any image subscribers
+      /// \return True if there are image subscribers, false otherwise
+      public: virtual bool HasImageConnections() const;
+
+      /// \brief Check if there are any frustum subscribers
+      /// \return True if there are info subscribers, false otherwise
+      public: virtual bool HasFrustumConnections() const;
+
       /// \brief Get the latest image. An image is an instance of
       /// msgs::LogicalCameraImage, which contains a list of detected models.
       /// \return List of detected models.

src/DepthCameraSensor.cc

@@ -439,14 +439,10 @@ bool DepthCameraSensor::CreateCamera()
         std::placeholders::_4, std::placeholders::_5));
 
   // Initialize the point message.
-  // \todo(anyone) The true value in the following function call forces
-  // the xyz and rgb fields to be aligned to memory boundaries. This is need
-  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
-  // alignment should be configured.
   msgs::InitPointCloudPacked(
         this->dataPtr->pointMsg,
         this->OpticalFrameId(),
-        true,
+        false,
         {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
          {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
 

src/GpuLidarSensor.cc

@@ -129,12 +129,7 @@ bool GpuLidarSensor::Load(const sdf::Sensor &_sdf)
   }
 
   // Initialize the point message.
-  // \todo(anyone) The true value in the following function call forces
-  // the xyz and rgb fields to be aligned to memory boundaries. This is need
-  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
-  // alignment should be configured. This same problem is in the
-  // RgbdCameraSensor.
-  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), true,
+  msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->FrameId(), false,
       {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
       {"intensity", msgs::PointCloudPacked::Field::FLOAT32},
       {"ring", msgs::PointCloudPacked::Field::UINT16}});

src/LogicalCameraSensor.cc

@@ -36,9 +36,15 @@ class gz::sensors::LogicalCameraSensorPrivate
   /// \brief node to create publisher
   public: transport::Node node;
 
+  /// \brief node to create publisher for frustum
+  public: transport::Node nodeLogic;
+
   /// \brief publisher to publish logical camera messages.
   public: transport::Node::Publisher pub;
 
+  /// \brief Publisher to publish logical camera frustum information
+  public: transport::Node::Publisher pubLogic;
+
   /// \brief true if Load() has been called and was successful
   public: bool initialized = false;
 
@@ -55,7 +61,10 @@ class gz::sensors::LogicalCameraSensorPrivate
   public: std::map<std::string, math::Pose3d> models;
 
   /// \brief Msg containg info on models detected by logical camera
-  msgs::LogicalCameraImage msg;
+  public: msgs::LogicalCameraImage msg;
+
+  /// \brief Msg containing logical camera frustum info
+  public: msgs::LogicalCameraSensor msgLogic;
 };
 
 //////////////////////////////////////////////////
@@ -110,12 +119,23 @@ bool LogicalCameraSensor::Load(sdf::ElementPtr _sdf)
       this->dataPtr->node.Advertise<msgs::LogicalCameraImage>(
       this->Topic());
 
+  this->dataPtr->pubLogic =
+      this->dataPtr->nodeLogic.Advertise<msgs::LogicalCameraSensor>(
+      this->Topic() + "/frustum");
+
   if (!this->dataPtr->pub)
   {
     gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n";
     return false;
   }
 
+  if (!this->dataPtr->pubLogic)
+  {
+    gzerr << "Unable to create publisher on topic[" << this->Topic()
+          << "/frustum].\n";
+    return false;
+  }
+
   gzdbg << "Logical images for [" << this->Name() << "] advertised on ["
          << this->Topic() << "]" << std::endl;
 
@@ -166,9 +186,25 @@ bool LogicalCameraSensor::Update(
   frame->set_key("frame_id");
   frame->add_value(this->FrameId());
 
+  *this->dataPtr->msgLogic.mutable_header()->mutable_stamp() =
+    msgs::Convert(_now);
+  this->dataPtr->msgLogic.mutable_header()->clear_data();
+  auto frame_log = this->dataPtr->msgLogic.mutable_header()->add_data();
+
+  frame_log->set_key("frame_id");
+  frame_log->add_value(this->FrameId());
+
   // publish
+  this->dataPtr->msgLogic.set_near_clip(this->dataPtr->frustum.Near());
+  this->dataPtr->msgLogic.set_far_clip(this->dataPtr->frustum.Far());
+  this->dataPtr->msgLogic.set_horizontal_fov(
+    this->dataPtr->frustum.FOV().Radian());
+  this->dataPtr->msgLogic.set_aspect_ratio(
+    this->dataPtr->frustum.AspectRatio());
   this->AddSequence(this->dataPtr->msg.mutable_header());
+
   this->dataPtr->pub.Publish(this->dataPtr->msg);
+  this->dataPtr->pubLogic.Publish(this->dataPtr->msgLogic);
 
   return true;
 }
@@ -206,7 +242,18 @@ msgs::LogicalCameraImage LogicalCameraSensor::Image() const
 
 //////////////////////////////////////////////////
 bool LogicalCameraSensor::HasConnections() const
+{
+  return this->HasImageConnections() || this->HasFrustumConnections();
+}
+
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::HasImageConnections() const
 {
   return this->dataPtr->pub && this->dataPtr->pub.HasConnections();
 }
 
+//////////////////////////////////////////////////
+bool LogicalCameraSensor::HasFrustumConnections() const
+{
+  return this->dataPtr->pubLogic && this->dataPtr->pubLogic.HasConnections();
+}

src/RgbdCameraSensor.cc

@@ -395,12 +395,8 @@ bool RgbdCameraSensor::CreateCameras()
         std::placeholders::_4, std::placeholders::_5));
 
   // Initialize the point message.
-  // \todo(anyone) The true value in the following function call forces
-  // the xyz and rgb fields to be aligned to memory boundaries. This is need
-  // by ROS1: https://github.com/ros/common_msgs/pull/77. Ideally, memory
-  // alignment should be configured.
   msgs::InitPointCloudPacked(this->dataPtr->pointMsg, this->OpticalFrameId(),
-      true,
+      false,
       {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
        {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
 

test/integration/gpu_lidar_sensor.cc

@@ -431,10 +431,10 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine)
   EXPECT_EQ(static_cast<uint32_t>(vertSamples), pointMsgs.back().height());
   EXPECT_EQ(static_cast<uint32_t>(horzSamples), pointMsgs.back().width());
   EXPECT_FALSE(pointMsgs.back().is_bigendian());
-  EXPECT_EQ(32u, pointMsgs.back().point_step());
-  EXPECT_EQ(32u * horzSamples, pointMsgs.back().row_step());
+  EXPECT_EQ(18u, pointMsgs.back().point_step());
+  EXPECT_EQ(18u * horzSamples, pointMsgs.back().row_step());
   EXPECT_FALSE(pointMsgs.back().is_dense());
-  EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size());
+  EXPECT_EQ(18u * horzSamples * vertSamples, pointMsgs.back().data().size());
 
   EXPECT_TRUE(pointMsgs.back().has_header());
   EXPECT_LT(1, pointMsgs.back().header().data().size());

test/integration/rgbd_camera.cc

@@ -475,7 +475,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF(
   {
     // init the point cloud msg to be filled
     msgs::PointCloudPacked pointsMsg;
-    msgs::InitPointCloudPacked(pointsMsg, "depth2Image", true,
+    msgs::InitPointCloudPacked(pointsMsg, "depth2Image", false,
         {{"xyz", msgs::PointCloudPacked::Field::FLOAT32},
          {"rgb", msgs::PointCloudPacked::Field::FLOAT32}});
     pointsMsg.set_width(rgbdSensor->ImageWidth());