Browse Source

ROS2 camera sensor (#65)

* ROS2 camera sensor
* review fixes

Co-authored-by: Piotr Zyskowski <[email protected]>
Signed-off-by: Adam Dąbrowski <[email protected]>
Adam Dąbrowski 3 years ago
parent
commit
fd44b36697

+ 4 - 0
Gems/ROS2/Code/CMakeLists.txt

@@ -28,6 +28,8 @@ ly_add_target(
             AZ::AzCore
             AZ::AzCore
             AZ::AzFramework
             AZ::AzFramework
             Gem::Atom_RPI.Public
             Gem::Atom_RPI.Public
+            Gem::Atom_Feature_Common.Static
+            Gem::Atom_Component_DebugCamera.Static
 )
 )
 
 
 target_depends_on_ros2_packages(ROS2.Static rclcpp builtin_interfaces std_msgs sensor_msgs urdfdom tf2_ros)
 target_depends_on_ros2_packages(ROS2.Static rclcpp builtin_interfaces std_msgs sensor_msgs urdfdom tf2_ros)
@@ -56,6 +58,7 @@ ly_add_target(
     BUILD_DEPENDENCIES
     BUILD_DEPENDENCIES
         PRIVATE
         PRIVATE
             Gem::ROS2.Static
             Gem::ROS2.Static
+            Gem::Atom_Feature_Common.Static
 )
 )
 
 
 # By default, we will specify that the above target ROS2 would be used by
 # By default, we will specify that the above target ROS2 would be used by
@@ -99,6 +102,7 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
         BUILD_DEPENDENCIES
         BUILD_DEPENDENCIES
             PUBLIC
             PUBLIC
                 Gem::ROS2.Editor.Static
                 Gem::ROS2.Editor.Static
+                Gem::Atom_Feature_Common.Static
     )
     )
 
 
     # By default, we will specify that the above target ROS2 would be used by
     # By default, we will specify that the above target ROS2 would be used by

+ 90 - 0
Gems/ROS2/Code/Source/Camera/CameraCaptureScheduler.cpp

@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#include "Camera/CameraCaptureScheduler.h"
+
+namespace ROS2 {
+
+CameraCaptureScheduler& CameraCaptureScheduler::Get() {
+    static CameraCaptureScheduler i;
+    return i;
+}
+
+CameraCaptureScheduler::CameraCaptureScheduler() {
+    AZ::Render::FrameCaptureNotificationBus::Handler::BusConnect();
+}
+
+CameraCaptureScheduler::~CameraCaptureScheduler() {
+    AZ::Render::FrameCaptureNotificationBus::Handler::BusDisconnect();
+}
+
+bool CameraCaptureScheduler::TryRequestFrame(const AZStd::vector <AZStd::string>& passHierarchy,
+                                             std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult
+                                                           &result)> frameRenderedCallback,
+                                             AZ::RPI::RenderPipelinePtr pipeline) {
+    if (passHierarchy.size() != 2) {
+        AZ_TracePrintf("CameraCaptureScheduler", "Pipeline hierarchy must have exactly size of 2. Has %lu\n",
+                       passHierarchy.size());
+        return false;
+    }
+
+    if (!pipeline) {
+        AZ_TracePrintf("CameraCaptureScheduler", "Pipeline is nullptr\n");
+        return false;
+    }
+
+    if (!frameRenderedCallback) {
+        AZ_TracePrintf("CameraCaptureScheduler", "Callback is empty\n");
+        return false;
+    }
+
+    CameraCaptureRequest request(passHierarchy, frameRenderedCallback, pipeline);
+    auto found_it = std::find(captureRequestQueue.cbegin(), captureRequestQueue.cend(), request);
+    if (found_it != captureRequestQueue.cend()) {
+        return false;
+    }
+
+    captureRequestQueue.emplace_back(std::move(request));
+
+    if (captureInProgres) {
+        return false;
+    }
+
+    auto callback = [this](const AZ::RPI::AttachmentReadback::ReadbackResult &result) {
+        auto currentRequest = captureRequestQueue.begin();
+        currentRequest->frameRenderedCallback(result);
+    };
+
+    pipeline->AddToRenderTickOnce();
+    AZ::Render::FrameCaptureRequestBus::Broadcast(
+            &AZ::Render::FrameCaptureRequestBus::Events::CapturePassAttachmentWithCallback,
+            passHierarchy,
+            AZStd::string("Output"),
+            callback,
+            AZ::RPI::PassAttachmentReadbackOption::Output);
+    captureInProgres = true;
+    return true;
+}
+
+void CameraCaptureScheduler::OnCaptureFinished(AZ::Render::FrameCaptureResult result, const AZStd::string &info) {
+    captureInProgres = false;
+    captureRequestQueue.erase(captureRequestQueue.begin());
+}
+
+bool CameraCaptureScheduler::CameraCaptureRequest::operator==(const CameraCaptureScheduler::CameraCaptureRequest &s) const {
+    return pipelineHierarchy[0] == s.pipelineHierarchy[0];
+}
+
+CameraCaptureScheduler::CameraCaptureRequest::CameraCaptureRequest(
+        AZStd::vector<AZStd::string> pipelineHierarchy,
+        std::function<void(
+                const AZ::RPI::AttachmentReadback::ReadbackResult &result)> frameRenderedCallback,
+        AZ::RPI::RenderPipelinePtr pipeline)
+    : pipelineHierarchy(std::move(pipelineHierarchy))
+    , frameRenderedCallback(std::move(frameRenderedCallback))
+    , pipeline(std::move(pipeline)) {}
+}

