Merge pull request #832 from RobotecAI/merging_point-release_24092_to…

…_main

Merge `point-release/24092` to `main` for release

Gems/ROS2/Code/CMakeLists.txt

@@ -70,8 +70,7 @@ ly_add_target(
             Gem::LmbrCentral.API
 )
 
-target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs)
-target_depends_on_ros2_package(${gem_name}.Static control_toolbox 2.2.0 REQUIRED)
+target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs control_msgs)
 
 ly_add_target(
     NAME ${gem_name}.API HEADERONLY
@@ -256,4 +255,3 @@ if(PAL_TRAIT_BUILD_TESTS_SUPPORTED)
     endif()
 endif()
 
-add_subdirectory(PythonTests)

Gems/ROS2/Code/Include/ROS2/ROS2GemUtilities.h

@@ -9,13 +9,25 @@
 
 #include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Component/Entity.h>
+#include <AzCore/Component/EntityUtils.h>
 #ifdef ROS2_EDITOR
 #include <AzToolsFramework/ToolsComponents/GenericComponentWrapper.h>
 #endif
+
 namespace ROS2
 {
     namespace Utils
     {
+        //! Checks whether the entity has a component of the given type
+        //! @param entity pointer to entity
+        //! @param typeId type of the component
+        //! @returns true if entity has component with given type
+        inline bool HasComponentOfType(const AZ::Entity* entity, const AZ::Uuid typeId)
+        {
+            auto components = AZ::EntityUtils::FindDerivedComponents(entity, typeId);
+            return !components.empty();
+        }
+
         /// Create component for a given entity in safe way.
         /// @param entityId entity that will own component
         /// @param componentType Uuid of component to create
@@ -26,7 +38,6 @@ namespace ROS2
         /// We should use that that we are not sure if we access eg ROS2FrameComponent in game mode or from Editor
         /// @param entity pointer to entity eg with GetEntity()
         /// @return pointer to component with type T
-
         template<class ComponentType>
         ComponentType* GetGameOrEditorComponent(const AZ::Entity* entity)
         {

Gems/ROS2/Code/Include/ROS2/Utilities/Controllers/PidConfiguration.h

@@ -8,11 +8,11 @@
 #pragma once
 
 #include <AzCore/Serialization/SerializeContext.h>
-#include <control_toolbox/pid.hpp>
 
 namespace ROS2::Controllers
 {
-    //! A wrapper for ROS 2 control_toolbox pid controller.
+    //! A PID controller.
+    //! Based on a ROS 2 control_toolbox implementation.
     //! @see <a href="https://github.com/ros-controls/control_toolbox">control_toolbox</a>.
     class PidConfiguration
     {
@@ -38,7 +38,7 @@ namespace ROS2::Controllers
             const bool antiWindup,
             const double outputLimit);
 
-        //! Initialize PID using member fields as set by the user.
+        //! Initialize the controller
         void InitializePid();
 
         //! Compute the value of PID command.
@@ -54,8 +54,9 @@ namespace ROS2::Controllers
         double m_iMax = 10.0; //!< maximal allowable integral term.
         double m_iMin = -10.0; //!< minimal allowable integral term.
         bool m_antiWindup = false; //!< prevents condition of integrator overflow in integral action.
+        bool m_initialized = false; //!< is PID initialized.
         double m_outputLimit = 0.0; //!< limit PID output; set to 0.0 to disable.
-
-        control_toolbox::Pid m_pid; //!< PID implementation object from control_toolbox (of ros2_control).
+        double m_previousError = 0.0; //!< previous recorded error.
+        double m_integral = 0.0; //!< integral accumulator.
     };
 } // namespace ROS2::Controllers

Gems/ROS2/Code/Source/Camera/CameraSensor.cpp

@@ -158,15 +158,33 @@ namespace ROS2
         m_passHierarchy.push_back("CopyToSwapChain");
 
         m_pipeline->SetDefaultView(m_view);
-        const AZ::RPI::ViewPtr targetView = m_scene->GetDefaultRenderPipeline()->GetDefaultView();
+        UpdateViewAlias();
+
+        Camera::CameraNotificationBus::Handler::BusConnect();
+    }
+
+    void CameraSensor::OnCameraRemoved(const AZ::EntityId& cameraEntityId)
+    {
+        UpdateViewAlias();
+    }
+
+    void CameraSensor::OnActiveViewChanged(const AZ::EntityId& cameraEntityId)
+    {
+        UpdateViewAlias();
+    }
+
+    void CameraSensor::UpdateViewAlias()
+    {
         if (auto* fp = m_scene->GetFeatureProcessor<AZ::Render::PostProcessFeatureProcessor>())
         {
+            const AZ::RPI::ViewPtr targetView = m_scene->GetDefaultRenderPipeline()->GetDefaultView();
             fp->SetViewAlias(m_view, targetView);
         }
     }
 
     CameraSensor::~CameraSensor()
     {
+        Camera::CameraNotificationBus::Handler::BusDisconnect();
         if (m_scene)
         {
             if (auto* fp = m_scene->GetFeatureProcessor<AZ::Render::PostProcessFeatureProcessor>())

Gems/ROS2/Code/Source/Camera/CameraSensor.h

@@ -10,6 +10,7 @@
 #include "CameraPublishers.h"
 #include <Atom/Feature/Utils/FrameCaptureBus.h>
 #include <AzCore/std/containers/span.h>
+#include <AzFramework/Components/CameraBus.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <rclcpp/publisher.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
@@ -20,7 +21,7 @@ namespace ROS2
 {
     //! Class to create camera sensor using Atom renderer
     //! It creates dedicated rendering pipeline for each camera
-    class CameraSensor
+    class CameraSensor : public Camera::CameraNotificationBus::Handler
     {
     public:
         //! Initializes rendering pipeline for the camera sensor.
@@ -40,6 +41,12 @@ namespace ROS2
         [[nodiscard]] const CameraSensorDescription& GetCameraSensorDescription() const;
 
     private:
+        // CameraNotificationBus overrides
+        void OnCameraRemoved(const AZ::EntityId& cameraEntityId) override;
+        void OnActiveViewChanged(const AZ::EntityId& cameraEntityId) override;
+
+        void UpdateViewAlias();
+
         AZStd::vector<AZStd::string> m_passHierarchy;
         AZ::RPI::ViewPtr m_view;
         AZ::RPI::Scene* m_scene = nullptr;

Gems/ROS2/Code/Source/Frame/Conversions/FrameConversion.py

@@ -47,6 +47,8 @@ def search_for_components(item, foundComponents, insidePatches=False):
                     "m_template/Joint Name"
                 ]):
                     item["path"] = item["path"].replace("m_template", "ROS2FrameConfiguration")
+                if item["op"] == "remove" and "ROS2FrameComponent" in item["path"]:
+                    item["path"] = item["path"].replace("ROS2FrameComponent", "ROS2FrameEditorComponent")
 
             for key, value in item.items():
                 search_for_components(value, foundComponents, insidePatches)

Gems/ROS2/Code/Source/Frame/ROS2FrameComponent.cpp

@@ -8,7 +8,6 @@
 
 #include "ROS2FrameSystemComponent.h"
 #include <AzCore/Component/Entity.h>
-#include <AzCore/Component/EntityUtils.h>
 #include <AzCore/RTTI/ReflectContext.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/EditContextConstants.inl>
@@ -21,6 +20,9 @@
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/ROS2GemUtilities.h>
 #include <ROS2/Utilities/ROS2Names.h>
+#include <Source/ArticulationLinkComponent.h>
+#include <Source/FixedJointComponent.h>
+#include <Source/JointComponent.h>
 #include <rapidjson/document.h>
 #include <rapidjson/stringbuffer.h>
 
@@ -68,17 +70,6 @@ namespace ROS2
             // Found the component!
             return component;
         }
-
-        //! Checks whether the entity has a component of the given type
-        //! @param entity pointer to entity
-        //! @param typeId type of the component
-        //! @returns true if entity has component with given type
-        static bool CheckIfEntityHasComponentOfType(const AZ::Entity* entity, const AZ::Uuid typeId)
-        {
-            auto components = AZ::EntityUtils::FindDerivedComponents(entity, typeId);
-            return !components.empty();
-        }
-
     } // namespace Internal
 
     AZ::JsonSerializationResult::Result JsonFrameComponentConfigSerializer::Load(
@@ -160,12 +151,9 @@ namespace ROS2
             // Otherwise it'll be dynamic when it has joints and it's not a fixed joint.
             else
             {
-                const bool hasJoints = Internal::CheckIfEntityHasComponentOfType(
-                    m_entity, AZ::Uuid("{B01FD1D2-1D91-438D-874A-BF5EB7E919A8}")); // Physx::JointComponent;
-                const bool hasFixedJoints = Internal::CheckIfEntityHasComponentOfType(
-                    m_entity, AZ::Uuid("{02E6C633-8F44-4CEE-AE94-DCB06DE36422}")); // Physx::FixedJointComponent
-                const bool hasArticulations = Internal::CheckIfEntityHasComponentOfType(
-                    m_entity, AZ::Uuid("{48751E98-B35F-4A2F-A908-D9CDD5230264}")); // Physx::ArticulationComponent
+                const bool hasJoints = Utils::HasComponentOfType(m_entity, PhysX::JointComponent::TYPEINFO_Uuid());
+                const bool hasFixedJoints = Utils::HasComponentOfType(m_entity, PhysX::FixedJointComponent::TYPEINFO_Uuid());
+                const bool hasArticulations = Utils::HasComponentOfType(m_entity, PhysX::ArticulationLinkComponent::TYPEINFO_Uuid());
                 m_isDynamic = (hasJoints && !hasFixedJoints) || hasArticulations;
             }