Browse Source

Update ROS 2 sensors base implementation

Update ROS 2 sensors base implementation
Mike Chang 1 year ago
parent
commit
72a7f1073b
24 changed files with 222 additions and 11 deletions
  1. 20 0
      Gems/ROS2/Code/Include/ROS2/ROS2SensorTypesIds.h
  2. 13 0
      Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h
  3. 33 1
      Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h
  4. 21 3
      Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfigurationRequestBus.h
  5. 30 0
      Gems/ROS2/Code/Include/ROS2/Sensor/SensorHelper.h
  6. 2 0
      Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp
  7. 2 1
      Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h
  8. 2 0
      Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp
  9. 2 1
      Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h
  10. 2 0
      Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp
  11. 2 1
      Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h
  12. 2 0
      Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp
  13. 1 1
      Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h
  14. 2 0
      Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp
  15. 1 1
      Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h
  16. 2 0
      Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp
  17. 1 1
      Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h
  18. 2 0
      Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp
  19. 2 0
      Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp
  20. 1 1
      Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h
  21. 75 0
      Gems/ROS2/Code/Source/Sensor/SensorHelpers.cpp
  22. 1 0
      Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp
  23. 1 0
      Gems/ROS2/Code/ros2_files.cmake
  24. 2 0
      Gems/ROS2/Code/ros2_header_files.cmake

+ 20 - 0
Gems/ROS2/Code/Include/ROS2/ROS2SensorTypesIds.h

@@ -0,0 +1,20 @@
+/*
+ * 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
+
+namespace ROS2
+{
+    inline constexpr const char* ROS2CameraSensorComponentTypeId = "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}";
+    inline constexpr const char* ROS2ContactSensorComponentTypeId = "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}";
+    inline constexpr const char* ROS2GNSSSensorComponentTypeId = "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}";
+    inline constexpr const char* ROS2ImuSensorComponentTypeId = "{502A955E-7742-4E23-AD77-5E4063739DCA}";
+    inline constexpr const char* ROS2LidarSensorComponentTypeId = "{502A955F-7742-4E23-AD77-5E4063739DCA}";
+    inline constexpr const char* ROS2Lidar2DSensorComponentTypeId = "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}";
+    inline constexpr const char* ROS2WheelOdometryComponentTypeId = "{9bdb8c23-ac76-4c25-8d35-37aaa9f43fac}";
+    inline constexpr const char* ROS2OdometrySensorComponent = "{61387448-63AA-4563-AF87-60C72B05B863}";
+} // namespace ROS2

+ 13 - 0
Gems/ROS2/Code/Include/ROS2/Sensor/Events/EventSourceAdapter.h

@@ -107,6 +107,7 @@ namespace ROS2
                         return;
                     }
 
+                    m_lastDeltaTime =  m_adaptedDeltaTime;
                     m_sensorAdaptedEvent.Signal(m_adaptedDeltaTime, AZStd::forward<decltype(args)>(args)...);
                     m_adaptedDeltaTime = 0.0f;
                 });
@@ -134,6 +135,17 @@ namespace ROS2
             m_adaptedFrequency = adaptedFrequency;
         }
 
+        //! Gets adapter working frequency, based on the last obtained delta time between adapted events.
+        //! If the adapter has not been started yet, the frequency will be zero.
+        [[nodiscard]] float GetEffectiveFrequency() const
+        {
+            if (m_lastDeltaTime == 0.0f)
+            {
+                return 0.0f;
+            }
+            return 1.0f / m_lastDeltaTime;
+        }
+
         //! Connects given event handler to source event (ROS2::SensorEventSource). That event is signalled regardless of adapted frequency
         //! set for event source adapter (ROS2::EventSourceAdapter::SetFrequency). Its frequency depends only on specific event source
         //! implementation. If different working frequency is required (main purpose of ROS2::EventSourceAdapter), user should see
@@ -185,6 +197,7 @@ namespace ROS2
         typename EventSourceT::AdaptedEventType m_sensorAdaptedEvent{};
 
         float m_adaptedFrequency{ 30.0f }; ///< Adapted frequency value.
+        float m_lastDeltaTime{ 0.0f }; ///< Last difference in time between adapted events, used to compute effective frequency value.
         float m_adaptedDeltaTime{ 0.0f }; ///< Accumulator for calculating adapted delta time.
         int m_tickCounter{ 0 }; ///< Internal counter for controlling adapter frequency.
     };

+ 33 - 1
Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h

@@ -12,6 +12,7 @@
 #include <AzCore/Serialization/EditContext.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/ROS2GemUtilities.h>
+#include <ROS2/ROS2SensorTypesIds.h>
 #include <ROS2/Sensor/Events/EventSourceAdapter.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
 #include <ROS2/Sensor/SensorConfigurationRequestBus.h>
@@ -68,19 +69,50 @@ namespace ROS2
             return m_sensorConfiguration;
         }
 
-        void EnablePublishing(bool publishingEnabled) override
+        void SetSensorEnabled(bool sensorEnabled) override
+        {
+            if (sensorEnabled)
+            {
+                m_eventSourceAdapter.Start();
+            }
+            else
+            {
+                m_eventSourceAdapter.Stop();
+            }
+        }
+
+        void SetPublishingEnabled(bool publishingEnabled) override
         {
             m_sensorConfiguration.m_publishingEnabled = publishingEnabled;
         }
 
+        void SetVisualizeEnabled(bool visualizeEnabled) override
+        {
+            m_sensorConfiguration.m_visualize = visualizeEnabled;
+        }
+
+        float GetEffectiveFrequency() const override
+        {
+            return m_eventSourceAdapter.GetEffectiveFrequency();
+        }
+
+        void SetDesiredFrequency(float frequency) override
+        {
+            m_sensorConfiguration.m_frequency = frequency;
+            m_eventSourceAdapter.SetFrequency(frequency);
+        }
+
         virtual ~ROS2SensorComponentBase() = default;
 
         void Activate() override
         {
+            AZ::EntityComponentIdPair entityComponentIdPair(GetEntityId(), GetId());
+            SensorConfigurationRequestBus::Handler::BusConnect(entityComponentIdPair);
         }
 
         void Deactivate() override
         {
+            SensorConfigurationRequestBus::Handler::BusDisconnect();
         }
 
     protected:

+ 21 - 3
Gems/ROS2/Code/Include/ROS2/Sensor/SensorConfigurationRequestBus.h

@@ -7,22 +7,40 @@
  */
 #pragma once
 
+#include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/EBus/EBus.h>
 #include <AzCore/Interface/Interface.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
-
 namespace ROS2
 {
     //! Interface that allows to get sensor configuration and switch publish state.
     class SensorConfigurationRequest : public AZ::EBusTraits
     {
     public:
-        using BusIdType = AZ::EntityId;
+        AZ_RTTI(SensorConfigurationRequest, "{01904eab-fa33-7487-b634-e3f8361eb5fb}");
+        using BusIdType = AZ::EntityComponentIdPair;
         static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
 
+        //! Get the configuration of the sensor.
         virtual SensorConfiguration GetSensorConfiguration() const = 0;
-        virtual void EnablePublishing(bool publishingEnabled) = 0;
+
+        //! Enable or disable the sensor, completely stopping the sensor from running.
+        virtual void SetSensorEnabled(bool enable) = 0;
+
+        //! Enable or disable publishing of the sensor to ROS 2.
+        //! The sensor implementation will still be running, but the data will not be published.
+        virtual void SetPublishingEnabled(bool publishingEnabled) = 0;
+
+        //! Enable or disable visualization of the sensor.
+        //! The sensor implementation will still be running, but the data will not be visualized in the viewport.
+        virtual void SetVisualizeEnabled(bool visualizeEnabled) = 0;
+
+        //! Get the the current frequency of the sensor in hertz.
+        virtual float GetEffectiveFrequency() const = 0;
+
+        //! Sets desired frequency of the sensor in hertz.
+        virtual void SetDesiredFrequency(float frequency) = 0;
     };
 
     using SensorConfigurationRequestBus = AZ::EBus<SensorConfigurationRequest>;

+ 30 - 0
Gems/ROS2/Code/Include/ROS2/Sensor/SensorHelper.h

@@ -0,0 +1,30 @@
+/*
+ * 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/Component/Component.h>
+
+namespace ROS2
+{
+    //! Function to check if the component is a sensor component.
+    //! @param component Component to check
+    //! @return True if the component is a sensor component
+    bool IsComponentROS2Sensor(const AZ::Component* component);
+
+    //! Function to get Ids of all sensors attached to the entity.
+    //! @param entityId Id of the entity
+    //! @return Vector of EntityComponentIdPair of all sensors attached to the entity
+    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsForEntity(const AZ::EntityId& entityId);
+
+    //! Function to get Ids of all sensors attached to the entity
+    //! @param entityId Id of the entity
+    //! @param sensorType Type of the sensor, see @file ROS2SensorTypesIds.h
+    //! @return Vector of EntityComponentIdPair of all sensors attached to the entity
+    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsOfType(const AZ::EntityId& entityId, const AZ::Uuid& sensorType);
+
+} // namespace ROS2

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

@@ -33,6 +33,7 @@ namespace ROS2
 
     void ROS2CameraSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera)
         {
             SetImageSource<CameraRGBDSensor>();
@@ -68,6 +69,7 @@ namespace ROS2
         StopSensor();
         m_cameraSensor.reset();
         ROS2::CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId());
+        ROS2SensorComponentBase::Deactivate();
     }
 
     AZ::Matrix3x3 ROS2CameraSensorComponent::GetCameraMatrix() const

+ 2 - 1
Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h

@@ -18,6 +18,7 @@
 #include "CameraSensorConfiguration.h"
 #include <ROS2/Camera/CameraCalibrationRequestBus.h>
 #include <ROS2/ROS2Bus.h>
+#include <ROS2/ROS2SensorTypesIds.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
 #include <ROS2/Sensor/ROS2SensorComponentBase.h>
 
@@ -42,7 +43,7 @@ namespace ROS2
         static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         ~ROS2CameraSensorComponent() override = default;
 
-        AZ_COMPONENT(ROS2CameraSensorComponent, "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}", SensorBaseType);
+        AZ_COMPONENT(ROS2CameraSensorComponent, ROS2CameraSensorComponentTypeId, SensorBaseType);
         static void Reflect(AZ::ReflectContext* context);
 
         // AzToolsFramework::Components::EditorComponentBase overrides ..

+ 2 - 0
Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp

@@ -59,6 +59,7 @@ namespace ROS2
 
     void ROS2ContactSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         m_entityId = GetEntityId();
         AZ::Entity* entity = nullptr;
         AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId);
@@ -109,6 +110,7 @@ namespace ROS2
         m_onCollisionBeginHandler.Disconnect();
         m_onCollisionPersistHandler.Disconnect();
         m_onCollisionEndHandler.Disconnect();
+        ROS2SensorComponentBase::Deactivate();
     }
 
     void ROS2ContactSensorComponent::FrequencyTick()

+ 2 - 1
Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h

@@ -14,6 +14,7 @@
 #include <AzCore/std/parallel/mutex.h>
 #include <AzCore/std/string/string.h>
 #include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <ROS2/ROS2SensorTypesIds.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
 #include <ROS2/Sensor/ROS2SensorComponentBase.h>
 #include <gazebo_msgs/msg/contact_state.hpp>
@@ -29,7 +30,7 @@ namespace ROS2
     class ROS2ContactSensorComponent : public ROS2SensorComponentBase<TickBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2ContactSensorComponent, "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}", SensorBaseType);
+        AZ_COMPONENT(ROS2ContactSensorComponent, ROS2ContactSensorComponentTypeId, SensorBaseType);
         ROS2ContactSensorComponent();
         ~ROS2ContactSensorComponent() = default;
 

+ 2 - 0
Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp

@@ -58,6 +58,7 @@ namespace ROS2
 
     void ROS2GNSSSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         auto ros2Node = ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for GNSS sensor");
 
@@ -88,6 +89,7 @@ namespace ROS2
     {
         StopSensor();
         m_gnssPublisher.reset();
+        ROS2SensorComponentBase::Deactivate();
     }
 
     void ROS2GNSSSensorComponent::FrequencyTick()

+ 2 - 1
Gems/ROS2/Code/Source/GNSS/ROS2GNSSSensorComponent.h

@@ -9,6 +9,7 @@
 
 #include <AzCore/Math/Transform.h>
 #include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2/ROS2SensorTypesIds.h>
 #include <ROS2/Sensor/Events/TickBasedSource.h>
 #include <ROS2/Sensor/ROS2SensorComponentBase.h>
 #include <rclcpp/publisher.hpp>
@@ -23,7 +24,7 @@ namespace ROS2
     class ROS2GNSSSensorComponent : public ROS2SensorComponentBase<TickBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2GNSSSensorComponent, "{55B4A299-7FA3-496A-88F0-764C75B0E9A7}", SensorBaseType);
+        AZ_COMPONENT(ROS2GNSSSensorComponent, ROS2GNSSSensorComponentTypeId, SensorBaseType);
         ROS2GNSSSensorComponent();
         ROS2GNSSSensorComponent(const SensorConfiguration& sensorConfiguration);
         ~ROS2GNSSSensorComponent() = default;

+ 2 - 0
Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -81,6 +81,7 @@ namespace ROS2
 
     void ROS2ImuSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         auto ros2Node = ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for IMU sensor");
         m_imuMsg.header.frame_id = GetFrameID().c_str();
@@ -113,6 +114,7 @@ namespace ROS2
         StopSensor();
         m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
         m_imuPublisher.reset();
+        ROS2SensorComponentBase::Deactivate();
     }
 
     void ROS2ImuSensorComponent::OnPhysicsEvent(AzPhysics::SceneHandle sceneHandle)

+ 1 - 1
Gems/ROS2/Code/Source/Imu/ROS2ImuSensorComponent.h

@@ -28,7 +28,7 @@ namespace ROS2
     class ROS2ImuSensorComponent : public ROS2SensorComponentBase<PhysicsBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2ImuSensorComponent, "{502A955E-7742-4E23-AD77-5E4063739DCA}", SensorBaseType);
+        AZ_COMPONENT(ROS2ImuSensorComponent, ROS2ImuSensorComponentTypeId, SensorBaseType);
         ROS2ImuSensorComponent();
         ROS2ImuSensorComponent(const SensorConfiguration& sensorConfiguration, const ImuSensorConfiguration& imuConfiguration);
         ~ROS2ImuSensorComponent() = default;

+ 2 - 0
Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp

@@ -65,6 +65,7 @@ namespace ROS2
 
     void ROS2Lidar2DSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         m_lidarCore.Init(GetEntityId());
 
         auto ros2Node = ROS2Interface::Get()->GetNode();
@@ -99,6 +100,7 @@ namespace ROS2
         StopSensor();
         m_laserScanPublisher.reset();
         m_lidarCore.Deinit();
+        ROS2SensorComponentBase::Deactivate();
     }
 
     void ROS2Lidar2DSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)

+ 1 - 1
Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h

@@ -28,7 +28,7 @@ namespace ROS2
     class ROS2Lidar2DSensorComponent : public ROS2SensorComponentBase<TickBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2Lidar2DSensorComponent, "{F4C2D970-1D69-40F2-9D4D-B52DCFDD2704}", SensorBaseType);
+        AZ_COMPONENT(ROS2Lidar2DSensorComponent, ROS2Lidar2DSensorComponentTypeId, SensorBaseType);
         ROS2Lidar2DSensorComponent();
         ROS2Lidar2DSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration);
         ~ROS2Lidar2DSensorComponent() = default;

+ 2 - 0
Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -70,6 +70,7 @@ namespace ROS2
 
     void ROS2LidarSensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         m_lidarCore.Init(GetEntityId());
 
         m_lidarRaycasterId = m_lidarCore.GetLidarRaycasterId();
@@ -127,6 +128,7 @@ namespace ROS2
         StopSensor();
         m_pointCloudPublisher.reset();
         m_lidarCore.Deinit();
+        ROS2SensorComponentBase::Deactivate();
     }
 
     void ROS2LidarSensorComponent::FrequencyTick()

+ 1 - 1
Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h

@@ -29,7 +29,7 @@ namespace ROS2
     class ROS2LidarSensorComponent : public ROS2SensorComponentBase<TickBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2LidarSensorComponent, "{502A955F-7742-4E23-AD77-5E4063739DCA}", SensorBaseType);
+        AZ_COMPONENT(ROS2LidarSensorComponent, ROS2LidarSensorComponentTypeId, SensorBaseType);
         ROS2LidarSensorComponent();
         ROS2LidarSensorComponent(const SensorConfiguration& sensorConfiguration, const LidarSensorConfiguration& lidarConfiguration);
         ~ROS2LidarSensorComponent() = default;

+ 2 - 0
Gems/ROS2/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp

@@ -97,6 +97,7 @@ namespace ROS2
     }
     void ROS2OdometrySensorComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         // "odom" is globally fixed frame for all robots, no matter the namespace
         m_odometryMsg.header.frame_id = ROS2Names::GetNamespacedName(GetNamespace(), "odom").c_str();
         m_odometryMsg.child_frame_id = GetFrameID().c_str();
@@ -123,5 +124,6 @@ namespace ROS2
     {
         StopSensor();
         m_odometryPublisher.reset();
+        ROS2SensorComponentBase::Deactivate();
     }
 } // namespace ROS2

+ 2 - 0
Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.cpp

@@ -100,6 +100,7 @@ namespace ROS2
 
     void ROS2WheelOdometryComponent::Activate()
     {
+        ROS2SensorComponentBase::Activate();
         m_robotPose = AZ::Vector3{ 0 };
         m_robotRotation = AZ::Quaternion{ 0, 0, 0, 1 };
 
@@ -134,5 +135,6 @@ namespace ROS2
     {
         StopSensor();
         m_odometryPublisher.reset();
+        ROS2SensorComponentBase::Deactivate();
     }
 } // namespace ROS2

+ 1 - 1
Gems/ROS2/Code/Source/Odometry/ROS2WheelOdometry.h

@@ -25,7 +25,7 @@ namespace ROS2
         : public ROS2SensorComponentBase<PhysicsBasedSource>
     {
     public:
-        AZ_COMPONENT(ROS2WheelOdometryComponent, "{9bdb8c23-ac76-4c25-8d35-37aaa9f43fac}", SensorBaseType);
+        AZ_COMPONENT(ROS2WheelOdometryComponent, ROS2WheelOdometryComponentTypeId, SensorBaseType);
         ROS2WheelOdometryComponent();
         ~ROS2WheelOdometryComponent() = default;
         static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);

+ 75 - 0
Gems/ROS2/Code/Source/Sensor/SensorHelpers.cpp

@@ -0,0 +1,75 @@
+/*
+ * 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 <ROS2/Sensor/Events/PhysicsBasedSource.h>
+#include <ROS2/Sensor/Events/TickBasedSource.h>
+#include <ROS2/Sensor/ROS2SensorComponentBase.h>
+#include <ROS2/Sensor/SensorHelper.h>
+
+namespace ROS2
+{
+
+    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsForEntity(const AZ::EntityId& entityId)
+    {
+        AZStd::vector<AZ::EntityComponentIdPair> sensors;
+        if (AZ::Entity* entity = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->FindEntity(entityId))
+        {
+            auto components = entity->GetComponents();
+            for (const auto* component : components)
+            {
+                AZ_Assert(component, "Component not found");
+                if (IsComponentROS2Sensor(component))
+                {
+                    sensors.push_back(AZ::EntityComponentIdPair(entityId, component->GetId()));
+                }
+            }
+        }
+        else
+        {
+            AZ_Warning("SensorHelpers", false, "Entity not found");
+        }
+        return sensors;
+    }
+
+    AZStd::vector<AZ::EntityComponentIdPair> GetSensorsOfType(const AZ::EntityId& entityId, const AZ::Uuid& sensorType)
+    {
+        AZStd::vector<AZ::EntityComponentIdPair> sensors;
+        if (AZ::Entity* entity = AZ::Interface<AZ::ComponentApplicationRequests>::Get()->FindEntity(entityId))
+        {
+            auto components = entity->GetComponents();
+            for (const auto* component : components)
+            {
+                AZ_Assert(component, "Component not found");
+                if (component->RTTI_IsTypeOf(sensorType))
+                {
+                    sensors.push_back(AZ::EntityComponentIdPair(entityId, component->GetId()));
+                }
+            }
+        }
+        else
+        {
+            AZ_Warning("SensorHelpers", false, "Entity not found");
+        }
+        return sensors;
+    }
+
+    bool IsComponentROS2Sensor(const AZ::Component* component)
+    {
+        // In ROS2 gem we have at this moment two types of base classes for sensors, we need to check if the component is derived from one
+        // of them. If we add more base classes for sensors in the future, we need to update this function.
+        if (azrtti_cast<const ROS2::ROS2SensorComponentBase<ROS2::TickBasedSource>*>(component))
+        {
+            return true;
+        }
+        if (azrtti_cast<const ROS2::ROS2SensorComponentBase<ROS2::PhysicsBasedSource>*>(component))
+        {
+            return true;
+        }
+        return false;
+    }
+}; // namespace ROS2

+ 1 - 0
Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp

@@ -29,6 +29,7 @@
 #include <AzCore/std/smart_ptr/make_shared.h>
 #include <AzCore/std/string/string_view.h>
 #include <AzFramework/API/ApplicationAPI.h>
+#include <ROS2/Sensor/SensorConfigurationRequestBus.h>
 
 namespace ROS2
 {

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

@@ -123,6 +123,7 @@ set(FILES
         Source/Sensor/Events/PhysicsBasedSource.cpp
         Source/Sensor/Events/TickBasedSource.cpp
         Source/Sensor/SensorConfiguration.cpp
+        Source/Sensor/SensorHelpers.cpp
         Source/SimulationUtils/FollowingCameraConfiguration.cpp
         Source/SimulationUtils/FollowingCameraConfiguration.h
         Source/SimulationUtils/FollowingCameraComponent.cpp

+ 2 - 0
Gems/ROS2/Code/ros2_header_files.cmake

@@ -36,6 +36,7 @@ set(FILES
         Include/ROS2/RobotControl/ControlSubscriptionHandler.h
         Include/ROS2/RobotImporter/SDFormatModelPluginImporterHook.h
         Include/ROS2/RobotImporter/SDFormatSensorImporterHook.h
+        Include/ROS2/ROS2SensorTypesIds.h
         Include/ROS2/Lidar/LidarRaycasterBus.h
         Include/ROS2/Lidar/LidarSystemBus.h
         Include/ROS2/Lidar/LidarRegistrarBus.h
@@ -48,6 +49,7 @@ set(FILES
         Include/ROS2/Sensor/ROS2SensorComponentBase.h
         Include/ROS2/Sensor/SensorConfiguration.h
         Include/ROS2/Sensor/SensorConfigurationRequestBus.h
+        Include/ROS2/Sensor/SensorHelper.h
         Include/ROS2/Spawner/SpawnerBus.h
         Include/ROS2/Utilities/Controllers/PidConfiguration.h
         Include/ROS2/Utilities/ROS2Conversions.h