+ 69 - 0
Gems/ROS2/Code/Source/Camera/CameraCaptureScheduler.h

@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/std/string/string.h>
+#include <AzCore/std/containers/vector.h>
+#include <Atom/Feature/Utils/FrameCaptureBus.h>
+
+#include <Clock/SimulationClock.h>
+
+#include <Atom/RPI.Public/RenderPipeline.h>
+
+#include <list>
+
+namespace ROS2 {
+
+//! Singleton class to initialize and synchronize frame captures for multiple cameras
+//! It supports the following features:
+//! - only one frame is rendered and captured at a time
+//! - scheduler waits for a notification that capture is finished
+//! - queue with camera captures collects only one capture request per camera
+//! @note This is a temporary solution until the multi camera continuous capture is implemented on Atom side
+//! @note Components are synchronous by default, so no multithreading guards were placed
+class CameraCaptureScheduler : public AZ::Render::FrameCaptureNotificationBus::Handler {
+public:
+    //! Get underlying singleton
+    static CameraCaptureScheduler& Get();
+
+    //! Requests frame rendering if applicable, sets callback and adds to rendering queue is necessary
+    //! Calling this function does not guarantee start of the rendering.
+    //!
+    //! @note Pass hierarchy is expected to have two elements, where first is the unique pipeline name
+    bool TryRequestFrame(const AZStd::vector <AZStd::string>& passHierarchy,
+                         std::function<void(
+                              const AZ::RPI::AttachmentReadback::ReadbackResult &result)> frameRenderedCallback,
+                         AZ::RPI::RenderPipelinePtr pipeline);
+
+private:
+    void OnCaptureFinished(AZ::Render::FrameCaptureResult result, const AZStd::string &info) override;
+
+    CameraCaptureScheduler();
+
+    ~CameraCaptureScheduler() override;
+
+    struct CameraCaptureRequest {
+        AZStd::vector<AZStd::string> pipelineHierarchy;
+        std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult &result)> frameRenderedCallback;
+        AZ::RPI::RenderPipelinePtr pipeline;
+
+        //! Compares only the pipeline name
+        bool operator==(const CameraCaptureRequest &rhs) const;
+        CameraCaptureRequest(
+                AZStd::vector<AZStd::string> pipelineHierarchy,
+                std::function<void(
+                        const AZ::RPI::AttachmentReadback::ReadbackResult &result)> frameRenderedCallback,
+                AZ::RPI::RenderPipelinePtr pipeline);
+        CameraCaptureRequest(CameraCaptureRequest &&other) = default;
+        CameraCaptureRequest &operator=(CameraCaptureRequest&& other) = default;
+    };
+
+    bool captureInProgres = false;
+    std::list<CameraCaptureRequest> captureRequestQueue;
+};
+}

