ROS2-related cherries for 2409.2 point release

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/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;
             }
 

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp

@@ -23,16 +23,6 @@ namespace ROS2
             AZStd::bind(&FollowJointTrajectoryActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1));
     }
 
-    JointsTrajectoryRequests::TrajectoryActionStatus FollowJointTrajectoryActionServer::GetGoalStatus() const
-    {
-        return m_goalStatus;
-    }
-
-    void FollowJointTrajectoryActionServer::SetGoalSuccess()
-    {
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
-    }
-
     void FollowJointTrajectoryActionServer::CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result)
     {
         AZ_Assert(m_goalHandle, "Invalid goal handle!");
@@ -50,7 +40,6 @@ namespace ROS2
         {
             AZ_Trace("FollowJointTrajectoryActionServer", "Goal succeeded\n");
             m_goalHandle->succeed(result);
-            m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
         }
     }
 
@@ -102,7 +91,6 @@ namespace ROS2
             return rclcpp_action::CancelResponse::REJECT;
         }
 
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled;
         return rclcpp_action::CancelResponse::ACCEPT;
     }
 
@@ -149,6 +137,5 @@ namespace ROS2
 
         m_goalHandle = goalHandle;
         // m_goalHandle->execute(); // No need to call this, as we are already executing the goal due to ACCEPT_AND_EXECUTE
-        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Executing;
     }
 } // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h

@@ -32,17 +32,10 @@ namespace ROS2
         //! server documentation </a>
         FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId);
 
-        //! Return trajectory action status.
-        //! @return Status of the trajectory execution.
-        JointsTrajectoryRequests::TrajectoryActionStatus GetGoalStatus() const;
-
         //! Cancel the current goal.
         //! @param result Result to be passed to through action server to the client.
         void CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result);
 
-        //! Sets the goal status to success
-        void SetGoalSuccess();
-
         //! Report goal success to the action server.
         //! @param result Result which contains success code.
         void GoalSuccess(std::shared_ptr<FollowJointTrajectory::Result> result);
@@ -56,7 +49,6 @@ namespace ROS2
         using TrajectoryActionStatus = JointsTrajectoryRequests::TrajectoryActionStatus;
 
         AZ::EntityId m_entityId;
-        TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle;
         rclcpp_action::Server<FollowJointTrajectory>::SharedPtr m_actionServer;
         std::shared_ptr<GoalHandle> m_goalHandle;
 

Gems/ROS2/Code/Source/Manipulation/JointUtils.cpp

