Rework RGBD sensor. (#117)

* Rework RGBD sensor.

The RGBD sensor is running only one pipeline with this change.

Signed-off-by: Michał Pełka <[email protected]>

* Adjusted to review

Signed-off-by: Michał Pełka <[email protected]>

* Apply suggestions from code review, adjusted code to recent API changes.

Co-authored-by: Steve Pham <[email protected]>
Signed-off-by: Michał Pełka <[email protected]>

---------

Signed-off-by: Michał Pełka <[email protected]>
Signed-off-by: Michał Pełka <[email protected]>
Co-authored-by: Steve Pham <[email protected]>

Gems/ROS2/Assets/Passes/PipelineRenderToTextureROSColor.pass

@@ -4,7 +4,7 @@
     "ClassName": "PassAsset",
     "ClassData": {
         "PassTemplate": {
-            "Name": "PipelineRenderToTextureROSDepth",
+            "Name": "PipelineRenderToTextureROSColor",
             "PassClass": "RenderToTexturePass",
             "PassData": {
                 "$type": "RenderToTexturePassData",

Gems/ROS2/Code/Source/Camera/CameraSensor.cpp

@@ -52,7 +52,7 @@ namespace ROS2
         };
 
     } // namespace Internal
-    CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height)
+    CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height, AZ::EntityId entityId)
         : m_verticalFieldOfViewDeg(verticalFov)
         , m_width(width)
         , m_height(height)
@@ -60,6 +60,7 @@ namespace ROS2
         , m_aspectRatio(static_cast<float>(width) / static_cast<float>(height))
         , m_viewToClipMatrix(MakeViewToClipMatrix())
         , m_cameraIntrinsics(MakeCameraIntrinsics())
+        , m_entityId(entityId)
     {
         ValidateParameters();
     }
@@ -116,11 +117,11 @@ namespace ROS2
         m_view->SetViewToClipMatrix(m_cameraSensorDescription.m_viewToClipMatrix);
         m_scene = AZ::RPI::RPISystemInterface::Get()->GetSceneByName(AZ::Name("Main"));
 
-        const AZStd::string pipelineName = m_cameraSensorDescription.m_cameraName + "Pipeline" + GetPipelineTypeName();
-
+        m_pipelineName = AZStd::string::format("%sPipeline%s%s",m_cameraSensorDescription.m_cameraName.c_str(), GetPipelineTypeName().c_str(),
+                                               m_cameraSensorDescription.m_entityId.ToString().c_str() );
         AZ::RPI::RenderPipelineDescriptor pipelineDesc;
         pipelineDesc.m_mainViewTagName = "MainCamera";
-        pipelineDesc.m_name = pipelineName;
+        pipelineDesc.m_name = m_pipelineName;
 
         pipelineDesc.m_rootPassTemplate = GetPipelineTemplateName();
 
@@ -135,7 +136,7 @@ namespace ROS2
 
         m_scene->AddRenderPipeline(m_pipeline);
 
-        m_passHierarchy.push_back(pipelineName);
+        m_passHierarchy.push_back(m_pipelineName);
         m_passHierarchy.push_back("CopyToSwapChain");
 
         m_pipeline->SetDefaultView(m_view);
@@ -198,20 +199,34 @@ namespace ROS2
             cameraPose,
             [header, publisher](const AZ::RPI::AttachmentReadback::ReadbackResult& result)
             {
-                const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor;
-                const auto format = descriptor.m_format;
-                AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast<uint32_t>(format));
-                sensor_msgs::msg::Image message;
-                message.encoding = Internal::FormatMappings.at(format);
-                message.width = descriptor.m_size.m_width;
-                message.height = descriptor.m_size.m_height;
-                message.step = message.width * Internal::BitDepth.at(format);
-                message.data = std::vector<uint8_t>(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size());
-                message.header = header;
-                publisher->publish(message);
+                if (result.m_state == AZ::RPI::AttachmentReadback::ReadbackState::Success)
+                {
+                    const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor;
+                    const auto format = descriptor.m_format;
+                    AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast<uint32_t>(format));
+                    sensor_msgs::msg::Image message;
+                    message.encoding = Internal::FormatMappings.at(format);
+                    message.width = descriptor.m_size.m_width;
+                    message.height = descriptor.m_size.m_height;
+                    message.step = message.width * Internal::BitDepth.at(format);
+                    message.data = std::vector<uint8_t>(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size());
+                    message.header = header;
+                    publisher->publish(message);
+                }
             });
     }
 