+ 106 - 0
Gems/ROS2/Code/Source/Camera/CameraSensor.cpp

@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#include "CameraSensor.h"
+
+#include <AzCore/Math/MatrixUtils.h>
+
+#include <Atom/RPI.Public/RPISystemInterface.h>
+#include <Atom/RPI.Public/Scene.h>
+#include <Atom/RPI.Public/RenderPipeline.h>
+#include <Atom/RPI.Public/Base.h>
+
+#include <AzFramework/Components/TransformComponent.h>
+#include <AzFramework/Scene/SceneSystemInterface.h>
+
+#include <Atom/RPI.Public/Pass/Specific/RenderToTexturePass.h>
+
+#include <PostProcess/PostProcessFeatureProcessor.h>
+
+#include <Camera/CameraCaptureScheduler.h>
+
+namespace ROS2 {
+    CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height)
+    : verticalFieldOfViewDeg(verticalFov)
+    , width(width)
+    , height(height)
+    , cameraName(cameraName)
+    , aspectRatio(static_cast<float>(width)/static_cast<float>(height))
+    , viewToClipMatrix(MakeViewToClipMatrix()) {
+        validateParameters();
+    }
+
+    AZ::Matrix4x4 CameraSensorDescription::MakeViewToClipMatrix() const {
+        const float nearDist = 0.1f, farDist = 100.0f;
+        AZ::Matrix4x4 localViewToClipMatrix;
+        AZ::MakePerspectiveFovMatrixRH(localViewToClipMatrix, AZ::DegToRad(verticalFieldOfViewDeg), aspectRatio, nearDist, farDist, true);
+        return localViewToClipMatrix;
+    }
+
+    void CameraSensorDescription::validateParameters() const {
+        AZ_Assert(verticalFieldOfViewDeg > 0.0f && verticalFieldOfViewDeg < 180.0f, "Vertical fov should be in range 0.0 < FoV < 180.0 degrees");
+        AZ_Assert(!cameraName.empty(), "Camera name cannot be empty");
+    }
+
+    CameraSensor::CameraSensor(const CameraSensorDescription& cameraSensorDescription) {
+        AZ_TracePrintf("CameraSensor", "Initializing pipeline for %s", cameraSensorDescription.cameraName.c_str());
+
+        AZ::Name viewName = AZ::Name("MainCamera");
+        m_view = AZ::RPI::View::CreateView(viewName, AZ::RPI::View::UsageCamera);
+        m_view->SetViewToClipMatrix(cameraSensorDescription.viewToClipMatrix);
+        m_scene = AZ::RPI::RPISystemInterface::Get()->GetSceneByName(AZ::Name("Main"));
+
+        AZStd::string pipelineName = cameraSensorDescription.cameraName + "Pipeline";
+
+        AZ::RPI::RenderPipelineDescriptor pipelineDesc;
+        pipelineDesc.m_mainViewTagName = "MainCamera";
+        pipelineDesc.m_name = pipelineName;
+        pipelineDesc.m_rootPassTemplate = "MainPipelineRenderToTexture";
+        // TODO: expose sample count to user as it might substantially affect the performance
+        pipelineDesc.m_renderSettings.m_multisampleState.m_samples = 4;
+        m_pipeline = AZ::RPI::RenderPipeline::CreateRenderPipeline(pipelineDesc);
+        m_pipeline->RemoveFromRenderTick();
+
+        if (auto renderToTexturePass = azrtti_cast<AZ::RPI::RenderToTexturePass*>(m_pipeline->GetRootPass().get()))
+        {
+            renderToTexturePass->ResizeOutput(cameraSensorDescription.width, cameraSensorDescription.height);
+        }
+
+        m_scene->AddRenderPipeline(m_pipeline);
+
+        m_passHierarchy.push_back(pipelineName);
+        m_passHierarchy.push_back("CopyToSwapChain");
+
+        m_pipeline->SetDefaultView(m_view);
+        AZ::RPI::ViewPtr targetView = m_scene->GetDefaultRenderPipeline()->GetDefaultView();
+        if (auto* fp = m_scene->GetFeatureProcessor<AZ::Render::PostProcessFeatureProcessor>()) {
+            fp->SetViewAlias(m_view, targetView);
+        }
+    }
+
+    CameraSensor::~CameraSensor() {
+        if (m_scene) {
+            if (auto* fp = m_scene->GetFeatureProcessor<AZ::Render::PostProcessFeatureProcessor>())
+            {
+                fp->RemoveViewAlias(m_view);
+            }
+            m_scene->RemoveRenderPipeline(m_pipeline->GetId());
+            m_scene = nullptr;
+        }
+        m_passHierarchy.clear();
+        m_pipeline.reset();
+        m_view.reset();
+    }
+
+    void CameraSensor::RequestFrame(const AZ::Transform& cameraPose, std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback) {
+        AZ::Transform inverse = cameraPose.GetInverse();
+        m_view->SetWorldToViewMatrix(AZ::Matrix4x4::CreateFromQuaternionAndTranslation(inverse.GetRotation(),
+                                                                                       inverse.GetTranslation()));
+
+        CameraCaptureScheduler::Get().TryRequestFrame(m_passHierarchy, std::move(callback), m_pipeline);
+    }
+}

