Added util to manipulate cameras. (#331)

* Added util to manipulate cameras.
---------

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

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -37,13 +37,13 @@
 #include <RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h>
 #include <RobotControl/ROS2RobotControlComponent.h>
 #include <RobotImporter/ROS2RobotImporterSystemComponent.h>
+#include <SimulationUtils/FollowingCameraComponent.h>
 #include <Spawner/ROS2SpawnPointComponent.h>
 #include <Spawner/ROS2SpawnerComponent.h>
 #include <VehicleDynamics/ModelComponents/AckermannModelComponent.h>
 #include <VehicleDynamics/ModelComponents/SkidSteeringModelComponent.h>
 #include <VehicleDynamics/VehicleModelComponent.h>
 #include <VehicleDynamics/WheelControllerComponent.h>
-
 namespace ROS2
 {
     class ROS2ModuleInterface : public AZ::Module
@@ -95,7 +95,8 @@ namespace ROS2
                     VacuumGripperComponent::CreateDescriptor(),
                     ConveyorBeltComponent::CreateDescriptor(),
                     FingerGripperComponent::CreateDescriptor(),
-                    ROS2ContactSensorComponent::CreateDescriptor()
+                    ROS2ContactSensorComponent::CreateDescriptor(),
+                    FollowingCameraComponent::CreateDescriptor(),
                 });
         }
 

Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp

@@ -0,0 +1,226 @@
+/*
+ * 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 "FollowingCameraComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Input/Devices/Keyboard/InputDeviceKeyboard.h>
+#include <MathConversion.h>
+
+namespace ROS2
+{
+
+    // Default keyboard mapping for predefined views.
+    const AZStd::unordered_map<AzFramework::InputChannelId, int> KeysToView{
+        { AzFramework::InputDeviceKeyboard::Key::Alphanumeric1, 0 }, { AzFramework::InputDeviceKeyboard::Key::Alphanumeric2, 1 },
+        { AzFramework::InputDeviceKeyboard::Key::Alphanumeric3, 2 }, { AzFramework::InputDeviceKeyboard::Key::Alphanumeric4, 3 },
+        { AzFramework::InputDeviceKeyboard::Key::Alphanumeric5, 4 }, { AzFramework::InputDeviceKeyboard::Key::Alphanumeric6, 5 },
+        { AzFramework::InputDeviceKeyboard::Key::Alphanumeric7, 6 }, { AzFramework::InputDeviceKeyboard::Key::Alphanumeric8, 7 },
+        { AzFramework::InputDeviceKeyboard::Key::Alphanumeric9, 8 }, { AzFramework::InputDeviceKeyboard::Key::Alphanumeric0, 9 }
+    };
+
+    FollowingCameraComponent::FollowingCameraComponent(const FollowingCameraConfiguration& configuration)
+        : m_configuration(configuration)
+    {
+    }
+
+    void FollowingCameraComponent::Reflect(AZ::ReflectContext* reflection)
+    {
+        FollowingCameraConfiguration::Reflect(reflection);
+        AZ::SerializeContext* serializeContext = azrtti_cast<AZ::SerializeContext*>(reflection);
+        if (serializeContext)
+        {
+            serializeContext->Class<FollowingCameraComponent, AZ::Component>()->Version(1)->Field(
+                "FollowingCameraConfiguration", &FollowingCameraComponent::m_configuration);
+            AZ::EditContext* editContext = serializeContext->GetEditContext();
+            if (editContext)
+            {
+                editContext->Class<FollowingCameraComponent>("Following Camera", "Camera following entity with predefined views")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2 Utilities")
+                    ->Attribute(AZ::Edit::Attributes::Icon, "Editor/Icons/Components/Camera.svg")
+                    ->UIElement(AZ::Edit::UIHandlers::Label, "", "")
+                    ->Attribute(
+                        AZ::Edit::Attributes::ValueText,
+                        "This Component allows to switch camera view between predefined views. "
+                        "It also allows to zoom in/out and rotate around parent transformation. "
+                        "Use 0-9 keys to switch views and W, S, A, D keys to manipulate current view.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FollowingCameraComponent::m_configuration,
+                        "FollowingCameraConfiguration",
+                        "FollowingCameraConfiguration")
+                    ->Attribute(AZ::Edit::Attributes::Visibility, AZ::Edit::PropertyVisibility::ShowChildrenOnly);
+            }
+        }
+    }
+
+    void FollowingCameraComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC("FollowingCameraService"));
+    }
+
+    void FollowingCameraComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC("FollowingCameraService"));
+    }
+
+    void FollowingCameraComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC("TransformService"));
+        required.push_back(AZ_CRC("CameraService"));
+    }
+
+    void FollowingCameraComponent::Activate()
+    {
+        if (m_configuration.m_predefinedViews.size() == 0)
+        {
+            AZ_Warning("FollowingCameraComponent", false, "No predefined views");
+            return;
+        }
+        if (m_configuration.m_defaultView < m_configuration.m_predefinedViews.size())
+        {
+            m_currentView = m_configuration.m_predefinedViews[m_configuration.m_defaultView];
+        }
+        InputChannelEventListener::Connect();
+        AZ::TickBus::Handler::BusConnect();
+    }
+
+    void FollowingCameraComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        InputChannelEventListener::Disconnect();
+    }
+
+    void FollowingCameraComponent::CacheTransform(const AZ::Transform& transform, float deltaTime)
+    {
+        // update the smoothing buffer
+        m_lastTranslationsBuffer.push_back(AZStd::make_pair(transform.GetTranslation(), deltaTime));
+        m_lastRotationsBuffer.push_back(AZStd::make_pair(transform.GetRotation(), deltaTime));
+
+        if (m_lastTranslationsBuffer.size() > m_configuration.m_smoothingBuffer)
+        {
+            m_lastTranslationsBuffer.pop_front();
+        }
+        if (m_lastRotationsBuffer.size() > m_configuration.m_smoothingBuffer)
+        {
+            m_lastRotationsBuffer.pop_front();
+        }
+    }
+    void FollowingCameraComponent::OnTick(float deltaTime, AZ::ScriptTimePoint /*time*/)
+    {
+        AZ_Warning("FollowingCameraComponent", m_currentView.IsValid(), "View is not valid");
+        if (!m_currentView.IsValid())
+        {
+            return;
+        }
+        // obtain the current view transform
+        AZ::Transform target_local_transform;
+        AZ::Transform target_world_transform;
+
+        AZ::TransformBus::Event(m_currentView, &AZ::TransformBus::Events::GetLocalAndWorld, target_local_transform, target_world_transform);
+
+        // get parent's transform
+        const AZ::Transform parent_transform = target_world_transform * target_local_transform.GetInverse();
+
+        CacheTransform(parent_transform, deltaTime);
+
+        // get the averaged translation and quaternion
+        AZ::Transform filtered_parent_transform = { SmoothTranslation(), SmoothRotation(), 1.f };
+
+        auto modifiedTransformZoom = AZ::Transform::CreateIdentity();
+        modifiedTransformZoom.SetTranslation(AZ::Vector3::CreateAxisY(m_opticalAxisTranslation));
+
+        // adjust the camera's transform
+        //  - rotation is applied in the parent's frame
+        //  - translation is applied in the camera's frame
+        AZ::Transform filteredTransformAdjusted = filtered_parent_transform *
+            AZ::Transform::CreateFromQuaternion(AZ::Quaternion::CreateFromAxisAngle(AZ::Vector3::CreateAxisZ(1.f), m_rotationOffset)) *
+            target_local_transform * modifiedTransformZoom;
+
+        // apply the transform to the camera
+        AZ::TransformBus::Event(GetEntityId(), &AZ::TransformBus::Events::SetWorldTM, filteredTransformAdjusted);
+    }
+
+    AZ::Vector3 FollowingCameraComponent::AverageVector(const AZStd::deque<AZStd::pair<AZ::Vector3, float>>& buffer) const
+    {
+        AZ::Vector3 sum{ 0 };
+        float normalization{ 0 };
+        for (const auto& p : buffer)
+        {
+            sum += p.first * p.second;
+            normalization += p.second;
+        }
+        return sum / normalization;
+    }
+
+    AZ::Vector3 FollowingCameraComponent::SmoothTranslation() const
+    {
+        return AverageVector(m_lastTranslationsBuffer);
+    }
+
+    AZ::Quaternion FollowingCameraComponent::SmoothRotation() const
+    {
+        AZ::Quaternion q = m_lastRotationsBuffer.front().first;
+        for (int i = 1; i < m_lastRotationsBuffer.size(); i++)
+        {
+            q = q.Slerp(m_lastRotationsBuffer[i].first, m_lastRotationsBuffer[i].second);
+        }
+        return q;
+    }
+
+    bool FollowingCameraComponent::OnInputChannelEventFiltered(const AzFramework::InputChannel& inputChannel)
+    {
+        const AzFramework::InputDeviceId& deviceId = inputChannel.GetInputDevice().GetInputDeviceId();
+
+        if (AzFramework::InputDeviceKeyboard::IsKeyboardDevice(deviceId) && inputChannel.IsStateBegan())
+        {
+            OnKeyboardEvent(inputChannel);
+        }
+
+        return false;
+    }
+
+    void FollowingCameraComponent::OnKeyboardEvent(const AzFramework::InputChannel& inputChannel)
+    {
+        const AzFramework::InputChannelId& channelId = inputChannel.GetInputChannelId();
+        if (channelId == AzFramework::InputDeviceKeyboard::Key::AlphanumericW)
+        {
+            m_opticalAxisTranslation += m_configuration.m_zoomSpeed;
+            m_opticalAxisTranslation = AZStd::min(m_opticalAxisTranslation, m_configuration.m_opticalAxisTranslationMin);
+            return;
+        }
+        if (channelId == AzFramework::InputDeviceKeyboard::Key::AlphanumericS)
+        {
+            m_opticalAxisTranslation -= m_configuration.m_zoomSpeed;
+            return;
+        }
+        if (channelId == AzFramework::InputDeviceKeyboard::Key::AlphanumericA)
+        {
+            m_rotationOffset -= m_configuration.m_rotationSpeed;
+            return;
+        }
+        if (channelId == AzFramework::InputDeviceKeyboard::Key::AlphanumericD)
+        {
+            m_rotationOffset += m_configuration.m_rotationSpeed;
+            return;
+        }
+
+        // channelId is a numeric key (Key::Alphanumeric0-Key::Alphanumeric9)
+        if (auto it = KeysToView.find(channelId); it != KeysToView.end())
+        {
+            if (int viewId = it->second; viewId < m_configuration.m_predefinedViews.size())
+            {
+                m_currentView = m_configuration.m_predefinedViews[viewId];
+                m_lastTranslationsBuffer.clear();
+                m_lastRotationsBuffer.clear();
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h

@@ -0,0 +1,81 @@
+/*
+ * 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 "FollowingCameraConfiguration.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <AzFramework/Input/Events/InputChannelEventListener.h>
+
+namespace ROS2
+{
+    //! The component used for cameras that follow moving objects
+    //! It allows to switch between different cameras attached to entities, and to control the active camera using keyboard.
+    class FollowingCameraComponent
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+        , public AzFramework::InputChannelEventListener
+    {
+    public:
+        FollowingCameraComponent() = default;
+        FollowingCameraComponent(const FollowingCameraConfiguration& configuration);
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+
+        static void Reflect(AZ::ReflectContext* reflection);
+
+        AZ_COMPONENT(FollowingCameraComponent, "{6a21768a-f327-11ed-a05b-0242ac120003}", AZ::Component);
+
+        // AZ::Component
+        void Activate() override;
+        void Deactivate() override;
+
+    private:
+        // AZ::TickBus overrides ..
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        // AzFramework::InputChannelEventListener overrides ...
+        bool OnInputChannelEventFiltered(const AzFramework::InputChannel& inputChannel) override;
+        void OnKeyboardEvent(const AzFramework::InputChannel& inputChannel);
+
+        //! Compute weighted average of the vectors in the buffer.
+        //! @param buffer The buffer to compute the average.
+        //! @return The average vector.
+        AZ::Vector3 AverageVector(const AZStd::deque<AZStd::pair<AZ::Vector3, float>>& buffer) const;
+
+        //! Compute weighted average of translation in the buffer.
+        //! @return The average translation.
+        AZ::Vector3 SmoothTranslation() const;
+
+        //! Compute weighted average of rotation in the buffer.
+        //! @return The average rotation.
+        AZ::Quaternion SmoothRotation() const;
+
+        //! Cache the transform in smoothing buffer.
+        //! @param transform The transform to cache.
+        //! @param deltaTime The time between the last frame and the current frame.
+        void CacheTransform(const AZ::Transform& transform, float deltaTime);
+
+        //! The smoothing buffer for translation, the first element is the translation, the second element is the weight.
+        AZStd::deque<AZStd::pair<AZ::Vector3, float>> m_lastTranslationsBuffer;
+
+        //! The smoothing buffer for rotation, the first element is the tangential vector, the second element is the weight.
+        AZStd::deque<AZStd::pair<AZ::Quaternion, float>> m_lastRotationsBuffer;
+
+        float m_rotationOffset = 0.0f; //!< The rotation change from the input.
+        float m_opticalAxisTranslation = 0.0f; //!< The zoom change from the input.
+        AZ::EntityId m_currentView; //!< Current used view point.
+
+        FollowingCameraConfiguration m_configuration; //!< The configuration of the following camera.
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraConfiguration.cpp

@@ -0,0 +1,56 @@
+/*
+ * 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 "FollowingCameraConfiguration.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+namespace ROS2
+{
+    void FollowingCameraConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<FollowingCameraConfiguration>()
+                ->Version(1)
+                ->Field("PredefinedViews", &FollowingCameraConfiguration::m_predefinedViews)
+                ->Field("SmoothingLength", &FollowingCameraConfiguration::m_smoothingBuffer)
+                ->Field("ZoomSpeed", &FollowingCameraConfiguration::m_zoomSpeed)
+                ->Field("RotationSpeed", &FollowingCameraConfiguration::m_rotationSpeed)
+                ->Field("DefaultView", &FollowingCameraConfiguration::m_defaultView);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<FollowingCameraConfiguration>("Follow Camera Configuration", "Configuration for the Following Camera Component")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FollowingCameraConfiguration::m_smoothingBuffer,
+                        "Smoothing Length",
+                        "Number of past transforms used to smooth, larger value gives smoother result, but more lag")
+                    ->Attribute(AZ::Edit::Attributes::Min, 1)
+                    ->Attribute(AZ::Edit::Attributes::Max, 100)
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &FollowingCameraConfiguration::m_zoomSpeed, "Zoom Speed", "Speed of zooming")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FollowingCameraConfiguration::m_rotationSpeed,
+                        "Rotation Speed",
+                        "Rotation Speed around the target")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &FollowingCameraConfiguration::m_predefinedViews, "Views", "Views to follow")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &FollowingCameraConfiguration::m_defaultView,
+                        "Default View",
+                        "Default View to follow")
+                    ->Attribute(AZ::Edit::Attributes::Min, 0);
+            }
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraConfiguration.h

@@ -0,0 +1,31 @@
+/*
+ * 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/EntityId.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/containers/vector.h>
+#include <AzCore/std/string/string.h>
+
+namespace ROS2
+{
+    //! A structure capturing configuration of Following Camera.
+    struct FollowingCameraConfiguration
+    {
+        AZ_TYPE_INFO(FollowingCameraConfiguration, "{605fec3d-0152-44f3-b885-669cdcf201eb}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        AZStd::vector<AZ::EntityId> m_predefinedViews; //!< List of predefined views.
+        int m_defaultView{ 0 }; //!< Index of the default view.
+        int m_smoothingBuffer = 30; //!< Number of past transforms used to smooth, larger value gives smoother result, but more lag
+        float m_zoomSpeed = 0.06f; //!< Speed of zooming
+        float m_rotationSpeed = 0.05f; //!< Rotation Speed around the target
+        const float m_opticalAxisTranslationMin = 0.0f; //!< Minimum zoom distance
+    };
+} // namespace ROS2

Gems/ROS2/Code/ros2_files.cmake

@@ -116,6 +116,10 @@ set(FILES
         Source/ROS2SystemComponent.h
         Source/Sensor/ROS2SensorComponent.cpp
         Source/Sensor/SensorConfiguration.cpp
+        Source/SimulationUtils/FollowingCameraConfiguration.cpp
+        Source/SimulationUtils/FollowingCameraConfiguration.h
+        Source/SimulationUtils/FollowingCameraComponent.cpp
+        Source/SimulationUtils/FollowingCameraComponent.h
         Source/Spawner/ROS2SpawnerComponent.cpp
         Source/Spawner/ROS2SpawnerComponent.h
         Source/Spawner/ROS2SpawnPointComponent.cpp