[ROS2] Add support for lidar intensity (o3de#737)

Signed-off-by: Aleksander Kamiński <[email protected]>
RobotecAI · Aug 7, 2024 · 7b860fc · 7b860fc
1 parent ce584e2
commit 7b860fc
Show file tree

Hide file tree

Showing 7 changed files with 86 additions and 29 deletions.
diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h
@@ -88,6 +88,7 @@ namespace ROS2
     {
         Points = (1 << 0), //!< return 3D point coordinates
         Ranges = (1 << 1), //!< return array of distances
+        Intensities = (1 << 2), //!< return intensity data
     };
 
     //! Bitwise operators for RaycastResultFlags
@@ -97,6 +98,7 @@ namespace ROS2
     {
         AZStd::vector<AZ::Vector3> m_points;
         AZStd::vector<float> m_ranges;
+        AZStd::vector<float> m_intensities;
     };
 
     //! Interface class that allows for communication with a single Lidar instance.

diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h
@@ -22,6 +22,7 @@ namespace ROS2
         EntityExclusion         = 1 << 2,
         MaxRangePoints          = 1 << 3,
         PointcloudPublishing    = 1 << 4,
+        Intensity               = 1 << 5,
         All                     = 0b1111111111111111,
     };
 

diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp
@@ -75,6 +75,10 @@ namespace ROS2
         }
 
         RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges | RaycastResultFlags::Points;
+        if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity)
+        {
+            requestedFlags |= RaycastResultFlags::Intensities;
+        }
 
         LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags);
 
@@ -161,8 +165,10 @@ namespace ROS2
         return m_lidarRaycasterId;
     }
 
-    RaycastResult LidarCore::PerformRaycast()
+    const RaycastResult& LidarCore::PerformRaycast()
     {
+        static const RaycastResult EmptyResults{};
+
         AZ::Entity* entity = nullptr;
         AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId);
         const auto entityTransform = entity->FindComponent<AzFramework::TransformComponent>();
@@ -172,7 +178,7 @@ namespace ROS2
         if (m_lastScanResults.m_points.empty())
         {
             AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n");
-            return RaycastResult();
+            return EmptyResults;
         }
         return m_lastScanResults;
     }

diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.h b/Gems/ROS2/Code/Source/Lidar/LidarCore.h
@@ -37,7 +37,7 @@ namespace ROS2
 
         //! Perform a raycast.
         //! @return Results of the raycast.
-        RaycastResult PerformRaycast();
+        const RaycastResult& PerformRaycast();
         //! Visualize the results of the last performed raycast.
         void VisualizeResults() const;
 

diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp
@@ -108,7 +108,7 @@ namespace ROS2
 
     void ROS2Lidar2DSensorComponent::FrequencyTick()
     {
-        RaycastResult lastScanResults = m_lidarCore.PerformRaycast();
+        const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast();
 
         auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
         auto message = sensor_msgs::msg::LaserScan();

diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp
@@ -12,6 +12,7 @@
 #include <Lidar/ROS2LidarSensorComponent.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Utilities/ROS2Names.h>
+#include <sensor_msgs/point_cloud2_iterator.hpp>
 
 namespace ROS2
 {
@@ -131,9 +132,7 @@ namespace ROS2
 
     void ROS2LidarSensorComponent::FrequencyTick()
     {
-        auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
-
-        if (m_canRaycasterPublish)
+        if (m_canRaycasterPublish && m_sensorConfiguration.m_publishingEnabled)
         {
             const builtin_interfaces::msg::Time timestamp = ROS2Interface::Get()->GetROSTimestamp();
             LidarRaycasterRequestBus::Event(
@@ -142,43 +141,91 @@ namespace ROS2
                 aznumeric_cast<AZ::u64>(timestamp.sec) * aznumeric_cast<AZ::u64>(1.0e9f) + timestamp.nanosec);
         }
 
-        auto lastScanResults = m_lidarCore.PerformRaycast();
+        const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast();
 
-        if (m_canRaycasterPublish)
+        if (m_canRaycasterPublish || !m_sensorConfiguration.m_publishingEnabled)
         { // Skip publishing when it can be handled by the raycaster.
             return;
         }
 
-        const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();
-        for (auto& point : lastScanResults.m_points)
-        {
-            point = inverseLidarTM.TransformPoint(point);
-        }
+        PublishRaycastResults(lastScanResults);
+    }
+
+    void ROS2LidarSensorComponent::PublishRaycastResults(const RaycastResult& results)
+    {
+        const bool isIntensityEnabled = m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity;
 
         auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
         auto message = sensor_msgs::msg::PointCloud2();
         message.header.frame_id = ros2Frame->GetFrameID().data();
         message.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
         message.height = 1;
-        message.width = lastScanResults.m_points.size();
-        message.point_step = sizeof(AZ::Vector3);
-        message.row_step = message.width * message.point_step;
+        message.width = results.m_points.size();
+
+        sensor_msgs::PointCloud2Modifier modifier(message);
+        if (isIntensityEnabled)
+        {
+            modifier.setPointCloud2Fields(
+                4,
+                "x",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32,
+                "y",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32,
+                "z",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32,
+                "intensity",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32);
+        }
+        else
+        {
+            modifier.setPointCloud2Fields(
+                3,
+                "x",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32,
+                "y",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32,
+                "z",
+                1,
+                sensor_msgs::msg::PointField::FLOAT32);
+        }
+        modifier.resize(results.m_points.size());
+
+        sensor_msgs::PointCloud2Iterator<float> xIt(message, "x");
+        sensor_msgs::PointCloud2Iterator<float> yIt(message, "y");
+        sensor_msgs::PointCloud2Iterator<float> zIt(message, "z");
+
+        AZStd::optional<sensor_msgs::PointCloud2Iterator<float>> intensityIt;
+        if (isIntensityEnabled)
+        {
+            intensityIt = sensor_msgs::PointCloud2Iterator<float>(message, "intensity");
+        }
 
-        AZStd::array<const char*, 3> point_field_names = { "x", "y", "z" };
-        for (int i = 0; i < point_field_names.size(); i++)
+        const auto entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
+        const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse();
+        for (size_t i = 0; i < results.m_points.size(); ++i)
         {
-            sensor_msgs::msg::PointField pf;
-            pf.name = point_field_names[i];
-            pf.offset = i * 4;
-            pf.datatype = sensor_msgs::msg::PointField::FLOAT32;
-            pf.count = 1;
-            message.fields.push_back(pf);
+            const AZ::Vector3 globalPoint = inverseLidarTM.TransformPoint(results.m_points[i]);
+            *xIt = globalPoint.GetX();
+            *yIt = globalPoint.GetY();
+            *zIt = globalPoint.GetZ();
+
+            if (isIntensityEnabled)
+            {
+                *intensityIt.value() = results.m_intensities[i];
+                ++intensityIt.value();
+            }
+
+            ++xIt;
+            ++yIt;
+            ++zIt;
         }
 
-        size_t sizeInBytes = lastScanResults.m_points.size() * sizeof(AZ::Vector3);
-        message.data.resize(sizeInBytes);
-        AZ_Assert(message.row_step * message.height == sizeInBytes, "Inconsistency in the size of point cloud data");
-        memcpy(message.data.data(), lastScanResults.m_points.data(), sizeInBytes);
         m_pointCloudPublisher->publish(message);
     }
 } // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h
@@ -44,6 +44,7 @@ namespace ROS2
     private:
         //////////////////////////////////////////////////////////////////////////
         void FrequencyTick();
+        void PublishRaycastResults(const RaycastResult& results);
 
         bool m_canRaycasterPublish = false;
         std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::PointCloud2>> m_pointCloudPublisher;