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 <michal.pelka@robotec.ai>
Co-authored-by: Alex Peterson <26804013+AMZN-alexpete@users.noreply.github.com>
Co-authored-by: Piotr Jaroszek <piotr.jaroszek@robotec.ai>

Signed-off-by: Adam Dąbrowski <adam.dabrowski@robotec.ai>

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