diff --git a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp index 1512af2e2..0cd391c09 100644 --- a/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/SystemComponents/ROS2SystemComponent.cpp @@ -76,7 +76,6 @@ namespace ROS2 void ROS2SystemComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC("AssetDatabaseService", 0x3abf5601)); - required.push_back(AZ_CRC("RPISystem", 0xf2add773)); } void ROS2SystemComponent::GetDependentServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& dependent) diff --git a/Gems/ROS2Sensors/Code/CMakeLists.txt b/Gems/ROS2Sensors/Code/CMakeLists.txt index cd0d77819..18d25e060 100644 --- a/Gems/ROS2Sensors/Code/CMakeLists.txt +++ b/Gems/ROS2Sensors/Code/CMakeLists.txt @@ -265,6 +265,9 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED) BUILD_DEPENDENCIES PRIVATE AZ::AzTest + AZ::AzTestShared + AZ::AzToolsFramework + AZ::AzManipulatorTestFramework.Static Gem::${gem_name}.Editor.Private.Object ) diff --git a/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraSensorConfiguration.h similarity index 100% rename from Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Camera/CameraSensorConfiguration.h diff --git a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Configuration/ConfigurationBus.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Configuration/ConfigurationBus.h new file mode 100644 index 000000000..78e0d53f8 --- /dev/null +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Configuration/ConfigurationBus.h @@ -0,0 +1,35 @@ +/* + * 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 +#include + +namespace ROS2 { + template + class ConfigurationRequests : public AZ::ComponentBus + { + public: + using BusIdType = AZ::EntityId; + static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById; + static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Multiple; + + //! Returns the current configuration of the component. + virtual const Configuration GetConfiguration() const = 0; + + //! Sets the configuration of the component. + //! Each component should handle the configuration change without fully reinitializing the ROS2 publisher. + //! This will allow to change the configuration of the component at runtime. + //! @param configuration The new configuration to set. + virtual void SetConfiguration(const Configuration configuration) = 0; + }; + + template + using ConfigurationBus = AZ::EBus>; +} diff --git a/Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuSensorConfiguration.h similarity index 100% rename from Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Imu/ImuSensorConfiguration.h diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h similarity index 92% rename from Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h index da6115ad9..d566609d9 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.h +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h @@ -11,9 +11,8 @@ #include #include #include -#include -#include -#include +#include +#include #include namespace ROS2 @@ -59,7 +58,7 @@ namespace ROS2 //! Get all available lidar systems. AZStd::vector FetchLidarSystemList(); - const AZStd::vector m_availableModels; + AZStd::vector m_availableModels; AZStd::string m_lidarModelName = "CustomLidar2D"; void UpdateShowNoise(); diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplate.h similarity index 100% rename from Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplate.h diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplateUtils.h similarity index 97% rename from Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplateUtils.h index 3fbf069ac..c4e1590a7 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.h +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Lidar/LidarTemplateUtils.h @@ -9,7 +9,7 @@ #include #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h similarity index 100% rename from Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.h rename to Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h diff --git a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2WheelOdometryConfiguration.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2WheelOdometryConfiguration.h new file mode 100644 index 000000000..dc2a6b374 --- /dev/null +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Odometry/ROS2WheelOdometryConfiguration.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 +#include +#include + +namespace ROS2 { + + //! A structure capturing configuration of a wheel odometry sensor. + struct ROS2WheelOdometryConfiguration + { + AZ_RTTI(ROS2WheelOdometryConfiguration, "{9dc58d89-e674-4d7f-9ea9-afe3ae7fd2eb}"); + + ROS2WheelOdometryConfiguration() = default; + virtual ~ROS2WheelOdometryConfiguration() = default; + + static void Reflect(AZ::ReflectContext* context); + + ROS2OdometryCovariance m_poseCovariance; + ROS2OdometryCovariance m_twistCovariance; + }; +} diff --git a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h index 0a797d6cd..e73a47c21 100644 --- a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/ROS2SensorsTypeIds.h @@ -21,6 +21,9 @@ namespace ROS2Sensors // so they use the Same TypeId inline constexpr const char* ROS2SensorsEditorModuleTypeId = ROS2SensorsModuleTypeId; + // Sensor Components Base TypeIds + inline constexpr const char* ROS2SensorComponentBaseTypeId = "{A0C2D1B4-8E5F-4E7C-8A3D-6F9B5F1A0E7D}"; + // System Sensor Components TypeIds inline constexpr const char* ROS2SystemCameraComponentTypeId = "{B4665D39-78FD-40DE-8518-2F6BD345A831}"; inline constexpr const char* ROS2EditorCameraSystemComponentTypeId = "{407F51C0-92C9-11EE-B9D1-0242AC120002}"; diff --git a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h index f63bfd369..cc2a45ad8 100644 --- a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h @@ -10,12 +10,16 @@ #include #include +#include #include #include #include +#include #include #include #include +#include +#include namespace ROS2 { @@ -30,29 +34,32 @@ namespace ROS2 //! chosen event source implementation). //! @see ROS2::TickBasedSource //! @see ROS2::PhysicsBasedSource - template + template class ROS2SensorComponentBase : public AZ::Component , public SensorConfigurationRequestBus::Handler + , public ConfigurationBus::Handler { public: - using SensorBaseType = ROS2SensorComponentBase; + using SensorBaseType = ROS2SensorComponentBase; - AZ_COMPONENT_DECL((ROS2SensorComponentBase, AZ_CLASS)); + AZ_COMPONENT_DECL((ROS2SensorComponentBase, AZ_CLASS, AZ_CLASS)); static void Reflect(AZ::ReflectContext* context) { if (auto* serializeContext = azrtti_cast(context)) { - serializeContext->Class, AZ::Component>()->Version(1)->Field( - "SensorConfiguration", &ROS2SensorComponentBase::m_sensorConfiguration); + serializeContext->Class, AZ::Component>()->Version(1)->Field( + "SensorConfiguration", &ROS2SensorComponentBase::m_sensorConfiguration); if (auto* editContext = serializeContext->GetEditContext()) { - editContext->Class>("ROS2 Sensor Component Base", "Base component for sensors") + editContext + ->Class>( + "ROS2 Sensor Component Base", "Base component for sensors") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2SensorComponentBase::m_sensorConfiguration, + &ROS2SensorComponentBase::m_sensorConfiguration, "Sensor configuration", "Sensor configuration"); } @@ -108,13 +115,82 @@ namespace ROS2 { AZ::EntityComponentIdPair entityComponentIdPair(GetEntityId(), GetId()); SensorConfigurationRequestBus::Handler::BusConnect(entityComponentIdPair); + ConfigurationBus::Handler::BusConnect(GetEntityId()); + + if (m_sensorConfiguration.m_configurableFromROS2) + { + AZStd::string ns = GetNamespace(); + AZStd::string configurationName = ComponentConfigurationT::TYPEINFO_Name(); + AZStd::string getTopic = ns + "/" + "Get" + configurationName; + AZStd::string setTopic = ns + "/" + "Set" + configurationName; + m_configurationPublisher = ROS2Interface::Get()->GetNode()->create_publisher( + getTopic.data(), rclcpp::QoS(1).transient_local()); + m_configurationSubscriber = ROS2Interface::Get()->GetNode()->create_subscription( + setTopic.data(), + rclcpp::QoS(1).reliable(), + [this](const std_msgs::msg::String::SharedPtr msg) + { + ProcessConfigurationMsg(AZStd::string(msg->data.c_str())); + }); + + PublishConfiguration(GetConfiguration()); + } } void Deactivate() override { + m_configurationPublisher.reset(); + m_configurationSubscriber.reset(); + + ConfigurationBus::Handler::BusDisconnect(); SensorConfigurationRequestBus::Handler::BusDisconnect(); } + AZ::TypeId GetUnderlyingComponentType() const override + { + return AZ::TypeId(ROS2Sensors::ROS2SensorComponentBaseTypeId); + } + + const ComponentConfigurationT GetConfiguration() const override + { + return GetComponentConfiguration(); + } + + void ProcessConfigurationMsg(AZStd::string msg) + { + rapidjson::Document document; + document.Parse(msg.c_str()); + if (document.HasParseError()) + { + AZ_Error("ROS2Sensors", false, "Failed to parse configuration message"); + return; + } + + AZ::JsonDeserializerSettings deserializerSettings; + + ComponentConfigurationT configuration = GetConfiguration(); + auto result = AZ::JsonSerialization::Load(configuration, document, deserializerSettings); + auto processing = result.GetProcessing(); + if (processing == AZ::JsonSerializationResult::Processing::Completed) + { + SetConfiguration(configuration); + } + else + { + AZ_Error("ROS2Sensors", false, "Failed to deserialize configuration message: %s", result.ToString("").c_str()); + } + } + + void SetConfiguration(const ComponentConfigurationT configuration) override + { + SetComponentConfiguration(configuration); + + PublishConfiguration(configuration); + } + + virtual const ComponentConfigurationT GetComponentConfiguration() const = 0; + virtual void SetComponentConfiguration(const ComponentConfigurationT configuration) = 0; + protected: //! Starts sensor with passed frequency and adapted event callback. Optionally, user can pass source event callback, that will be //! called with event source frequency. @@ -142,7 +218,7 @@ namespace ROS2 m_eventSourceAdapter.Start(); } - //! Stops sensor and disconnects event callbacks passed through RSO2::ROS2SensorComponentBase::StartSensor. + //! Stops sensor and disconnects event callbacks passed through ROS2::ROS2SensorComponentBase::StartSensor. void StopSensor() { m_eventSourceAdapter.Stop(); @@ -173,8 +249,29 @@ namespace ROS2 //! Handler for adapted event. Requires manual assignment and connecting to adapted event in derived class. typename EventSourceT::AdaptedEventHandlerType m_adaptedEventHandler; + + rclcpp::Publisher::SharedPtr m_configurationPublisher; ///< Publisher for the sensor configuration. + rclcpp::Subscription::SharedPtr m_configurationSubscriber; ///< Subscriber for the sensor configuration. + + private: + void PublishConfiguration(const ComponentConfigurationT& configuration) + { + if (m_configurationPublisher != nullptr) + { + rapidjson::Document document(rapidjson::kObjectType); + AZ::JsonSerializerSettings serializerSettings; + serializerSettings.m_keepDefaults = true; + AZ::JsonSerialization::Store(document, document.GetAllocator(), configuration, serializerSettings); + rapidjson::StringBuffer buffer; + rapidjson::Writer writer(buffer); + document.Accept(writer); + std_msgs::msg::String msg; + msg.data = buffer.GetString(); + m_configurationPublisher->publish(msg); + } + } }; AZ_COMPONENT_IMPL_INLINE( - (ROS2SensorComponentBase, AZ_CLASS), "ROS2SensorComponentBase", "{2DF9A652-DF5D-43B1-932F-B6A838E36E97}", AZ::Component) + (ROS2SensorComponentBase, AZ_CLASS, AZ_CLASS), "ROS2SensorComponentBase", ROS2Sensors::ROS2SensorComponentBaseTypeId, AZ::Component) } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h index d807c47ef..724d2c0e7 100644 --- a/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h +++ b/Gems/ROS2Sensors/Code/Include/ROS2Sensors/Sensor/SensorConfiguration.h @@ -35,6 +35,9 @@ namespace ROS2 bool m_publishingEnabled = true; //!< Determines whether the sensor is publishing (sending data to ROS 2 ecosystem). bool m_visualize = true; //!< Determines whether the sensor is visualized in O3DE (for example, point cloud is drawn for LIDAR). + + bool m_configurableFromROS2 = + false; //!< Determines whether the sensor can be configured from ROS 2 (for example, camera resolution). private: // Frequency limit is once per day. static constexpr float m_minFrequency = AZStd::numeric_limits::epsilon(); diff --git a/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp b/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp index ece6cacc2..0b3c446f6 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp +++ b/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorConfiguration.cpp @@ -6,9 +6,9 @@ * */ -#include "CameraSensorConfiguration.h" #include #include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h b/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h index a062978ab..143764523 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h +++ b/Gems/ROS2Sensors/Code/Source/Camera/CameraSensorDescription.h @@ -7,7 +7,7 @@ */ #pragma once -#include "CameraSensorConfiguration.h" +#include #include #include diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp index 5c4dbef0e..939ad1f6c 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp @@ -22,6 +22,7 @@ namespace ROS2 void ROS2CameraSensorComponent::Reflect(AZ::ReflectContext* context) { CameraSensorConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); auto* serialize = azrtti_cast(context); if (serialize) @@ -34,18 +35,8 @@ namespace ROS2 void ROS2CameraSensorComponent::Activate() { ROS2SensorComponentBase::Activate(); - if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera) - { - SetImageSource(); - } - else if (m_cameraConfiguration.m_colorCamera) - { - SetImageSource(); - } - else if (m_cameraConfiguration.m_depthCamera) - { - SetImageSource(); - } + + SetCameraSensorConfiguration(); const auto* component = GetEntity()->FindComponent(); AZ_Assert(component, "Entity has no ROS2FrameComponent"); @@ -136,4 +127,39 @@ namespace ROS2 } return AZStd::string{}; } + + void ROS2CameraSensorComponent::SetCameraSensorConfiguration() + { + if (m_cameraConfiguration.m_colorCamera && m_cameraConfiguration.m_depthCamera) + { + SetImageSource(); + } + else if (m_cameraConfiguration.m_colorCamera) + { + SetImageSource(); + } + else if (m_cameraConfiguration.m_depthCamera) + { + SetImageSource(); + } + } + + void ROS2CameraSensorComponent::SetComponentConfiguration(const CameraSensorConfiguration configuration) + { + m_cameraConfiguration = configuration; + m_cameraSensor.reset(); + // SetCameraSensorConfiguration() is called in the next tick to ensure that the camera sensor is + // reset. + AZ::SystemTickBus::QueueFunction( + [this]() + { + SetCameraSensorConfiguration(); + }); + } + + const CameraSensorConfiguration ROS2CameraSensorComponent::GetComponentConfiguration() const + { + return m_cameraConfiguration; + } + } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h index c6fa00e1c..a7140629c 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.h @@ -15,10 +15,10 @@ #include #include "CameraSensor.h" -#include "CameraSensorConfiguration.h" #include #include #include +#include #include #include @@ -32,7 +32,7 @@ namespace ROS2 //! - 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 ROS2SensorComponentBase + : public ROS2SensorComponentBase , public CameraCalibrationRequestBus::Handler { public: @@ -73,6 +73,13 @@ namespace ROS2 ///! Requests message publication from camera sensor. void FrequencyTick(); + //! Sets the camera sensor and image source. + void SetCameraSensorConfiguration(); + + // ConfigurationBus overrides + void SetComponentConfiguration(const CameraSensorConfiguration configuration) override; + const CameraSensorConfiguration GetComponentConfiguration() const override; + CameraSensorConfiguration m_cameraConfiguration; AZStd::string m_frameName; AZStd::shared_ptr m_cameraSensor; diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h index fee784d3a..317ad4187 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorEditorComponent.h @@ -12,10 +12,10 @@ #include #include -#include "CameraSensorConfiguration.h" #include #include #include +#include #include namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp index e820b5649..faf07b6b2 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.cpp @@ -41,7 +41,11 @@ namespace ROS2 void ROS2SystemCameraComponent::GetRequiredServices([[maybe_unused]] AZ::ComponentDescriptor::DependencyArrayType& required) { required.push_back(AZ_CRC_CE("ROS2Service")); - required.push_back(AZ_CRC_CE("RPISystem")); + } + + void ROS2SystemCameraComponent::GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + dependent.push_back(AZ_CRC_CE("RPISystem")); } void ROS2SystemCameraComponent::InitPassTemplateMappingsHandler() diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h index 592afa957..d85b1ea92 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSystemComponent.h @@ -24,6 +24,7 @@ namespace ROS2 static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); void InitPassTemplateMappingsHandler(); diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp b/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp index 1af6a2f85..8125a314e 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.cpp @@ -36,6 +36,11 @@ namespace ROS2 BaseSystemComponent::GetRequiredServices(required); } + void ROS2EditorCameraSystemComponent::GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent) + { + BaseSystemComponent::GetDependentServices(dependent); + } + void ROS2EditorCameraSystemComponent::Activate() { AzToolsFramework::EditorEntityContextNotificationBus::Handler::BusConnect(); diff --git a/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h b/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h index ee2358404..edb1fdbd5 100644 --- a/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Camera/ROS2EditorCameraSystemComponent.h @@ -29,6 +29,7 @@ namespace ROS2 static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided); static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible); static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required); + static void GetDependentServices(AZ::ComponentDescriptor::DependencyArrayType& dependent); ////////////////////////////////////////////////////////////////////////// // Component overrides diff --git a/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp index 8f0f8de88..ef275fcce 100644 --- a/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp @@ -23,6 +23,15 @@ namespace ROS2 constexpr float ContactMaximumSeparation = 0.0001f; } + void ROS2ContactSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + // No configuration parameters + if (auto* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(1); + } + } + ROS2ContactSensorComponent::ROS2ContactSensorComponent() { TopicConfiguration tc; @@ -35,6 +44,9 @@ namespace ROS2 void ROS2ContactSensorComponent::Reflect(AZ::ReflectContext* context) { + ROS2ContactSensorConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); + if (auto* serialize = azrtti_cast(context)) { serialize->Class()->Version(2); @@ -197,4 +209,14 @@ namespace ROS2 m_activeContacts[event.m_body2->GetEntityId()] = AZStd::move(state); } } + + const ROS2ContactSensorConfiguration ROS2ContactSensorComponent::GetComponentConfiguration() const + { + return {}; + } + + void ROS2ContactSensorComponent::SetComponentConfiguration([[maybe_unused]] const ROS2ContactSensorConfiguration configuration) + { + // No configuration parameters + } } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.h b/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.h index 19c836258..a2710129d 100644 --- a/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.h @@ -23,11 +23,23 @@ namespace ROS2 { + //! This sensor does not have any configuration parameters. + //! This structure is required to be able to use the SensorConfigurationRequestBus. + struct ROS2ContactSensorConfiguration + { + AZ_RTTI(ROS2ContactSensorConfiguration, "{f7ebfb9d-2d49-4059-a88f-f577a52cd68c}"); + + ROS2ContactSensorConfiguration() = default; + virtual ~ROS2ContactSensorConfiguration() = default; + + static void Reflect(AZ::ReflectContext* context); + }; + //! Contact sensor detects collisions between two objects. //! It reports the location of the contact associated forces. //! This component publishes a contact_sensor topic. //! It doesn't measure torque. - class ROS2ContactSensorComponent : public ROS2SensorComponentBase + class ROS2ContactSensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2ContactSensorComponent, ROS2Sensors::ROS2ContactSensorComponentTypeId, SensorBaseType); @@ -48,6 +60,10 @@ namespace ROS2 void AddNewContact(const AzPhysics::CollisionEvent& event); + // Configuration Bus overrides + void SetComponentConfiguration(const ROS2ContactSensorConfiguration configuration) override; + const ROS2ContactSensorConfiguration GetComponentConfiguration() const override; + AZ::EntityId m_entityId; AZStd::string m_entityName = ""; diff --git a/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp index 2100877ac..0080e61cf 100644 --- a/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.cpp @@ -22,8 +22,19 @@ namespace ROS2 const char* GNSSMsgType = "sensor_msgs::msg::NavSatFix"; } + void ROS2GNSSSensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(1); + } + } + void ROS2GNSSSensorComponent::Reflect(AZ::ReflectContext* context) { + ROS2GNSSSensorConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); + if (auto* serialize = azrtti_cast(context)) { serialize->Class()->Version(4); @@ -111,4 +122,14 @@ namespace ROS2 m_gnssPublisher->publish(m_gnssMsg); } + void ROS2GNSSSensorComponent::SetComponentConfiguration([[maybe_unused]] const ROS2GNSSSensorConfiguration configuration) + { + // This sensor does not have any configuration parameters. + } + const ROS2GNSSSensorConfiguration ROS2GNSSSensorComponent::GetComponentConfiguration() const + { + // This sensor does not have any configuration parameters. + return {}; + } + } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.h b/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.h index 270cd8f43..c266cdafd 100644 --- a/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/GNSS/ROS2GNSSSensorComponent.h @@ -17,11 +17,23 @@ namespace ROS2 { + //! This sensor does not have any configuration parameters. + //! This structure is required to be able to use the SensorConfigurationRequestBus. + struct ROS2GNSSSensorConfiguration + { + AZ_RTTI(ROS2GNSSSensorConfiguration, "{e85947cc-2821-4fa7-93d0-b3150999a588}"); + + ROS2GNSSSensorConfiguration() = default; + virtual ~ROS2GNSSSensorConfiguration() = default; + + static void Reflect(AZ::ReflectContext* context); + }; + //! Global Navigation Satellite Systems (GNSS) sensor component class //! It provides NavSatFix data of sensor's position in GNSS frame which is defined by GNSS origin offset //! Offset is provided as latitude [deg], longitude [deg], altitude [m] of o3de global frame //! It is assumed that o3de global frame overlaps with ENU coordinate system - class ROS2GNSSSensorComponent : public ROS2SensorComponentBase + class ROS2GNSSSensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2GNSSSensorComponent, ROS2Sensors::ROS2GNSSSensorComponentTypeId, SensorBaseType); @@ -45,6 +57,10 @@ namespace ROS2 //! @return Current entity position. [[nodiscard]] AZ::Transform GetCurrentPose() const; + // Configuration Bus overrides + void SetComponentConfiguration(const ROS2GNSSSensorConfiguration configuration) override; + const ROS2GNSSSensorConfiguration GetComponentConfiguration() const override; + std::shared_ptr> m_gnssPublisher; sensor_msgs::msg::NavSatFix m_gnssMsg; }; diff --git a/Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp b/Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp index df6cce9b9..9227bdf70 100644 --- a/Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp +++ b/Gems/ROS2Sensors/Code/Source/Imu/ImuSensorConfiguration.cpp @@ -6,9 +6,9 @@ * */ -#include "ImuSensorConfiguration.h" #include #include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp index 052093718..bee005581 100644 --- a/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp @@ -32,6 +32,7 @@ namespace ROS2 void ROS2ImuSensorComponent::Reflect(AZ::ReflectContext* context) { ImuSensorConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); if (auto* serializeContext = azrtti_cast(context)) { @@ -82,6 +83,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(); @@ -89,9 +91,7 @@ namespace ROS2 const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic); m_imuPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig.GetQoS()); - m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance); - m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance); - m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance); + ConfigureSensor(); StartSensor( m_sensorConfiguration.m_frequency, @@ -201,4 +201,22 @@ namespace ROS2 return covarianceMatrix; } + void ROS2ImuSensorComponent::ConfigureSensor() + { + m_linearAccelerationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_linearAccelerationVariance); + m_angularVelocityCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_angularVelocityVariance); + m_orientationCovariance = ToDiagonalCovarianceMatrix(m_imuConfiguration.m_orientationVariance); + } + + const ImuSensorConfiguration ROS2ImuSensorComponent::GetComponentConfiguration() const + { + return m_imuConfiguration; + } + + void ROS2ImuSensorComponent::SetComponentConfiguration(const ImuSensorConfiguration configuration) + { + m_imuConfiguration = configuration; + ConfigureSensor(); + } + } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h b/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h index cc925152d..8f8c3b141 100644 --- a/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.h @@ -14,18 +14,18 @@ #include #include #include +#include +#include #include #include #include -#include "ImuSensorConfiguration.h" - namespace ROS2 { //! An IMU (Inertial Measurement Unit) sensor Component. //! IMUs typically include gyroscopes, accelerometers and magnetometers. This component encapsulates data //! acquisition and its publishing to ROS2 ecosystem. IMU Component requires ROS2FrameComponent. - class ROS2ImuSensorComponent : public ROS2SensorComponentBase + class ROS2ImuSensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2ImuSensorComponent, ROS2Sensors::ROS2ImuSensorComponentTypeId, SensorBaseType); @@ -62,6 +62,12 @@ namespace ROS2 AZ::Matrix3x3 ToDiagonalCovarianceMatrix(const AZ::Vector3& variance); + void ConfigureSensor(); + + // Configuration Bus + void SetComponentConfiguration(const ImuSensorConfiguration configuration) override; + const ImuSensorConfiguration GetComponentConfiguration() const override; + // Handle to the simulated physical body AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; }; diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.cpp index 12dd78f11..b80e0389c 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.cpp @@ -188,6 +188,11 @@ namespace ROS2 m_implementationToRaycasterMap.clear(); } + void LidarCore::UpdateConfig(const LidarSensorConfiguration& lidarConfiguration) + { + m_lidarConfiguration = lidarConfiguration; + } + LidarId LidarCore::GetLidarRaycasterId() const { return m_lidarRaycasterId; diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h b/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h index 1e7ddc2c1..3ba73b84b 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarCore.h @@ -11,10 +11,10 @@ #include #include #include +#include #include #include "LidarRaycaster.h" -#include "LidarSensorConfiguration.h" namespace ROS2 { @@ -35,6 +35,10 @@ namespace ROS2 //! Deinitialize when deactivating the lidar. void Deinit(); + //! Update the lidar configuration. + //! This function should only be called when the lidar is not active. + void UpdateConfig(const LidarSensorConfiguration& lidarConfiguration); + //! Perform a raycast. //! @return Results of the raycast. AZStd::optional PerformRaycast(); diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp index f798c3a52..a52bc043e 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarRaycaster.cpp @@ -13,7 +13,7 @@ #include #include #include -#include +#include #include namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp index f476a4693..d630917d7 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarRegistrarSystemComponent.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp index ceaf74744..e572055a0 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarSensorConfiguration.cpp @@ -6,9 +6,10 @@ * */ -#include "LidarSensorConfiguration.h" +#include "Lidar/LidarRegistrarSystemComponent.h" #include #include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp index 8d44449fd..bea4e4f5d 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplate.cpp @@ -7,7 +7,7 @@ */ #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp index 3c963cc6a..06095a20c 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/LidarTemplateUtils.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index dd302a05b..8f5810b6a 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -144,4 +144,16 @@ namespace ROS2 m_laserScanPublisher->publish(message); } + + const LidarSensorConfiguration ROS2Lidar2DSensorComponent::GetComponentConfiguration() const + { + return m_lidarCore.m_lidarConfiguration; + } + + void ROS2Lidar2DSensorComponent::SetComponentConfiguration(const LidarSensorConfiguration configuration) + { + m_lidarCore.Deinit(); + m_lidarCore.UpdateConfig(configuration); + m_lidarCore.Init(GetEntityId()); + } } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h index 4a7e5b8cb..707459fe8 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.h @@ -25,7 +25,7 @@ namespace ROS2 //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection. //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation //! and data publishing. It requires ROS2FrameComponent. - class ROS2Lidar2DSensorComponent : public ROS2SensorComponentBase + class ROS2Lidar2DSensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2Lidar2DSensorComponent, ROS2Sensors::ROS2Lidar2DSensorComponentTypeId, SensorBaseType); @@ -46,6 +46,10 @@ namespace ROS2 void PublishRaycastResults(const RaycastResults& results); + // ConfigurationBus overrides + const LidarSensorConfiguration GetComponentConfiguration() const override; + void SetComponentConfiguration(const LidarSensorConfiguration configuration) override; + std::shared_ptr> m_laserScanPublisher; LidarCore m_lidarCore; diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index bba9fb46a..b4cae8d05 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -25,6 +25,8 @@ namespace ROS2 void ROS2LidarSensorComponent::Reflect(AZ::ReflectContext* context) { + ROS2SensorComponentBase::Reflect(context); + if (auto* serializeContext = azrtti_cast(context)) { serializeContext->Class()->Version(3)->Field( @@ -283,4 +285,17 @@ namespace ROS2 m_segmentationClassesPublisher->publish(segmentationClasses); } } + + void ROS2LidarSensorComponent::SetComponentConfiguration(const LidarSensorConfiguration configuration) + { + m_lidarCore.Deinit(); + m_lidarCore.UpdateConfig(configuration); + m_lidarCore.Init(GetEntityId()); + } + + const LidarSensorConfiguration ROS2LidarSensorComponent::GetComponentConfiguration() const + { + return m_lidarCore.m_lidarConfiguration; + } + } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h index 041852fa7..9bde136e3 100644 --- a/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -19,7 +20,6 @@ #include "LidarCore.h" #include "LidarRaycaster.h" -#include "LidarSensorConfiguration.h" namespace ROS2 { @@ -27,7 +27,7 @@ namespace ROS2 //! Lidars (Light Detection and Ranging) emit laser light and measure it after reflection. //! Lidar Component allows customization of lidar type and behavior and encapsulates both simulation //! and data publishing. It requires ROS2FrameComponent. - class ROS2LidarSensorComponent : public ROS2SensorComponentBase + class ROS2LidarSensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2LidarSensorComponent, ROS2Sensors::ROS2LidarSensorComponentTypeId, SensorBaseType); @@ -47,6 +47,10 @@ namespace ROS2 void FrequencyTick(); void PublishRaycastResults(const RaycastResults& results); + // ConfigurationBus overrides + const LidarSensorConfiguration GetComponentConfiguration() const override; + void SetComponentConfiguration(const LidarSensorConfiguration configuration) override; + bool m_canRaycasterPublish = false; std::shared_ptr> m_pointCloudPublisher; std::shared_ptr> m_segmentationClassesPublisher; diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp index 0a0c4df54..35e338c30 100644 --- a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometryCovariance.cpp @@ -6,11 +6,11 @@ * */ -#include "ROS2OdometryCovariance.h" #include #include #include #include +#include #include namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp index 84011ef07..e1e1ca86b 100644 --- a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp @@ -20,8 +20,19 @@ namespace ROS2 const char* OdometryMsgType = "nav_msgs::msg::Odometry"; } + void ROS2OdometrySensorConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto* serialize = azrtti_cast(context)) + { + serialize->Class()->Version(1); + } + } + void ROS2OdometrySensorComponent::Reflect(AZ::ReflectContext* context) { + ROS2OdometrySensorConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); + if (auto* serialize = azrtti_cast(context)) { serialize->Class()->Version(2); @@ -131,4 +142,16 @@ namespace ROS2 m_odometryPublisher.reset(); ROS2SensorComponentBase::Deactivate(); } + + const ROS2OdometrySensorConfiguration ROS2OdometrySensorComponent::GetComponentConfiguration() const + { + // This sensor does not have any configuration parameters. + return {}; + } + + void ROS2OdometrySensorComponent::SetComponentConfiguration([[maybe_unused]] ROS2OdometrySensorConfiguration configuration) + { + // This sensor does not have any configuration parameters. + } + } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.h b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.h index d82aa26e2..89196e073 100644 --- a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.h +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.h @@ -17,11 +17,23 @@ namespace ROS2 { + //! This sensor does not have any configuration parameters. + //! This structure is required to be able to use the SensorConfigurationRequestBus. + struct ROS2OdometrySensorConfiguration + { + AZ_RTTI(ROS2OdometrySensorConfiguration, "{04c5d154-6fa7-4c12-a4e3-025ac853198a}"); + + ROS2OdometrySensorConfiguration() = default; + virtual ~ROS2OdometrySensorConfiguration() = default; + + static void Reflect(AZ::ReflectContext* context); + }; + //! Odometry sensor Component. //! It constructs and publishes an odometry message, which contains information about vehicle velocity and position in space. //! This is a ground truth "sensor", which can be helpful for development and machine learning. //! @see nav_msgs package. - class ROS2OdometrySensorComponent : public ROS2SensorComponentBase + class ROS2OdometrySensorComponent : public ROS2SensorComponentBase { public: AZ_COMPONENT(ROS2OdometrySensorComponent, ROS2Sensors::ROS2OdometrySensorComponent, SensorBaseType); @@ -37,6 +49,10 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: + // ConfigurationBus overrides + const ROS2OdometrySensorConfiguration GetComponentConfiguration() const override; + void SetComponentConfiguration(const ROS2OdometrySensorConfiguration configuration) override; + AzPhysics::SimulatedBodyHandle m_bodyHandle = AzPhysics::InvalidSimulatedBodyHandle; std::shared_ptr> m_odometryPublisher; nav_msgs::msg::Odometry m_odometryMsg; diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp index 43b9c3508..0236e1aaa 100644 --- a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.cpp @@ -7,10 +7,13 @@ */ #include "ROS2WheelOdometry.h" -#include "Odometry/ROS2OdometryCovariance.h" +#include +#include +#include #include #include #include +#include namespace ROS2 { @@ -19,16 +22,95 @@ namespace ROS2 const char* WheelOdometryMsgType = "nav_msgs::msg::Odometry"; } + // Manual conversion between version without a configuration struct and with a configuration struct. + // This is done to maintain backwards compatibility with older versions of the component. + // This function checks if in the prefab there exits a member called "Twist covariance" or "Pose covariance". + // If it does, it will load the values into the new configuration struct. + // If it doesn't, it will treat the loaded json as it would load the new version of the component. + // Checking old members is used instead of checking if there is a member called "Odometry configuration" + // because is default values are used O3DE does not create an empty member and initializes the component with default values. + // Meaning if there does not exist a member called "Odometry configuration" this component could use old values or default ones. + AZ::JsonSerializationResult::Result JsonROS2WheelOdometryComponentConfigSerializer::Load( + void* outputValue, const AZ::Uuid& outputValueTypeId, const rapidjson::Value& inputValue, AZ::JsonDeserializerContext& context) + { + namespace JSR = AZ::JsonSerializationResult; + + auto configInstance = reinterpret_cast(outputValue); + AZ_Assert(configInstance, "Output value for JsonROS2WheelOdometryComponentConfigSerializer can't be null."); + + JSR::ResultCode result(JSR::Tasks::ReadField); + + const bool hasOldMembers = inputValue.HasMember("Twist covariance") || inputValue.HasMember("Pose covariance"); + if (hasOldMembers) + { + { + JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField( + &configInstance->m_odometryConfiguration.m_poseCovariance, + azrtti_typeidm_odometryConfiguration.m_poseCovariance)>(), + inputValue, + "Pose covariance", + context); + + result.Combine(componentIdLoadResult); + } + + { + JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField( + &configInstance->m_odometryConfiguration.m_twistCovariance, + azrtti_typeidm_odometryConfiguration.m_twistCovariance)>(), + inputValue, + "Twist covariance", + context); + + result.Combine(componentIdLoadResult); + } + } + else + { + { + JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField( + &configInstance->m_odometryConfiguration, + azrtti_typeidm_odometryConfiguration)>(), + inputValue, + "Odometry configuration", + context); + + result.Combine(componentIdLoadResult); + } + } + { + JSR::ResultCode componentIdLoadResult = ContinueLoadingFromJsonObjectField( + &configInstance->m_sensorConfiguration, + azrtti_typeidm_sensorConfiguration)>(), + inputValue, + "Sensor configuration", + context); + + result.Combine(componentIdLoadResult); + } + + return context.Report( + result, + result.GetProcessing() != JSR::Processing::Halted ? "Successfully loaded ROS2FrameComponent information." + : "Failed to load ROS2FrameComponent information."); + } + + AZ_CLASS_ALLOCATOR_IMPL(JsonROS2WheelOdometryComponentConfigSerializer, AZ::SystemAllocator); + void ROS2WheelOdometryComponent::Reflect(AZ::ReflectContext* context) { - ROS2OdometryCovariance::Reflect(context); + if (auto jsonContext = azrtti_cast(context)) + { + jsonContext->Serializer()->HandlesType(); + } + + ROS2WheelOdometryConfiguration::Reflect(context); + ROS2SensorComponentBase::Reflect(context); if (auto* serialize = azrtti_cast(context)) { - serialize->Class() - ->Version(2) - ->Field("Twist covariance", &ROS2WheelOdometryComponent::m_twistCovariance) - ->Field("Pose covariance", &ROS2WheelOdometryComponent::m_poseCovariance); + serialize->Class()->Version(2)->Field( + "Odometry configuration", &ROS2WheelOdometryComponent::m_odometryConfiguration); if (auto* editContext = serialize->GetEditContext()) { @@ -40,14 +122,10 @@ namespace ROS2 ->Attribute(AZ::Edit::Attributes::ViewportIcon, "Editor/Icons/Components/Viewport/ROS2WheelOdometrySensor.svg") ->DataElement( AZ::Edit::UIHandlers::Default, - &ROS2WheelOdometryComponent::m_twistCovariance, - "Twist covariance", - "Set ROS twist covariance") - ->DataElement( - AZ::Edit::UIHandlers::Default, - &ROS2WheelOdometryComponent::m_poseCovariance, - "Pose covariance", - "Set ROS pose covariance"); + &ROS2WheelOdometryComponent::m_odometryConfiguration, + "Odometry configuration", + "Odometry sensor configuration") + ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly); } } } @@ -72,7 +150,7 @@ namespace ROS2 { m_odometryMsg.pose.pose.position = ROS2Conversions::ToROS2Point(m_robotPose); m_odometryMsg.pose.pose.orientation = ROS2Conversions::ToROS2Quaternion(m_robotRotation); - m_odometryMsg.pose.covariance = m_poseCovariance.GetRosCovariance(); + m_odometryMsg.pose.covariance = m_odometryConfiguration.m_poseCovariance.GetRosCovariance(); m_odometryPublisher->publish(m_odometryMsg); } @@ -87,7 +165,7 @@ namespace ROS2 m_odometryMsg.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); m_odometryMsg.twist.twist.linear = ROS2Conversions::ToROS2Vector3(vt.first); m_odometryMsg.twist.twist.angular = ROS2Conversions::ToROS2Vector3(vt.second); - m_odometryMsg.twist.covariance = m_twistCovariance.GetRosCovariance(); + m_odometryMsg.twist.covariance = m_odometryConfiguration.m_twistCovariance.GetRosCovariance(); if (m_sensorConfiguration.m_frequency > 0) { @@ -137,4 +215,14 @@ namespace ROS2 m_odometryPublisher.reset(); ROS2SensorComponentBase::Deactivate(); } + + const ROS2WheelOdometryConfiguration ROS2WheelOdometryComponent::GetComponentConfiguration() const + { + return m_odometryConfiguration; + } + + void ROS2WheelOdometryComponent::SetComponentConfiguration(const ROS2WheelOdometryConfiguration configuration) + { + m_odometryConfiguration = configuration; + } } // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h index 45aa15779..6cb9a2a30 100644 --- a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometry.h @@ -9,20 +9,34 @@ #include #include +#include #include #include #include -#include "ROS2OdometryCovariance.h" - namespace ROS2 { + class JsonROS2WheelOdometryComponentConfigSerializer : public AZ::BaseJsonSerializer + { + public: + AZ_RTTI(JsonROS2WheelOdometryComponentConfigSerializer, "{3f7f9be6-d964-4a55-b856-c03cc5754df0}", AZ::BaseJsonSerializer); + AZ_CLASS_ALLOCATOR_DECL; + + AZ::JsonSerializationResult::Result Load( + void* outputValue, + const AZ::Uuid& outputValueTypeId, + const rapidjson::Value& inputValue, + AZ::JsonDeserializerContext& context) override; + }; + //! Wheel odometry sensor component. //! It constructs and publishes an odometry message, which contains information about the vehicle's velocity and position in space. //! This is a physical sensor that takes a vehicle's configuration and computes updates from the wheels' rotations. //! @see nav_msgs package. - class ROS2WheelOdometryComponent : public ROS2SensorComponentBase + class ROS2WheelOdometryComponent : public ROS2SensorComponentBase { + friend class JsonROS2WheelOdometryComponentConfigSerializer; + public: AZ_COMPONENT(ROS2WheelOdometryComponent, ROS2Sensors::ROS2WheelOdometryComponentTypeId, SensorBaseType); ROS2WheelOdometryComponent(); @@ -36,12 +50,15 @@ namespace ROS2 ////////////////////////////////////////////////////////////////////////// private: + // ConfigurationBus overrides + const ROS2WheelOdometryConfiguration GetComponentConfiguration() const override; + void SetComponentConfiguration(const ROS2WheelOdometryConfiguration configuration) override; + std::shared_ptr> m_odometryPublisher; nav_msgs::msg::Odometry m_odometryMsg; AZ::Vector3 m_robotPose{ 0 }; AZ::Quaternion m_robotRotation{ 0, 0, 0, 1 }; - ROS2OdometryCovariance m_poseCovariance; - ROS2OdometryCovariance m_twistCovariance; + ROS2WheelOdometryConfiguration m_odometryConfiguration; void OnOdometryEvent(); void OnPhysicsEvent(float physicsDeltaTime); diff --git a/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometryConfiguration.cpp b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometryConfiguration.cpp new file mode 100644 index 000000000..e4bb8141c --- /dev/null +++ b/Gems/ROS2Sensors/Code/Source/Odometry/ROS2WheelOdometryConfiguration.cpp @@ -0,0 +1,42 @@ +/* + * 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 +#include +#include + +namespace ROS2 +{ + void ROS2WheelOdometryConfiguration::Reflect(AZ::ReflectContext* context) + { + ROS2OdometryCovariance::Reflect(context); + + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->Version(1) + ->Field("Pose covariance", &ROS2WheelOdometryConfiguration::m_poseCovariance) + ->Field("Twist covariance", &ROS2WheelOdometryConfiguration::m_twistCovariance); + + if (AZ::EditContext* editContext = serialize->GetEditContext()) + { + editContext->Class("ROS2 Wheel Odometry configuration", "Wheel odometry sensor configuration") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2WheelOdometryConfiguration::m_poseCovariance, + "Pose covariance", + "Set ROS pose covariance") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &ROS2WheelOdometryConfiguration::m_twistCovariance, + "Twist covariance", + "Set ROS twist covariance"); + } + } + } +} // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp b/Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp index 088ab466c..cb83988c4 100644 --- a/Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp +++ b/Gems/ROS2Sensors/Code/Source/ROS2SensorsModuleInterface.cpp @@ -45,8 +45,6 @@ namespace ROS2Sensors m_descriptors.end(), { ROS2SensorsSystemComponent::CreateDescriptor(), - ROS2::ROS2SensorComponentBase::CreateDescriptor(), - ROS2::ROS2SensorComponentBase::CreateDescriptor(), ROS2::ROS2CameraSensorComponent::CreateDescriptor(), ROS2::ROS2SystemCameraComponent::CreateDescriptor(), ROS2::ROS2ImageEncodingConversionComponent::CreateDescriptor(), diff --git a/Gems/ROS2Sensors/Code/Source/Sensor/SensorConfiguration.cpp b/Gems/ROS2Sensors/Code/Source/Sensor/SensorConfiguration.cpp index 61a9be998..92cdfd657 100644 --- a/Gems/ROS2Sensors/Code/Source/Sensor/SensorConfiguration.cpp +++ b/Gems/ROS2Sensors/Code/Source/Sensor/SensorConfiguration.cpp @@ -23,7 +23,8 @@ namespace ROS2 ->Field("Visualize", &SensorConfiguration::m_visualize) ->Field("Publishing Enabled", &SensorConfiguration::m_publishingEnabled) ->Field("Frequency (HZ)", &SensorConfiguration::m_frequency) - ->Field("Publishers", &SensorConfiguration::m_publishersConfigurations); + ->Field("Publishers", &SensorConfiguration::m_publishersConfigurations) + ->Field("Configurable from ROS2", &SensorConfiguration::m_configurableFromROS2); if (AZ::EditContext* ec = serializeContext->GetEditContext()) { @@ -35,6 +36,11 @@ namespace ROS2 &SensorConfiguration::m_publishingEnabled, "Publishing Enabled", "Toggle publishing for topic") + ->DataElement( + AZ::Edit::UIHandlers::Default, + &SensorConfiguration::m_configurableFromROS2, + "Configurable from ROS2", + "Enables JSON configuration from ROS2") ->DataElement( AZ::Edit::UIHandlers::Default, &SensorConfiguration::m_frequency, "Frequency", "Frequency of publishing [Hz]") ->Attribute(AZ::Edit::Attributes::Min, SensorConfiguration::m_minFrequency) diff --git a/Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp b/Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp index 723b03da6..879da7ad0 100644 --- a/Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp +++ b/Gems/ROS2Sensors/Code/Source/Sensor/SensorHelpers.cpp @@ -6,6 +6,7 @@ * */ +#include "ROS2Sensors/ROS2SensorsTypeIds.h" #include #include #include @@ -60,16 +61,13 @@ namespace ROS2 bool IsComponentROS2Sensor(const AZ::Component* component) { - // In ROS2Sensors 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*>(component)) - { - return true; - } - if (azrtti_cast*>(component)) + // The Base component implements the GetUndelyingComponentType() method. This enables us to check if the component + // is a ROS2 sensor, without the need to specify any template parameters. + if (component->GetUnderlyingComponentType() == AZ::TypeId(ROS2Sensors::ROS2SensorComponentBaseTypeId)) { return true; } + return false; } }; // namespace ROS2 diff --git a/Gems/ROS2Sensors/Code/Tests/Odometry/ROS2WheelOdometrySerializationTest.cpp b/Gems/ROS2Sensors/Code/Tests/Odometry/ROS2WheelOdometrySerializationTest.cpp new file mode 100644 index 000000000..a994024ca --- /dev/null +++ b/Gems/ROS2Sensors/Code/Tests/Odometry/ROS2WheelOdometrySerializationTest.cpp @@ -0,0 +1,138 @@ +/* + * 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 +#include +#include +#include +#include + +namespace UnitTest +{ + class ROS2SensorsTestFixture : public ::testing::Test + { + }; + + // Store test for the old schema, where there is no configuration class. + TEST_F(ROS2SensorsTestFixture, Deserialize_WhenOldSchemaProvided_StoresSuccessfully) + { + ROS2::ROS2WheelOdometryComponent wheelOdometryComponent; + + rapidjson::Document jsonDocument(rapidjson::kObjectType); + jsonDocument.Parse(R"({ + "Twist covariance": { + "Linear covariance": [ + 123.0, + 456.0, + 789.0 + ], + "Angular covariance": [ + 987.0, + 654.0, + 321.0 + ] + }, + "Pose covariance": { + "Linear covariance": [ + 321.0, + 654.0, + 987.0 + ], + "Angular covariance": [ + 789.0, + 456.0, + 123.0 + ] + } + })"); + + AZ::JsonDeserializerSettings settings; + auto storeResult = AZ::JsonSerialization::Load(wheelOdometryComponent, jsonDocument, settings); + + EXPECT_EQ(storeResult.GetOutcome(), AZ::JsonSerializationResult::Outcomes::PartialDefaults); + + rapidjson::Document document(rapidjson::kObjectType); + document.Parse("{}"); + + AZ::JsonSerializerSettings serializerSettings; + serializerSettings.m_keepDefaults = true; + + AZ::JsonSerializationResult::ResultCode resultCode = + AZ::JsonSerialization::Store(document, document.GetAllocator(), wheelOdometryComponent, serializerSettings); + + EXPECT_EQ(resultCode.GetOutcome(), AZ::JsonSerializationResult::Outcomes::Success); + + rapidjson::StringBuffer buffer; + rapidjson::Writer writer(buffer); + document.Accept(writer); + + const char* expectedOutput = + R"~({"Id":0,"SensorConfiguration":{"Visualize":true,"Publishing Enabled":true,"Frequency (HZ)":10.0,"Publishers":{"nav_msgs::msg::Odometry":{"Type":"nav_msgs::msg::Odometry","Topic":"odom","QoS":{"Reliability":2,"Durability":2,"Depth":5}}}},"Odometry configuration":{"Pose covariance":{"Linear covariance":[321.0,654.0,987.0],"Angular covariance":[789.0,456.0,123.0]},"Twist covariance":{"Linear covariance":[123.0,456.0,789.0],"Angular covariance":[987.0,654.0,321.0]}}})~"; + EXPECT_STREQ(buffer.GetString(), expectedOutput); + } + + // Store test for the new schema, where there is a configuration class. + TEST_F(ROS2SensorsTestFixture, Deserialize_WhenNewSchemaProvided_StoresSuccessfully) + { + ROS2::ROS2WheelOdometryComponent wheelOdometryComponent; + + rapidjson::Document jsonDocument(rapidjson::kObjectType); + jsonDocument.Parse(R"({ + "Odometry configuration": { + "Twist covariance": { + "Linear covariance": [ + 123.0, + 456.0, + 789.0 + ], + "Angular covariance": [ + 987.0, + 654.0, + 321.0 + ] + }, + "Pose covariance": { + "Linear covariance": [ + 321.0, + 654.0, + 987.0 + ], + "Angular covariance": [ + 789.0, + 456.0, + 123.0 + ] + } + } + })"); + + AZ::JsonDeserializerSettings settings; + auto storeResult = AZ::JsonSerialization::Load(wheelOdometryComponent, jsonDocument, settings); + + EXPECT_EQ(storeResult.GetOutcome(), AZ::JsonSerializationResult::Outcomes::PartialDefaults); + + rapidjson::Document document(rapidjson::kObjectType); + document.Parse("{}"); + + AZ::JsonSerializerSettings serializerSettings; + serializerSettings.m_keepDefaults = true; + + AZ::JsonSerializationResult::ResultCode resultCode = + AZ::JsonSerialization::Store(document, document.GetAllocator(), wheelOdometryComponent, serializerSettings); + + EXPECT_EQ(resultCode.GetOutcome(), AZ::JsonSerializationResult::Outcomes::Success); + + rapidjson::StringBuffer buffer; + rapidjson::Writer writer(buffer); + document.Accept(writer); + + const char* expectedOutput = + R"~({"Id":0,"SensorConfiguration":{"Visualize":true,"Publishing Enabled":true,"Frequency (HZ)":10.0,"Publishers":{"nav_msgs::msg::Odometry":{"Type":"nav_msgs::msg::Odometry","Topic":"odom","QoS":{"Reliability":2,"Durability":2,"Depth":5}}}},"Odometry configuration":{"Pose covariance":{"Linear covariance":[321.0,654.0,987.0],"Angular covariance":[789.0,456.0,123.0]},"Twist covariance":{"Linear covariance":[123.0,456.0,789.0],"Angular covariance":[987.0,654.0,321.0]}}})~"; + EXPECT_STREQ(buffer.GetString(), expectedOutput); + } +} // namespace UnitTest diff --git a/Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp b/Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp index 40217ff9b..2f2ed4532 100644 --- a/Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp +++ b/Gems/ROS2Sensors/Code/Tests/Tools/ROS2SensorsEditorTest.cpp @@ -6,6 +6,50 @@ * */ +#include #include +#include +#include -AZ_UNIT_TEST_HOOK(DEFAULT_UNIT_TEST_ENV); +namespace UnitTest +{ + class ROS2SensorsTestEnvironment : public AZ::Test::GemTestEnvironment + { + // AZ::Test::GemTestEnvironment overrides ... + void AddGemsAndComponents() override; + AZ::ComponentApplication* CreateApplicationInstance() override; + void PostSystemEntityActivate() override; + + public: + ROS2SensorsTestEnvironment() = default; + ~ROS2SensorsTestEnvironment() override = default; + }; + + void ROS2SensorsTestEnvironment::AddGemsAndComponents() + { + AddActiveGems(AZStd::to_array({ "ROS2", "ROS2Sensors" })); + AddDynamicModulePaths({ "ROS2", "ROS2Sensors" }); + } + + AZ::ComponentApplication* ROS2SensorsTestEnvironment::CreateApplicationInstance() + { + // Using ToolsTestApplication to have AzFramework and AzToolsFramework components. + return aznew UnitTest::ToolsTestApplication("ROS2SensorsTestEnvironment"); + } + + void ROS2SensorsTestEnvironment::PostSystemEntityActivate() + { + AZ::UserSettingsComponentRequestBus::Broadcast(&AZ::UserSettingsComponentRequests::DisableSaveOnFinalize); + } +} // namespace UnitTest + +AZTEST_EXPORT int AZ_UNIT_TEST_HOOK_NAME(int argc, char** argv) +{ + ::testing::InitGoogleMock(&argc, argv); + AZ::Test::printUnusedParametersWarning(argc, argv); + AZ::Test::addTestEnvironments({ new UnitTest::ROS2SensorsTestEnvironment() }); + int result = RUN_ALL_TESTS(); + return result; +} + +IMPLEMENT_TEST_EXECUTABLE_MAIN(); diff --git a/Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake b/Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake index 0f1dc1312..583c4dfbe 100644 --- a/Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake +++ b/Gems/ROS2Sensors/Code/ros2sensors_api_files.cmake @@ -7,16 +7,24 @@ # set(FILES - Include/ROS2Sensors/ROS2SensorsTypeIds.h - Include/ROS2Sensors/Sensor/SensorConfiguration.h - Include/ROS2Sensors/Sensor/SensorConfigurationRequestBus.h - Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h - Include/ROS2Sensors/Sensor/SensorHelper.h Include/ROS2Sensors/Camera/CameraCalibrationRequestBus.h Include/ROS2Sensors/Camera/CameraPostProcessingRequestBus.h + Include/ROS2Sensors/Camera/CameraSensorConfiguration.h + Include/ROS2Sensors/Configuration/ConfigurationBus.h Include/ROS2Sensors/GNSS/GNSSPostProcessingRequestBus.h + Include/ROS2Sensors/Imu/ImuSensorConfiguration.h Include/ROS2Sensors/Lidar/ClassSegmentationBus.h Include/ROS2Sensors/Lidar/LidarRaycasterBus.h - Include/ROS2Sensors/Lidar/LidarSystemBus.h Include/ROS2Sensors/Lidar/LidarRegistrarBus.h + Include/ROS2Sensors/Lidar/LidarSensorConfiguration.h + Include/ROS2Sensors/Lidar/LidarSystemBus.h + Include/ROS2Sensors/Lidar/LidarTemplate.h + Include/ROS2Sensors/Lidar/LidarTemplateUtils.h + Include/ROS2Sensors/Odometry/ROS2OdometryCovariance.h + Include/ROS2Sensors/Odometry/ROS2WheelOdometryConfiguration.h + Include/ROS2Sensors/ROS2SensorsTypeIds.h + Include/ROS2Sensors/Sensor/ROS2SensorComponentBase.h + Include/ROS2Sensors/Sensor/SensorConfiguration.h + Include/ROS2Sensors/Sensor/SensorConfigurationRequestBus.h + Include/ROS2Sensors/Sensor/SensorHelper.h ) diff --git a/Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake b/Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake index 564e5dfc2..99a4c3805 100644 --- a/Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake +++ b/Gems/ROS2Sensors/Code/ros2sensors_editor_tests_files.cmake @@ -8,4 +8,5 @@ set(FILES Tests/Tools/ROS2SensorsEditorTest.cpp + Tests/Odometry/ROS2WheelOdometrySerializationTest.cpp ) diff --git a/Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake b/Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake index 059d6a5b4..b4504dd58 100644 --- a/Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake +++ b/Gems/ROS2Sensors/Code/ros2sensors_private_files.cmake @@ -7,35 +7,29 @@ # set(FILES - Source/ROS2SensorsModuleInterface.cpp - Source/ROS2SensorsModuleInterface.h - Source/Clients/ROS2SensorsSystemComponent.cpp - Source/Clients/ROS2SensorsSystemComponent.h - Source/Sensor/SensorConfiguration.cpp - Source/Sensor/SensorHelpers.cpp - Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.cpp - Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.h Source/Camera/CameraConstants.h Source/Camera/CameraPublishers.cpp Source/Camera/CameraPublishers.h Source/Camera/CameraSensor.cpp Source/Camera/CameraSensor.h + Source/Camera/CameraSensorConfiguration.cpp Source/Camera/CameraSensorDescription.cpp Source/Camera/CameraSensorDescription.h - Source/Camera/CameraSensorConfiguration.cpp - Source/Camera/CameraSensorConfiguration.h Source/Camera/CameraUtilities.cpp Source/Camera/CameraUtilities.h + Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.cpp + Source/Camera/PostProcessing/ROS2ImageEncodingConversionComponent.h Source/Camera/ROS2CameraSensorComponent.cpp Source/Camera/ROS2CameraSensorComponent.h Source/Camera/ROS2CameraSystemComponent.cpp Source/Camera/ROS2CameraSystemComponent.h + Source/Clients/ROS2SensorsSystemComponent.cpp + Source/Clients/ROS2SensorsSystemComponent.h Source/ContactSensor/ROS2ContactSensorComponent.cpp Source/ContactSensor/ROS2ContactSensorComponent.h Source/GNSS/ROS2GNSSSensorComponent.cpp Source/GNSS/ROS2GNSSSensorComponent.h Source/Imu/ImuSensorConfiguration.cpp - Source/Imu/ImuSensorConfiguration.h Source/Imu/ROS2ImuSensorComponent.cpp Source/Imu/ROS2ImuSensorComponent.h Source/Lidar/ClassSegmentationConfigurationComponent.cpp @@ -47,23 +41,24 @@ set(FILES Source/Lidar/LidarRegistrarSystemComponent.cpp Source/Lidar/LidarRegistrarSystemComponent.h Source/Lidar/LidarSensorConfiguration.cpp - Source/Lidar/LidarSensorConfiguration.h Source/Lidar/LidarSystem.cpp Source/Lidar/LidarSystem.h Source/Lidar/LidarTemplate.cpp - Source/Lidar/LidarTemplate.h Source/Lidar/LidarTemplateUtils.cpp - Source/Lidar/LidarTemplateUtils.h Source/Lidar/PointCloudMessageBuilder.cpp Source/Lidar/PointCloudMessageBuilder.h Source/Lidar/ROS2Lidar2DSensorComponent.cpp Source/Lidar/ROS2Lidar2DSensorComponent.h Source/Lidar/ROS2LidarSensorComponent.cpp Source/Lidar/ROS2LidarSensorComponent.h + Source/Odometry/ROS2OdometryCovariance.cpp Source/Odometry/ROS2OdometrySensorComponent.cpp Source/Odometry/ROS2OdometrySensorComponent.h Source/Odometry/ROS2WheelOdometry.cpp Source/Odometry/ROS2WheelOdometry.h - Source/Odometry/ROS2OdometryCovariance.cpp - Source/Odometry/ROS2OdometryCovariance.h + Source/Odometry/ROS2WheelOdometryConfiguration.cpp + Source/Sensor/SensorConfiguration.cpp + Source/Sensor/SensorHelpers.cpp + Source/ROS2SensorsModuleInterface.cpp + Source/ROS2SensorsModuleInterface.h ) diff --git a/repo.json b/repo.json index 42c3c0e2b..067d00fdd 100644 --- a/repo.json +++ b/repo.json @@ -166,7 +166,7 @@ "Atom_Feature_Common", "Atom_Component_DebugCamera", "CommonFeaturesAtom", - "PhysX", + "PhysX5", "StartingPointInput" ], "restricted": "ROS2",