Manipulation Components redesign & refactor (#366)

Redesign of manipulation components
- Fixed dependencies and improved handling of actions.
- New interfaces.
- Major changes (breaking manipulation API).
- Moved motorized joints to a subfolder.
- Adding articulation controller
- Adding MoveIt2 test
- Cleanup of interfaces
- Added Editor Component for Joint Manipulation

Co-authored-by: Michał Pełka <[email protected]>
Co-authored-by: Alex Peterson <[email protected]>
Co-authored-by: Piotr Jaroszek <[email protected]>

Signed-off-by: Adam Dąbrowski <[email protected]>

Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h

@@ -0,0 +1,27 @@
+/*
+* 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/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <ROS2/Communication/TopicConfiguration.h>
+#include <rclcpp/publisher.hpp>
+
+namespace ROS2
+{
+    struct PublisherConfiguration
+    {
+        AZ_TYPE_INFO(PublisherConfiguration, "{E50D1926-0322-4832-BD72-C48F854131C8}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        bool m_publish = true;  //!< A switch controlling whether publishing happens.
+        TopicConfiguration m_topicConfiguration; //!< Configuration of the published topic.
+        float m_frequency = 10.0f; //!< Frequency of the published topic (Hz)
+    };
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h

@@ -0,0 +1,53 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/std/string/string.h>
+#include <ROS2/Manipulation/JointInfo.h>
+
+namespace ROS2
+{
+    //! Interface for controllers that execute the simple movement between two positions one step at a time.
+    class JointsPositionControllerRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Whether the implementing controller component is based on articulations.
+        //! @return true if the controller works with articulations, false otherwise.
+        virtual bool SupportsArticulation()
+        {
+            return false;
+        };
+
+        //! Whether the implementing controller component is based on articulations.
+        //! @return true if the controller works with classic joints, false otherwise.
+        virtual bool SupportsClassicJoints()
+        {
+            return false;
+        };
+
+        //! Control a joint through specification of its target position.
+        //! @param jointName name of the joint to move.
+        //! @param joint specification of the joint.
+        //! @param currentPosition current position of the joint.
+        //! @param targetPosition target position of the joint.
+        //! @param deltaTime how much time elapsed in simulation the movement should represent.
+        //! @return nothing on success, error message if the command cannot be realize due to controller or entity configuration.
+        virtual AZ::Outcome<void, AZStd::string> PositionControl(
+            const AZStd::string& jointName,
+            JointInfo joint,
+            JointPosition currentPosition,
+            JointPosition targetPosition,
+            float deltaTime) = 0;
+    };
+    using JointsPositionControllerRequestBus = AZ::EBus<JointsPositionControllerRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.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/ComponentBus.h>
+#include <AzCore/RTTI/RTTI.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/containers/unordered_map.h>
+#include <AzCore/std/string/string.h>
+#include <PhysX/ArticulationTypes.h>
+
+namespace ROS2
+{
+    using JointPosition = float;
+    struct JointInfo
+    {
+        AZ_TYPE_INFO(JointInfo, "{2E33E4D0-78DD-436D-B3AB-F752E744F421}");
+        static void Reflect(AZ::ReflectContext* context);
+
+        bool m_isArticulation = false;
+        PhysX::ArticulationJointAxis m_axis = PhysX::ArticulationJointAxis::Twist;
+        AZ::EntityComponentIdPair m_entityComponentIdPair;
+        JointPosition m_restPosition = 0.0f; //!< Keeps this position if no commands are given (for example, opposing gravity).
+    };
+    using ManipulationJoints = AZStd::unordered_map<AZStd::string, JointInfo>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointPublisherComponent.h

@@ -1,50 +0,0 @@
-#pragma once
-
-#include <AzCore/Component/Component.h>
-#include <AzCore/Component/TickBus.h>
-#include <Source/HingeJointComponent.h>
-#include <rclcpp/publisher.hpp>
-#include <sensor_msgs/msg/joint_state.hpp>
-
-namespace ROS2
-{
-    //! A component responsible for publishing the joint positions on ROS2 /joint_states topic.
-    //!< @see <a href="https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html">jointState message</a>.
-    class JointPublisherComponent
-        : public AZ::Component
-        , public AZ::TickBus::Handler
-    {
-    public:
-        AZ_COMPONENT(JointPublisherComponent, "{a679c2e4-a602-46de-8db4-4b33d83317f4}", AZ::Component);
-        JointPublisherComponent() = default;
-
-        //////////////////////////////////////////////////////////////////////////
-        // Component overrides
-        void Activate() override;
-        void Deactivate() override;
-        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
-        //////////////////////////////////////////////////////////////////////////
-        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
-        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
-        static void Reflect(AZ::ReflectContext* context);
-
-        AZStd::unordered_map<AZ::Name, PhysX::HingeJointComponent> &GetHierarchyMap();
-
-    private:
-        void PublishMessage();
-        void UpdateMessage();
-        void Initialize();
-        AZStd::string GetFrameID() const;
-
-        float GetJointPosition(const AZ::Component& hingeComponent) const;
-
-        AZStd::unordered_map<AZ::Name, PhysX::HingeJointComponent> m_hierarchyMap;
-        std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> m_jointstatePublisher;
-        sensor_msgs::msg::JointState m_jointstateMsg;
-        bool m_initialized{false};
-        float m_timeElapsedSinceLastTick = 0.0f;
-
-        //! Frequency in Hz (1/s).
-        float m_frequency = 10;
-    };
-} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h

@@ -0,0 +1,61 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Outcome/Outcome.h>
+#include <AzCore/std/containers/vector.h>
+#include <AzCore/std/string/string.h>
+#include <ROS2/Manipulation/JointInfo.h>
+
+namespace ROS2
+{
+    //! Interface for general requests for joint systems such as manipulator arms.
+    //! This interface supports only systems with joints or articulation links with a single degree of freedom (DOF) each.
+    class JointsManipulationRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        using JointsPositionsMap = AZStd::unordered_map<AZStd::string, JointPosition>;
+
+        //! Get all entity tree joints, including joint or articulation component hierarchy.
+        //! @return An unordered map of joint names to joint info structure.
+        //! @note Only free joints are returned (no fixed ones).
+        virtual ManipulationJoints GetJoints() = 0;
+
+        //! Get position of a joint by name.
+        //! Works with hinge joints and articulation links.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @return outcome with relative position in degree of motion range if joint exists.
+        //! If it does not exist or some other error happened, error message is returned.
+        virtual AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) = 0;
+
+        //! Return positions of all single DOF joints.
+        //! @return a vector of all joints relative positions in degree of motion range or error message.
+        virtual JointsPositionsMap GetAllJointsPositions() = 0;
+
+        //! Move specified joints into positions.
+        //! @param new positions for each named joint. Use names queried through GetJoints().
+        //! @return nothing on success, error message on failure.
+        //! @note the movement is realized by a specific controller and not instant. The joints will then keep these positions.
+        virtual AZ::Outcome<void, AZStd::string> MoveJointsToPositions(const JointsPositionsMap& positions) = 0;
+
+        //! Move a single joint into desired relative position.
+        //! @param jointName name of the joint. Use names acquired from GetJoints() query.
+        //! @param position relative position in degree of motion range to achieve.
+        //! @return nothing on success, error message on failure.
+        //! @note the movement is realized by a specific controller and not instant. The joints will then keep this position.
+        virtual AZ::Outcome<void, AZStd::string> MoveJointToPosition(const AZStd::string& jointName, JointPosition position) = 0;
+
+        //! Stop the joints movement in progress. It will keep the position in which it stopped.
+        virtual void Stop() = 0;
+    };
+    using JointsManipulationRequestBus = AZ::EBus<JointsManipulationRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h

@@ -0,0 +1,55 @@
+/*
+ * 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/EBus/EBus.h>
+#include <AzCore/Outcome/Outcome.h>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+namespace ROS2
+{
+    //! Interface for commanding a system of joints such as robotic arm (manipulator) through FollowJointTrajectory actions.
+    //@see <a href="https://github.com/ros-controls/control_msgs/blob/humble/control_msgs/action/FollowJointTrajectory.action">FollowJointTrajectory</a>
+    class JointsTrajectoryRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        using TrajectoryGoal = control_msgs::action::FollowJointTrajectory::Goal;
+        using TrajectoryGoalPtr = std::shared_ptr<const TrajectoryGoal>;
+        using TrajectoryResultPtr = std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result>;
+
+        //! Status of trajectory action.
+        enum class TrajectoryActionStatus
+        {
+            Idle, //!< No action has been ordered yet.
+            Executing, //!< Ongoing trajectory goal.
+            Cancelled, //!< Cancelled goal, either by the client user or simulation side.
+            Succeeded //!< Goal reached.
+        };
+
+        //! Validate and, if validation is successful, start a trajectory goal.
+        //! @param trajectoryGoal Specified trajectory including points with timing and tolerances.
+        //! @return Nothing on success, error message if failed.
+        //! @note The call will return an error if the goal trajectory mismatches joints system.
+        virtual AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) = 0;
+
+        //! Cancel current trajectory goal.
+        //! @param result Result of trajectory goal with explanation on why it was cancelled.
+        //! @return nothing on success, error if the goal could not be cancelled.
+        virtual AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr result) = 0;
+
+        //! Retrieve current trajectory goal status.
+        //! @return Status of trajectory goal.
+        virtual TrajectoryActionStatus GetGoalStatus() = 0;
+    };
+
+    using JointsTrajectoryRequestBus = AZ::EBus<JointsTrajectoryRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/ManipulatorControllerComponent.h

@@ -1,62 +0,0 @@
-#pragma once
-
-#include <AzCore/Component/Component.h>
-#include <AzCore/Component/TickBus.h>
-#include <AzCore/Name/Name.h>
-#include <ROS2/Utilities/Controllers/PidConfiguration.h>
-#include <control_msgs/action/follow_joint_trajectory.hpp>
-
-
-namespace ROS2
-{
-    // forward declaration
-    class FollowJointTrajectoryActionServer;
-
-    //! Component responsible for controlling a robotic arm made up of hinge joints.
-    class ManipulatorControllerComponent
-        : public AZ::Component
-        , public AZ::TickBus::Handler
-    {
-    public:
-        enum class Controller
-        {
-            FeedForward, //!< @see <a href="https://en.wikipedia.org/wiki/Feed_forward_(control)">FeedForward</a>.
-            PID          //!< @see <a href="https://en.wikipedia.org/wiki/PID_controller">PID</a>.
-        };
-
-        AZ_COMPONENT(ManipulatorControllerComponent, "{3da9abfc-0028-4e3e-8d04-4e4440d2e319}", AZ::Component); // , ManipulatorRequestBus::Handler);
-
-        ManipulatorControllerComponent();
-        ~ManipulatorControllerComponent();
-
-        //////////////////////////////////////////////////////////////////////////
-        // Component overrides
-        void Activate() override;
-        void Deactivate() override;
-        //////////////////////////////////////////////////////////////////////////
-        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
-        static void Reflect(AZ::ReflectContext* context);
-        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
-
-    private:
-        void InitializePid();
-        void InitializeCurrentPosition();
-        void KeepStillPosition(const uint64_t deltaTimeNs);
-        void ExecuteTrajectory(const uint64_t deltaTimeNs);
-        float GetJointPosition(const AZ::Component& hingeComponent);
-        float ComputeFFJointVelocity(const float currentPosition, const float desiredPosition, const rclcpp::Duration & duration) const;
-        float ComputePIDJointVelocity(const float currentPosition, const float desiredPosition, const uint64_t & deltaTimeNs, int & jointIndex);
-        void SetJointVelocity(AZ::Component& hingeComponent, const float desiredVelocity);
-
-        AZStd::unique_ptr<FollowJointTrajectoryActionServer> m_actionServerClass;
-        AZStd::string m_ROS2ControllerName;
-        bool m_initialized{false};
-        bool m_initializedTrajectory{false};
-        Controller m_controllerType = Controller::FeedForward;
-        bool m_keepStillPositionInitialize{false};
-        AZStd::vector<Controllers::PidConfiguration> m_pidConfigurationVector;
-        AZStd::unordered_map<AZ::Name, float> m_jointKeepStillPosition;
-        trajectory_msgs::msg::JointTrajectory m_trajectory;
-        rclcpp::Time m_timeStartingExecutionTraj;
-    };
-} // namespace ROS2

Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerComponent.h → Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h

@@ -12,7 +12,7 @@
 #include <AzCore/Component/TickBus.h>
 #include <AzCore/Serialization/SerializeContext.h>
 #include <ImGuiBus.h>
-#include <ROS2/Manipulation/JointMotorControllerConfiguration.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h>
 
 namespace ROS2
 {

Gems/ROS2/Code/Include/ROS2/Manipulation/ManualMotorControllerComponent.h → Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h

@@ -8,7 +8,7 @@
 #pragma once
 
 #include <AzCore/Serialization/SerializeContext.h>
-#include <ROS2/Manipulation/JointMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h>
 
 namespace ROS2
 {

Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerComponent.h → Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h

@@ -9,8 +9,8 @@
 
 #include <AzCore/Serialization/SerializeContext.h>
 #include <AzFramework/Entity/EntityDebugDisplayBus.h>
-#include <ROS2/Manipulation/JointMotorControllerComponent.h>
-#include <ROS2/Manipulation/PidMotorControllerBus.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h>
 #include <ROS2/Utilities/Controllers/PidConfiguration.h>
 
 namespace ROS2

Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp

@@ -0,0 +1,38 @@
+/*
+ * 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/Serialization/EditContext.h>
+#include <ROS2/Communication/PublisherConfiguration.h>
+#include <ROS2/Utilities/ROS2Names.h>
+
+namespace ROS2
+{
+    void PublisherConfiguration::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<PublisherConfiguration>()
+                ->Version(1)
+                ->Field("Publish", &PublisherConfiguration::m_publish)
+                ->Field("Topic", &PublisherConfiguration::m_topicConfiguration)
+                ->Field("Frequency", &PublisherConfiguration::m_frequency);
+
+            if (AZ::EditContext* ec = serializeContext->GetEditContext())
+            {
+                ec->Class<PublisherConfiguration>("Publisher", "Configuration of publisher")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_publish, "Publish", "Whether the publisher is on")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_topicConfiguration, "Topic", "Topic configuration")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &PublisherConfiguration::m_frequency, "Frequency (Hz)", "Publishing frequency");
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp

@@ -0,0 +1,69 @@
+/*
+ * 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 "JointsArticulationControllerComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <PhysX/ArticulationJointBus.h>
+
+namespace ROS2
+{
+    void JointsArticulationControllerComponent::Activate()
+    {
+        JointsPositionControllerRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void JointsArticulationControllerComponent::Deactivate()
+    {
+        JointsPositionControllerRequestBus::Handler::BusDisconnect();
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsArticulationControllerComponent::PositionControl(
+        const AZStd::string& jointName,
+        JointInfo joint,
+        [[maybe_unused]] JointPosition currentPosition,
+        JointPosition targetPosition,
+        [[maybe_unused]] float deltaTime)
+    {
+        if (!joint.m_isArticulation)
+        {
+            return AZ::Failure(AZStd::string::format(
+                "Joint %s is not an articulation link, use JointsPIDControllerComponent instead", jointName.c_str()));
+        }
+
+        PhysX::ArticulationJointRequestBus::Event(
+            joint.m_entityComponentIdPair.GetEntityId(), &PhysX::ArticulationJointRequests::SetDriveTarget, joint.m_axis, targetPosition);
+        return AZ::Success();
+    }
+
+    void JointsArticulationControllerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsArticulationControllerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsArticulationControllerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<JointsArticulationControllerComponent, AZ::Component>()->Version(0);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<JointsArticulationControllerComponent>(
+                      "JointsArticulationControllerComponent", "Component to move articulation joints")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2");
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h

@@ -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
+ *
+ */
+
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
+
+namespace ROS2
+{
+    //! Handles position control commands for joints using Articulations.
+    class JointsArticulationControllerComponent
+        : public AZ::Component
+        , public JointsPositionControllerRequestBus::Handler
+    {
+    public:
+        JointsArticulationControllerComponent() = default;
+        ~JointsArticulationControllerComponent() = default;
+        AZ_COMPONENT(JointsArticulationControllerComponent, "{243E9F07-5F84-4F83-9E6D-D1DA04D7CEF9}", AZ::Component);
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void Reflect(AZ::ReflectContext* context);
+
+        // JointsPositionControllerRequestBus::Handler overrides ...
+        //! @see ROS2::JointsPositionControllerRequestBus::SupportsArticulation
+        bool SupportsArticulation() override
+        {
+            return true;
+        }
+
+        //! @see ROS2::JointsPositionControllerRequestBus::SupportsClassicJoints
+        bool SupportsClassicJoints() override
+        {
+            return false;
+        }
+
+        //! @see ROS2::JointsPositionControllerRequestBus::PositionControl
+        AZ::Outcome<void, AZStd::string> PositionControl(
+            const AZStd::string& jointName,
+            JointInfo joint,
+            JointPosition currentPosition,
+            JointPosition targetPosition,
+            float deltaTime) override;
+
+    private:
+        // Component overrides ...
+        void Activate() override;
+        void Deactivate() override;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp

@@ -0,0 +1,96 @@
+/*
+ * 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 "JointsPIDControllerComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <PhysX/Joint/PhysXJointRequestsBus.h>
+
+namespace ROS2
+{
+    void JointsPIDControllerComponent::Activate()
+    {
+        JointsPositionControllerRequestBus::Handler::BusConnect(GetEntityId());
+        InitializePIDs();
+    }
+
+    void JointsPIDControllerComponent::Deactivate()
+    {
+        JointsPositionControllerRequestBus::Handler::BusDisconnect();
+    }
+
+    void JointsPIDControllerComponent::InitializePIDs()
+    {
+        for (auto& [jointName, pid] : m_pidConfiguration)
+        {
+            pid.InitializePid();
+        }
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsPIDControllerComponent::PositionControl(
+        const AZStd::string& jointName,
+        JointInfo joint,
+        JointPosition currentPosition,
+        JointPosition targetPosition,
+        float deltaTime)
+    {
+        if (joint.m_isArticulation)
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s is articulation link, JointsPIDControllerComponent only handles classic Hinge joints. Use "
+                               "JointsArticulationControllerComponent instead", jointName.c_str()));
+        }
+
+        bool jointPIDdefined = m_pidConfiguration.find(jointName) != m_pidConfiguration.end();
+        AZ_Warning(
+            "JointsPIDControllerComponent",
+            jointPIDdefined,
+            "PID not defined for joint %s, using a default, the behavior is likely to be wrong for this joint",
+            jointName.c_str());
+
+        Controllers::PidConfiguration defaultConfiguration;
+        defaultConfiguration.InitializePid();
+        auto applicablePidConfiguration = jointPIDdefined ? m_pidConfiguration.at(jointName) : defaultConfiguration;
+
+        uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
+        auto positionError = targetPosition - currentPosition;
+        float desiredVelocity = applicablePidConfiguration.ComputeCommand(positionError, deltaTimeNs);
+        PhysX::JointRequestBus::Event(joint.m_entityComponentIdPair, &PhysX::JointRequests::SetVelocity, desiredVelocity);
+        return AZ::Success();
+    }
+
+    void JointsPIDControllerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsPIDControllerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsPIDControllerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<JointsPIDControllerComponent, AZ::Component>()->Version(0)->Field(
+                "JointsPIDs", &JointsPIDControllerComponent::m_pidConfiguration);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<JointsPIDControllerComponent>("JointsPIDControllerComponent", "Component to move joints")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &JointsPIDControllerComponent::m_pidConfiguration,
+                        "Joint PIDs",
+                        "PID configuration for each free joint in this entity hierarchy");
+            }
+        }
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h

@@ -0,0 +1,60 @@
+/*
+ * 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/Component.h>
+#include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
+#include <ROS2/Utilities/Controllers/PidConfiguration.h>
+
+namespace ROS2
+{
+    //! Handles position control commands for joints.
+    class JointsPIDControllerComponent
+        : public AZ::Component
+        , public JointsPositionControllerRequestBus::Handler
+    {
+    public:
+        JointsPIDControllerComponent() = default;
+        ~JointsPIDControllerComponent() = default;
+        AZ_COMPONENT(JointsPIDControllerComponent, "{41A31EDB-90B0-412E-BBFA-D35D45546A8E}", AZ::Component);
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void Reflect(AZ::ReflectContext* context);
+
+        // JointsPositionControllerRequestBus::Handler overrides ...
+        //! @see ROS2::JointsPositionControllerRequestBus::SupportsArticulation
+        bool SupportsArticulation() override
+        {
+            return false;
+        }
+
+        //! @see ROS2::JointsPositionControllerRequestBus::SupportsClassicJoints
+        bool SupportsClassicJoints() override
+        {
+            return true;
+        }
+
+        //! @see ROS2::JointsPositionControllerRequestBus::PositionControl
+        AZ::Outcome<void, AZStd::string> PositionControl(
+            const AZStd::string& jointName,
+            JointInfo joint,
+            JointPosition currentPosition,
+            JointPosition targetPosition,
+            float deltaTime) override;
+
+    private:
+        // Component overrides ...
+        void Activate() override;
+        void Deactivate() override;
+        void InitializePIDs();
+
+        AZStd::unordered_map<AZStd::string, Controllers::PidConfiguration> m_pidConfiguration;
+    };
+} // namespace ROS2

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

@@ -0,0 +1,139 @@
+/*
+ * 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 "FollowJointTrajectoryActionServer.h"
+#include <AzCore/std/functional.h>
+#include <ROS2/ROS2Bus.h>
+
+namespace ROS2
+{
+    FollowJointTrajectoryActionServer::FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId)
+        : m_entityId(entityId)
+    {
+        m_actionServer = rclcpp_action::create_server<FollowJointTrajectory>(
+            ROS2Interface::Get()->GetNode(),
+            actionName.c_str(),
+            AZStd::bind(&FollowJointTrajectoryActionServer::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2),
+            AZStd::bind(&FollowJointTrajectoryActionServer::GoalCancelledCallback, this, AZStd::placeholders::_1),
+            AZStd::bind(&FollowJointTrajectoryActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1));
+    }
+
+    JointsTrajectoryRequests::TrajectoryActionStatus FollowJointTrajectoryActionServer::GetGoalStatus() const
+    {
+        return m_goalStatus;
+    }
+
+    void FollowJointTrajectoryActionServer::CancelGoal(std::shared_ptr<FollowJointTrajectory::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_executing())
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling goal\n");
+            m_goalHandle->canceled(result);
+        }
+    }
+
+    void FollowJointTrajectoryActionServer::GoalSuccess(std::shared_ptr<FollowJointTrajectory::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_executing())
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Goal succeeded\n");
+            m_goalHandle->succeed(result);
+            m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Succeeded;
+        }
+    }
+
+    void FollowJointTrajectoryActionServer::PublishFeedback(std::shared_ptr<FollowJointTrajectory::Feedback> feedback)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_executing())
+        {
+            m_goalHandle->publish_feedback(feedback);
+        }
+    }
+
+    bool FollowJointTrajectoryActionServer::IsGoalActiveState() const
+    {
+        return m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling();
+    }
+
+    bool FollowJointTrajectoryActionServer::IsReadyForExecution() const
+    {
+        // Has a goal handle yet - can be accepted.
+        if (!m_goalHandle)
+        {
+            return true;
+        }
+        // accept goal if previous is terminal state
+        return IsGoalActiveState() == false;
+    }
+
+    bool FollowJointTrajectoryActionServer::IsExecuting() const
+    {
+        return m_goalHandle && m_goalHandle->is_executing();
+    }
+
+    rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::GoalReceivedCallback(
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal)
+    { // Accept each received goal unless other goal is active (no deferring/queuing)
+        if (!IsReadyForExecution())
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Goal rejected: server is not ready for execution!");
+            if (m_goalHandle)
+            {
+                AZ_Trace(
+                    "FollowJointTrajectoryActionServer",
+                    " is_active: %d,  is_executing %d, is_canceling : %d",
+                    m_goalHandle->is_active(),
+                    m_goalHandle->is_executing(),
+                    m_goalHandle->is_canceling());
+            }
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+
+        AZ::Outcome<void, AZStd::string> executionOrderOutcome;
+        JointsTrajectoryRequestBus::EventResult(executionOrderOutcome, m_entityId, &JointsTrajectoryRequests::StartTrajectoryGoal, goal);
+
+        if (!executionOrderOutcome)
+        {
+            AZ_Trace("FollowJointTrajectoryActionServer", "Execution not be accepted: %s", executionOrderOutcome.GetError().c_str());
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse FollowJointTrajectoryActionServer::GoalCancelledCallback(
+        [[maybe_unused]] const std::shared_ptr<GoalHandle> goalHandle)
+    { // Accept each cancel attempt
+        auto result = std::make_shared<FollowJointTrajectory::Result>();
+        result->error_string = "User Cancelled";
+        result->error_code = FollowJointTrajectory::Result::SUCCESSFUL;
+
+        AZ::Outcome<void, AZStd::string> cancelOutcome;
+        JointsTrajectoryRequestBus::EventResult(cancelOutcome, m_entityId, &JointsTrajectoryRequests::CancelTrajectoryGoal, result);
+
+        if (!cancelOutcome)
+        { // This will not happen in simulation unless intentionally done for behavior validation
+            AZ_Trace("FollowJointTrajectoryActionServer", "Cancelling could not be accepted: %s\n", cancelOutcome.GetError().c_str());
+            return rclcpp_action::CancelResponse::REJECT;
+        }
+
+        m_goalStatus = JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled;
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void FollowJointTrajectoryActionServer::GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goalHandle)
+    {
+        AZ_Trace("FollowJointTrajectoryActionServer", "Goal accepted\n");
+        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

@@ -1,89 +1,71 @@
 /*
-* 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
-*
-*/
+ * 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/std/string/string.h>
-#include <AzCore/std/functional.h>
-#include <rclcpp_action/server.hpp>
-#include <rclcpp_action/rclcpp_action.hpp>
+#include <ROS2/Manipulation/JointsTrajectoryRequests.h>
 #include <control_msgs/action/follow_joint_trajectory.hpp>
-#include <ROS2/ROS2Bus.h>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/server.hpp>
 
 namespace ROS2
 {
-    enum class GoalStatus
-    {
-        Pending,
-        Active,
-        Concluded
-    };
-
-    // ROS2 Action Server class
+    //! A class wrapping ROS 2 action server for joint trajectory controller.
+    //! @see <a href="https://control.ros.org/master/doc/ros2_controllers/joint_trajectory_controller/doc/userdoc.html"> joint trajectory
+    //! controller </a>.
     class FollowJointTrajectoryActionServer
     {
     public:
-        using GoalHandleFollowJointTrajectory = rclcpp_action::ServerGoalHandle<control_msgs::action::FollowJointTrajectory>;
         using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
-        FollowJointTrajectoryActionServer() = default;
-        ~FollowJointTrajectoryActionServer() = default;
-        void CreateServer(AZStd::string ROS2ControllerName);
 
-        rclcpp_action::Server<FollowJointTrajectory>::SharedPtr m_actionServer;
-        std::shared_ptr<GoalHandleFollowJointTrajectory> m_goalHandle;
+        //! Create an action server for FollowJointTrajectory action and bind Goal callbacks.
+        //! @param actionName Name of the action, similar to topic or service name.
+        //! @param entityId entity which will execute callbacks through JointsTrajectoryRequestBus.
+        //! @see <a href="https://docs.ros.org/en/humble/p/rclcpp_action/generated/classrclcpp__action_1_1Server.html"> ROS 2 action
+        //! server documentation </a>
+        FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId);
 
-        GoalStatus m_goalStatus = GoalStatus::Pending;
+        //! Return trajectory action status.
+        //! @return Status of the trajectory execution.
+        JointsTrajectoryRequests::TrajectoryActionStatus GetGoalStatus() const;
 
-    protected:
-        // callbacks for action_server_
-        rclcpp_action::GoalResponse goal_received_callback(
-            const rclcpp_action::GoalUUID & uuid, 
-            std::shared_ptr<const FollowJointTrajectory::Goal> goal);
-        rclcpp_action::CancelResponse goal_cancelled_callback(
-            const std::shared_ptr<GoalHandleFollowJointTrajectory> goal_handle);
-        void goal_accepted_callback(
-            std::shared_ptr<GoalHandleFollowJointTrajectory> goal_handle);
-    };
+        //! 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);
 
-    void FollowJointTrajectoryActionServer::CreateServer(AZStd::string ROS2ControllerName)
-    {
-        auto ros2Node = ROS2Interface::Get()->GetNode();
-        // Create the ROS2 action server
-        this->m_actionServer = rclcpp_action::create_server<FollowJointTrajectory>(
-            ros2Node,
-            ROS2ControllerName.append("/follow_joint_trajectory").data(),
-            AZStd::bind(&FollowJointTrajectoryActionServer::goal_received_callback, this, AZStd::placeholders::_1, AZStd::placeholders::_2),
-            AZStd::bind(&FollowJointTrajectoryActionServer::goal_cancelled_callback, this, AZStd::placeholders::_1),
-            AZStd::bind(&FollowJointTrajectoryActionServer::goal_accepted_callback, this, AZStd::placeholders::_1));
-    }
+        //! Report goal success to the action server.
+        //! @param result Result which contains success code.
+        void GoalSuccess(std::shared_ptr<FollowJointTrajectory::Result> result);
 
-    rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::goal_received_callback(
-            [[maybe_unused]] const rclcpp_action::GoalUUID & uuid,
-            [[maybe_unused]] std::shared_ptr<const FollowJointTrajectory::Goal> goal)
-    {
-        // Dummy implementation
-        AZ_TracePrintf("ManipulatorControllerComponent", "FollowJointTrajectory manipulator Goal received");
-        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-    }
+        //! Publish feedback during an active action.
+        //! @param feedback An action feedback message informing about the progress.
+        void PublishFeedback(std::shared_ptr<FollowJointTrajectory::Feedback> feedback);
 
-    rclcpp_action::CancelResponse FollowJointTrajectoryActionServer::goal_cancelled_callback(
-            [[maybe_unused]] const std::shared_ptr<GoalHandleFollowJointTrajectory> goal_handle)
-    {
-        // Dummy implementation
-        AZ_TracePrintf("ManipulatorControllerComponent", "FollowJointTrajectory manipulator Goal canceled");
-        return rclcpp_action::CancelResponse::ACCEPT;
-    }
+    private:
+        using GoalHandle = rclcpp_action::ServerGoalHandle<FollowJointTrajectory>;
+        using TrajectoryActionStatus = JointsTrajectoryRequests::TrajectoryActionStatus;
 
-    void FollowJointTrajectoryActionServer::goal_accepted_callback(
-            const std::shared_ptr<GoalHandleFollowJointTrajectory> goal_handle)
-    {
-        AZ_TracePrintf("ManipulatorControllerComponent", "FollowJointTrajectory manipulator Goal accepted");
-        this->m_goalHandle = goal_handle;
-        this->m_goalStatus = GoalStatus::Active;     
-    }
-}
+        AZ::EntityId m_entityId;
+        TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle;
+        rclcpp_action::Server<FollowJointTrajectory>::SharedPtr m_actionServer;
+        std::shared_ptr<GoalHandle> m_goalHandle;
+
+        bool IsGoalActiveState() const;
+        bool IsReadyForExecution() const;
+        bool IsExecuting() const;
+
+        rclcpp_action::GoalResponse GoalReceivedCallback(
+            const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const FollowJointTrajectory::Goal> goal);
+
+        rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr<GoalHandle> goalHandle);
+
+        void GoalAcceptedCallback(const std::shared_ptr<GoalHandle> goalHandle);
+    };
+} // namespace ROS2

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