+    void CameraSensor::RequestMessagePublication(
+        AZStd::span<std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> publishers,
+        const AZ::Transform& cameraPose,
+        const std_msgs::msg::Header& header)
+    {
+        if (!publishers.empty())
+        {
+            RequestMessagePublication(publishers.front(), cameraPose, header);
+        }
+    }
+
     CameraDepthSensor::CameraDepthSensor(const CameraSensorDescription& cameraSensorDescription)
         : CameraSensor(cameraSensorDescription)
     {
@@ -244,4 +259,51 @@ namespace ROS2
         return "Color";
     };
 
+    CameraRGBDSensor::CameraRGBDSensor(const CameraSensorDescription& cameraSensorDescription)
+        : CameraColorSensor(cameraSensorDescription)
+    {
+    }
+
+    void CameraRGBDSensor::ReadBackDepth(
+        AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback)
+    {
+            AZ::Render::FrameCaptureOutcome captureOutcome;
+            AZStd::vector<AZStd::string> passHierarchy{m_pipelineName,"DepthPrePass"};
+            AZ::Render::FrameCaptureRequestBus::BroadcastResult(
+                captureOutcome,
+                &AZ::Render::FrameCaptureRequestBus::Events::CapturePassAttachmentWithCallback,
+                callback,
+                passHierarchy,
+                AZStd::string("DepthLinear"),
+                AZ::RPI::PassAttachmentReadbackOption::Output);
+    }
+
+    void CameraRGBDSensor::RequestMessagePublication(
+        AZStd::span<std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> publishers,
+        const AZ::Transform& cameraPose,
+        const std_msgs::msg::Header& header)
+    {
+        AZ_Assert(publishers.size()==2, "RequestMessagePublication for CameraRGBDSensor should be called with exactly two publishers");
+        const auto publisherDepth = publishers.back();
+        ReadBackDepth(
+            [header, publisherDepth](const AZ::RPI::AttachmentReadback::ReadbackResult& result)
+            {
+                if (result.m_state == AZ::RPI::AttachmentReadback::ReadbackState::Success)
+                {
+                    const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor;
+                    const auto format = descriptor.m_format;
+                    AZ_Assert(Internal::FormatMappings.contains(format), "Unknown format in result %u", static_cast<uint32_t>(format));
+                    sensor_msgs::msg::Image message;
+                    message.encoding = Internal::FormatMappings.at(format);
+                    message.width = descriptor.m_size.m_width;
+                    message.height = descriptor.m_size.m_height;
+                    message.step = message.width * Internal::BitDepth.at(format);
+                    message.data = std::vector<uint8_t>(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size());
+                    message.header = header;
+                    publisherDepth->publish(message);
+                }
+            });
+        CameraSensor::RequestMessagePublication(publishers,cameraPose,header);
+    }
+
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/CameraSensor.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include <Atom/Feature/Utils/FrameCaptureBus.h>
+#include <AzCore/std/containers/span.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <chrono>
 #include <rclcpp/publisher.hpp>
@@ -25,7 +26,8 @@ namespace ROS2
         //! @param verticalFov - vertical field of view of camera sensor
         //! @param width - camera image width in pixels
         //! @param height - camera image height in pixels
-        CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height);
+        //! @param entityId - entityId of camera sensor
+        CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height, AZ::EntityId entityId);
 
         const float m_verticalFieldOfViewDeg; //!< camera vertical field of view
         const int m_width; //!< camera image width in pixels
@@ -35,7 +37,7 @@ namespace ROS2
         const float m_aspectRatio; //!< camera image aspect ratio; equal to (width / height)
         const AZ::Matrix4x4 m_viewToClipMatrix; //!< camera view to clip space transform matrix; derived from other parameters
         const AZStd::array<double, 9> m_cameraIntrinsics; //!< camera intrinsics; derived from other parameters
-
+        const AZ::EntityId m_entityId; //! Entity Id that is owning this sensor.
     private:
         AZ::Matrix4x4 MakeViewToClipMatrix() const;
 
@@ -57,11 +59,11 @@ namespace ROS2
         virtual ~CameraSensor();
 
         //! Publish Image Message frame from rendering pipeline