@@ -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
+ *
+ */
+
+#include "JointUtils.h"
+
+#include <ROS2/ROS2GemUtilities.h>
+#include <Source/EditorArticulationLinkComponent.h>
+#include <Source/EditorBallJointComponent.h>
+#include <Source/EditorFixedJointComponent.h>
+#include <Source/EditorHingeJointComponent.h>
+#include <Source/EditorPrismaticJointComponent.h>
+
+namespace ROS2::JointUtils
+{
+
+    bool HasNonFixedJoints(const AZ::Entity* entity)
+    {
+        const bool hasPrismaticJoints = Utils::HasComponentOfType(entity, PhysX::EditorPrismaticJointComponent::TYPEINFO_Uuid());
+        const bool hasBallJoints = Utils::HasComponentOfType(entity, PhysX::EditorBallJointComponent::TYPEINFO_Uuid());
+        const bool hasHingeJoints = Utils::HasComponentOfType(entity, PhysX::EditorHingeJointComponent::TYPEINFO_Uuid());
+        const bool hasArticulations = Utils::HasComponentOfType(entity, PhysX::EditorArticulationLinkComponent::TYPEINFO_Uuid());
+        const bool hasJoints = hasPrismaticJoints || hasBallJoints || hasHingeJoints || hasArticulations;
+
+        return hasJoints;
+    }
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Manipulation/JointUtils.h

@@ -0,0 +1,19 @@
+/*
+ * 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/Entity.h>
+
+namespace ROS2::JointUtils
+{
+    //! Check if the entity has any of the non fixed joint or articulation components.
+    //! @param entity Entity to check.
+    //! @return True if the entity has any of the joint or articulation components.
+    bool HasNonFixedJoints(const AZ::Entity* entity);
+} // namespace ROS2::Utils

Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp

@@ -7,6 +7,7 @@
  */
 
 #include "JointsManipulationEditorComponent.h"
+#include "JointUtils.h"
 #include "JointsManipulationComponent.h"
 #include <AzCore/Component/ComponentApplicationBus.h>
 #include <AzCore/Component/TransformBus.h>
@@ -108,8 +109,10 @@ namespace ROS2
                 azrtti_cast<ROS2::ROS2FrameEditorComponent*>(Utils::GetGameOrEditorComponent<ROS2::ROS2FrameEditorComponent>(entity));
             AZ_Assert(frameEditorComponent, "ROS2FrameEditorComponent does not exist!");
 
+            const bool hasNonFixedJoints = JointUtils::HasNonFixedJoints(entity);
+
             AZStd::string jointName(frameEditorComponent->GetJointName().GetCStr());
-            if (!jointName.empty())
+            if (!jointName.empty() && hasNonFixedJoints)
             {
                 m_initialPositions.emplace_back(AZStd::make_pair(jointName, configBackup[jointName]));
             }

Gems/ROS2/Code/Source/Manipulation/JointsPositionsEditorComponent.cpp

@@ -9,6 +9,7 @@
 
 #include "JointsPositionsEditorComponent.h"
 #include "JointPositionsSubscriptionHandler.h"
+#include "JointUtils.h"
 #include "JointsPositionsComponent.h"
 
 #include <AzCore/Serialization/EditContext.h>
@@ -76,14 +77,17 @@ namespace ROS2
 
     AZ::Crc32 JointsPositionsEditorComponent::FindAllJoints()
     {
+        m_jointNames.clear();
         AZStd::function<void(const AZ::Entity* entity)> getAllJointsHierarchy = [&](const AZ::Entity* entity)
         {
             auto* frameEditorComponent =
                 azrtti_cast<ROS2::ROS2FrameEditorComponent*>(Utils::GetGameOrEditorComponent<ROS2::ROS2FrameEditorComponent>(entity));
             AZ_Assert(frameEditorComponent, "ROS2FrameEditorComponent does not exist!");
 
+            const bool hasNonFixedJoints = JointUtils::HasNonFixedJoints(entity);
+
             AZStd::string jointName(frameEditorComponent->GetJointName().GetCStr());
-            if (!jointName.empty())
+            if (!jointName.empty() && hasNonFixedJoints)
             {
                 m_jointNames.emplace_back(jointName);
             }

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp

@@ -12,8 +12,8 @@
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2/Manipulation/JointsManipulationRequests.h>
 #include <ROS2/ROS2Bus.h>
-#include <ROS2/Utilities/ROS2Names.h>
 #include <ROS2/Utilities/ROS2Conversions.h>
+#include <ROS2/Utilities/ROS2Names.h>
 
 namespace ROS2
 {
@@ -92,7 +92,7 @@ namespace ROS2
     AZ::Outcome<void, JointsTrajectoryComponent::TrajectoryResult> JointsTrajectoryComponent::StartTrajectoryGoal(
         TrajectoryGoalPtr trajectoryGoal)
     {
-        if (m_trajectoryInProgress)
+        if (m_goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Executing)
         {
             auto result = JointsTrajectoryComponent::TrajectoryResult();
             result.error_code = JointsTrajectoryComponent::TrajectoryResult::INVALID_GOAL;
@@ -107,7 +107,7 @@ namespace ROS2
         }
         m_trajectoryGoal = *trajectoryGoal;
         m_trajectoryExecutionStartTime = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp());
-        m_trajectoryInProgress = true;
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Executing;
         return AZ::Success();
     }
 
@@ -186,13 +186,13 @@ namespace ROS2
     AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal()
     {
         m_trajectoryGoal.trajectory.points.clear();
-        m_trajectoryInProgress = false;
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled;
         return AZ::Success();
     }
 
     JointsTrajectoryRequests::TrajectoryActionStatus JointsTrajectoryComponent::GetGoalStatus()
     {
-        return m_followTrajectoryServer->GetGoalStatus();
+        return m_goalStatus;
     }
 
     void JointsTrajectoryComponent::FollowTrajectory(const uint64_t deltaTimeNs)
@@ -205,7 +205,6 @@ namespace ROS2
             result->error_string = "User Cancelled";
             result->error_code = FollowJointTrajectoryActionServer::FollowJointTrajectory::Result::SUCCESSFUL;
             m_followTrajectoryServer->CancelGoal(result);
-            m_followTrajectoryServer->SetGoalSuccess();
             return;
         }
 
@@ -219,7 +218,7 @@ namespace ROS2
             AZ_TracePrintf("JointsManipulationComponent", "Goal Concluded: all points reached\n");
             auto successResult = std::make_shared<control_msgs::action::FollowJointTrajectory::Result>(); //!< Empty defaults to success.
             m_followTrajectoryServer->GoalSuccess(successResult);
-            m_trajectoryInProgress = false;
+            m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
             return;
         }
 

Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h

@@ -64,9 +64,9 @@ namespace ROS2
         AZStd::string m_followTrajectoryActionName{ "arm_controller/follow_joint_trajectory" };
         AZStd::unique_ptr<FollowJointTrajectoryActionServer> m_followTrajectoryServer;
         TrajectoryGoal m_trajectoryGoal;
+        TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle;
         rclcpp::Time m_trajectoryExecutionStartTime;
         ManipulationJoints m_manipulationJoints;
-        bool m_trajectoryInProgress{ false };
         builtin_interfaces::msg::Time m_lastTickTimestamp; //!< ROS 2 Timestamp during last OnTick call
     };
 } // namespace ROS2