@@ -0,0 +1,25 @@
+/*
+ * 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 <ROS2/Manipulation/JointInfo.h>
+
+namespace ROS2
+{
+    void JointInfo::Reflect(AZ::ReflectContext* context)
+    {
+        if (auto serializeContext = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serializeContext->Class<JointInfo>()
+                ->Version(1)
+                ->Field("IsArticulation", &JointInfo::m_isArticulation)
+                ->Field("Axis", &JointInfo::m_axis)
+                ->Field("EntityComponentIdPair", &JointInfo::m_entityComponentIdPair)
+                ->Field("RestPosition", &JointInfo::m_restPosition);
+        }
+    }
+} // namespace ROS2

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

@@ -1,156 +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 <ROS2/Manipulation/JointPublisherComponent.h>
-#include <AzCore/Component/TransformBus.h>
-#include <AzCore/Component/ComponentApplicationBus.h>
-#include <AzCore/Serialization/EditContext.h>
-#include <rclcpp/qos.hpp>
-#include <PhysX/Joint/PhysXJointRequestsBus.h>
-#include <ROS2/Frame/ROS2FrameComponent.h>
-#include <ROS2/ROS2Bus.h>
-#include <ROS2/Utilities/ROS2Names.h>
-
-namespace ROS2
-{
-    void JointPublisherComponent::Activate()
-    {
-        AZ::TickBus::Handler::BusConnect();
-        auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
-        auto ros2Frame = GetEntity()->FindComponent<ROS2FrameComponent>();
-        AZStd::string namespacedTopic = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), "joint_states");
-        m_jointstatePublisher = ros2Node->create_publisher<sensor_msgs::msg::JointState>(namespacedTopic.data(), rclcpp::SystemDefaultsQoS());        // TODO: add QoS instead of "1"
-    }
-
-    void JointPublisherComponent::Deactivate()
-    {
-        AZ::TickBus::Handler::BusDisconnect();
-        m_jointstatePublisher.reset();
-    }
-
-    void JointPublisherComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
-    {
-        provided.push_back(AZ_CRC_CE("JointPublisherService"));
-    }
-
-    void JointPublisherComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
-    {
-        required.push_back(AZ_CRC_CE("ROS2Frame"));
-    }
-
-    void JointPublisherComponent::Reflect(AZ::ReflectContext* context)
-    {
-        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
-        {
-            serialize->Class<JointPublisherComponent, AZ::Component>()
-                ->Version(0)
-                ->Field("Frequency (HZ)", &JointPublisherComponent::m_frequency);
-
-            if (AZ::EditContext* ec = serialize->GetEditContext())
-            {
-                ec->Class<JointPublisherComponent>("JointPublisherComponent", "[Publish all the Hinge joint in the tree]")
-                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
-                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default, &JointPublisherComponent::m_frequency, "Frequency", "Frequency of publishing [Hz]")
-                    ->Attribute(AZ::Edit::Attributes::Min, 0.001f)
-                    ->Attribute(AZ::Edit::Attributes::Max, 1000.0f);
-            }
-        }
-    }
-
-    void JointPublisherComponent::Initialize()
-    {
-        AZStd::vector<AZ::EntityId> descendants;
-        AZ::TransformBus::EventResult(descendants, GetEntityId(), &AZ::TransformInterface::GetAllDescendants);
-
-        for (const AZ::EntityId& descendantID : descendants)
-        {
-            AZ::Entity* entity = nullptr;
-            AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, descendantID);
-            AZ_Assert(entity, "Unknown entity %s", descendantID.ToString().c_str());
-            auto* frameComponent = entity->FindComponent<ROS2FrameComponent>();
-            auto* hingeComponent = entity->FindComponent<PhysX::HingeJointComponent>();
-            if (frameComponent && hingeComponent)
-            {
-                AZ::Name jointName = frameComponent->GetJointName();
-                m_hierarchyMap[jointName] = *hingeComponent;
-                m_jointstateMsg.name.push_back(jointName.GetCStr());
-                m_jointstateMsg.position.push_back(0.0f);
-            }
-        }
-    }
-
-    AZStd::unordered_map<AZ::Name, PhysX::HingeJointComponent>  &JointPublisherComponent::GetHierarchyMap()
-    {
-        return m_hierarchyMap;
-    }
-
-    void JointPublisherComponent::UpdateMessage()
-    {
-        int i = 0;
-        std_msgs::msg::Header ros_header;
-        ros_header.frame_id = GetFrameID().data();
-        ros_header.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
-        m_jointstateMsg.header = ros_header;
-        for ([[maybe_unused]] auto& [name, hingeComponent] : m_hierarchyMap)
-        {
-            m_jointstateMsg.position[i] = GetJointPosition(hingeComponent);
-            m_jointstateMsg.name[i] = name.GetCStr();
-            i++;
-        }
-    }
-
-    float JointPublisherComponent::GetJointPosition(const AZ::Component& hingeComponent) const
-    {
-        float position{0};
-        auto componentId = hingeComponent.GetId();
-        auto entityId = hingeComponent.GetEntityId();
-        const AZ::EntityComponentIdPair id(entityId,componentId);
-        PhysX::JointRequestBus::EventResult(position, id, &PhysX::JointRequests::GetPosition);
-        return position;
-    }
-
-    void JointPublisherComponent::PublishMessage()
-    {
-        UpdateMessage();
-        m_jointstatePublisher->publish(m_jointstateMsg);
-    }
-
-    AZStd::string JointPublisherComponent::GetFrameID() const
-    {
-        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
-        return ros2Frame->GetFrameID();
-    }
-
-    void JointPublisherComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
-    {
-        if (!m_initialized)
-        {
-            Initialize();
-            m_initialized = true;
-        }
-
-        AZ_Assert(m_frequency > 0, "JointPublisher frequency must be greater than zero");
-        auto frameTime = 1 / m_frequency;
-
-        m_timeElapsedSinceLastTick += deltaTime;
-        if (m_timeElapsedSinceLastTick < frameTime)
-            return;
-
-        m_timeElapsedSinceLastTick -= frameTime;
-        if (deltaTime > frameTime)
-        { // Frequency higher than possible, not catching up, just keep going with each frame.
-            m_timeElapsedSinceLastTick = 0.0f;
-        }
-
-        // Note that the publisher frequency can be limited by simulation tick rate (if higher frequency is desired).
-        PublishMessage();
-    }
-} // namespace ROS2

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

@@ -0,0 +1,74 @@
+/*
+ * 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 "JointStatePublisher.h"
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Names.h>
+
+namespace ROS2
+{
+    JointStatePublisher::JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context)
+        : m_configuration(configuration)
+        , m_context(context)
+    {
+        auto topicConfiguration = m_configuration.m_topicConfiguration;
+        AZStd::string topic = ROS2Names::GetNamespacedName(context.m_publisherNamespace, topicConfiguration.m_topic);
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        m_jointStatePublisher = ros2Node->create_publisher<sensor_msgs::msg::JointState>(topic.data(), topicConfiguration.GetQoS());
+    }
+
+    void JointStatePublisher::PublishMessage()
+    {
+        std_msgs::msg::Header rosHeader;
+        rosHeader.frame_id = ROS2Names::GetNamespacedName(m_context.m_publisherNamespace, m_context.m_frameId).data();
+        rosHeader.stamp = ROS2::ROS2Interface::Get()->GetROSTimestamp();
+        m_jointStateMsg.header = rosHeader;
+
+        ManipulationJoints manipulatorJoints;
+        JointsManipulationRequestBus::EventResult(manipulatorJoints, m_context.m_entityId, &JointsManipulationRequests::GetJoints);
+
+        m_jointStateMsg.name.resize(manipulatorJoints.size());
+        m_jointStateMsg.position.resize(manipulatorJoints.size());
+        m_jointStateMsg.velocity.resize(manipulatorJoints.size());
+        m_jointStateMsg.effort.resize(manipulatorJoints.size());
+        size_t i = 0;
+        for (const auto& [jointName, jointInfo] : manipulatorJoints)
+        {
+            AZ::Outcome<float, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(result, m_context.m_entityId, &JointsManipulationRequests::GetJointPosition, jointName);
+            auto currentJointPosition = result.GetValue();
+
+            m_jointStateMsg.name[i] = jointName.c_str();
+            m_jointStateMsg.position[i] = currentJointPosition;
+            m_jointStateMsg.velocity[i] = 0.0;
+            m_jointStateMsg.effort[i] = 0.0;
+            i++;
+        }
+        m_jointStatePublisher->publish(m_jointStateMsg);
+    }
+
+    void JointStatePublisher::OnTick(float deltaTime)
+    {
+        AZ_Assert(m_configuration.m_frequency > 0.f, "JointPublisher frequency must be greater than zero");
+        auto frameTime = 1.f / m_configuration.m_frequency;
+
+        m_timeElapsedSinceLastTick += deltaTime;
+        if (m_timeElapsedSinceLastTick < frameTime)
+        {
+            return;
+        }
+
+        m_timeElapsedSinceLastTick -= frameTime;
+        if (deltaTime > frameTime)
+        { // Frequency higher than possible, not catching up, just keep going with each frame.
+            m_timeElapsedSinceLastTick = 0.0f;
+        }
+        PublishMessage();
+    }
+} // namespace ROS2

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

@@ -0,0 +1,45 @@
+/*
+ * 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 <ROS2/Communication/PublisherConfiguration.h>
+#include <rclcpp/publisher.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+namespace ROS2
+{
+    struct JointStatePublisherContext
+    {
+        AZ::EntityId m_entityId;
+        AZStd::string m_frameId;
+        AZStd::string m_publisherNamespace;
+    };
+
+    //! A class responsible for publishing the joint positions on ROS2 /joint_states topic.
+    //!< @see <a href="https://docs.ros2.org/latest/api/sensor_msgs/msg/JointState.html">jointState message</a>.
+    class JointStatePublisher
+    {
+    public:
+        JointStatePublisher(const PublisherConfiguration& configuration, const JointStatePublisherContext& context);
+
+        //! Update time tick. This will result in state publishing if timing matches frequency.
+        void OnTick(float deltaTime);
+
+    private:
+        void PublishMessage();
+
+        PublisherConfiguration m_configuration;
+        JointStatePublisherContext m_context;
+
+        std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> m_jointStatePublisher;
+        sensor_msgs::msg::JointState m_jointStateMsg;
+        float m_timeElapsedSinceLastTick = 0.0f;
+    };
+} // namespace ROS2

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

@@ -0,0 +1,339 @@
+/*
+ * 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 "JointsManipulationComponent.h"
+#include "Controllers/JointsArticulationControllerComponent.h"
+#include "Controllers/JointsPIDControllerComponent.h"
+#include "JointStatePublisher.h"
+#include <AzCore/Component/ComponentApplicationBus.h>
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <Source/ArticulationLinkComponent.h>
+#include <Source/HingeJointComponent.h>
+
+namespace ROS2
+{
+    namespace Internal
+    {
+        void AddHingeJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints)
+        {
+            if (joints.find(jointName) != joints.end())
+            {
+                AZ_Assert(false, "Joint names in hierarchy need to be unique (%s is not)!", jointName.c_str());
+                return;
+            }
+            AZ_Printf("JointsManipulationComponent", "Adding joint info for hinge joint %s\n", jointName.c_str());
+            JointInfo jointInfo;
+            jointInfo.m_isArticulation = false;
+            jointInfo.m_axis = static_cast<PhysX::ArticulationJointAxis>(0);
+            jointInfo.m_entityComponentIdPair = idPair;
+            joints[jointName] = jointInfo;
+        }
+
+        bool TryGetFreeArticulationAxis(const AZ::EntityId& entityId, PhysX::ArticulationJointAxis& axis)
+        {
+            for (AZ::u8 i = 0; i <= static_cast<AZ::u8>(PhysX::ArticulationJointAxis::Z); i++)
+            {
+                PhysX::ArticulationJointMotionType type = PhysX::ArticulationJointMotionType::Locked;
+                axis = static_cast<PhysX::ArticulationJointAxis>(i);
+                // Use bus to prevent compilation error without PhysX Articulation support.
+                PhysX::ArticulationJointRequestBus::EventResult(type, entityId, &PhysX::ArticulationJointRequests::GetMotion, axis);
+                if (type != PhysX::ArticulationJointMotionType::Locked)
+                {
+                    return true;
+                }
+            }
+            return false;
+        }
+
+        void AddArticulationJointInfo(const AZ::EntityComponentIdPair& idPair, const AZStd::string& jointName, ManipulationJoints& joints)
+        {
+            PhysX::ArticulationJointAxis freeAxis;
+            bool hasFreeAxis = TryGetFreeArticulationAxis(idPair.GetEntityId(), freeAxis);
+            if (!hasFreeAxis)
+            { // Do not add a joint since it is a fixed one
+                AZ_Printf("JointsManipulationComponent", "Articulation joint %s is fixed, skipping\n", jointName.c_str());
+                return;
+            }
+
+            if (joints.find(jointName) != joints.end())
+            {
+                AZ_Assert(false, "Joint names in hierarchy need to be unique (%s is not)!", jointName.c_str());
+                return;
+            }
+
+            AZ_Printf("JointsManipulationComponent", "Adding joint info for articulation link %s\n", jointName.c_str());
+            JointInfo jointInfo;
+            jointInfo.m_isArticulation = true;
+            jointInfo.m_axis = freeAxis;
+            jointInfo.m_entityComponentIdPair = idPair;
+            joints[jointName] = jointInfo;
+        }
+
+        ManipulationJoints GetAllEntityHierarchyJoints(const AZ::EntityId& entityId)
+        { // Look for either Articulation Links or Hinge joints in entity hierarchy and collect them into a map.
+            // Determine kind of joints through presence of appropriate controller
+            bool supportsArticulation = false;
+            bool supportsClassicJoints = false;
+            JointsPositionControllerRequestBus::EventResult(
+                supportsArticulation, entityId, &JointsPositionControllerRequests::SupportsArticulation);
+            JointsPositionControllerRequestBus::EventResult(
+                supportsClassicJoints, entityId, &JointsPositionControllerRequests::SupportsClassicJoints);
+            ManipulationJoints manipulationJoints;
+            if (!supportsArticulation && !supportsClassicJoints)
+            {
+                AZ_Warning("JointsManipulationComponent", false, "No suitable Position Controller Component in entity!");
+                return manipulationJoints;
+            }
+            if (supportsArticulation && supportsClassicJoints)
+            {
+                AZ_Warning("JointsManipulationComponent", false, "Cannot support both classic joint and articulations in one hierarchy");
+                return manipulationJoints;
+            }
+
+            // Get all descendants and iterate over joints
+            AZStd::vector<AZ::EntityId> descendants;
+            AZ::TransformBus::EventResult(descendants, entityId, &AZ::TransformInterface::GetEntityAndAllDescendants);
+            AZ_Warning("JointsManipulationComponent", descendants.size() > 0, "Entity %s has no descendants!", entityId.ToString().c_str());
+            for (const AZ::EntityId& descendantID : descendants)
+            {
+                AZ::Entity* entity = nullptr;
+                AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, descendantID);
+                AZ_Assert(entity, "Unknown entity %s", descendantID.ToString().c_str());
+
+                // If there is a Frame Component, take joint name stored in it.
+                auto* frameComponent = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(entity);
+                if (!frameComponent)
+                { // Frame Component is required for joints.
+                    continue;
+                }
+                const AZStd::string jointName(frameComponent->GetJointName().GetCStr());
+
+                auto* hingeComponent = Utils::GetGameOrEditorComponent<PhysX::HingeJointComponent>(entity);
+                auto* articulationComponent = Utils::GetGameOrEditorComponent<PhysX::ArticulationLinkComponent>(entity);
+                AZ_Warning(
+                    "JointsManipulationComponent",
+                    (hingeComponent && supportsClassicJoints) || !hingeComponent,
+                    "Found classic joints but the controller does not support them!");
+                AZ_Warning(
+                    "JointsManipulationComponent",
+                    (articulationComponent && supportsArticulation) || !articulationComponent,
+                    "Found articulations but the controller does not support them!");
+
+                // See if there is a Hinge Joint in the entity, add it to map.
+                if (supportsClassicJoints && hingeComponent)
+                {
+                    auto idPair = AZ::EntityComponentIdPair(hingeComponent->GetEntityId(), hingeComponent->GetId());
+                    Internal::AddHingeJointInfo(idPair, jointName, manipulationJoints);
+                }
+
+                // See if there is an Articulation Link in the entity, add it to map.
+                if (supportsArticulation && articulationComponent)
+                {
+                    auto idPair = AZ::EntityComponentIdPair(articulationComponent->GetEntityId(), articulationComponent->GetId());
+                    Internal::AddArticulationJointInfo(idPair, jointName, manipulationJoints);
+                }
+            }
+            return manipulationJoints;
+        }
+
+        void SetInitialPositions(ManipulationJoints& manipulationJoints, const AZStd::unordered_map<AZStd::string, float>& initialPositions)
+        {
+            // Set the initial / resting position to move to and keep.
+            for (const auto& [jointName, jointInfo] : manipulationJoints)
+            {
+                if (initialPositions.contains(jointName))
+                {
+                    manipulationJoints[jointName].m_restPosition = initialPositions.at(jointName);
+                }
+                else
+                {
+                    AZ_Warning("JointsManipulationComponent", false, "No set initial position for joint %s", jointName.c_str());
+                }
+            }
+        }
+    } // namespace Internal
+
+    JointsManipulationComponent::JointsManipulationComponent()
+    {
+    }
+
+    JointsManipulationComponent::JointsManipulationComponent(
+        const PublisherConfiguration& configuration, const AZStd::unordered_map<AZStd::string, JointPosition>& initialPositions)
+        : m_jointStatePublisherConfiguration(configuration)
+        , m_initialPositions(initialPositions)
+    {
+    }
+
+    void JointsManipulationComponent::Activate()
+    {
+        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        JointStatePublisherContext publisherContext;
+        publisherContext.m_publisherNamespace = ros2Frame->GetNamespace();
+        publisherContext.m_frameId = ros2Frame->GetFrameID();
+        publisherContext.m_entityId = GetEntityId();
+
+        m_jointStatePublisher = AZStd::make_unique<JointStatePublisher>(m_jointStatePublisherConfiguration, publisherContext);
+
+        AZ::TickBus::Handler::BusConnect();
+        JointsManipulationRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void JointsManipulationComponent::Deactivate()
+    {
+        JointsManipulationRequestBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+    }
+
+    ManipulationJoints JointsManipulationComponent::GetJoints()
+    {
+        return m_manipulationJoints;
+    }
+
+    AZ::Outcome<JointPosition, AZStd::string> JointsManipulationComponent::GetJointPosition(const AZStd::string& jointName)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+
+        auto jointInfo = m_manipulationJoints.at(jointName);
+        float position{ 0 };
+        if (jointInfo.m_isArticulation)
+        {
+            PhysX::ArticulationJointRequestBus::EventResult(
+                position,
+                jointInfo.m_entityComponentIdPair.GetEntityId(),
+                &PhysX::ArticulationJointRequests::GetJointPosition,
+                jointInfo.m_axis);
+        }
+        else
+        {
+            PhysX::JointRequestBus::EventResult(position, jointInfo.m_entityComponentIdPair, &PhysX::JointRequests::GetPosition);
+        }
+        return AZ::Success(position);
+    }
+
+    JointsManipulationRequests::JointsPositionsMap JointsManipulationComponent::GetAllJointsPositions()
+    {
+        JointsManipulationRequests::JointsPositionsMap positions;
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            positions[jointName] = GetJointPosition(jointName).GetValue();
+        }
+        return positions;
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsManipulationComponent::MoveJointToPosition(
+        const AZStd::string& jointName, JointPosition position)
+    {
+        if (!m_manipulationJoints.contains(jointName))
+        {
+            return AZ::Failure(AZStd::string::format("Joint %s does not exist", jointName.c_str()));
+        }
+        m_manipulationJoints[jointName].m_restPosition = position;
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsManipulationComponent::MoveJointsToPositions(
+        const JointsManipulationRequests::JointsPositionsMap& positions)
+    {
+        for (const auto& [jointName, position] : positions)
+        {
+            auto outcome = MoveJointToPosition(jointName, position);
+            if (!outcome)
+            {
+                return outcome;
+            }
+        }
+        return AZ::Success();
+    }
+
+    void JointsManipulationComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsManipulationComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("JointsManipulationService"));
+    }
+
+    void JointsManipulationComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("JointsManipulationService"));
+    }
+
+    void JointsManipulationComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<JointsManipulationComponent, AZ::Component>()
+                ->Version(1)
+                ->Field("JointStatesPublisherConfiguration", &JointsManipulationComponent::m_jointStatePublisherConfiguration)
+                ->Field("InitialJointPosition", &JointsManipulationComponent::m_initialPositions);
+        }
+    }
+
+    void JointsManipulationComponent::MoveToSetPositions(float deltaTime)
+    {
+        for (const auto& [jointName, jointInfo] : m_manipulationJoints)
+        {
+            float currentPosition = GetJointPosition(jointName).GetValue();
+            float desiredPosition = jointInfo.m_restPosition;
+
+            AZ::Outcome<void, AZStd::string> positionControlOutcome;
+            JointsPositionControllerRequestBus::EventResult(
+                positionControlOutcome,
+                GetEntityId(),
+                &JointsPositionControllerRequests::PositionControl,
+                jointName,
+                jointInfo,
+                currentPosition,
+                desiredPosition,
+                deltaTime);
+
+            AZ_Warning(
+                "JointsManipulationComponent",
+                positionControlOutcome,
+                "Position control failed for joint %s (%s): %s",
+                jointName.c_str(),
+                jointInfo.m_entityComponentIdPair.GetEntityId().ToString().c_str(),
+                positionControlOutcome.GetError().c_str());
+        }
+    }
+
+    void JointsManipulationComponent::Stop()
+    {
+        for (auto& [jointName, jointInfo] : m_manipulationJoints)
+        { // Set all target joint positions to their current positions.
+            jointInfo.m_restPosition = GetJointPosition(jointName).GetValue();
+        }
+    }
+
+    void JointsManipulationComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (m_manipulationJoints.empty())
+        {
+            m_manipulationJoints = Internal::GetAllEntityHierarchyJoints(GetEntityId());
+            Internal::SetInitialPositions(m_manipulationJoints, m_initialPositions);
+            if (m_manipulationJoints.empty())
+            {
+                AZ_Warning("JointsManipulationComponent", false, "No manipulation joints to handle!");
+                AZ::TickBus::Handler::BusDisconnect();
+                return;
+            }
+        }
+        m_jointStatePublisher->OnTick(deltaTime);
+        MoveToSetPositions(deltaTime);
+    }
+} // namespace ROS2

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

@@ -0,0 +1,69 @@
+/*
+ * 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/Component.h>
+#include <AzCore/Component/EntityBus.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzCore/Name/Name.h>
+
+#include "JointStatePublisher.h"
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+
+namespace ROS2
+{
+    //! Component responsible for controlling a hierarchical system of joints such as robotic arm with Articulations or Hinge Joints.
+    //! This manipulator component uses simple joint position interface. For trajectory control, see JointsTrajectoryComponent.
+    class JointsManipulationComponent
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+        , public JointsManipulationRequestBus::Handler
+    {
+    public:
+        JointsManipulationComponent();
+        JointsManipulationComponent(
+            const PublisherConfiguration& configuration, const AZStd::unordered_map<AZStd::string, JointPosition>& initialPositions);
+        ~JointsManipulationComponent() = default;
+        AZ_COMPONENT(JointsManipulationComponent, "{3da9abfc-0028-4e3e-8d04-4e4440d2e319}", AZ::Component);
+
+        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* context);
+
+        // JointsManipulationRequestBus::Handler overrides ...
+        //! @see ROS2::JointsManipulationRequestBus::GetJoints
+        ManipulationJoints GetJoints() override;
+        //! @see ROS2::JointsManipulationRequestBus::GetJointPosition
+        AZ::Outcome<JointPosition, AZStd::string> GetJointPosition(const AZStd::string& jointName) override;
+        //! @see ROS2::JointsManipulationRequestBus::GetAllJointsPositions
+        JointsPositionsMap GetAllJointsPositions() override;
+        //! @see ROS2::JointsManipulationRequestBus::MoveJointsToPositions
+        AZ::Outcome<void, AZStd::string> MoveJointsToPositions(const JointsPositionsMap& positions) override;
+        //! @see ROS2::JointsManipulationRequestBus::MoveJointToPosition
+        AZ::Outcome<void, AZStd::string> MoveJointToPosition(const AZStd::string& jointName, JointPosition position) override;
+        //! @see ROS2::JointsManipulationRequestBus::Stop
+        void Stop() override;
+
+    private:
+        // Component overrides ...
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        void MoveToSetPositions(float deltaTime);
+
+        AZStd::unique_ptr<JointStatePublisher> m_jointStatePublisher;
+        PublisherConfiguration m_jointStatePublisherConfiguration;
+        ManipulationJoints m_manipulationJoints;
+        AZStd::unordered_map<AZStd::string, JointPosition> m_initialPositions;
+    };
+} // namespace ROS2

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

@@ -0,0 +1,80 @@
+/*
+ * 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 "JointsManipulationEditorComponent.h"
+#include "JointsManipulationComponent.h"
+#include <AzCore/Component/ComponentApplicationBus.h>
+#include <AzCore/Component/TransformBus.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h>
+#include <ROS2/Manipulation/JointInfo.h>
+#include <ROS2/ROS2GemUtilities.h>
+#include <ROS2/Utilities/ROS2Names.h>
+#include <Source/ArticulationLinkComponent.h>
+#include <Source/HingeJointComponent.h>
+
+namespace ROS2
+{
+    JointsManipulationEditorComponent::JointsManipulationEditorComponent()
+    {
+        m_jointStatePublisherConfiguration.m_topicConfiguration.m_type = "sensor_msgs::msg::JointState";
+        m_jointStatePublisherConfiguration.m_topicConfiguration.m_topic = "joint_states";
+        m_jointStatePublisherConfiguration.m_frequency = 25.0f;
+    }
+
+    void JointsManipulationEditorComponent::BuildGameEntity(AZ::Entity* gameEntity)
+    {
+        gameEntity->CreateComponent<JointsManipulationComponent>(m_jointStatePublisherConfiguration, m_initialPositions);
+    }
+
+    void JointsManipulationEditorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("JointsControllerService"));
+    }
+
+    void JointsManipulationEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("JointsManipulationService"));
+    }
+
+    void JointsManipulationEditorComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("JointsManipulationService"));
+    }
+
+    void JointsManipulationEditorComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<JointsManipulationEditorComponent, AZ::Component>()
+                ->Version(0)
+                ->Field("JointStatePublisherConfiguration", &JointsManipulationEditorComponent::m_jointStatePublisherConfiguration)
+                ->Field("Initial positions", &JointsManipulationEditorComponent::m_initialPositions);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<JointsManipulationEditorComponent>("JointsManipulationEditorComponent", "Component for manipulation of joints")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &JointsManipulationEditorComponent::m_jointStatePublisherConfiguration,
+                        "Joint State Publisher",
+                        "Configuration of Joint State Publisher")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &JointsManipulationEditorComponent::m_initialPositions,
+                        "Initial positions",
+                        "Initial positions of all the joints");
+            }
+        }
+    }
+} // namespace ROS2

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

@@ -0,0 +1,38 @@
+/*
+ * 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/std/containers/unordered_map.h>
+#include <AzCore/std/string/string.h>
+#include <AzToolsFramework/ToolsComponents/EditorComponentBase.h>
+#include <ROS2/Communication/PublisherConfiguration.h>
+
+namespace ROS2
+{
+    //! Editor Component responsible for a hierarchical system of joints such as robotic arm with Articulations or Hinge Joints.
+    class JointsManipulationEditorComponent : public AzToolsFramework::Components::EditorComponentBase
+    {
+    public:
+        JointsManipulationEditorComponent();
+        ~JointsManipulationEditorComponent() = default;
+        AZ_EDITOR_COMPONENT(JointsManipulationEditorComponent, "{BF2F77FD-92FB-4730-92C7-DDEE54F508BF}");
+
+        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* context);
+
+        // AzToolsFramework::Components::EditorComponentBase overrides
+        void BuildGameEntity(AZ::Entity* gameEntity) override;
+
+    private:
+        PublisherConfiguration m_jointStatePublisherConfiguration;
+        AZStd::unordered_map<AZStd::string, float> m_initialPositions;
+    };
+} // namespace ROS2

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

@@ -0,0 +1,250 @@
+/*
+ * 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 "JointsTrajectoryComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <ROS2/ROS2Bus.h>
+#include <ROS2/Utilities/ROS2Names.h>
+
+namespace ROS2
+{
+    void JointsTrajectoryComponent::Activate()
+    {
+        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        AZ_Assert(ros2Frame, "Missing Frame Component!");
+        AZStd::string namespacedAction = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), m_followTrajectoryActionName);
+        m_followTrajectoryServer = AZStd::make_unique<FollowJointTrajectoryActionServer>(namespacedAction, GetEntityId());
+        AZ::TickBus::Handler::BusConnect();
+        JointsTrajectoryRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    ManipulationJoints& JointsTrajectoryComponent::GetManipulationJoints()
+    {
+        if (m_manipulationJoints.empty())
+        {
+            JointsManipulationRequestBus::EventResult(m_manipulationJoints, GetEntityId(), &JointsManipulationRequests::GetJoints);
+        }
+        return m_manipulationJoints;
+    }
+
+    void JointsTrajectoryComponent::Deactivate()
+    {
+        JointsTrajectoryRequestBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+        m_followTrajectoryServer.reset();
+    }
+
+    void JointsTrajectoryComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<JointsTrajectoryComponent, AZ::Component>()->Version(0)->Field(
+                "Action name", &JointsTrajectoryComponent::m_followTrajectoryActionName);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<JointsTrajectoryComponent>("JointsTrajectoryComponent", "Component to control a robotic arm using trajectories")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &JointsTrajectoryComponent::m_followTrajectoryActionName,
+                        "Action Name",
+                        "Name the follow trajectory action server to accept movement commands");
+            }
+        }
+    }
+
+    void JointsTrajectoryComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("JointsManipulationService"));
+    }
+
+    void JointsTrajectoryComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService"));
+    }
+
+    void JointsTrajectoryComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("ManipulatorJointTrajectoryService"));
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal)
+    {
+        if (m_trajectoryInProgress)
+        {
+            return AZ::Failure("Another trajectory goal is executing. Wait for completion or cancel it");
+        }
+
+        auto validationResult = ValidateGoal(trajectoryGoal);
+        if (!validationResult)
+        {
+            return validationResult;
+        }
+        m_trajectoryGoal = *trajectoryGoal;
+        m_trajectoryExecutionStartTime = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp());
+        m_trajectoryInProgress = true;
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::ValidateGoal(TrajectoryGoalPtr trajectoryGoal)
+    {
+        // Check joint names validity
+        for (const auto& jointName : trajectoryGoal->trajectory.joint_names)
+        {
+            AZStd::string azJointName(jointName.c_str());
+            if (m_manipulationJoints.find(azJointName) == m_manipulationJoints.end())
+            {
+                AZ_Printf(
+                    "JointsTrajectoryComponent",
+                    "Trajectory goal is invalid: no joint %s in manipulator",
+                    azJointName.c_str());
+                return AZ::Failure(AZStd::string::format("Trajectory goal is invalid: no joint %s in manipulator", azJointName.c_str()));
+            }
+        }
+        return AZ::Success();
+    }
+
+    void JointsTrajectoryComponent::UpdateFeedback()
+    {
+        auto feedback = std::make_shared<control_msgs::action::FollowJointTrajectory::Feedback>();
+        trajectory_msgs::msg::JointTrajectoryPoint desiredPoint;
+        for (const auto& [jointName, hingeComponent] : m_manipulationJoints)
+        {
+            std::string jointNameStdString(jointName.c_str());
+            feedback->joint_names.push_back(jointNameStdString);
+
+            AZ::Outcome<float, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(result, GetEntityId(), &JointsManipulationRequests::GetJointPosition, jointName);
+            auto currentJointPosition = result.GetValue();
+
+            desiredPoint.positions.push_back(static_cast<double>(currentJointPosition));
+            desiredPoint.velocities.push_back(0.0f); // Velocities not supported yet!
+            desiredPoint.accelerations.push_back(0.0f); // Accelerations not supported yet!
+            desiredPoint.effort.push_back(0.0f); // Effort not supported yet!
+        }
+
+        trajectory_msgs::msg::JointTrajectoryPoint actualPoint = desiredPoint;
+        trajectory_msgs::msg::JointTrajectoryPoint currentError;
+
+        std::transform(
+            desiredPoint.positions.begin(),
+            desiredPoint.positions.end(),
+            actualPoint.positions.begin(),
+            currentError.positions.begin(),
+            std::minus<double>());
+
+        std::transform(
+            desiredPoint.velocities.begin(),
+            desiredPoint.velocities.end(),
+            actualPoint.velocities.begin(),
+            currentError.velocities.begin(),
+            std::minus<double>());
+
+        std::transform(
+            desiredPoint.accelerations.begin(),
+            desiredPoint.accelerations.end(),
+            actualPoint.accelerations.begin(),
+            currentError.accelerations.begin(),
+            std::minus<double>());
+
+        std::transform(
+            desiredPoint.effort.begin(),
+            desiredPoint.effort.end(),
+            actualPoint.effort.begin(),
+            currentError.effort.begin(),
+            std::minus<double>());
+
+        feedback->desired = desiredPoint;
+        feedback->actual = actualPoint;
+        feedback->error = currentError;
+        m_followTrajectoryServer->PublishFeedback(feedback);
+    }
+
+    AZ::Outcome<void, AZStd::string> JointsTrajectoryComponent::CancelTrajectoryGoal(TrajectoryResultPtr result)
+    {
+        m_trajectoryGoal.trajectory.points.clear();
+        m_followTrajectoryServer->CancelGoal(result);
+        m_trajectoryInProgress = false;
+        return AZ::Success();
+    }
+
+    JointsTrajectoryRequests::TrajectoryActionStatus JointsTrajectoryComponent::GetGoalStatus()
+    {
+        return m_followTrajectoryServer->GetGoalStatus();
+    }
+
+    void JointsTrajectoryComponent::FollowTrajectory(const uint64_t deltaTimeNs)
+    {
+        auto goalStatus = GetGoalStatus();
+        if (goalStatus == JointsTrajectoryRequests::TrajectoryActionStatus::Cancelled)
+        {
+            JointsManipulationRequestBus::Event(GetEntityId(), &JointsManipulationRequests::Stop);
+            return;
+        }
+
+        if (goalStatus != JointsTrajectoryRequests::TrajectoryActionStatus::Executing)
+        {
+            return;
+        }
+
+        if (m_trajectoryGoal.trajectory.points.size() == 0)
+        { // The manipulator has reached the goal.
+            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;
+            return;
+        }
+
+        auto desiredGoal = m_trajectoryGoal.trajectory.points.front();
+        rclcpp::Duration targetGoalTime = rclcpp::Duration(desiredGoal.time_from_start); //!< Requested arrival time for trajectory point.
+        rclcpp::Time timeNow = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp()); //!< Current simulation time.
+        rclcpp::Duration threshold = rclcpp::Duration::from_nanoseconds(1e7);
+
+        if (m_trajectoryExecutionStartTime + targetGoalTime <= timeNow + threshold)
+        { // Jump to the next point if current simulation time is ahead of timeFromStart
+            m_trajectoryGoal.trajectory.points.erase(m_trajectoryGoal.trajectory.points.begin());
+            FollowTrajectory(deltaTimeNs);
+            return;
+        }
+
+        MoveToNextPoint(desiredGoal);
+    }
+
+    void JointsTrajectoryComponent::MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint)
+    {
+        for (int jointIndex = 0; jointIndex < m_trajectoryGoal.trajectory.joint_names.size(); jointIndex++)
+        { // Order each joint to be moved
+            AZStd::string jointName(m_trajectoryGoal.trajectory.joint_names[jointIndex].c_str());
+            AZ_Assert(m_manipulationJoints.find(jointName) != m_manipulationJoints.end(), "Invalid trajectory executing");
+
+            float targetPos = currentTrajectoryPoint.positions[jointIndex];
+            AZ::Outcome<void, AZStd::string> result;
+            JointsManipulationRequestBus::EventResult(
+                result, GetEntityId(), &JointsManipulationRequests::MoveJointToPosition, jointName, targetPos);
+            AZ_Warning("JointTrajectoryComponent", result, "Joint move cannot be realized: %s", result.GetError().c_str());
+        }
+    }
+
+    void JointsTrajectoryComponent::OnTick(float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
+    {
+        if (m_manipulationJoints.empty())
+        {
+            GetManipulationJoints();
+            return;
+        }
+        uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
+        FollowTrajectory(deltaTimeNs);
+    }
+} // namespace ROS2

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

@@ -0,0 +1,70 @@
+/*
+ * 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 "FollowJointTrajectoryActionServer.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/EntityBus.h>
+#include <AzCore/Component/TickBus.h>
+#include <ROS2/Manipulation/JointsManipulationRequests.h>
+#include <ROS2/Manipulation/JointsTrajectoryRequests.h>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+namespace ROS2
+{
+    //! Component responsible for execution of commands to move robotic arm (manipulator) based on set trajectory goal.
+    class JointsTrajectoryComponent
+        : public AZ::Component
+        , public AZ::TickBus::Handler
+        , public JointsTrajectoryRequestBus::Handler
+    {
+    public:
+        JointsTrajectoryComponent() = default;
+        ~JointsTrajectoryComponent() = default;
+        AZ_COMPONENT(JointsTrajectoryComponent, "{429DE04C-6B6D-4B2D-9D6C-3681F23CBF90}", AZ::Component);
+
+        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* context);
+
+        // JointsTrajectoryRequestBus::Handler overrides ...
+        //! @see ROS2::JointsTrajectoryRequestBus::StartTrajectoryGoal
+        AZ::Outcome<void, AZStd::string> StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override;
+        //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal
+        AZ::Outcome<void, AZStd::string> CancelTrajectoryGoal(TrajectoryResultPtr trajectoryResult) override;
+        //! @see ROS2::JointsTrajectoryRequestBus::GetGoalStatus
+        TrajectoryActionStatus GetGoalStatus() override;
+
+    private:
+        // Component overrides ...
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        //! Follow set trajectory.
+        //! @param deltaTimeNs frame time step, to advance trajectory by.
+        void FollowTrajectory(const uint64_t deltaTimeNs);
+        AZ::Outcome<void, AZStd::string> ValidateGoal(TrajectoryGoalPtr trajectoryGoal);
+        void MoveToNextPoint(const trajectory_msgs::msg::JointTrajectoryPoint currentTrajectoryPoint);
+        void UpdateFeedback();
+
+        //! Lazy initialize Manipulation joints on the start of simulation.
+        ManipulationJoints& GetManipulationJoints();
+
+        AZStd::string m_followTrajectoryActionName{ "arm_controller/follow_joint_trajectory" };
+        AZStd::unique_ptr<FollowJointTrajectoryActionServer> m_followTrajectoryServer;
+        TrajectoryGoal m_trajectoryGoal;
+        rclcpp::Time m_trajectoryExecutionStartTime;
+        ManipulationJoints m_manipulationJoints;
+        bool m_trajectoryInProgress{ false };
+    };
+} // namespace ROS2

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

@@ -1,275 +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 "FollowJointTrajectoryActionServer.h"
-#include <ROS2/Manipulation/ManipulatorControllerComponent.h>
-#include <ROS2/Manipulation/JointPublisherComponent.h>
-#include <AzCore/Component/ComponentApplicationBus.h>
-#include <AzCore/Component/TransformBus.h>
-#include <AzCore/Serialization/EditContext.h>
-#include <AzCore/std/functional.h>
-#include <PhysX/Joint/PhysXJointRequestsBus.h>
-#include <Source/HingeJointComponent.h>
-#include <ROS2/Frame/ROS2FrameComponent.h>
-
-namespace ROS2
-{
-    // ManipulatorControllerComponent class
-    using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
-    ManipulatorControllerComponent::ManipulatorControllerComponent() = default;
-    ManipulatorControllerComponent::~ManipulatorControllerComponent() = default;
-
-    void ManipulatorControllerComponent::Activate()
-    {
-        AZ::TickBus::Handler::BusConnect();
-        m_actionServerClass = AZStd::make_unique<FollowJointTrajectoryActionServer>();
-        m_actionServerClass->CreateServer(m_ROS2ControllerName);
-        InitializePid();        
-    }
-
-    void ManipulatorControllerComponent::Deactivate()
-    {
-        AZ::TickBus::Handler::BusDisconnect();
-        m_actionServerClass->m_actionServer.reset();
-    }
-
-
-    void ManipulatorControllerComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
-    {
-        required.push_back(AZ_CRC_CE("JointPublisherService"));
-    }
-
-
-    void ManipulatorControllerComponent::Reflect(AZ::ReflectContext* context)
-    {
-        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
-        {
-            serialize->Class<ManipulatorControllerComponent, AZ::Component>()
-                ->Version(0)
-                ->Field("ROS2 Controller name", &ManipulatorControllerComponent::m_ROS2ControllerName)
-                ->Field("Controller type", &ManipulatorControllerComponent::m_controllerType)
-                ->Field("PID Configuration Vector", &ManipulatorControllerComponent::m_pidConfigurationVector);
-
-            if (AZ::EditContext* ec = serialize->GetEditContext())
-            {
-                ec->Class<ManipulatorControllerComponent>("ManipulatorControllerComponent", "[Controller for a robotic arm (only hinge joints)]")
-                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
-                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default,
-                        &ManipulatorControllerComponent::m_ROS2ControllerName,
-                        "ROS2 Controller Name",
-                        "It should mirror the name of the ROS2 Controller used as prefix of the ROS2 FollowJointTrajectory Server")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::ComboBox,
-                        &ManipulatorControllerComponent::m_controllerType,
-                        "Controller type", 
-                        "Different controller types to command the joints of the manipulator")
-                    ->EnumAttribute(ManipulatorControllerComponent::Controller::FeedForward, "FeedForward")
-                    ->EnumAttribute(ManipulatorControllerComponent::Controller::PID, "PID")
-                    ->DataElement(
-                        AZ::Edit::UIHandlers::Default, 
-                        &ManipulatorControllerComponent::m_pidConfigurationVector,
-                        "PIDs Configuration", 
-                        "PID controllers configuration (for all the joints)");
-            }
-        }
-    }
-
-    void ManipulatorControllerComponent::InitializePid()
-    {
-        for (auto& pid : m_pidConfigurationVector)
-        {
-            pid.InitializePid();
-        }
-    }
-
-    void ManipulatorControllerComponent::InitializeCurrentPosition()
-    {
-        auto* jointPublisherComponent = GetEntity()->FindComponent<JointPublisherComponent>();
-        if (jointPublisherComponent)
-        {
-            for (auto & [jointName , hingeComponent] : jointPublisherComponent->GetHierarchyMap())
-            {
-                m_jointKeepStillPosition[jointName] = GetJointPosition(hingeComponent);
-            }
-        }
-    }
-
-    float ManipulatorControllerComponent::GetJointPosition(const AZ::Component& hingeComponent)
-    {
-        float position{0};
-        auto componentId = hingeComponent.GetId();
-        auto entityId = hingeComponent.GetEntityId();
-        const AZ::EntityComponentIdPair id(entityId,componentId);
-        PhysX::JointRequestBus::EventResult(position, id, &PhysX::JointRequests::GetPosition);
-        return position;
-    }
-
-    float ManipulatorControllerComponent::ComputeFFJointVelocity(const float currentPosition, const float desiredPosition, const rclcpp::Duration & duration) const
-    {
-        // FeedForward (dummy) method
-        float desiredVelocity = (desiredPosition - currentPosition) / duration.seconds();
-        return desiredVelocity;
-    }
-
-    float ManipulatorControllerComponent::ComputePIDJointVelocity(const float currentPosition, const float desiredPosition, const uint64_t & deltaTimeNs, int & jointIndex)
-    {
-        // PID method
-        float error = desiredPosition - currentPosition;
-        float command = m_pidConfigurationVector.at(jointIndex).ComputeCommand(error, deltaTimeNs);
-        return command;
-    }
-
-    void ManipulatorControllerComponent::SetJointVelocity(AZ::Component& hingeComponent, const float desiredVelocity)
-    {
-        auto componentId = hingeComponent.GetId();
-        auto entityId = hingeComponent.GetEntityId();
-        const AZ::EntityComponentIdPair id(entityId,componentId);
-        PhysX::JointRequestBus::Event(id, &PhysX::JointRequests::SetVelocity, desiredVelocity);
-    }
-
-    void ManipulatorControllerComponent::KeepStillPosition([[maybe_unused]] const uint64_t deltaTimeNs)
-    {
-        if (!m_keepStillPositionInitialize)
-        {
-            InitializeCurrentPosition();
-            m_keepStillPositionInitialize = true;
-        }
-        
-        int jointIndex = 0;
-        for (auto & [jointName , desiredPosition] : m_jointKeepStillPosition)
-        {
-            float currentPosition;
-
-            auto* jointPublisherComponent = GetEntity()->FindComponent<JointPublisherComponent>();
-            if (jointPublisherComponent)
-            {
-                currentPosition = GetJointPosition(jointPublisherComponent->GetHierarchyMap()[jointName]);
-                float desiredVelocity;
-                if (m_controllerType == Controller::FeedForward)
-                {
-                    desiredVelocity = ComputeFFJointVelocity(
-                            currentPosition, 
-                            desiredPosition, 
-                            rclcpp::Duration::from_nanoseconds(5e8)); // Dummy forward time reference 
-                }
-                else if(m_controllerType == Controller::PID)
-                {
-                    desiredVelocity = ComputePIDJointVelocity(
-                            currentPosition, 
-                            desiredPosition, 
-                            deltaTimeNs,
-                            jointIndex);
-                }
-                else
-                {
-                    desiredVelocity = 0.0f;
-                }
-                                
-                SetJointVelocity(jointPublisherComponent->GetHierarchyMap()[jointName], desiredVelocity);
-
-                jointIndex++;
-            }
-        }
-    }
-
-    void ManipulatorControllerComponent::ExecuteTrajectory([[maybe_unused]] const uint64_t deltaTimeNs)
-    {
-        // If the trajectory is thoroughly executed set the status to Concluded
-        if (m_trajectory.points.size() == 0)
-        {
-            m_initializedTrajectory = false;
-            m_actionServerClass->m_goalStatus = GoalStatus::Concluded;
-            AZ_TracePrintf("ManipulatorControllerComponent", "Goal Concluded: all points reached");
-            return;
-        }
-
-        auto desiredGoal = m_trajectory.points.front();
-
-        rclcpp::Duration timeFromStart = rclcpp::Duration(desiredGoal.time_from_start); // arrival time of the current desired trajectory point
-        rclcpp::Duration threshold = rclcpp::Duration::from_nanoseconds(1e7);
-        rclcpp::Time timeNow = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp()); // current simulation time
-
-        // Jump to the next point if current simulation time is ahead of timeFromStart
-        if(m_timeStartingExecutionTraj + timeFromStart  <= timeNow + threshold)
-        {
-            m_trajectory.points.erase(m_trajectory.points.begin());
-            ExecuteTrajectory(deltaTimeNs);
-            return;
-        }
-
-        int jointIndex = 0;
-        for (auto & jointName : m_trajectory.joint_names)
-        {
-            auto* jointPublisherComponent = GetEntity()->FindComponent<JointPublisherComponent>();
-            if (jointPublisherComponent)
-            {
-                float currentPosition = GetJointPosition(jointPublisherComponent->GetHierarchyMap()[AZ::Name(jointName.c_str())]);
-                float desiredPosition = desiredGoal.positions[jointIndex];
-                float desiredVelocity;
-                if (m_controllerType == Controller::FeedForward)
-                {
-                    desiredVelocity = ComputeFFJointVelocity(
-                            currentPosition, 
-                            desiredPosition, 
-                            m_timeStartingExecutionTraj + timeFromStart - timeNow);
-                }
-                else if (m_controllerType == Controller::PID)
-                {
-                    desiredVelocity = ComputePIDJointVelocity(
-                            currentPosition, 
-                            desiredPosition, 
-                            deltaTimeNs,
-                            jointIndex);
-                }
-                else
-                {
-                    desiredVelocity = 0.0f;
-                }
-                
-                SetJointVelocity(jointPublisherComponent->GetHierarchyMap()[AZ::Name(jointName.c_str())], desiredVelocity);
-
-                jointIndex++;
-            }
-        }
-
-        
-    }
-
-    void ManipulatorControllerComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
-    {
-        const uint64_t deltaTimeNs = deltaTime * 1'000'000'000;
-
-        if (m_actionServerClass->m_goalStatus == GoalStatus::Active)
-        {
-            if (!m_initializedTrajectory)
-            {
-                m_trajectory = m_actionServerClass->m_goalHandle->get_goal()->trajectory;
-                m_timeStartingExecutionTraj = rclcpp::Time(ROS2::ROS2Interface::Get()->GetROSTimestamp());
-                m_initializedTrajectory = true;
-            }
-
-            ExecuteTrajectory(deltaTimeNs);
-
-            if (m_actionServerClass->m_goalStatus == GoalStatus::Concluded)
-            {
-                m_actionServerClass->m_goalStatus = GoalStatus::Pending;
-                auto result = std::make_shared<FollowJointTrajectory::Result>();
-                m_actionServerClass->m_goalHandle->succeed(result);
-                m_keepStillPositionInitialize = false;
-            }
-        }
-        else
-        {
-            KeepStillPosition(deltaTimeNs);
-        }
-    }
-
-} // namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointMotorControllerComponent.cpp → Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp

@@ -6,12 +6,12 @@
  *
  */
 