-        //! @param publisher - ROS2 publisher to publish image in future
+        //! @param publishers - ROS2 publishers to publish image in future : color, depth ...
         //! @param header - header with filled message information (frame, timestamp, seq)
         //! @param cameraPose - current camera pose from which the rendering should take place
-        void RequestMessagePublication(
-            std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> publisher,
+        virtual void RequestMessagePublication(
+            AZStd::span<std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> publishers,
             const AZ::Transform& cameraPose,
             const std_msgs::msg::Header& header);
 
@@ -69,16 +71,17 @@ namespace ROS2
         [[nodiscard]] const CameraSensorDescription& GetCameraSensorDescription() const;
 
     private:
-        //! Request a frame from the rendering pipeline
+        //! Publish Image Message frame from rendering pipeline
+        //! @param publisher - ROS2 publisher to publish image in future
+        //! @param header - header with filled message information (frame, timestamp, seq)
         //! @param cameraPose - current camera pose from which the rendering should take place
-        //! @param callback - callback function object that will be called when capture is ready
-        //!                   it's argument is readback structure containing, among other thins, captured image
-        void RequestFrame(
-            const AZ::Transform& cameraPose, AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback);
+        void RequestMessagePublication(
+            std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> publisher,
+            const AZ::Transform& cameraPose,
+            const std_msgs::msg::Header& header);
 
         CameraSensorDescription m_cameraSensorDescription;
         AZStd::vector<AZStd::string> m_passHierarchy;
-        AZ::RPI::RenderPipelinePtr m_pipeline;
         AZ::RPI::ViewPtr m_view;
         AZ::RPI::Scene* m_scene = nullptr;
         const AZ::Transform AtomToRos{ AZ::Transform::CreateFromQuaternion(
@@ -87,6 +90,26 @@ namespace ROS2
         virtual AZStd::string GetPipelineTypeName() const = 0; //! Type of returned data eg Color, Depth, Optical flow
 
     protected:
+        AZ::RPI::RenderPipelinePtr m_pipeline;
+        AZStd::string m_pipelineName;
+
+        //! Request a frame from the rendering pipeline
+        //! @param cameraPose - current camera pose from which the rendering should take place
+        //! @param callback - callback function object that will be called when capture is ready.
+        //!                   It's argument is readback structure containing, among other things, a captured image
+        void RequestFrame(
+            const AZ::Transform& cameraPose, AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback);
+
+        //! Request an additional frame from the rendering pipeline
+        //! @param callback - callback function object that will be called when capture is ready.
+        //!                   It's argument is readback structure containing, among other things, a captured image
+        //! @param passName - pass name in pipeline, eg `DepthToLinearDepthPass`
+        //! @param slotName - slot name in selected pass, eg `Output`
+        void RequestAdditionalFrame(
+            AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback,
+            const AZStd::string& passName,
+            const AZStd::string& slotName);
+
         //! Read and setup Atom Passes
         void SetupPasses();
     };
@@ -113,4 +136,19 @@ namespace ROS2
         AZStd::string GetPipelineTypeName() const override;
     };
 
+    //! Implementation of camera sensors that runs pipeline which produces color image and readbacks a depth image from pipeline
+    class CameraRGBDSensor : public CameraColorSensor
+    {
+    public:
+        CameraRGBDSensor(const CameraSensorDescription& cameraSensorDescription);
+
+        // CameraSensor overrides
+        void RequestMessagePublication(
+            AZStd::span<std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>>> publishers,
+            const AZ::Transform& cameraPose,
+            const std_msgs::msg::Header& header) override;
+
+    private:
+        void ReadBackDepth(AZStd::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback);
+    };
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -66,7 +66,7 @@ namespace ROS2
             ros2Node->create_publisher<sensor_msgs::msg::CameraInfo>(cameraInfoFullTopic.data(), cameraInfoPublisherConfig.GetQoS());
 
         const CameraSensorDescription description{
-            GetCameraNameFromFrame(GetEntity()), m_verticalFieldOfViewDeg, m_width, m_height
+            GetCameraNameFromFrame(GetEntity()), m_verticalFieldOfViewDeg, m_width, m_height, GetEntityId()
         };
         if (m_colorCamera)
         {
@@ -74,7 +74,7 @@ namespace ROS2
             AZStd::string cameraImageFullTopic = ROS2Names::GetNamespacedName(GetNamespace(), cameraImagePublisherConfig.m_topic);
             auto publisher =
                 ros2Node->create_publisher<sensor_msgs::msg::Image>(cameraImageFullTopic.data(), cameraImagePublisherConfig.GetQoS());
-            m_cameraSensorsWithPublihsers.emplace_back(CreatePair<CameraColorSensor>(publisher, description));
+            m_imagePublishers.emplace_back(publisher);
         }
         if (m_depthCamera)
         {
@@ -82,8 +82,22 @@ namespace ROS2
             AZStd::string cameraImageFullTopic = ROS2Names::GetNamespacedName(GetNamespace(), cameraImagePublisherConfig.m_topic);
             auto publisher =
                 ros2Node->create_publisher<sensor_msgs::msg::Image>(cameraImageFullTopic.data(), cameraImagePublisherConfig.GetQoS());
-            m_cameraSensorsWithPublihsers.emplace_back(CreatePair<CameraDepthSensor>(publisher, description));
+            m_imagePublishers.emplace_back(publisher);
         }
+
+        if (m_colorCamera && m_depthCamera)
+        {
+            m_cameraSensor = AZStd::make_shared<CameraRGBDSensor>(description);
+        }
+        else if (m_colorCamera)
+        {
+            m_cameraSensor = AZStd::make_shared<CameraColorSensor>(description);
+        }
+        else if (m_depthCamera)
+        {
+            m_cameraSensor = AZStd::make_shared<CameraDepthSensor>(description);
+        }
+
         const auto* component = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
         AZ_Assert(component, "Entity has no ROS2FrameComponent");
         m_frameName = component->GetFrameID();
@@ -91,7 +105,8 @@ namespace ROS2
 
     void ROS2CameraSensorComponent::Deactivate()
     {
-        m_cameraSensorsWithPublihsers.clear();
+        m_cameraSensor.reset();
+        m_imagePublishers.clear();
         ROS2SensorComponent::Deactivate();
     }
 
@@ -100,10 +115,10 @@ namespace ROS2
         const AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM();
         const auto timestamp = ROS2Interface::Get()->GetROSTimestamp();
         std_msgs::msg::Header ros_header;
-        if (!m_cameraSensorsWithPublihsers.empty())
+        if (!m_imagePublishers.empty() && m_cameraSensor)
         {
-            const auto& camera_descritpion = m_cameraSensorsWithPublihsers.front().second->GetCameraSensorDescription();
-            const auto& cameraIntrinsics = camera_descritpion.m_cameraIntrinsics;
+            const auto& cameraDescription = m_cameraSensor->GetCameraSensorDescription();
+            const auto& cameraIntrinsics = cameraDescription.m_cameraIntrinsics;
             sensor_msgs::msg::CameraInfo cameraInfo;
             ros_header.stamp = timestamp;
             ros_header.frame_id = m_frameName.c_str();
@@ -117,10 +132,7 @@ namespace ROS2
             cameraInfo.p = { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2], 0, cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5], 0,
                              cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8], 0 };
             m_cameraInfoPublisher->publish(cameraInfo);