+ 63 - 0
Gems/ROS2/Code/Source/Camera/CameraSensor.h

@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <Atom/Feature/Utils/FrameCaptureBus.h>
+#include <chrono>
+
+class Entity;
+
+namespace ROS2 {
+
+    //! Structure containing all information required to create the camera sensor
+    struct CameraSensorDescription {
+        //! Constructor to create the description
+        //! @param cameraName - name of the camera; used to differentiate cameras in a multi-camera setup
+        //! @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);
+
+        const float verticalFieldOfViewDeg; //!< camera vertical field of view
+        const int width;                    //!< camera image width in pixels
+        const int height;                   //!< camera image height in pixels
+        const AZStd::string cameraName;     //!< camera name to differentiate cameras in a multi-camera setup
+
+        const float aspectRatio;              //!< camera image aspect ratio; equal to (width / height)
+        const AZ::Matrix4x4 viewToClipMatrix; //!< camera view to clip space transform matrix; derived from other parameters
+
+    private:
+        AZ::Matrix4x4 MakeViewToClipMatrix() const;
+        void validateParameters() const;
+    };
+
+    //! Class to create camera sensor using Atom renderer
+    //! It creates dedicated rendering pipeline for each camera
+    class CameraSensor {
+    public:
+        //! Initializes rendering pipeline for the camera sensor
+        //! @param cameraSensorDescription - camera sensor description used to create camera pipeline
+        CameraSensor(const CameraSensorDescription& cameraSensorDescription);
+
+        //! Deinitializes rendering pipeline for the camera sensor
+        ~CameraSensor();
+
+        //! Function requesting frame from 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 thins, captured image
+        void RequestFrame(const AZ::Transform& cameraPose, std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback);
+
+    private:
+        AZStd::vector<AZStd::string> m_passHierarchy;
+        AZ::RPI::RenderPipelinePtr m_pipeline;
+        AZ::RPI::ViewPtr m_view;
+        AZ::RPI::Scene* m_scene = nullptr;
+    };
+
+}