-#include <AzFramework/Entity/EntityDebugDisplayBus.h>
 #include <AzCore/Serialization/EditContext.h>
+#include <AzFramework/Entity/EntityDebugDisplayBus.h>
 #include <HingeJointComponent.h>
 #include <PhysX/Joint/PhysXJointRequestsBus.h>
 #include <PrismaticJointComponent.h>
-#include <ROS2/Manipulation/JointMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h>
 #include <imgui/imgui.h>
 
 namespace ROS2

Gems/ROS2/Code/Source/Manipulation/JointMotorControllerConfiguration.cpp → Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp

@@ -6,7 +6,7 @@
  *
  */
 #include <AzCore/Serialization/EditContext.h>
-#include <ROS2/Manipulation/JointMotorControllerConfiguration.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h>
 
 namespace ROS2
 {

Gems/ROS2/Code/Source/Manipulation/ManualMotorControllerComponent.cpp → Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp

@@ -6,7 +6,7 @@
  *
  */
 #include <AzCore/Serialization/EditContext.h>
-#include <ROS2/Manipulation/ManualMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h>
 #include <imgui/imgui.h>
 
 namespace ROS2
@@ -40,11 +40,7 @@ namespace ROS2
     void ManualMotorControllerComponent::DisplayControllerParameters()
     {
         ImGui::PushItemWidth(200.0f);
-        ImGui::SliderFloat(
-            "SetSpeed",
-            &m_setSpeed,
-            -5.0f,
-            5.0f);
+        ImGui::SliderFloat("SetSpeed", &m_setSpeed, -5.0f, 5.0f);
 
         ImGui::PopItemWidth();
 

Gems/ROS2/Code/Source/Manipulation/PidMotorControllerComponent.cpp → Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp

@@ -9,7 +9,7 @@
 #include <AzCore/Component/ComponentBus.h>
 #include <AzCore/Serialization/EditContext.h>
 #include <PhysX/Joint/PhysXJointRequestsBus.h>
-#include <ROS2/Manipulation/PidMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h>
 #include <imgui/imgui.h>
 
 namespace ROS2

Gems/ROS2/Code/Source/ROS2EditorModule.cpp

@@ -5,10 +5,11 @@
  * SPDX-License-Identifier: Apache-2.0 OR MIT
  *
  */
+#include <Camera/ROS2CameraSensorEditorComponent.h>
 #include <Lidar/LidarRegistrarEditorSystemComponent.h>
+#include <Manipulation/JointsManipulationEditorComponent.h>
 #include <ROS2EditorSystemComponent.h>
 #include <ROS2ModuleInterface.h>
-#include <Camera/ROS2CameraSensorEditorComponent.h>
 #include <RobotImporter/ROS2RobotImporterEditorSystemComponent.h>
 
 #include <QtCore/qglobal.h>
@@ -30,19 +31,18 @@ namespace ROS2
         ROS2EditorModule()
         {
             InitROS2Resources();
-            
+
             // Push results of [MyComponent]::CreateDescriptor() into m_descriptors here.
             // Add ALL components descriptors associated with this gem to m_descriptors.
             // This will associate the AzTypeInfo information for the components with the SerializeContext, BehaviorContext and
             // EditContext. This happens through the [MyComponent]::Reflect() function.
             m_descriptors.insert(
                 m_descriptors.end(),
-                {
-                    ROS2EditorSystemComponent::CreateDescriptor(),
-                    LidarRegistrarEditorSystemComponent::CreateDescriptor(),
-                    ROS2RobotImporterEditorSystemComponent::CreateDescriptor(),
-                    ROS2CameraSensorEditorComponent::CreateDescriptor(),
-                });
+                { ROS2EditorSystemComponent::CreateDescriptor(),
+                  LidarRegistrarEditorSystemComponent::CreateDescriptor(),
+                  ROS2RobotImporterEditorSystemComponent::CreateDescriptor(),
+                  ROS2CameraSensorEditorComponent::CreateDescriptor(),
+                  JointsManipulationEditorComponent::CreateDescriptor() });
         }
 
         /**

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -14,16 +14,18 @@
 #include <GNSS/ROS2GNSSSensorComponent.h>
 #include <Imu/ROS2ImuSensorComponent.h>
 #include <Lidar/LidarRegistrarSystemComponent.h>
-#include <Lidar/ROS2LidarSensorComponent.h>
 #include <Lidar/ROS2Lidar2DSensorComponent.h>
+#include <Lidar/ROS2LidarSensorComponent.h>
+#include <Manipulation/Controllers/JointsArticulationControllerComponent.h>
+#include <Manipulation/Controllers/JointsPIDControllerComponent.h>
+#include <Manipulation/JointsManipulationComponent.h>
+#include <Manipulation/JointsTrajectoryComponent.h>
 #include <Odometry/ROS2OdometrySensorComponent.h>
 #include <Odometry/ROS2WheelOdometry.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
-#include <ROS2/Manipulation/JointMotorControllerComponent.h>
-#include <ROS2/Manipulation/JointPublisherComponent.h>
-#include <ROS2/Manipulation/ManipulatorControllerComponent.h>
-#include <ROS2/Manipulation/ManualMotorControllerComponent.h>
-#include <ROS2/Manipulation/PidMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h>
+#include <ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h>
 #include <RobotControl/Controllers/AckermannController/AckermannControlComponent.h>
 #include <RobotControl/Controllers/RigidBodyController/RigidBodyTwistControlComponent.h>
 #include <RobotControl/Controllers/SkidSteeringController/SkidSteeringControlComponent.h>
@@ -77,8 +79,10 @@ namespace ROS2
                     VehicleDynamics::SkidSteeringModelComponent::CreateDescriptor(),
                     JointMotorControllerComponent::CreateDescriptor(),
                     ManualMotorControllerComponent::CreateDescriptor(),
-                    JointPublisherComponent::CreateDescriptor(),
-                    ManipulatorControllerComponent::CreateDescriptor(),
+                    JointsManipulationComponent::CreateDescriptor(),
+                    JointsArticulationControllerComponent::CreateDescriptor(),
+                    JointsPIDControllerComponent::CreateDescriptor(),
+                    JointsTrajectoryComponent::CreateDescriptor(),
                     PidMotorControllerComponent::CreateDescriptor(),
                 });
         }

Gems/ROS2/Code/Source/ROS2SystemComponent.cpp

@@ -6,6 +6,7 @@
  *
  */
 #include <ROS2/Clock/PhysicallyStableClock.h>
+#include <ROS2/Communication/PublisherConfiguration.h>
 #include <ROS2/Communication/QoS.h>
 #include <ROS2/Communication/TopicConfiguration.h>
 #include <ROS2/Utilities/Controllers/PidConfiguration.h>
@@ -31,6 +32,7 @@ namespace ROS2
         // Reflect structs not strictly owned by any single component
         QoS::Reflect(context);
         TopicConfiguration::Reflect(context);
+        PublisherConfiguration::Reflect(context);
         VehicleDynamics::VehicleModelComponent::Reflect(context);
         ROS2::Controllers::PidConfiguration::Reflect(context);
         if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))

Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp

@@ -184,36 +184,46 @@ namespace ROS2
             entity->Deactivate();
         }
 
-        if (!m_useArticulations)
+
+        auto joints = Utils::GetAllJoints(m_model->root_link_->child_links);
+        for (const auto& [name, jointPtr] : joints)
         {
-            auto joints = Utils::GetAllJoints(m_model->root_link_->child_links);
-            for (const auto& [name, jointPtr] : joints)
+            AZ_Assert(jointPtr, "joint %s is null", name.c_str());
+            AZ_TracePrintf(
+                "CreatePrefabFromURDF",
+                "Creating joint %s : %s -> %s\n",
+                name.c_str(),
+                jointPtr->parent_link_name.c_str(),
+                jointPtr->child_link_name.c_str());
+
+            auto leadEntity = createdLinks.at(jointPtr->parent_link_name.c_str());
+            auto childEntity = createdLinks.at(jointPtr->child_link_name.c_str());
+
+
+            AZ::Entity* childEntityPtr = AzToolsFramework::GetEntityById(childEntity.GetValue());
+            if (childEntityPtr)
             {
-                AZ_Assert(jointPtr, "joint %s is null", name.c_str());
-                AZ_TracePrintf(
-                    "CreatePrefabFromURDF",
-                    "Creating joint %s : %s -> %s\n",
-                    name.c_str(),
-                    jointPtr->parent_link_name.c_str(),
-                    jointPtr->child_link_name.c_str());
-
-                auto leadEntity = createdLinks.at(jointPtr->parent_link_name.c_str());
-                auto childEntity = createdLinks.at(jointPtr->child_link_name.c_str());
-                // check if both has RigidBody
-                if (leadEntity.IsSuccess() && childEntity.IsSuccess())
+                auto* component = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(childEntityPtr);
+                if (component)
                 {
-                    AZStd::lock_guard<AZStd::mutex> lck(m_statusLock);
-                    auto result = m_jointsMaker.AddJointComponent(jointPtr, childEntity.GetValue(), leadEntity.GetValue());
-                    m_status.emplace(
-                        name, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue()));
-                }
-                else
-                {
-                    AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", name.c_str());
+                    component->SetJointName(AZStd::string(name.c_str(), name.length()));
                 }
             }