-        }
-        for (auto& [publisher, sensor] : m_cameraSensorsWithPublihsers)
-        {
-            sensor->RequestMessagePublication(publisher, transform, ros_header);
+            m_cameraSensor->RequestMessagePublication(m_imagePublishers, transform, ros_header );
         }
     }
 
@@ -135,6 +147,5 @@ namespace ROS2
             return cameraName;
         }
         return AZStd::string{};
-
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h

@@ -89,11 +89,13 @@ namespace ROS2
         int m_height = 480;
         bool m_colorCamera = true;
         bool m_depthCamera = true;
+        AZStd::string m_frameName;
 
         void FrequencyTick() override;
-        AZStd::vector<PublisherSensorPtrPair> m_cameraSensorsWithPublihsers;
+
+        AZStd::vector<ImagePublisherPtrType> m_imagePublishers;
+        AZStd::shared_ptr<CameraSensor> m_cameraSensor;
         CameraInfoPublisherPtrType m_cameraInfoPublisher;
 
-        AZStd::string m_frameName;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h

@@ -39,7 +39,7 @@ namespace ROS2
         void Activate() override;
         void Deactivate() override;
 
-        //! AzToolsFramework::Components::EditorComponentBase override
+        // AzToolsFramework::Components::EditorComponentBase override
         void BuildGameEntity(AZ::Entity* gameEntity) override;
 
     private: