Update ROS2 <-> ROS2Controllers interface (#933)

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

Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameComponent.h

@@ -63,7 +63,7 @@ namespace ROS2
 
         //! Get a frame id, which is needed for any ROS2 message with a Header
         //! @return Frame id which includes the namespace, ready to send in a ROS2 message
-        AZStd::string GetFrameID() const;
+        AZStd::string GetNamespacedFrameID() const;
 
         //! Set a above-mentioned frame id
         void SetFrameID(const AZStd::string& frameId);
@@ -71,7 +71,7 @@ namespace ROS2
         //! Get the joint name including the namespace
         //! @note Supplementary metadata for Joint components, necessary in some cases for joints addressed by name in ROS 2
         //! @return The namespaced joint name, ready to send in a ROS2 message
-        AZ::Name GetJointName() const;
+        AZ::Name GetNamespacedJointName() const;
 
         //! Set the joint name
         //! @note May be populated during URDF import or set by the user in the Editor view

Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameEditorComponent.h

@@ -65,8 +65,8 @@ namespace ROS2
         void UpdateNamespaceConfiguration(const AZStd::string& ros2Namespace, const NamespaceConfiguration::NamespaceStrategy& strategy);
 
         // ROS2FrameEditorComponentBus::Handler overrides
-        AZStd::string GetFrameID() const override;
-        AZ::Name GetJointName() const override;
+        AZStd::string GetNamespacedFrameID() const override;
+        AZ::Name GetNamespacedJointName() const override;
         AZStd::string GetNamespace() const override;
         void UpdateNamespace(const AZStd::string& parentNamespace) override;
         AZStd::string GetGlobalFrameName() const override;
@@ -74,7 +74,6 @@ namespace ROS2
         AZ::EntityId GetFrameParent() const override;
         AZStd::set<AZ::EntityId> GetFrameChildren() const override;
 
-
         //! Get the configuration of this component.
         ROS2FrameConfiguration GetConfiguration() const;
 

Gems/ROS2/Code/Include/ROS2/Frame/ROS2FrameEditorComponentBus.h

@@ -31,12 +31,12 @@ namespace ROS2
 
         //! Get a frame id, which is needed for any ROS2 message with a Header
         //! @return Frame id which includes the namespace, ready to send in a ROS2 message
-        virtual AZStd::string GetFrameID() const = 0;
+        virtual AZStd::string GetNamespacedFrameID() const = 0;
 
         //! Get the joint name including the namespace
         //! @note Supplementary metadata for Joint components, necessary in some cases for joints addressed by name in ROS 2
         //! @return The namespaced joint name, ready to send in a ROS2 message
-        virtual AZ::Name GetJointName() const = 0;
+        virtual AZ::Name GetNamespacedJointName() const = 0;
 
         //! Get a namespace, which should be used for any publisher or subscriber in the same entity.
         //! @return A complete namespace (including parent namespaces)

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

@@ -158,10 +158,10 @@ namespace ROS2
         }
 
         //! Returns this sensor frame ID. The ID contains namespace.
-        [[nodiscard]] AZStd::string GetFrameID() const
+        [[nodiscard]] AZStd::string GetNamespacedFrameID() const
         {
             auto* ros2Frame = GetEntity()->template FindComponent<ROS2FrameComponent>();
-            return ros2Frame->GetFrameID();
+            return ros2Frame->GetNamespacedFrameID();
         }
 
         SensorConfiguration m_sensorConfiguration; ///< Basic sensor configuration.

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

@@ -143,7 +143,7 @@ namespace ROS2
 
         if (m_publishTransform)
         {
-            AZ_TracePrintf("ROS2FrameComponent", "Setting up %s", GetFrameID().data());
+            AZ_TracePrintf("ROS2FrameComponent", "Setting up %s", GetNamespacedFrameID().data());
 
             // The frame will always be dynamic if it's a top entity.
             if (IsTopLevel())
@@ -168,10 +168,10 @@ namespace ROS2
                 "Setting up %s transform between parent %s and child %s to be published %s\n",
                 IsDynamic() ? "dynamic" : "static",
                 GetParentFrameID().data(),
-                GetFrameID().data(),
+                GetNamespacedFrameID().data(),
                 IsDynamic() ? "continuously to /tf" : "once to /tf_static");
 
-            m_ros2Transform = AZStd::make_unique<ROS2Transform>(GetParentFrameID(), GetFrameID(), IsDynamic());
+            m_ros2Transform = AZStd::make_unique<ROS2Transform>(GetParentFrameID(), GetNamespacedFrameID(), IsDynamic());
             if (IsDynamic())
             {
                 AZ::TickBus::Handler::BusConnect();
@@ -246,13 +246,13 @@ namespace ROS2
     {
         if (auto parentFrame = GetParentROS2FrameComponent(); parentFrame != nullptr)
         {
-            return parentFrame->GetFrameID();
+            return parentFrame->GetNamespacedFrameID();
         }
         // If parent entity does not exist or does not have a ROS2FrameComponent, return ROS2 default global frame.
         return GetGlobalFrameName();
     }
 
-    AZStd::string ROS2FrameComponent::GetFrameID() const
+    AZStd::string ROS2FrameComponent::GetNamespacedFrameID() const
     {
         return ROS2Names::GetNamespacedName(GetNamespace(), m_frameName);
     }
@@ -273,7 +273,7 @@ namespace ROS2
         return m_namespaceConfiguration.GetNamespace(parentNamespace);
     }
 
-    AZ::Name ROS2FrameComponent::GetJointName() const
+    AZ::Name ROS2FrameComponent::GetNamespacedJointName() const
     {
         return AZ::Name(ROS2Names::GetNamespacedName(GetNamespace(), m_jointName).c_str());
     }

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

@@ -64,7 +64,7 @@ namespace ROS2
         return ROS2FrameSystemInterface::Get()->IsTopLevel(GetEntityId());
     }
 
-    AZStd::string ROS2FrameEditorComponent::GetFrameID() const
+    AZStd::string ROS2FrameEditorComponent::GetNamespacedFrameID() const
     {
         return ROS2Names::GetNamespacedName(GetNamespace(), m_configuration.m_frameName);
     }
@@ -99,7 +99,7 @@ namespace ROS2
         m_configuration.m_namespaceConfiguration.SetNamespace(ros2Namespace, strategy);
     }
 
-    AZ::Name ROS2FrameEditorComponent::GetJointName() const
+    AZ::Name ROS2FrameEditorComponent::GetNamespacedJointName() const
     {
         return AZ::Name(ROS2Names::GetNamespacedName(GetNamespace(), m_configuration.m_jointName).c_str());
     }

Gems/ROS2/Code/Source/ROS2EditorModule.cpp

@@ -1,72 +0,0 @@
-/*
- * 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 <AzCore/RTTI/RTTIMacros.h>
-#include <Camera/ROS2CameraSensorEditorComponent.h>
-#include <Camera/ROS2EditorCameraSystemComponent.h>
-#include <Frame/ROS2FrameSystemComponent.h>
-#include <Lidar/LidarRegistrarEditorSystemComponent.h>
-#include <Manipulation/JointsManipulationEditorComponent.h>
-#include <Manipulation/JointsPositionsEditorComponent.h>
-#include <QtCore/qglobal.h>
-#include <ROS2/Frame/ROS2FrameEditorComponent.h>
-#include <ROS2ModuleInterface.h>
-#include <RobotImporter/ROS2RobotImporterEditorSystemComponent.h>
-#include <SdfAssetBuilder/SdfAssetBuilderSystemComponent.h>
-#include <SystemComponents/ROS2EditorSystemComponent.h>
-#ifdef WITH_GAZEBO_MSGS
-#include <Spawner/ROS2SpawnPointEditorComponent.h>
-#include <Spawner/ROS2SpawnerEditorComponent.h>
-#endif
-void InitROS2Resources()
-{
-    // Registration of Qt (ROS2.qrc) resources
-    Q_INIT_RESOURCE(ROS2);
-}
-
-namespace ROS2
-{
-    class ROS2EditorModule : public ROS2ModuleInterface
-    {
-    public:
-        AZ_RTTI(ROS2EditorModule, "{3DDFC98F-D1CC-4658-BAF8-2CC34A9D39F3}", ROS2ModuleInterface);
-        AZ_CLASS_ALLOCATOR(ROS2EditorModule, AZ::SystemAllocator);
-
-        ROS2EditorModule()
-        {
-            InitROS2Resources();
-
-            m_descriptors.insert(
-                m_descriptors.end(),
-                { ROS2EditorSystemComponent::CreateDescriptor(),
-                  ROS2EditorCameraSystemComponent::CreateDescriptor(),
-                  LidarRegistrarEditorSystemComponent::CreateDescriptor(),
-                  ROS2RobotImporterEditorSystemComponent::CreateDescriptor(),
-                  ROS2CameraSensorEditorComponent::CreateDescriptor(),
-#ifdef WITH_GAZEBO_MSGS
-                  ROS2SpawnerEditorComponent::CreateDescriptor(),
-                  ROS2SpawnPointEditorComponent::CreateDescriptor(),
-#endif
-                  SdfAssetBuilderSystemComponent::CreateDescriptor(),
-                  JointsManipulationEditorComponent::CreateDescriptor(),
-                  JointsPositionsEditorComponent::CreateDescriptor(),
-                  ROS2FrameSystemComponent::CreateDescriptor(),
-                  ROS2FrameEditorComponent::CreateDescriptor() });
-        }
-
-        AZ::ComponentTypeList GetRequiredSystemComponents() const override
-        {
-            return AZ::ComponentTypeList{
-                azrtti_typeid<ROS2EditorSystemComponent>(),           azrtti_typeid<ROS2EditorCameraSystemComponent>(),
-                azrtti_typeid<LidarRegistrarEditorSystemComponent>(), azrtti_typeid<ROS2RobotImporterEditorSystemComponent>(),
-                azrtti_typeid<SdfAssetBuilderSystemComponent>(),      azrtti_typeid<ROS2FrameSystemComponent>(),
-            };
-        }
-    };
-} // namespace ROS2
-
-AZ_DECLARE_MODULE_CLASS(Gem_ROS2, ROS2::ROS2EditorModule)

Gems/ROS2/Code/Tests/Frame/ROS2FrameComponentTest.cpp

@@ -53,9 +53,8 @@ namespace UnitTest
     {
         AddActiveGems(AZStd::to_array<AZStd::string_view>({ "ROS2" }));
         AddDynamicModulePaths({});
-        AddComponentDescriptors(
-            AZStd::initializer_list<AZ::ComponentDescriptor*>{ ROS2::ROS2FrameComponent::CreateDescriptor(),
-                                                               ROS2::ROS2SystemComponent::CreateDescriptor() });
+        AddComponentDescriptors(AZStd::initializer_list<AZ::ComponentDescriptor*>{ ROS2::ROS2FrameComponent::CreateDescriptor(),
+                                                                                   ROS2::ROS2SystemComponent::CreateDescriptor() });
         AddRequiredComponents(AZStd::to_array<AZ::TypeId const>({ ROS2::ROS2SystemComponent::TYPEINFO_Uuid() }));
     }
 
@@ -86,8 +85,8 @@ namespace UnitTest
         entity.Init();
         entity.Activate();
 
-        const std::string jointName(frame->GetJointName().GetCStr());
-        const std::string frameId(frame->GetFrameID().c_str());
+        const std::string jointName(frame->GetNamespacedJointName().GetCStr());
+        const std::string frameId(frame->GetNamespacedFrameID().c_str());
 
         EXPECT_EQ(entity.GetState(), AZ::Entity::State::Active);
         EXPECT_STRCASEEQ(jointName.c_str(), ("o3de_" + entityName + "/").c_str());
@@ -128,8 +127,8 @@ namespace UnitTest
 
         for (int i = 0; i < numOfEntities; i++)
         {
-            const std::string jointName(frames[i]->GetJointName().GetCStr());
-            const std::string frameId(frames[i]->GetFrameID().c_str());
+            const std::string jointName(frames[i]->GetNamespacedJointName().GetCStr());
+            const std::string frameId(frames[i]->GetNamespacedFrameID().c_str());
 
             EXPECT_EQ(entities[i]->GetState(), AZ::Entity::State::Active);
             EXPECT_STRCASEEQ(jointName.c_str(), (entities[0]->GetName() + "/").c_str());

Gems/ROS2Controllers/Code/CMakeLists.txt

@@ -147,7 +147,7 @@ if(PAL_TRAIT_BUILD_HOST_TOOLS)
             PUBLIC
                 AZ::AzToolsFramework
                 ${gem_name}.Private.Object
-                Gem::ROS2.Editor.Static
+                Gem::ROS2.Editor.API
     )
 
     ly_add_target(

Gems/ROS2Controllers/Code/Source/Gripper/FingerGripperComponent.cpp

@@ -16,7 +16,6 @@
 #include <AzFramework/Physics/PhysicsSystem.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
 #include <ROS2Controllers/Manipulation/JointsManipulationRequests.h>
-#include <Utilities/JointUtilities.h>
 #include <imgui/imgui.h>
 
 namespace ROS2Controllers
@@ -106,15 +105,23 @@ namespace ROS2Controllers
         {
             JointsManipulationRequestBus::EventResult(allJoints, m_rootOfArticulation, &JointsManipulationRequests::GetJoints);
         }
+
         AZStd::vector<AZ::EntityId> descendantIds;
         AZ::TransformBus::EventResult(descendantIds, GetEntityId(), &AZ::TransformBus::Events::GetAllDescendants);
-
-        for (AZ::EntityId descendant : descendantIds)
+        for (const auto& descendantId : descendantIds)
         {
-            AZStd::string jointName = Utils::GetJointName(descendant);
+            AZ::Entity* entity{ nullptr };
+            AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, descendantId);
+            AZ_Assert(entity, "Entity %s not found.", descendantId.ToString().c_str());
+            ROS2::ROS2FrameComponent* component = entity->FindComponent<ROS2::ROS2FrameComponent>();
+            if (!component)
+            {
+                break;
+            }
+
+            const auto jointName = component->GetNamespacedJointName().GetStringView();
             if (!jointName.empty())
             {
-                AZ_Printf("FingerGripperComponent", "Adding finger joint %s", jointName.c_str());
                 m_fingerJoints[jointName] = allJoints[jointName];
             }
         }

Gems/ROS2Controllers/Code/Source/Manipulation/JointsManipulationComponent.cpp

@@ -104,7 +104,7 @@ namespace ROS2Controllers
                 { // Frame Component is required for joints.
                     continue;
                 }
-                const AZStd::string jointName(frameComponent->GetJointName().GetCStr());
+                const AZStd::string jointName(frameComponent->GetNamespacedJointName().GetCStr());
 
                 auto* hingeComponent = entity->FindComponent<PhysX::HingeJointComponent>();
                 auto* prismaticComponent = entity->FindComponent<PhysX::PrismaticJointComponent>();
@@ -143,18 +143,19 @@ namespace ROS2Controllers
             return manipulationJoints;
         }
 
-        void SetInitialPositions(ManipulationJoints& manipulationJoints, const AZStd::unordered_map<AZStd::string, float>& initialPositions)
+        void SetInitialPositions(
+            ManipulationJoints& manipulationJoints, const AZStd::vector<AZStd::pair<AZStd::string, float>>& initialPositions)
         {
             // Set the initial / resting position to move to and keep.
-            for (const auto& [jointName, jointInfo] : manipulationJoints)
+            for (const auto& [jointName, position] : initialPositions)
             {
-                if (initialPositions.contains(jointName))
+                if (manipulationJoints.contains(jointName))
                 {
-                    manipulationJoints[jointName].m_restPosition = initialPositions.at(jointName);
+                    manipulationJoints[jointName].m_restPosition = position;
                 }
                 else
                 {
-                    AZ_Warning("JointsManipulationComponent", false, "No set initial position for joint %s", jointName.c_str());
+                    AZ_Warning("JointsManipulationComponent", false, "No joint %s to set initial position", jointName.c_str());
                 }
             }
         }
@@ -177,7 +178,7 @@ namespace ROS2Controllers
         auto* ros2Frame = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
         JointStatePublisherContext publisherContext;
         publisherContext.m_publisherNamespace = ros2Frame->GetNamespace();
-        publisherContext.m_frameId = ros2Frame->GetFrameID();
+        publisherContext.m_frameId = ros2Frame->GetNamespacedFrameID();
         publisherContext.m_entityId = GetEntityId();
 
         m_jointStatePublisher = AZStd::make_unique<JointStatePublisher>(m_jointStatePublisherConfiguration, publisherContext);
@@ -414,31 +415,12 @@ namespace ROS2Controllers
         }
     }
 
-    AZStd::string JointsManipulationComponent::GetManipulatorNamespace() const
-    {
-        auto* frameComponent = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
-        AZ_Assert(frameComponent, "ROS2FrameComponent is required for joints.");
-        return frameComponent->GetNamespace();
-    }
-
     void JointsManipulationComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
     {
         if (m_manipulationJoints.empty())
         {
-            const AZStd::string manipulatorNamespace = GetManipulatorNamespace();
-            AZStd::unordered_map<AZStd::string, JointPosition> initialPositionNamespaced;
-            AZStd::transform(
-                m_initialPositions.begin(),
-                m_initialPositions.end(),
-                AZStd::inserter(initialPositionNamespaced, initialPositionNamespaced.end()),
-                [&manipulatorNamespace](const auto& pair)
-                {
-                    return AZStd::make_pair(ROS2::ROS2Names::GetNamespacedName(manipulatorNamespace, pair.first), pair.second);
-                });
-
             m_manipulationJoints = Internal::GetAllEntityHierarchyJoints(GetEntityId());
-
-            Internal::SetInitialPositions(m_manipulationJoints, initialPositionNamespaced);
+            Internal::SetInitialPositions(m_manipulationJoints, m_initialPositions);
             if (m_manipulationJoints.empty())
             {
                 AZ_Warning("JointsManipulationComponent", false, "No manipulation joints to handle!");

Gems/ROS2Controllers/Code/Source/Manipulation/JointsManipulationComponent.h

@@ -76,8 +76,6 @@ namespace ROS2Controllers
 
         void MoveToSetPositions(float deltaTime);
 
-        AZStd::string GetManipulatorNamespace() const;
-
         AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const JointInfo& jointInfo);
         AZ::Outcome<JointVelocity, AZStd::string> GetJointVelocity(const JointInfo& jointInfo);
         AZ::Outcome<JointEffort, AZStd::string> GetJointEffort(const JointInfo& jointInfo);

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

@@ -14,7 +14,7 @@
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/std/functional.h>
 #include <AzToolsFramework/API/ToolsApplicationAPI.h>
-#include <ROS2/Frame/ROS2FrameEditorComponent.h>
+#include <ROS2/Frame/ROS2FrameEditorComponentBus.h>
 #include <ROS2Controllers/Manipulation/Controllers/JointsPositionControllerRequests.h>
 #include <ROS2Controllers/Manipulation/JointInfo.h>
 #include <Source/ArticulationLinkComponent.h>
@@ -104,18 +104,20 @@ namespace ROS2Controllers
 
         AZStd::function<void(const AZ::Entity* entity)> getAllJointsHierarchy = [&](const AZ::Entity* entity)
         {
-            auto* frameEditorComponent = entity->FindComponent<ROS2::ROS2FrameEditorComponent>();
-            AZ_Assert(frameEditorComponent, "ROS2FrameEditorComponent does not exist!");
+            AZ::Name jointName;
+            ROS2::ROS2FrameEditorComponentBus::EventResult(
+                jointName, entity->GetId(), &ROS2::ROS2FrameEditorComponentBus::Events::GetNamespacedJointName);
 
+            const AZStd::string jointNameStr = jointName.GetCStr();
             const bool hasNonFixedJoints = JointUtils::HasNonFixedJoints(entity);
-
-            const AZStd::string jointName(frameEditorComponent->GetConfiguration().m_jointName);
-            if (!jointName.empty() && hasNonFixedJoints)
+            if (!jointNameStr.empty() && hasNonFixedJoints)
             {
-                m_initialPositions.emplace_back(AZStd::make_pair(jointName, configBackup[jointName]));
+                m_initialPositions.emplace_back(AZStd::make_pair(jointNameStr, configBackup[jointNameStr]));
             }
 
-            const auto& childrenEntityIds = frameEditorComponent->GetFrameChildren();
+            AZStd::set<AZ::EntityId> childrenEntityIds;
+            ROS2::ROS2FrameEditorComponentBus::EventResult(
+                childrenEntityIds, entity->GetId(), &ROS2::ROS2FrameEditorComponentBus::Events::GetFrameChildren);
             if (!childrenEntityIds.empty())
             {
                 for (const auto& entityId : childrenEntityIds)

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

@@ -17,7 +17,7 @@
 
 #include <AzToolsFramework/API/ToolsApplicationAPI.h>
 #include <AzToolsFramework/ToolsComponents/EditorComponentBase.h>
-#include <ROS2/Frame/ROS2FrameEditorComponent.h>
+#include <ROS2/Frame/ROS2FrameEditorComponentBus.h>
 #include <ROS2Controllers/Manipulation/JointsManipulationRequests.h>
 
 namespace ROS2Controllers
@@ -81,18 +81,20 @@ namespace ROS2Controllers
         m_jointNames.clear();
         AZStd::function<void(const AZ::Entity* entity)> getAllJointsHierarchy = [&](const AZ::Entity* entity)
         {
-            auto* frameEditorComponent = entity->FindComponent<ROS2::ROS2FrameEditorComponent>();
-            AZ_Assert(frameEditorComponent, "ROS2FrameEditorComponent does not exist!");
+            AZ::Name jointName;
+            ROS2::ROS2FrameEditorComponentBus::EventResult(
+                jointName, entity->GetId(), &ROS2::ROS2FrameEditorComponentBus::Events::GetNamespacedJointName);
 
+            const AZStd::string jointNameStr = jointName.GetCStr();
             const bool hasNonFixedJoints = JointUtils::HasNonFixedJoints(entity);
-
-            const AZStd::string jointName(frameEditorComponent->GetJointName().GetCStr());
-            if (!jointName.empty() && hasNonFixedJoints)
+            if (!jointNameStr.empty() && hasNonFixedJoints)
             {
-                m_jointNames.emplace_back(jointName);
+                m_jointNames.emplace_back(jointNameStr);
             }
 
-            const auto& childrenEntityIds = frameEditorComponent->GetFrameChildren();
+            AZStd::set<AZ::EntityId> childrenEntityIds;
+            ROS2::ROS2FrameEditorComponentBus::EventResult(
+                childrenEntityIds, entity->GetId(), &ROS2::ROS2FrameEditorComponentBus::Events::GetFrameChildren);
             if (!childrenEntityIds.empty())
             {
                 for (const auto& entityId : childrenEntityIds)

Gems/ROS2Controllers/Code/Source/Sensors/ROS2WheelOdometrySensorComponent.cpp

@@ -183,7 +183,7 @@ namespace ROS2Controllers
 
         // "odom" is globally fixed frame for all robots, no matter the namespace
         m_odometryMsg.header.frame_id = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), "odom").c_str();
-        m_odometryMsg.child_frame_id = GetFrameID().c_str();
+        m_odometryMsg.child_frame_id = GetNamespacedFrameID().c_str();
 
         auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor");

Gems/ROS2Controllers/Code/Source/Utilities/JointUtilities.cpp

@@ -1,29 +0,0 @@
-/*
- * 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 "JointUtilities.h"
-#include <ROS2/Frame/ROS2FrameComponent.h>
-
-namespace ROS2Controllers::Utils
-{
-    AZStd::string GetJointName(AZ::EntityId entityId)
-    {
-        AZ::Entity* entity{ nullptr };
-        AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, entityId);
-        AZ_Assert(entity, "Entity %s not found.", entityId.ToString().c_str());
-        if (entity)
-        {
-            ROS2::ROS2FrameComponent* component = entity->FindComponent<ROS2::ROS2FrameComponent>();
-            if (component)
-            {
-                return component->GetJointName().GetStringView();
-            }
-        }
-        return AZStd::string();
-    }
-} // namespace ROS2Controllers::Utils

Gems/ROS2Controllers/Code/Source/Utilities/JointUtilities.h

@@ -1,15 +0,0 @@
-/*
- * 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 ROS2Controllers::Utils
-{
-    AZStd::string GetJointName(AZ::EntityId entityId);
-}

Gems/ROS2Controllers/Code/ros2controllers_private_files.cmake

@@ -60,8 +60,6 @@ set(FILES
     Source/Sensors/WheelOdometrySensorConfiguration.cpp
     Source/Utilities/ArticulationsUtilities.cpp
     Source/Utilities/ArticulationsUtilities.h
-    Source/Utilities/JointUtilities.cpp
-    Source/Utilities/JointUtilities.h
     Source/Utilities/Controllers/PidConfiguration.cpp
     Source/VehicleDynamics/AxleConfiguration.cpp
     Source/VehicleDynamics/DriveModel.cpp

Gems/ROS2Sensors/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -39,7 +39,7 @@ namespace ROS2Sensors
 
         const auto* component = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
         AZ_Assert(component, "Entity has no ROS2FrameComponent");
-        m_frameName = component->GetFrameID();
+        m_frameName = component->GetNamespacedFrameID();
         CameraConfigurationRequestBus::Handler::BusConnect(GetEntityId());
 
         StartSensor(
@@ -99,7 +99,7 @@ namespace ROS2Sensors
         AZ_Assert(component, "Entity %s has no ROS2CameraSensorComponent", entity->GetName().c_str());
         if (component)
         {
-            AZStd::string cameraName = component->GetFrameID();
+            AZStd::string cameraName = component->GetNamespacedFrameID();
             AZStd::replace(cameraName.begin(), cameraName.end(), '/', '_');
             return cameraName;
         }

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

@@ -142,7 +142,7 @@ namespace ROS2Sensors
         gazebo_msgs::msg::ContactsState msg;
         const auto* ros2Frame = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
         AZ_Assert(ros2Frame, "Invalid component pointer value");
-        msg.header.frame_id = ros2Frame->GetFrameID().data();
+        msg.header.frame_id = ros2Frame->GetNamespacedFrameID().data();
         msg.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
 
         {

Gems/ROS2Sensors/Code/Source/Imu/ROS2ImuSensorComponent.cpp

@@ -84,7 +84,7 @@ namespace ROS2Sensors
 
         auto ros2Node = ROS2::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();
+        m_imuMsg.header.frame_id = GetNamespacedFrameID().c_str();
         const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations[Internal::kImuMsgType];
         const auto fullTopic = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
         m_imuPublisher = ros2Node->create_publisher<sensor_msgs::msg::Imu>(fullTopic.data(), publisherConfig.GetQoS());

Gems/ROS2Sensors/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp

@@ -125,7 +125,7 @@ namespace ROS2Sensors
 
         auto* ros2Frame = GetEntity()->FindComponent<ROS2::ROS2FrameComponent>();
         auto message = sensor_msgs::msg::LaserScan();
-        message.header.frame_id = ros2Frame->GetFrameID().data();
+        message.header.frame_id = ros2Frame->GetNamespacedFrameID().data();
         message.header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
         message.angle_min = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_minHAngle);
         message.angle_max = AZ::DegToRad(m_lidarCore.m_lidarConfiguration.m_lidarParameters.m_maxHAngle);

Gems/ROS2Sensors/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -93,7 +93,7 @@ namespace ROS2Sensors
                 m_lidarRaycasterId,
                 &LidarRaycasterRequestBus::Events::ConfigurePointCloudPublisher,
                 ROS2::ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic),
-                ros2Frame->GetFrameID().data(),
+                ros2Frame->GetNamespacedFrameID().data(),
                 publisherConfig.GetQoS());
         }
         else
@@ -167,7 +167,7 @@ namespace ROS2Sensors
     void ROS2LidarSensorComponent::PublishRaycastResults(const RaycastResults& results)
     {
         auto builder = PointCloud2MessageBuilder(
-            GetEntity()->FindComponent<ROS2::ROS2FrameComponent>()->GetFrameID(),
+            GetEntity()->FindComponent<ROS2::ROS2FrameComponent>()->GetNamespacedFrameID(),
             ROS2::ROS2Interface::Get()->GetROSTimestamp(),
             results.GetCount());
 

Gems/ROS2Sensors/Code/Source/Odometry/ROS2OdometrySensorComponent.cpp

@@ -105,7 +105,7 @@ namespace ROS2Sensors
         ROS2SensorComponentBase::Activate();
         // "odom" is globally fixed frame for all robots, no matter the namespace
         m_odometryMsg.header.frame_id = ROS2::ROS2Names::GetNamespacedName(GetNamespace(), "odom").c_str();
-        m_odometryMsg.child_frame_id = GetFrameID().c_str();
+        m_odometryMsg.child_frame_id = GetNamespacedFrameID().c_str();
         auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
         AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Odometry sensor");
 

Gems/SimulationInterfaces/Code/Source/Clients/SimulationEntitiesManager.cpp

@@ -643,9 +643,8 @@ namespace SimulationInterfaces
         if (!initialPose.IsOrthogonal())
         {
             AZ_Warning("SimulationInterfaces", false, "Initial pose is not orthogonal");
-            completedCb(
-                AZ::Failure(FailedResult(
-                    simulation_interfaces::srv::SpawnEntity::Response::INVALID_POSE, "Initial pose is not orthogonal"))); //  INVALID_POSE
+            completedCb(AZ::Failure(FailedResult(
+                simulation_interfaces::srv::SpawnEntity::Response::INVALID_POSE, "Initial pose is not orthogonal"))); //  INVALID_POSE
             return;
         }
 
@@ -689,7 +688,7 @@ namespace SimulationInterfaces
                 ROS2::ROS2FrameComponent* frameComponent = entity->template FindComponent<ROS2::ROS2FrameComponent>();
                 if (frameComponent)
                 {
-                    const AZStd::string f = frameComponent->GetFrameID();
+                    const AZStd::string f = frameComponent->GetNamespacedFrameID();
                     if (f.empty())
                     {
                         frameComponent->SetFrameID(name);