+ 103 - 0
Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+
+#include "Camera/ROS2CameraSensorComponent.h"
+#include "Frame/ROS2FrameComponent.h"
+#include "ROS2/ROS2Bus.h"
+
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Component/Entity.h>
+#include <AzCore/Component/TransformBus.h>
+
+#include <sensor_msgs/image_encodings.hpp>
+#include <Utilities/ROS2Names.h>
+
+namespace ROS2
+{
+    namespace Internal
+    {
+        const char* kImageMessageType = "sensor_msgs::msg::Image";
+    }
+
+    ROS2CameraSensorComponent::ROS2CameraSensorComponent() {
+        PublisherConfiguration pc;
+        auto type = Internal::kImageMessageType;
+        pc.m_type = type;
+        pc.m_topic = "camera_image";
+        m_sensorConfiguration.m_frequency = 10;
+        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(type, pc));
+    }
+
+    void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context)
+    {
+        auto* serialize = azrtti_cast<AZ::SerializeContext*>(context);
+        if (serialize)
+        {
+            serialize->Class<ROS2CameraSensorComponent, ROS2SensorComponent>()
+                    ->Version(1)
+                    ->Field("CameraName", &ROS2CameraSensorComponent::m_cameraName)
+                    ->Field("VerticalFieldOfViewDeg", &ROS2CameraSensorComponent::m_VerticalFieldOfViewDeg)
+                    ->Field("Width", &ROS2CameraSensorComponent::m_width)
+                    ->Field("Height", &ROS2CameraSensorComponent::m_height);
+
+            AZ::EditContext* ec = serialize->GetEditContext();
+            if (ec) {
+                ec->Class<ROS2CameraSensorComponent>("ROS2 Camera Sensor", "[Camera component]")
+                        ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2CameraSensorComponent::m_cameraName, "Camera Name", "This is the camera name.")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2CameraSensorComponent::m_VerticalFieldOfViewDeg, "Vertical field of view", "Camera's vertical (y axis) field of view in degrees.")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2CameraSensorComponent::m_width, "Image width", "Image width")
+                        ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2CameraSensorComponent::m_height, "Image height", "Image height");
+            }
+        }
+    }
+
+    void ROS2CameraSensorComponent::Activate()
+    {
+        ROS2SensorComponent::Activate();
+
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for camera sensor");
+        const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImageMessageType];
+        AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
+
+        m_imagePublisher = ros2Node->create_publisher<sensor_msgs::msg::Image>(fullTopic.data(), publisherConfig.GetQoS());
+
+        m_cameraSensor.emplace(CameraSensorDescription{m_cameraName, m_VerticalFieldOfViewDeg, m_width, m_height});
+    }
+
+    void ROS2CameraSensorComponent::Deactivate()
+    {
+        m_cameraSensor.reset();
+        ROS2SensorComponent::Deactivate();
+    }
+
+    void ROS2CameraSensorComponent::FrequencyTick()
+    {
+        AZ::Transform transform = GetEntity()->GetTransform()->GetWorldTM();
+
+        if (!m_cameraSensor) {
+            return;
+        }
+        m_cameraSensor->RequestFrame(transform, [this](const AZ::RPI::AttachmentReadback::ReadbackResult& result) {
+            const AZ::RHI::ImageDescriptor& descriptor = result.m_imageDescriptor;
+
+            sensor_msgs::msg::Image message;
+            message.encoding = sensor_msgs::image_encodings::RGBA8;
+
+            message.width = descriptor.m_size.m_width;
+            message.height = descriptor.m_size.m_height;
+            message.data = std::vector<uint8_t>(result.m_dataBuffer->data(), result.m_dataBuffer->data() + result.m_dataBuffer->size());
+            message.header.frame_id = GetEntity()->FindComponent<ROS2FrameComponent>()->GetFrameID().c_str();
+
+            m_imagePublisher->publish(message);
+        }) ;
+    }
+} // namespace ROS2

