Sync changes from development before merge

Signed-off-by: Jan Hanca <[email protected]>

Gems/ROS2/Code/Include/ROS2/Clock/ITimeSource.h

@@ -8,6 +8,8 @@
 #pragma once
 
 #include <builtin_interfaces/msg/time.hpp>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/string/string.h>
 
 namespace ROS2
 {
@@ -19,6 +21,11 @@ namespace ROS2
 
         virtual ~ITimeSource() = default;
 
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time & time) = 0;
+
         //! Get time as ROS 2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const = 0;

Gems/ROS2/Code/Include/ROS2/Clock/ROS2Clock.h

@@ -8,8 +8,10 @@
 #pragma once
 
 #include "ITimeSource.h"
+#include <AzCore/Outcome/Outcome.h>
 #include <AzCore/std/chrono/chrono.h>
 #include <AzCore/std/smart_ptr/unique_ptr.h>
+#include <AzCore/std/string/string.h>
 #include <rclcpp/publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 
@@ -21,7 +23,6 @@ namespace ROS2
     //! the /use_sim_time parameter set to true.
     class ROS2Clock
     {
-
     public:
         ROS2Clock();
         ROS2Clock(AZStd::unique_ptr<ITimeSource> timeSource, bool publishClock);
@@ -32,6 +33,11 @@ namespace ROS2
 
         builtin_interfaces::msg::Time GetROSTimestamp() const;
 
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) const;
+
         //! Update time in the ROS 2 ecosystem.
         //! This will publish current time to the ROS 2 `/clock` topic, if Clock is configured to do it.
         void Tick();

Gems/ROS2/Code/Include/ROS2/Clock/ROS2TimeSource.h

@@ -25,6 +25,12 @@ namespace ROS2
         //! Get ROS 2 time as ROS2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;
+
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
     };
 
 } // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Clock/RealTimeSource.h

@@ -25,6 +25,11 @@ namespace ROS2
         virtual void Activate() override{};
         virtual void Deactivate() override{};
 
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
         //! Get simulation time as ROS 2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;

Gems/ROS2/Code/Include/ROS2/Clock/SimulationTimeSource.h

@@ -30,6 +30,11 @@ namespace ROS2
         virtual void Activate() override;
         virtual void Deactivate() override;
 
+        //! Sets the time source to the given time.
+        //! @param time The time to set the time source to.
+        //! @return An outcome indicating success or failure.
+        virtual AZ::Outcome<void, AZStd::string> AdjustTime(const builtin_interfaces::msg::Time& time) override;
+
         //! Get ROS 2 time as ROS2 message.
         //! @see ROS2Requests::GetROSTimestamp() for more details.
         virtual builtin_interfaces::msg::Time GetROSTimestamp() const override;

Gems/ROS2/Code/Include/ROS2/Sensor/ROS2SensorComponentBase.h

@@ -153,7 +153,7 @@ namespace ROS2
         //! Returns a complete namespace for this sensor topics and frame ids.
         [[nodiscard]] AZStd::string GetNamespace() const
         {
-            auto* ros2Frame = GetEntity()->template FindComponent<ROS2::ROS2FrameComponent>();
+            auto* ros2Frame = GetEntity()->template FindComponent<ROS2FrameComponent>();
 
             return ros2Frame->GetNamespace();
         }
@@ -161,12 +161,12 @@ namespace ROS2
         //! Returns this sensor frame ID. The ID contains namespace.
         [[nodiscard]] AZStd::string GetFrameID() const
         {
-            auto* ros2Frame = GetEntity()->template FindComponent<ROS2::ROS2FrameComponent>();
+            auto* ros2Frame = GetEntity()->template FindComponent<ROS2FrameComponent>();
             return ros2Frame->GetFrameID();
         }
 
         SensorConfiguration m_sensorConfiguration; ///< Basic sensor configuration.
-        ROS2::EventSourceAdapter<EventSourceT> m_eventSourceAdapter; ///< Adapter for selected event source (see this class documentation).
+        EventSourceAdapter<EventSourceT> m_eventSourceAdapter; ///< Adapter for selected event source (see this class documentation).
 
         //! Handler for source event. Requires manual assignment and connecting to source event in derived class.
         typename EventSourceT::SourceEventHandlerType m_sourceEventHandler;

Gems/ROS2/Code/Source/Clock/ROS2Clock.cpp

@@ -58,4 +58,9 @@ namespace ROS2
         msg.clock = GetROSTimestamp();
         m_clockPublisher->publish(msg);
     }
