From 6052835ded01a867b10a574230d15e4900df1467 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Adam=20D=C4=85browski?= Date: Tue, 4 Jul 2023 17:32:08 +0200 Subject: [PATCH] Manipulation Components redesign & refactor (#366) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 Co-authored-by: Alex Peterson <26804013+AMZN-alexpete@users.noreply.github.com> Co-authored-by: Piotr Jaroszek Signed-off-by: Adam Dąbrowski --- .../Communication/PublisherConfiguration.h | 27 ++ .../JointsPositionControllerRequests.h | 53 +++ .../Include/ROS2/Manipulation/JointInfo.h | 31 ++ .../Manipulation/JointPublisherComponent.h | 50 --- .../Manipulation/JointsManipulationRequests.h | 61 ++++ .../Manipulation/JointsTrajectoryRequests.h | 55 +++ .../ManipulatorControllerComponent.h | 62 ---- .../JointMotorControllerComponent.h | 2 +- .../JointMotorControllerConfiguration.h | 0 .../ManualMotorControllerComponent.h | 2 +- .../PidMotorControllerBus.h | 0 .../PidMotorControllerComponent.h | 4 +- .../Communication/PublisherConfiguration.cpp | 38 ++ .../JointsArticulationControllerComponent.cpp | 69 ++++ .../JointsArticulationControllerComponent.h | 56 +++ .../JointsPIDControllerComponent.cpp | 96 +++++ .../JointsPIDControllerComponent.h | 60 ++++ .../FollowJointTrajectoryActionServer.cpp | 139 +++++++ .../FollowJointTrajectoryActionServer.h | 120 +++---- .../Code/Source/Manipulation/JointInfo.cpp | 25 ++ .../Manipulation/JointPublisherComponent.cpp | 156 -------- .../Manipulation/JointStatePublisher.cpp | 74 ++++ .../Source/Manipulation/JointStatePublisher.h | 45 +++ .../JointsManipulationComponent.cpp | 339 ++++++++++++++++++ .../JointsManipulationComponent.h | 69 ++++ .../JointsManipulationEditorComponent.cpp | 80 +++++ .../JointsManipulationEditorComponent.h | 38 ++ .../JointsTrajectoryComponent.cpp | 250 +++++++++++++ .../Manipulation/JointsTrajectoryComponent.h | 70 ++++ .../ManipulatorControllerComponent.cpp | 275 -------------- .../JointMotorControllerComponent.cpp | 4 +- .../JointMotorControllerConfiguration.cpp | 2 +- .../ManualMotorControllerComponent.cpp | 8 +- .../PidMotorControllerComponent.cpp | 2 +- Gems/ROS2/Code/Source/ROS2EditorModule.cpp | 16 +- Gems/ROS2/Code/Source/ROS2ModuleInterface.h | 20 +- Gems/ROS2/Code/Source/ROS2SystemComponent.cpp | 2 + .../RobotImporter/URDF/URDFPrefabMaker.cpp | 56 +-- Gems/ROS2/Code/ros2_editor_files.cmake | 2 + Gems/ROS2/Code/ros2_files.cmake | 26 +- Gems/ROS2/Code/ros2_header_files.cmake | 19 +- Gems/ROS2/gem.json | 2 +- .../Storage_on_wheels.prefab | 2 +- 43 files changed, 1826 insertions(+), 681 deletions(-) create mode 100644 Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h create mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h create mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h delete mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/JointPublisherComponent.h create mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h create mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h delete mode 100644 Gems/ROS2/Code/Include/ROS2/Manipulation/ManipulatorControllerComponent.h rename Gems/ROS2/Code/Include/ROS2/Manipulation/{ => MotorizedJoints}/JointMotorControllerComponent.h (96%) rename Gems/ROS2/Code/Include/ROS2/Manipulation/{ => MotorizedJoints}/JointMotorControllerConfiguration.h (100%) rename Gems/ROS2/Code/Include/ROS2/Manipulation/{ => MotorizedJoints}/ManualMotorControllerComponent.h (92%) rename Gems/ROS2/Code/Include/ROS2/Manipulation/{ => MotorizedJoints}/PidMotorControllerBus.h (100%) rename Gems/ROS2/Code/Include/ROS2/Manipulation/{ => MotorizedJoints}/PidMotorControllerComponent.h (92%) create mode 100644 Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h create mode 100644 Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h create mode 100644 Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp delete mode 100644 Gems/ROS2/Code/Source/Manipulation/JointPublisherComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp create mode 100644 Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h delete mode 100644 Gems/ROS2/Code/Source/Manipulation/ManipulatorControllerComponent.cpp rename Gems/ROS2/Code/Source/Manipulation/{ => MotorizedJoints}/JointMotorControllerComponent.cpp (98%) rename Gems/ROS2/Code/Source/Manipulation/{ => MotorizedJoints}/JointMotorControllerConfiguration.cpp (94%) rename Gems/ROS2/Code/Source/Manipulation/{ => MotorizedJoints}/ManualMotorControllerComponent.cpp (90%) rename Gems/ROS2/Code/Source/Manipulation/{ => MotorizedJoints}/PidMotorControllerComponent.cpp (97%) diff --git a/Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Communication/PublisherConfiguration.h new file mode 100644 index 000000000..ff0bdc87e --- /dev/null +++ b/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 +#include +#include +#include + +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 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h new file mode 100644 index 000000000..54f6d06fa --- /dev/null +++ b/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 +#include +#include +#include + +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 PositionControl( + const AZStd::string& jointName, + JointInfo joint, + JointPosition currentPosition, + JointPosition targetPosition, + float deltaTime) = 0; + }; + using JointsPositionControllerRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointInfo.h new file mode 100644 index 000000000..6caf22782 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointPublisherComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointPublisherComponent.h deleted file mode 100644 index a16c0dd15..000000000 --- a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointPublisherComponent.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace ROS2 -{ - //! A component responsible for publishing the joint positions on ROS2 /joint_states topic. - //!< @see jointState message. - 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 &GetHierarchyMap(); - - private: - void PublishMessage(); - void UpdateMessage(); - void Initialize(); - AZStd::string GetFrameID() const; - - float GetJointPosition(const AZ::Component& hingeComponent) const; - - AZStd::unordered_map m_hierarchyMap; - std::shared_ptr> 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 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsManipulationRequests.h new file mode 100644 index 000000000..095720960 --- /dev/null +++ b/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 +#include +#include +#include +#include + +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; + + //! 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 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 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 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; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/JointsTrajectoryRequests.h new file mode 100644 index 000000000..5af3cd536 --- /dev/null +++ b/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 +#include +#include +#include + +namespace ROS2 +{ + //! Interface for commanding a system of joints such as robotic arm (manipulator) through FollowJointTrajectory actions. + //@see FollowJointTrajectory + 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; + using TrajectoryResultPtr = std::shared_ptr; + + //! 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 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 CancelTrajectoryGoal(TrajectoryResultPtr result) = 0; + + //! Retrieve current trajectory goal status. + //! @return Status of trajectory goal. + virtual TrajectoryActionStatus GetGoalStatus() = 0; + }; + + using JointsTrajectoryRequestBus = AZ::EBus; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/ManipulatorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/ManipulatorControllerComponent.h deleted file mode 100644 index 75bdd1d66..000000000 --- a/Gems/ROS2/Code/Include/ROS2/Manipulation/ManipulatorControllerComponent.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - - -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 FeedForward. - PID //!< @see PID. - }; - - 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 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 m_pidConfigurationVector; - AZStd::unordered_map m_jointKeepStillPosition; - trajectory_msgs::msg::JointTrajectory m_trajectory; - rclcpp::Time m_timeStartingExecutionTraj; - }; -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h similarity index 96% rename from Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerComponent.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h index 47f221628..3880684d6 100644 --- a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerComponent.h +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerComponent.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerConfiguration.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h similarity index 100% rename from Gems/ROS2/Code/Include/ROS2/Manipulation/JointMotorControllerConfiguration.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.h diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/ManualMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h similarity index 92% rename from Gems/ROS2/Code/Include/ROS2/Manipulation/ManualMotorControllerComponent.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h index 22ed42bbc..7c8808f09 100644 --- a/Gems/ROS2/Code/Include/ROS2/Manipulation/ManualMotorControllerComponent.h +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/ManualMotorControllerComponent.h @@ -8,7 +8,7 @@ #pragma once #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerBus.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h similarity index 100% rename from Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerBus.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerBus.h diff --git a/Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerComponent.h b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h similarity index 92% rename from Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerComponent.h rename to Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h index 7da8c9c60..018d2781a 100644 --- a/Gems/ROS2/Code/Include/ROS2/Manipulation/PidMotorControllerComponent.h +++ b/Gems/ROS2/Code/Include/ROS2/Manipulation/MotorizedJoints/PidMotorControllerComponent.h @@ -9,8 +9,8 @@ #include #include -#include -#include +#include +#include #include namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp b/Gems/ROS2/Code/Source/Communication/PublisherConfiguration.cpp new file mode 100644 index 000000000..08e2ea6d2 --- /dev/null +++ b/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 +#include +#include + +namespace ROS2 +{ + void PublisherConfiguration::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->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("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 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.cpp new file mode 100644 index 000000000..e0e8de59e --- /dev/null +++ b/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 +#include + +namespace ROS2 +{ + void JointsArticulationControllerComponent::Activate() + { + JointsPositionControllerRequestBus::Handler::BusConnect(GetEntityId()); + } + + void JointsArticulationControllerComponent::Deactivate() + { + JointsPositionControllerRequestBus::Handler::BusDisconnect(); + } + + AZ::Outcome 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(context)) + { + serialize->Class()->Version(0); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class( + "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 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsArticulationControllerComponent.h new file mode 100644 index 000000000..07aed5736 --- /dev/null +++ b/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 +#include + +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 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 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.cpp new file mode 100644 index 000000000..3e9611c86 --- /dev/null +++ b/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 +#include + +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 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(context)) + { + serialize->Class()->Version(0)->Field( + "JointsPIDs", &JointsPIDControllerComponent::m_pidConfiguration); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("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 diff --git a/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h b/Gems/ROS2/Code/Source/Manipulation/Controllers/JointsPIDControllerComponent.h new file mode 100644 index 000000000..bd83905f4 --- /dev/null +++ b/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 +#include +#include + +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 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 m_pidConfiguration; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.cpp new file mode 100644 index 000000000..bbf2ce09b --- /dev/null +++ b/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 +#include + +namespace ROS2 +{ + FollowJointTrajectoryActionServer::FollowJointTrajectoryActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId) + : m_entityId(entityId) + { + m_actionServer = rclcpp_action::create_server( + 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 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 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 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 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 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) + { // Accept each cancel attempt + auto result = std::make_shared(); + result->error_string = "User Cancelled"; + result->error_code = FollowJointTrajectory::Result::SUCCESSFUL; + + AZ::Outcome 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) + { + 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 diff --git a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h b/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h index 8419b2220..a5b5e241a 100644 --- a/Gems/ROS2/Code/Source/Manipulation/FollowJointTrajectoryActionServer.h +++ b/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 #include -#include -#include -#include +#include #include -#include +#include +#include namespace ROS2 { - enum class GoalStatus - { - Pending, - Active, - Concluded - }; - - // ROS2 Action Server class + //! A class wrapping ROS 2 action server for joint trajectory controller. + //! @see joint trajectory + //! controller . class FollowJointTrajectoryActionServer { public: - using GoalHandleFollowJointTrajectory = rclcpp_action::ServerGoalHandle; using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; - FollowJointTrajectoryActionServer() = default; - ~FollowJointTrajectoryActionServer() = default; - void CreateServer(AZStd::string ROS2ControllerName); - rclcpp_action::Server::SharedPtr m_actionServer; - std::shared_ptr 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 ROS 2 action + //! server documentation + 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 goal); - rclcpp_action::CancelResponse goal_cancelled_callback( - const std::shared_ptr goal_handle); - void goal_accepted_callback( - std::shared_ptr goal_handle); - }; + //! Cancel the current goal. + //! @param result Result to be passed to through action server to the client. + void CancelGoal(std::shared_ptr result); - void FollowJointTrajectoryActionServer::CreateServer(AZStd::string ROS2ControllerName) - { - auto ros2Node = ROS2Interface::Get()->GetNode(); - // Create the ROS2 action server - this->m_actionServer = rclcpp_action::create_server( - 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 result); - rclcpp_action::GoalResponse FollowJointTrajectoryActionServer::goal_received_callback( - [[maybe_unused]] const rclcpp_action::GoalUUID & uuid, - [[maybe_unused]] std::shared_ptr 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 feedback); - rclcpp_action::CancelResponse FollowJointTrajectoryActionServer::goal_cancelled_callback( - [[maybe_unused]] const std::shared_ptr goal_handle) - { - // Dummy implementation - AZ_TracePrintf("ManipulatorControllerComponent", "FollowJointTrajectory manipulator Goal canceled"); - return rclcpp_action::CancelResponse::ACCEPT; - } + private: + using GoalHandle = rclcpp_action::ServerGoalHandle; + using TrajectoryActionStatus = JointsTrajectoryRequests::TrajectoryActionStatus; - void FollowJointTrajectoryActionServer::goal_accepted_callback( - const std::shared_ptr goal_handle) - { - AZ_TracePrintf("ManipulatorControllerComponent", "FollowJointTrajectory manipulator Goal accepted"); - this->m_goalHandle = goal_handle; - this->m_goalStatus = GoalStatus::Active; - } -} \ No newline at end of file + AZ::EntityId m_entityId; + TrajectoryActionStatus m_goalStatus = TrajectoryActionStatus::Idle; + rclcpp_action::Server::SharedPtr m_actionServer; + std::shared_ptr m_goalHandle; + + bool IsGoalActiveState() const; + bool IsReadyForExecution() const; + bool IsExecuting() const; + + rclcpp_action::GoalResponse GoalReceivedCallback( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + + rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr goalHandle); + + void GoalAcceptedCallback(const std::shared_ptr goalHandle); + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp b/Gems/ROS2/Code/Source/Manipulation/JointInfo.cpp new file mode 100644 index 000000000..c29df69f0 --- /dev/null +++ b/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 + +namespace ROS2 +{ + void JointInfo::Reflect(AZ::ReflectContext* context) + { + if (auto serializeContext = azrtti_cast(context)) + { + serializeContext->Class() + ->Version(1) + ->Field("IsArticulation", &JointInfo::m_isArticulation) + ->Field("Axis", &JointInfo::m_axis) + ->Field("EntityComponentIdPair", &JointInfo::m_entityComponentIdPair) + ->Field("RestPosition", &JointInfo::m_restPosition); + } + } +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointPublisherComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointPublisherComponent.cpp deleted file mode 100644 index 9817413d4..000000000 --- a/Gems/ROS2/Code/Source/Manipulation/JointPublisherComponent.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ROS2 -{ - void JointPublisherComponent::Activate() - { - AZ::TickBus::Handler::BusConnect(); - auto ros2Node = ROS2::ROS2Interface::Get()->GetNode(); - auto ros2Frame = GetEntity()->FindComponent(); - AZStd::string namespacedTopic = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), "joint_states"); - m_jointstatePublisher = ros2Node->create_publisher(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(context)) - { - serialize->Class() - ->Version(0) - ->Field("Frequency (HZ)", &JointPublisherComponent::m_frequency); - - if (AZ::EditContext* ec = serialize->GetEditContext()) - { - ec->Class("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 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(); - auto* hingeComponent = entity->FindComponent(); - 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 &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(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 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.cpp new file mode 100644 index 000000000..0d14faf4d --- /dev/null +++ b/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 +#include +#include + +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(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 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 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h b/Gems/ROS2/Code/Source/Manipulation/JointStatePublisher.h new file mode 100644 index 000000000..b236b0310 --- /dev/null +++ b/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 +#include +#include +#include + +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 jointState message. + 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> m_jointStatePublisher; + sensor_msgs::msg::JointState m_jointStateMsg; + float m_timeElapsedSinceLastTick = 0.0f; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.cpp new file mode 100644 index 000000000..eabdc5d99 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include + +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(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(PhysX::ArticulationJointAxis::Z); i++) + { + PhysX::ArticulationJointMotionType type = PhysX::ArticulationJointMotionType::Locked; + axis = static_cast(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 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(entity); + if (!frameComponent) + { // Frame Component is required for joints. + continue; + } + const AZStd::string jointName(frameComponent->GetJointName().GetCStr()); + + auto* hingeComponent = Utils::GetGameOrEditorComponent(entity); + auto* articulationComponent = Utils::GetGameOrEditorComponent(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& 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& initialPositions) + : m_jointStatePublisherConfiguration(configuration) + , m_initialPositions(initialPositions) + { + } + + void JointsManipulationComponent::Activate() + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + JointStatePublisherContext publisherContext; + publisherContext.m_publisherNamespace = ros2Frame->GetNamespace(); + publisherContext.m_frameId = ros2Frame->GetFrameID(); + publisherContext.m_entityId = GetEntityId(); + + m_jointStatePublisher = AZStd::make_unique(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 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 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 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(context)) + { + serialize->Class() + ->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 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 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationComponent.h new file mode 100644 index 000000000..fe2221d65 --- /dev/null +++ b/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 +#include +#include +#include + +#include "JointStatePublisher.h" +#include + +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& 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 GetJointPosition(const AZStd::string& jointName) override; + //! @see ROS2::JointsManipulationRequestBus::GetAllJointsPositions + JointsPositionsMap GetAllJointsPositions() override; + //! @see ROS2::JointsManipulationRequestBus::MoveJointsToPositions + AZ::Outcome MoveJointsToPositions(const JointsPositionsMap& positions) override; + //! @see ROS2::JointsManipulationRequestBus::MoveJointToPosition + AZ::Outcome 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 m_jointStatePublisher; + PublisherConfiguration m_jointStatePublisherConfiguration; + ManipulationJoints m_manipulationJoints; + AZStd::unordered_map m_initialPositions; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.cpp new file mode 100644 index 000000000..533843328 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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(context)) + { + serialize->Class() + ->Version(0) + ->Field("JointStatePublisherConfiguration", &JointsManipulationEditorComponent::m_jointStatePublisherConfiguration) + ->Field("Initial positions", &JointsManipulationEditorComponent::m_initialPositions); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("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 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsManipulationEditorComponent.h new file mode 100644 index 000000000..66f8df2f3 --- /dev/null +++ b/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 +#include +#include +#include + +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 m_initialPositions; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.cpp new file mode 100644 index 000000000..06cb74775 --- /dev/null +++ b/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 +#include +#include +#include +#include + +namespace ROS2 +{ + void JointsTrajectoryComponent::Activate() + { + auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); + AZ_Assert(ros2Frame, "Missing Frame Component!"); + AZStd::string namespacedAction = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), m_followTrajectoryActionName); + m_followTrajectoryServer = AZStd::make_unique(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(context)) + { + serialize->Class()->Version(0)->Field( + "Action name", &JointsTrajectoryComponent::m_followTrajectoryActionName); + + if (AZ::EditContext* ec = serialize->GetEditContext()) + { + ec->Class("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 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 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(); + 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 result; + JointsManipulationRequestBus::EventResult(result, GetEntityId(), &JointsManipulationRequests::GetJointPosition, jointName); + auto currentJointPosition = result.GetValue(); + + desiredPoint.positions.push_back(static_cast(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()); + + std::transform( + desiredPoint.velocities.begin(), + desiredPoint.velocities.end(), + actualPoint.velocities.begin(), + currentError.velocities.begin(), + std::minus()); + + std::transform( + desiredPoint.accelerations.begin(), + desiredPoint.accelerations.end(), + actualPoint.accelerations.begin(), + currentError.accelerations.begin(), + std::minus()); + + std::transform( + desiredPoint.effort.begin(), + desiredPoint.effort.end(), + actualPoint.effort.begin(), + currentError.effort.begin(), + std::minus()); + + feedback->desired = desiredPoint; + feedback->actual = actualPoint; + feedback->error = currentError; + m_followTrajectoryServer->PublishFeedback(feedback); + } + + AZ::Outcome 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(); //!< 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 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 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h b/Gems/ROS2/Code/Source/Manipulation/JointsTrajectoryComponent.h new file mode 100644 index 000000000..f09f70e10 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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 StartTrajectoryGoal(TrajectoryGoalPtr trajectoryGoal) override; + //! @see ROS2::JointsTrajectoryRequestBus::CancelTrajectoryGoal + AZ::Outcome 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 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 m_followTrajectoryServer; + TrajectoryGoal m_trajectoryGoal; + rclcpp::Time m_trajectoryExecutionStartTime; + ManipulationJoints m_manipulationJoints; + bool m_trajectoryInProgress{ false }; + }; +} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/ManipulatorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/ManipulatorControllerComponent.cpp deleted file mode 100644 index 17343b41a..000000000 --- a/Gems/ROS2/Code/Source/Manipulation/ManipulatorControllerComponent.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -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(); - 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(context)) - { - serialize->Class() - ->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", "[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(); - 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(); - 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(); - 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(); - m_actionServerClass->m_goalHandle->succeed(result); - m_keepStillPositionInitialize = false; - } - } - else - { - KeepStillPosition(deltaTimeNs); - } - } - -} // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp similarity index 98% rename from Gems/ROS2/Code/Source/Manipulation/JointMotorControllerComponent.cpp rename to Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp index c7b35a761..090803c62 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointMotorControllerComponent.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerComponent.cpp @@ -6,12 +6,12 @@ * */ -#include #include +#include #include #include #include -#include +#include #include namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Manipulation/JointMotorControllerConfiguration.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp similarity index 94% rename from Gems/ROS2/Code/Source/Manipulation/JointMotorControllerConfiguration.cpp rename to Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp index 16f82f154..293b92526 100644 --- a/Gems/ROS2/Code/Source/Manipulation/JointMotorControllerConfiguration.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/JointMotorControllerConfiguration.cpp @@ -6,7 +6,7 @@ * */ #include -#include +#include namespace ROS2 { diff --git a/Gems/ROS2/Code/Source/Manipulation/ManualMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp similarity index 90% rename from Gems/ROS2/Code/Source/Manipulation/ManualMotorControllerComponent.cpp rename to Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp index d9d33477d..c97346f5e 100644 --- a/Gems/ROS2/Code/Source/Manipulation/ManualMotorControllerComponent.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/ManualMotorControllerComponent.cpp @@ -6,7 +6,7 @@ * */ #include -#include +#include #include 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(); diff --git a/Gems/ROS2/Code/Source/Manipulation/PidMotorControllerComponent.cpp b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp similarity index 97% rename from Gems/ROS2/Code/Source/Manipulation/PidMotorControllerComponent.cpp rename to Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp index 05266ce78..57e8791bb 100644 --- a/Gems/ROS2/Code/Source/Manipulation/PidMotorControllerComponent.cpp +++ b/Gems/ROS2/Code/Source/Manipulation/MotorizedJoints/PidMotorControllerComponent.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include namespace ROS2 diff --git a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp index 3435a4f15..ae1809124 100644 --- a/Gems/ROS2/Code/Source/ROS2EditorModule.cpp +++ b/Gems/ROS2/Code/Source/ROS2EditorModule.cpp @@ -5,10 +5,11 @@ * SPDX-License-Identifier: Apache-2.0 OR MIT * */ +#include #include +#include #include #include -#include #include #include @@ -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() }); } /** diff --git a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h index d90f90c3e..c56c55727 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -14,16 +14,18 @@ #include #include #include -#include #include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include #include #include #include @@ -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(), }); } diff --git a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp index c276a92c0..dc4314bb1 100644 --- a/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp +++ b/Gems/ROS2/Code/Source/ROS2SystemComponent.cpp @@ -6,6 +6,7 @@ * */ #include +#include #include #include #include @@ -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(context)) diff --git a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp b/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp index 36cad81f9..5aca8cafb 100644 --- a/Gems/ROS2/Code/Source/RobotImporter/URDF/URDFPrefabMaker.cpp +++ b/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(childEntityPtr); + if (component) { - AZStd::lock_guard 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 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(); diff --git a/Gems/ROS2/Code/ros2_editor_files.cmake b/Gems/ROS2/Code/ros2_editor_files.cmake index c889a6735..752934b14 100644 --- a/Gems/ROS2/Code/ros2_editor_files.cmake +++ b/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 diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake index bbec5e3bd..31dcb34cb 100644 --- a/Gems/ROS2/Code/ros2_files.cmake +++ b/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 -) + ) diff --git a/Gems/ROS2/Code/ros2_header_files.cmake b/Gems/ROS2/Code/ros2_header_files.cmake index d990bc846..9f3832023 100644 --- a/Gems/ROS2/Code/ros2_header_files.cmake +++ b/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 diff --git a/Gems/ROS2/gem.json b/Gems/ROS2/gem.json index ec0b081dd..1bd8f4b80 100644 --- a/Gems/ROS2/gem.json +++ b/Gems/ROS2/gem.json @@ -1,6 +1,6 @@ { "gem_name": "ROS2", - "version": "1.0.0", + "version": "2.0.0", "platforms": [ "Linux" ], diff --git a/Gems/WarehouseAssets/Assets/Prefabs/Warehouse_storage/Storage_on_wheels.prefab b/Gems/WarehouseAssets/Assets/Prefabs/Warehouse_storage/Storage_on_wheels.prefab index 5f1434228..980be49f1 100755 --- a/Gems/WarehouseAssets/Assets/Prefabs/Warehouse_storage/Storage_on_wheels.prefab +++ b/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": [