+            // check if both has RigidBody and we are not creating articulation
+            if (!m_useArticulations && leadEntity.IsSuccess() && childEntity.IsSuccess())
+            {
+                AZStd::lock_guard<AZStd::mutex> lck(m_statusLock);
+                auto result = m_jointsMaker.AddJointComponent(jointPtr, childEntity.GetValue(), leadEntity.GetValue());
+                m_status.emplace(
+                    name, AZStd::string::format(" %s %llu", result.IsSuccess() ? "created as" : "Failed", result.GetValue()));
+            }
+            else
+            {
+                AZ_Warning("CreatePrefabFromURDF", false, "cannot create joint %s", name.c_str());
+            }
         }
 
+
         MoveEntityToDefaultSpawnPoint(createEntityRoot.GetValue());
 
         auto contentEntityId = createEntityRoot.GetValue();

Gems/ROS2/Code/ros2_editor_files.cmake

@@ -10,6 +10,8 @@ set(FILES
     Source/Camera/ROS2CameraSensorEditorComponent.h
     Source/Lidar/LidarRegistrarEditorSystemComponent.cpp
     Source/Lidar/LidarRegistrarEditorSystemComponent.h
+    Source/Manipulation/JointsManipulationEditorComponent.cpp
+    Source/Manipulation/JointsManipulationEditorComponent.h
     Source/RobotImporter/Pages/CheckAssetPage.cpp
     Source/RobotImporter/Pages/CheckAssetPage.h
     Source/RobotImporter/Pages/CheckUrdfPage.cpp

Gems/ROS2/Code/ros2_files.cmake

@@ -25,6 +25,7 @@ set(FILES
         Source/Clock/PhysicallyStableClock.cpp
         Source/Clock/SimulationClock.cpp
         Source/Communication/QoS.cpp
+        Source/Communication/PublisherConfiguration.cpp
         Source/Communication/TopicConfiguration.cpp
         Source/Frame/NamespaceConfiguration.cpp
         Source/Frame/ROS2FrameComponent.cpp
@@ -49,12 +50,23 @@ set(FILES
         Source/Lidar/ROS2Lidar2DSensorComponent.h
         Source/Lidar/ROS2LidarSensorComponent.cpp
         Source/Lidar/ROS2LidarSensorComponent.h
-        Source/Manipulation/JointMotorControllerComponent.cpp
-        Source/Manipulation/JointMotorControllerConfiguration.cpp
-        Source/Manipulation/JointPublisherComponent.cpp
-        Source/Manipulation/ManipulatorControllerComponent.cpp
-        Source/Manipulation/ManualMotorControllerComponent.cpp
-        Source/Manipulation/PidMotorControllerComponent.cpp
+        Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp
+        Source/Manipulation/Controllers/JointsArticulationControllerComponent.h
+        Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp
+        Source/Manipulation/Controllers/JointsPIDControllerComponent.h
+        Source/Manipulation/JointInfo.cpp
+        Source/Manipulation/JointStatePublisher.cpp
+        Source/Manipulation/JointStatePublisher.h
+        Source/Manipulation/JointsManipulationComponent.cpp
+        Source/Manipulation/JointsManipulationComponent.h
+        Source/Manipulation/JointsTrajectoryComponent.cpp
+        Source/Manipulation/JointsTrajectoryComponent.h
+        Source/Manipulation/FollowJointTrajectoryActionServer.cpp
+        Source/Manipulation/FollowJointTrajectoryActionServer.h
+        Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp
+        Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp
+        Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp
+        Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp
         Source/Odometry/ROS2OdometrySensorComponent.cpp
         Source/Odometry/ROS2OdometrySensorComponent.h
         Source/Odometry/ROS2WheelOdometry.cpp
@@ -119,4 +131,4 @@ set(FILES
         Source/VehicleDynamics/WheelControllerComponent.cpp
         Source/VehicleDynamics/WheelControllerComponent.h
         Source/VehicleDynamics/WheelDynamicsData.h
-)
+        )

Gems/ROS2/Code/ros2_header_files.cmake

@@ -8,16 +8,21 @@ set(FILES
         Include/ROS2/Camera/CameraPostProcessingRequestBus.h
         Include/ROS2/Clock/PhysicallyStableClock.h
         Include/ROS2/Clock/SimulationClock.h
+        Include/ROS2/Communication/PublisherConfiguration.h
+        Include/ROS2/Communication/TopicConfiguration.h
+        Include/ROS2/Communication/QoS.h
         Include/ROS2/Frame/NamespaceConfiguration.h
         Include/ROS2/Frame/ROS2FrameComponent.h
         Include/ROS2/Frame/ROS2Transform.h
-        Include/ROS2/Manipulation/JointMotorControllerComponent.h
-        Include/ROS2/Manipulation/JointMotorControllerConfiguration.h
-        Include/ROS2/Manipulation/JointPublisherComponent.h
-        Include/ROS2/Manipulation/ManipulatorControllerComponent.h
-        Include/ROS2/Manipulation/ManualMotorControllerComponent.h
-        Include/ROS2/Manipulation/PidMotorControllerBus.h
-        Include/ROS2/Manipulation/PidMotorControllerComponent.h
+        Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h
+        Include/ROS2/Manipulation/JointInfo.h
+        Include/ROS2/Manipulation/JointsManipulationRequests.h
+        Include/ROS2/Manipulation/JointsTrajectoryRequests.h
+        Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h
+        Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h
+        Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h
+        Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h
+        Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h
         Include/ROS2/RobotControl/ControlConfiguration.h
         Include/ROS2/RobotControl/ControlSubscriptionHandler.h
         Include/ROS2/Lidar/LidarRaycasterBus.h

Gems/ROS2/gem.json

@@ -1,6 +1,6 @@
 {
     "gem_name": "ROS2",
-    "version": "1.0.0",
+    "version": "2.0.0",
     "platforms": [
         "Linux"
     ],

Gems/WarehouseAssets/Assets/Prefabs/Warehouse_storage/Storage_on_wheels.prefab

@@ -178,7 +178,7 @@
                         "Position": [
                             0.0,
                             0.0,
-                            0.9333269596099854
+                            1.0
                         ],
                         "MaterialSlots": {
                             "Slots": [