+ 51 - 0
Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h

@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <rclcpp/publisher.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include "Sensor/ROS2SensorComponent.h"
+
+#include <AzCore/Component/Component.h>
+
+#include "CameraSensor.h"
+
+namespace ROS2
+{
+    //! ROS2 Camera sensor component class
+    //! Allows turning an entity into a camera sensor
+    //! Can be parametrized with following values:
+    //!   - camera name
+    //!   - camera image width and height in pixels
+    //!   - camera vertical field of view in degrees
+    //! Camera frustum is facing negative Z axis; image plane is parallel to X,Y plane: X - right, Y - up
+    class ROS2CameraSensorComponent
+        : public ROS2SensorComponent
+    {
+    public:
+        ROS2CameraSensorComponent();
+        ~ROS2CameraSensorComponent() override = default;
+        AZ_COMPONENT(ROS2CameraSensorComponent, "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}", ROS2SensorComponent);
+        static void Reflect(AZ::ReflectContext* context);
+
+        void Activate() override;
+        void Deactivate() override;
+
+    private:
+        float m_VerticalFieldOfViewDeg = 90.0f;
+        int m_width = 640;
+        int m_height = 480;
+        AZStd::string m_cameraName = "camera";
+
+        void FrequencyTick() override;
+
+        std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::Image>> m_imagePublisher;
+        std::optional<CameraSensor> m_cameraSensor;
+    };
+}  // namespace ROS2

+ 1 - 1
Gems/ROS2/Code/Source/Frame/ROS2Transform.h

@@ -34,4 +34,4 @@ namespace ROS2
        bool m_isPublished = false;
        bool m_isPublished = false;
        bool m_isDynamic;
        bool m_isDynamic;
    };
    };
-}  // namespace ROS2
+}  // namespace ROS2

+ 3 - 1
Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -13,6 +13,7 @@
 #include "Lidar/ROS2LidarSensorComponent.h"
 #include "Lidar/ROS2LidarSensorComponent.h"
 #include "RobotControl/ROS2RobotControlComponent.h"
 #include "RobotControl/ROS2RobotControlComponent.h"
 #include "ROS2SystemComponent.h"
 #include "ROS2SystemComponent.h"
+#include "Camera/ROS2CameraSensorComponent.h"
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Module/Module.h>
 #include <AzCore/Module/Module.h>
 
 
@@ -38,7 +39,8 @@ namespace ROS2
                 ROS2GNSSSensorComponent::CreateDescriptor(),
                 ROS2GNSSSensorComponent::CreateDescriptor(),
                 ROS2LidarSensorComponent::CreateDescriptor(),
                 ROS2LidarSensorComponent::CreateDescriptor(),
                 ROS2FrameComponent::CreateDescriptor(),
                 ROS2FrameComponent::CreateDescriptor(),
-                ROS2RobotControlComponent::CreateDescriptor()
+                ROS2RobotControlComponent::CreateDescriptor(),
+                ROS2CameraSensorComponent::CreateDescriptor()
                 });
                 });
         }
         }
 
 

+ 6 - 0
Gems/ROS2/Code/ros2_files.cmake

@@ -4,6 +4,12 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 
 set(FILES
 set(FILES
+    Source/Camera/CameraCaptureScheduler.cpp
+    Source/Camera/CameraCaptureScheduler.h
+    Source/Camera/CameraSensor.cpp
+    Source/Camera/CameraSensor.h
+    Source/Camera/ROS2CameraSensorComponent.cpp
+    Source/Camera/ROS2CameraSensorComponent.h
     Source/Clock/SimulationClock.cpp
     Source/Clock/SimulationClock.cpp
     Source/Clock/SimulationClock.h
     Source/Clock/SimulationClock.h
     Source/Lidar/LidarRaycaster.cpp
     Source/Lidar/LidarRaycaster.cpp