+
+    AZ::Outcome<void, AZStd::string> ROS2Clock::AdjustTime(const builtin_interfaces::msg::Time & time) const
+    {
+        return m_timeSource->AdjustTime(time);
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/ROS2TimeSource.cpp

@@ -17,4 +17,10 @@ namespace ROS2
         builtin_interfaces::msg::Time timeStamp = ros2Node->get_clock()->now();
         return timeStamp;
     }
+
+    AZ::Outcome<void, AZStd::string> ROS2TimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        return AZ::Failure(AZStd::string("ROS2TimeSource does not support setting a specific time."));
+    }
+
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/RealTimeSource.cpp

@@ -33,4 +33,9 @@ namespace ROS2
             return 0;
         }
     }
+
+    AZ::Outcome<void, AZStd::string> RealTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        return AZ::Failure(AZStd::string("RealTimeSource does not support setting a specific time."));
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Clock/SimulationTimeSource.cpp

@@ -83,4 +83,11 @@ namespace ROS2
         timeStamp.nanosec = static_cast<uint32_t>((m_elapsed - timeStamp.sec) * 1e9);
         return timeStamp;
     }
+
+    AZ::Outcome<void, AZStd::string> SimulationTimeSource::AdjustTime(const builtin_interfaces::msg::Time& time)
+    {
+        const double timeSec = static_cast<double>(time.sec) + static_cast<double>(time.nanosec) * 1e-9;
+        m_elapsed = timeSec;
+        return AZ::Success();
+    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnPointComponent.h"
 #include "Spawner/ROS2SpawnPointComponentController.h"
 #include <AzCore/Component/Entity.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponent.h

@@ -6,9 +6,7 @@
  *
  */
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "Spawner/ROS2SpawnPointComponentController.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Math/Transform.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "Spawner/ROS2SpawnPointComponentController.h"
 #include "Spawner/ROS2SpawnerComponentController.h"
 #include <AzCore/Component/TransformBus.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointComponentController.h

@@ -6,9 +6,7 @@
  *
  */
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Math/Transform.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnPointEditorComponent.h"
 #include "Spawner/ROS2SpawnPointComponentController.h"
 #include "Spawner/ROS2SpawnerEditorComponent.h"

Gems/ROS2/Code/Source/Spawner/ROS2SpawnPointEditorComponent.h

@@ -6,9 +6,7 @@
  *
  */
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "Spawner/ROS2SpawnPointComponent.h"
 #include "Spawner/ROS2SpawnPointComponentController.h"
 #include "Spawner/ROS2SpawnerEditorComponent.h"

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnerComponent.h"
 #include "Spawner/ROS2SpawnerComponentController.h"
 #include <AzCore/Component/EntityId.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponent.h

@@ -6,9 +6,7 @@
  *
  */
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnPointComponent.h"
 #include "Spawner/ROS2SpawnerComponentController.h"
 #include <AzCore/Asset/AssetCommon.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnerComponentController.h"
 #include "Spawner/ROS2SpawnPointComponent.h"
 #include "Spawner/ROS2SpawnerComponent.h"

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerComponentController.h

@@ -7,9 +7,7 @@
  */
 
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2/Spawner/SpawnerBus.h"
 #include "ROS2SpawnPointComponent.h"
 #include <AzCore/Component/ComponentBus.h>

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2SpawnerEditorComponent.h"
 #include "AzCore/Debug/Trace.h"
 #include "ROS2SpawnPointEditorComponent.h"

Gems/ROS2/Code/Source/Spawner/ROS2SpawnerEditorComponent.h

@@ -6,9 +6,7 @@
  *
  */
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "Spawner/ROS2SpawnerComponent.h"
 #include "Spawner/ROS2SpawnerComponentController.h"
 #include <AzToolsFramework/ToolsComponents/EditorComponentAdapter.h>

Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.cpp

@@ -5,9 +5,7 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
+
 #include "ROS2ContactSensorComponent.h"
 #include <AzFramework/Physics/Collision/CollisionEvents.h>
 #include <AzFramework/Physics/Common/PhysicsSimulatedBody.h>

Gems/ROS2Sensors/Code/Source/ContactSensor/ROS2ContactSensorComponent.h

@@ -7,10 +7,6 @@
  */
 
 #pragma once
-#ifndef WITH_GAZEBO_MSGS
-static_assert(false, "This file should not be included in the build, without WITH_GAZEBO_MSGS defined.");
-#endif
-
 #include <AzCore/Component/EntityId.h>
 #include <AzCore/RTTI/ReflectContext.h>
 #include <AzCore/std/containers/unordered_map.h>