Vacuum gripper with action server (#369)

Vacuum Gripper component and gripper action server.

Co-authored-by: Steve Pham <[email protected]>
Co-authored-by: Artur Kamieniecki <[email protected]>
Co-authored-by: Antoni Puch <[email protected]>

Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/CMakeLists.txt

@@ -66,6 +66,7 @@ ly_add_target(
             Gem::Atom_Component_DebugCamera.Static
             Gem::StartingPointInput
             Gem::PhysX.Static
+            Gem::LmbrCentral.API
 )
 
 target_depends_on_ros2_packages(${gem_name}.Static rclcpp builtin_interfaces std_msgs sensor_msgs nav_msgs tf2_ros ackermann_msgs gazebo_msgs)

Gems/ROS2/Code/Include/ROS2/Gripper/GripperRequestBus.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Interface/Interface.h>
+
+namespace ROS2
+{
+    //! The interface allows to control gripper components through GripperCommand actions.
+    //! It is a bus that allows communication between ROS2 GripperCommand Action server with the particular implementation of the gripper.
+    //! It encapsulates GripperCommand commands and getters that allow to build the feedback and result messages.
+    //! @see <a href="https://github.com/ros-controls/control_msgs/blob/humble/control_msgs/action/GripperCommand.action">GripperCommand</a>
+    class GripperRequests : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+        static constexpr AZ::EBusHandlerPolicy HandlerPolicy = AZ::EBusHandlerPolicy::Single;
+
+        //! Send new command to the gripper.
+        //! @param position position of the gripper (as a gap size in meters) to be set, for vacuum/electromagnetic gripper it is 0 or 1.
+        //! @param maxEffort maximum effort of the gripper to be set in Newtons.
+        //! @return Nothing on success, error message if failed.
+        virtual AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) = 0;
+
+        //! Cancel the current command to the gripper.
+        //! @return Nothing on success, error message if failed.
+        virtual AZ::Outcome<void, AZStd::string> CancelGripperCommand() = 0;
+
+        //! Get the current position of the gripper.
+        //! @return Position of the gripper.
+        virtual float GetGripperPosition() const = 0;
+
+        //! Get the current effort of the gripper.
+        //! @return Position (gap size in meters) of the gripper.
+        virtual float GetGripperEffort() const = 0;
+
+        //! Get if the gripper is not moving (stalled).
+        virtual bool IsGripperNotMoving() const = 0;
+
+        //! Get if the gripper reached the set with GripperCommand position.
+        virtual bool HasGripperReachedGoal() const = 0;
+
+        //! Get if the gripper command has been cancelled.
+        virtual bool HasGripperCommandBeenCancelled() const = 0;
+    };
+
+    using GripperRequestBus = AZ::EBus<GripperRequests>;
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServer.cpp

@@ -0,0 +1,129 @@
+/*
+ * 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 "Utils.h"
+
+#include "GripperActionServer.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+
+namespace ROS2
+{
+    GripperActionServer::GripperActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId)
+        : m_entityId(entityId)
+    {
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        actionServer = rclcpp_action::create_server<GripperCommand>(
+            ros2Node,
+            actionName.data(),
+            AZStd::bind(&GripperActionServer::GoalReceivedCallback, this, AZStd::placeholders::_1, AZStd::placeholders::_2),
+            AZStd::bind(&GripperActionServer::GoalCancelledCallback, this, AZStd::placeholders::_1),
+            AZStd::bind(&GripperActionServer::GoalAcceptedCallback, this, AZStd::placeholders::_1));
+    }
+
+    bool GripperActionServer::IsGoalActiveState() const
+    {
+        if (!m_goalHandle)
+        {
+            return false;
+        }
+        return m_goalHandle->is_active() || m_goalHandle->is_executing() || m_goalHandle->is_canceling();
+    }
+
+    bool GripperActionServer::IsReadyForExecution() const
+    {
+        // Has no goal handle yet - can be accepted.
+        if (!m_goalHandle)
+        {
+            return true;
+        }
+        // Accept if the previous one is in a terminal state.
+        return IsGoalActiveState() == false;
+    }
+
+    rclcpp_action::GoalResponse GripperActionServer::GoalReceivedCallback(
+        [[maybe_unused]] const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal)
+    {
+        if (!IsReadyForExecution())
+        {
+            AZ_Printf("GripperActionServer", "GripperActionServer::handleGoal: Rejected goal, already executing\n");
+            if (m_goalHandle)
+            {
+                AZ_Trace(
+                    "GripperActionServer",
+                    " is_active: %d,  is_executing %d, is_canceling : %d\n",
+                    m_goalHandle->is_active(),
+                    m_goalHandle->is_executing(),
+                    m_goalHandle->is_canceling());
+            }
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+
+        AZ::Outcome<void, AZStd::string> commandOutcome = AZ::Failure(AZStd::string("No gripper component found!"));
+        GripperRequestBus::EventResult(
+            commandOutcome, m_entityId, &GripperRequestBus::Events::GripperCommand, goal->command.position, goal->command.max_effort);
+        if (!commandOutcome.IsSuccess())
+        {
+            AZ_Trace("GripperActionServer", "GripperCommand could not be accepted: %s\n", commandOutcome.GetError().c_str());
+            return rclcpp_action::GoalResponse::REJECT;
+        }
+        AZ_Trace("GripperActionServer", "GripperActionServer::handleGoal: GripperCommand accepted!\n");
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+    }
+
+    rclcpp_action::CancelResponse GripperActionServer::GoalCancelledCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle)
+    {
+        AZ::Outcome<void, AZStd::string> cancelOutcome = AZ::Failure(AZStd::string("No gripper component found!"));
+        GripperRequestBus::EventResult(cancelOutcome, m_entityId, &GripperRequestBus::Events::CancelGripperCommand);
+
+        if (!cancelOutcome)
+        { // This will not happen in a simulation unless intentionally done for behavior validation
+            AZ_Trace("GripperActionServer", "Cancelling could not be accepted: %s\n", cancelOutcome.GetError().c_str());
+            return rclcpp_action::CancelResponse::REJECT;
+        }
+        return rclcpp_action::CancelResponse::ACCEPT;
+    }
+
+    void GripperActionServer::GoalAcceptedCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle)
+    {
+        AZ_Trace("GripperActionServer", "Goal accepted!\n");
+        m_goalHandle = goal_handle;
+    }
+
+    void GripperActionServer::CancelGoal(std::shared_ptr<GripperCommand::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_canceling())
+        {
+            AZ_Trace("GripperActionServer", "Cancelling goal\n");
+            m_goalHandle->canceled(result);
+        }
+    }
+
+    void GripperActionServer::GoalSuccess(std::shared_ptr<GripperCommand::Result> result)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && (m_goalHandle->is_executing() || m_goalHandle->is_canceling()))
+        {
+            AZ_Trace("GripperActionServer", "Goal succeeded\n");
+            m_goalHandle->succeed(result);
+        }
+    }
+
+    void GripperActionServer::PublishFeedback(std::shared_ptr<GripperCommand::Feedback> feedback)
+    {
+        AZ_Assert(m_goalHandle, "Invalid goal handle!");
+        if (m_goalHandle && m_goalHandle->is_executing())
+        {
+            m_goalHandle->publish_feedback(feedback);
+        }
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServer.h

@@ -0,0 +1,63 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root
+ * of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+namespace ROS2
+{
+    //! GripperActionServer is a class responsible for managing an action server controlling a gripper
+    //! @see <a href="https://docs.ros.org/en/humble/p/rclcpp_action/generated/classrclcpp__action_1_1Server.html"> ROS 2 action
+    //! server documentation </a>
+    //! @see <a href="https://docs.ros.org/en/api/control_msgs/html/msg/GripperCommand.html"> GripperCommand documentation </a>
+    class GripperActionServer
+    {
+    public:
+        using GripperCommand = control_msgs::action::GripperCommand;
+        using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle<control_msgs::action::GripperCommand>;
+
+        //! Create an action server for GripperAction 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 GripperRequestBus.
+        GripperActionServer(const AZStd::string& actionName, const AZ::EntityId& entityId);
+
+        //! Cancel the current goal.
+        //! @param result Result to be passed to through action server to the client.
+        void CancelGoal(std::shared_ptr<GripperCommand::Result> result);
+
+        //! Report goal success to the action server.
+        //! @param result Result which contains success code.
+        void GoalSuccess(std::shared_ptr<GripperCommand::Result> result);
+
+        //! Publish feedback during an active action.
+        //! @param feedback An action feedback message informing about the progress.
+        void PublishFeedback(std::shared_ptr<GripperCommand::Feedback> feedback);
+
+        //! Check if the goal is in an active state
+        bool IsGoalActiveState() const;
+
+        //! Check if the goal is in a pending state
+        bool IsReadyForExecution() const;
+
+    private:
+        rclcpp_action::GoalResponse GoalReceivedCallback(
+            const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal);
+        rclcpp_action::CancelResponse GoalCancelledCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle);
+        void GoalAcceptedCallback(const std::shared_ptr<GoalHandleGripperCommand> goal_handle);
+
+        rclcpp_action::Server<GripperCommand>::SharedPtr actionServer;
+        std::shared_ptr<GoalHandleGripperCommand> m_goalHandle;
+        AZ::EntityId m_entityId; //! Entity that has target gripper component
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.cpp

@@ -0,0 +1,132 @@
+/*
+ * 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 "Utils.h"
+
+#include "GripperActionServerComponent.h"
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+
+namespace ROS2
+{
+    void GripperActionServerComponent::Activate()
+    {
+        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        AZ_Assert(ros2Frame, "Missing Frame Component!");
+        AZStd::string namespacedAction = ROS2Names::GetNamespacedName(ros2Frame->GetNamespace(), m_gripperActionServerName);
+        AZ_Printf("GripperActionServerComponent", "Creating Gripper Action Server: %s\n", namespacedAction.c_str());
+        m_gripperActionServer = AZStd::make_unique<GripperActionServer>(namespacedAction, GetEntityId());
+        AZ::TickBus::Handler::BusConnect();
+    }
+
+    void GripperActionServerComponent::Deactivate()
+    {
+        AZ::TickBus::Handler::BusDisconnect();
+        m_gripperActionServer.reset();
+    }
+
+    void GripperActionServerComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<GripperActionServerComponent, AZ::Component>()
+                ->Field("ActionServerName", &GripperActionServerComponent::m_gripperActionServerName)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<GripperActionServerComponent>("GripperActionServerComponent", "Component for the gripper action server")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "GripperActionServer")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &GripperActionServerComponent::m_gripperActionServerName,
+                        "Gripper Action Server",
+                        "Action name for the gripper server.");
+            }
+        }
+    }
+
+    void GripperActionServerComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+        required.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void GripperActionServerComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperServerService"));
+    }
+    void GripperActionServerComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperServerService"));
+    }
+
+    std::shared_ptr<GripperActionServer::GripperCommand::Feedback> GripperActionServerComponent::ProduceFeedback() const
+    {
+        auto feedback = std::make_shared<GripperCommand::Feedback>();
+        float position = 0.0f;
+        float effort = 0.0f;
+        GripperRequestBus::EventResult(position, GetEntityId(), &GripperRequestBus::Events::GetGripperPosition);
+        GripperRequestBus::EventResult(effort, GetEntityId(), &GripperRequestBus::Events::GetGripperEffort);
+        feedback->position = position;
+        feedback->position = effort;
+        feedback->reached_goal = false;
+        feedback->stalled = false;
+        return feedback;
+    }
+
+    std::shared_ptr<GripperActionServer::GripperCommand::Result> GripperActionServerComponent::ProduceResult() const
+    {
+        auto result = std::make_shared<GripperCommand::Result>();
+        float position = 0.0f;
+        float effort = 0.0f;
+        bool stalled = false;
+        bool reachedGoal = false;
+        GripperRequestBus::EventResult(position, GetEntityId(), &GripperRequestBus::Events::GetGripperPosition);
+        GripperRequestBus::EventResult(effort, GetEntityId(), &GripperRequestBus::Events::GetGripperEffort);
+        GripperRequestBus::EventResult(stalled, GetEntityId(), &GripperRequestBus::Events::IsGripperNotMoving);
+        GripperRequestBus::EventResult(reachedGoal, GetEntityId(), &GripperRequestBus::Events::HasGripperReachedGoal);
+        result->position = position;
+        result->position = effort;
+        result->reached_goal = reachedGoal;
+        result->stalled = stalled;
+        return result;
+    }
+
+    void GripperActionServerComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
+    {
+        AZ_Assert(m_gripperActionServer, "GripperActionServer::OnTick: GripperActionServer is null!");
+        if (!m_gripperActionServer->IsGoalActiveState())
+        {
+            return;
+        }
+        bool isDone = false;
+        bool isStalled;
+        bool isCancelled = false;
+        GripperRequestBus::EventResult(isDone, GetEntityId(), &GripperRequestBus::Events::HasGripperReachedGoal);
+        GripperRequestBus::EventResult(isStalled, GetEntityId(), &GripperRequestBus::Events::IsGripperNotMoving);
+        GripperRequestBus::EventResult(isCancelled, GetEntityId(), &GripperRequestBus::Events::HasGripperCommandBeenCancelled);
+        if (isCancelled)
+        {
+            m_gripperActionServer->CancelGoal(ProduceResult());
+            return;
+        }
+        if (isDone || isStalled)
+        {
+            AZ_Printf("GripperActionServer::OnTick", "GripperActionServer::OnTick: Gripper reached goal!");
+            m_gripperActionServer->GoalSuccess(ProduceResult());
+            return;
+        }
+        m_gripperActionServer->PublishFeedback(ProduceFeedback());
+        return;
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/GripperActionServerComponent.h

@@ -0,0 +1,50 @@
+/*
+ * 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 "GripperActionServer.h"
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+#include <control_msgs/action/gripper_command.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+namespace ROS2
+{
+    //! GripperActionServerComponent is a component that encapsulates gripper action server.
+    //! It is responsible for creating and managing the action server, producing feedback and result.
+    class GripperActionServerComponent
+        : public AZ::Component
+        , private AZ::TickBus::Handler
+    {
+    public:
+        using GripperCommand = control_msgs::action::GripperCommand;
+        using GoalHandleGripperCommand = rclcpp_action::ServerGoalHandle<control_msgs::action::GripperCommand>;
+        AZ_COMPONENT(GripperActionServerComponent, "{6A4417AC-1D85-4AB0-A116-1E77D40FC816}", AZ::Component);
+        GripperActionServerComponent() = default;
+        ~GripperActionServerComponent() = default;
+        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);
+
+    private:
+        // AZ::Component overrides...
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler overrides
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        std::shared_ptr<GripperActionServer::GripperCommand::Feedback> ProduceFeedback() const;
+        std::shared_ptr<GripperActionServer::GripperCommand::Result> ProduceResult() const;
+        AZStd::string m_gripperActionServerName{ "gripper_server" }; //! name of the GripperCommand action server
+        AZStd::unique_ptr<GripperActionServer> m_gripperActionServer; //! action server for GripperCommand
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.cpp

@@ -0,0 +1,361 @@
+/*
+ * 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 "VacuumGripperComponent.h"
+#include "Source/ArticulationLinkComponent.h"
+#include "Utils.h"
+
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzFramework/Components/TransformComponent.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <AzFramework/Physics/SimulatedBodies/RigidBody.h>
+#include <imgui/imgui.h>
+
+namespace ROS2
+{
+    void VacuumGripperComponent::Activate()
+    {
+        m_gripperEffectorBodyHandle = AzPhysics::InvalidSimulatedBodyHandle;
+        m_onTriggerEnterHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler(
+            [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
+            {
+                const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
+                const bool isGrippable = isObjectGrippable(grippedEntityCandidateId);
+                if (isGrippable)
+                {
+                    m_grippedObjectInEffector = grippedEntityCandidateId;
+                }
+            });
+
+        m_onTriggerExitHandler = AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler(
+            [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event)
+            {
+                const auto grippedEntityCandidateId = event.m_otherBody->GetEntityId();
+                if (m_grippedObjectInEffector == grippedEntityCandidateId)
+                {
+                    m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
+                }
+            });
+
+        m_vacuumJoint = AzPhysics::InvalidJointHandle;
+        m_grippedObjectInEffector = AZ::EntityId(AZ::EntityId::InvalidEntityId);
+        m_tryingToGrip = false;
+        m_cancelGripperCommand = false;
+        AZ::TickBus::Handler::BusConnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusConnect();
+        GripperRequestBus::Handler::BusConnect(GetEntityId());
+    }
+
+    void VacuumGripperComponent::Deactivate()
+    {
+        m_onTriggerEnterHandler.Disconnect();
+        m_onTriggerExitHandler.Disconnect();
+        GripperRequestBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+        ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect();
+    }
+
+    void VacuumGripperComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("GripperService"));
+    }
+    void VacuumGripperComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("GripperService"));
+    }
+
+    void VacuumGripperComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<VacuumGripperComponent, AZ::Component>()
+                ->Field("EffectorCollider", &VacuumGripperComponent::m_gripperEffectorCollider)
+                ->Field("EffectorArticulation", &VacuumGripperComponent::m_gripperEffectorArticulationLink)
+                ->Version(1);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<VacuumGripperComponent>("VacuumGripperComponent", "Component for control of a vacuum gripper.")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "VacuumGripper")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &VacuumGripperComponent::m_gripperEffectorCollider,
+                        "Effector Trigger Collider",
+                        "The entity with trigger collider that will detect objects that can be successfully gripped.")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId,
+                        &VacuumGripperComponent::m_gripperEffectorArticulationLink,
+                        "Effector Articulation Link",
+                        "The entity that is the articulation link of the effector.");
+            }
+        }
+    }
+
+    void VacuumGripperComponent::OnTick(float delta, AZ::ScriptTimePoint timePoint)
+    {
+        AzPhysics::SystemInterface* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get();
+        AZ_Assert(physicsSystem, "No physics system.");
+
+        AzPhysics::SceneInterface* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AZ_Assert(sceneInterface, "No scene intreface.");
+
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle.");
+
+        // Connect the trigger handlers if not already connected, it is circumventing the issue GH-16188, the
+        // RigidbodyNotificationBus should be used instead.
+        if (!m_onTriggerEnterHandler.IsConnected() || !m_onTriggerExitHandler.IsConnected())
+        {
+            if (auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get())
+            {
+                AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
+                    physicsSystem->FindAttachedBodyHandleFromEntityId(m_gripperEffectorCollider);
+                AZ_Warning(
+                    "VacuumGripper", foundBody.first != AzPhysics::InvalidSceneHandle, "No body found for m_gripperEffectorCollider.");
+                if (foundBody.first != AzPhysics::InvalidSceneHandle)
+                {
+                    AzPhysics::SimulatedBodyEvents::RegisterOnTriggerEnterHandler(
+                        foundBody.first, foundBody.second, m_onTriggerEnterHandler);
+                    AzPhysics::SimulatedBodyEvents::RegisterOnTriggerExitHandler(foundBody.first, foundBody.second, m_onTriggerExitHandler);
+                }
+            }
+
+            AZ::EntityId rootArticulationEntity = GetRootOfArticulation(m_gripperEffectorArticulationLink);
+            AZ::Entity* rootEntity = nullptr;
+            AZ::ComponentApplicationBus::BroadcastResult(rootEntity, &AZ::ComponentApplicationRequests::FindEntity, rootArticulationEntity);
+
+            AZ_Trace("VacuumGripper", "Root articulation entity name: %s\n", rootEntity->GetName().c_str());
+
+            PhysX::ArticulationLinkComponent* component = rootEntity->FindComponent<PhysX::ArticulationLinkComponent>();
+            AZStd::vector<AzPhysics::SimulatedBodyHandle> articulationHandles = component->GetSimulatedBodyHandles();
+
+            AZ_Assert(articulationHandles.size() > 1, "Expected more than one body handles in articulations");
+            for (AzPhysics::SimulatedBodyHandle handle : articulationHandles)
+            {
+                AzPhysics::SimulatedBody* body = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, handle);
+                AZ_Assert(body, "Expected valid body pointer");
+                if (body->GetEntityId() == m_gripperEffectorArticulationLink)
+                {
+                    m_gripperEffectorBodyHandle = handle;
+                }
+            }
+        }
+        if (m_tryingToGrip)
+        {
+            TryToGripObject();
+        }
+    }
+    AZ::EntityId VacuumGripperComponent::GetRootOfArticulation(AZ::EntityId entityId)
+    {
+        AZ::EntityId parentEntityId{ AZ::EntityId::InvalidEntityId };
+        AZ::Entity* parentEntity = nullptr;
+        AZ::TransformBus::EventResult(parentEntityId, entityId, &AZ::TransformBus::Events::GetParentId);
+        AZ::ComponentApplicationBus::BroadcastResult(parentEntity, &AZ::ComponentApplicationRequests::FindEntity, parentEntityId);
+
+        if (parentEntity == nullptr)
+        {
+            return AZ::EntityId(AZ::EntityId::InvalidEntityId);
+        }
+
+        // Get articulation link component, if not found for parent, return current entity
+        PhysX::ArticulationLinkComponent* component = parentEntity->FindComponent<PhysX::ArticulationLinkComponent>();
+        if (component == nullptr)
+        {
+            return entityId;
+        }
+        return GetRootOfArticulation(parentEntity->GetId());
+    }
+
+    bool VacuumGripperComponent::isObjectGrippable(const AZ::EntityId entityId)
+    {
+        bool isGrippable = false;
+        LmbrCentral::TagComponentRequestBus::EventResult(isGrippable, entityId, &LmbrCentral::TagComponentRequests::HasTag, GrippableTag);
+        return isGrippable;
+    }
+
+    bool VacuumGripperComponent::TryToGripObject()
+    {
+        AZ_Warning(
+            "VacuumGripper",
+            m_gripperEffectorBodyHandle != AzPhysics::InvalidSimulatedBodyHandle,
+            "Invalid body handle for gripperEffectorBody");
+        if (m_gripperEffectorBodyHandle == AzPhysics::InvalidSimulatedBodyHandle)
+        {
+            // No articulation link found
+            return true;
+        }
+        if (m_vacuumJoint != AzPhysics::InvalidJointHandle)
+        {
+            // Object is already gripped
+            return true;
+        }
+
+        if (!m_grippedObjectInEffector.IsValid())
+        {
+            // No object to grip
+            return false;
+        }
+        PhysX::ArticulationLinkComponent* component = m_entity->FindComponent<PhysX::ArticulationLinkComponent>();
+        AZ_Assert(component, "No PhysX::ArticulationLinkComponent found on entity ");
+
+        auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        // Get gripped rigid body
+        AzPhysics::RigidBody* grippedRigidBody = nullptr;
+        Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
+        AZ_Assert(grippedRigidBody, "No RigidBody found on entity grippedRigidBody");
+
+        // Gripper is the end of the articulation chain
+        AzPhysics::SimulatedBody* gripperBody = nullptr;
+        gripperBody = sceneInterface->GetSimulatedBodyFromHandle(defaultSceneHandle, m_gripperEffectorBodyHandle);
+        AZ_Assert(gripperBody, "No gripper body found");
+
+        AttachToGripper(gripperBody, grippedRigidBody, sceneInterface);
+
+        return true;
+    }
+
+    void VacuumGripperComponent::AttachToGripper(
+        AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface)
+    {
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+
+        // Find Transform of the child in parent's frame
+        AZ::Transform childTransformWorld = grippedRigidBody->GetTransform();
+        AZ::Transform parentsTranformWorld = gripperBody->GetTransform();
+        AZ::Transform childTransformParent = parentsTranformWorld.GetInverse() * childTransformWorld;
+        childTransformParent.Invert();
+
+        // Configure new joint
+        PhysX::FixedJointConfiguration jointConfig;
+        jointConfig.m_debugName = "VacuumJoint";
+        jointConfig.m_parentLocalRotation = AZ::Quaternion::CreateIdentity();
+        jointConfig.m_parentLocalPosition = AZ::Vector3::CreateZero();
+        jointConfig.m_childLocalRotation = childTransformParent.GetRotation();
+        jointConfig.m_childLocalPosition = childTransformParent.GetTranslation();
+        jointConfig.m_startSimulationEnabled = true;
+
+        // Create new joint
+        m_vacuumJoint =
+            sceneInterface->AddJoint(defaultSceneHandle, &jointConfig, m_gripperEffectorBodyHandle, grippedRigidBody->m_bodyHandle);
+    }
+
+    void VacuumGripperComponent::ReleaseGrippedObject()
+    {
+        m_tryingToGrip = false;
+        if (m_vacuumJoint == AzPhysics::InvalidJointHandle)
+        {
+            return;
+        }
+        auto* sceneInterface = AZ::Interface<AzPhysics::SceneInterface>::Get();
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle");
+        // Wake up the body to prevent it from not moving after release
+        AzPhysics::RigidBody* grippedRigidBody = nullptr;
+        Physics::RigidBodyRequestBus::EventResult(grippedRigidBody, m_grippedObjectInEffector, &Physics::RigidBodyRequests::GetRigidBody);
+        if (grippedRigidBody)
+        {
+            grippedRigidBody->ForceAwake();
+        }
+        sceneInterface->RemoveJoint(defaultSceneHandle, m_vacuumJoint);
+        m_vacuumJoint = AzPhysics::InvalidJointHandle;
+    }
+
+    void VacuumGripperComponent::OnImGuiUpdate()
+    {
+        ImGui::Begin("VacuumGripperDebugger");
+
+        AZStd::string grippedObjectInEffectorName;
+        AZ::ComponentApplicationBus::BroadcastResult(
+            grippedObjectInEffectorName, &AZ::ComponentApplicationRequests::GetEntityName, m_grippedObjectInEffector);
+
+        ImGui::Text("Grippable object : %s", grippedObjectInEffectorName.c_str());
+        ImGui::Text("Vacuum joint created : %d ", m_vacuumJoint != AzPhysics::InvalidJointHandle);
+
+        ImGui::Checkbox("Gripping", &m_tryingToGrip);
+
+        if (ImGui::Button("Grip Command "))
+        {
+            m_tryingToGrip = true;
+        }
+
+        if (ImGui::Button("Release Command"))
+        {
+            ReleaseGrippedObject();
+        }
+        ImGui::End();
+    }
+
+    AZ::Outcome<void, AZStd::string> VacuumGripperComponent::GripperCommand(float position, float maxEffort)
+    {
+        AZ_Trace("VacuumGripper", "GripperCommand %f\n", position);
+        m_cancelGripperCommand = false;
+        if (position == 0.0f)
+        {
+            m_tryingToGrip = true;
+        }
+        else
+        {
+            ReleaseGrippedObject();
+        }
+        return AZ::Success();
+    }
+
+    AZ::Outcome<void, AZStd::string> VacuumGripperComponent::CancelGripperCommand()
+    {
+        ReleaseGrippedObject();
+        m_cancelGripperCommand = true;
+        return AZ::Success();
+    }
+
+    bool VacuumGripperComponent::HasGripperCommandBeenCancelled() const
+    {
+        if (m_cancelGripperCommand && m_vacuumJoint == AzPhysics::InvalidJointHandle)
+        {
+            return true;
+        }
+        return false;
+    }
+
+    float VacuumGripperComponent::GetGripperPosition() const
+    {
+        return m_tryingToGrip ? 0.0f : 1.0f;
+    }
+
+    float VacuumGripperComponent::GetGripperEffort() const
+    {
+        return m_vacuumJoint == AzPhysics::InvalidJointHandle ? 0.0f : 1.0f;
+    }
+    bool VacuumGripperComponent::IsGripperNotMoving() const
+    {
+        return true;
+    }
+
+    bool VacuumGripperComponent::HasGripperReachedGoal() const
+    {
+        const bool isObjectAttached = (m_vacuumJoint != AzPhysics::InvalidJointHandle);
+        if (m_tryingToGrip && isObjectAttached)
+        {
+            return true;
+        }
+        if (!m_tryingToGrip && !isObjectAttached)
+        {
+            return true;
+        }
+        return false;
+    }
+
+} // namespace ROS2

Gems/ROS2/Code/Source/Gripper/VacuumGripperComponent.h

@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root
+ * of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/Component.h>
+#include <AzCore/Component/TickBus.h>
+#include <AzFramework/Physics/Common/PhysicsEvents.h>
+#include <AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h>
+#include <AzFramework/Physics/PhysicsSystem.h>
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include <ImGuiBus.h>
+#include <LmbrCentral/Scripting/TagComponentBus.h>
+#include <ROS2/Gripper/GripperRequestBus.h>
+
+namespace ROS2
+{
+    //! This component implements vacuum gripper functionality.
+    //! It allows to attach to gripper objects that are in inside designated trigger collider to objects that has "Grippable" tag.
+    //! To use component:
+    //!  - Attach component to root of robot articulation.
+    //!  - Assign to m_gripperEffectorCollider EntityId of the collider that will be used as the gripper effector.
+    //!  - Add tag "Grippable" to objects that can be gripped.
+    class VacuumGripperComponent
+        : public AZ::Component
+        , public GripperRequestBus::Handler
+        , public ImGui::ImGuiUpdateListenerBus::Handler
+        , public AZ::TickBus::Handler
+    {
+    public:
+        static constexpr AZ::Crc32 GrippableTag = AZ_CRC_CE("Grippable");
+        AZ_COMPONENT(VacuumGripperComponent, "{a29eb4fa-0f6f-11ee-be56-0242ac120002}", AZ::Component);
+        VacuumGripperComponent() = default;
+        ~VacuumGripperComponent() = default;
+
+        // AZ::Component overrides...
+        void Activate() override;
+        void Deactivate() override;
+
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        // GripperRequestBus::Handler overrides...
+        AZ::Outcome<void, AZStd::string> GripperCommand(float position, float maxEffort) override;
+        AZ::Outcome<void, AZStd::string> CancelGripperCommand() override;
+        float GetGripperPosition() const override;
+        float GetGripperEffort() const override;
+        bool IsGripperNotMoving() const override;
+        bool HasGripperReachedGoal() const override;
+        bool HasGripperCommandBeenCancelled() const override;
+        // AZ::TickBus::Handler overrides...
+        void OnTick(float delta, AZ::ScriptTimePoint timePoint) override;
+
+        // ImGui::ImGuiUpdateListenerBus::Handler overrides...
+        void OnImGuiUpdate() override;
+
+        //! Entity that contains the collider that will be used as the gripper
+        //! effector/ The collider must be a trigger collider.
+        AZ::EntityId m_gripperEffectorCollider;
+
+        //! Entity that contains the articulation link that will be used as the gripper
+        AZ::EntityId m_gripperEffectorArticulationLink;
+
+        //! The physics body handle to m_gripperEffectorArticulationLink.
+        AzPhysics::SimulatedBodyHandle m_gripperEffectorBodyHandle;
+
+        //! EntityId of the object that is currently gripped by the gripper effector.
+        AZ::EntityId m_grippedObjectInEffector;
+
+        //! Handle to joint created by vacuum gripper.
+        AzPhysics::JointHandle m_vacuumJoint;
+
+        bool m_tryingToGrip{ false };
+
+        bool m_cancelGripperCommand{ false };
+
+        AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler m_onTriggerEnterHandler;
+        AzPhysics::SimulatedBodyEvents::OnTriggerExit::Handler m_onTriggerExitHandler;
+
+        //! Checks if object is grippable (has Tag).
+        bool isObjectGrippable(const AZ::EntityId entityId);
+
+        //! Checks if an object is in the gripper effector collider and creates a joint between gripper effector and object.
+        bool TryToGripObject();
+
+        //! Releases object from gripper effector.
+        void ReleaseGrippedObject();
+
+        AZ::EntityId GetRootOfArticulation(AZ::EntityId entityId);
+
+        void AttachToGripper(
+            AzPhysics::SimulatedBody* gripperBody, AzPhysics::RigidBody* grippedRigidBody, AzPhysics::SceneInterface* sceneInterface);
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -12,6 +12,8 @@
 #include <AzCore/Module/Module.h>
 #include <Camera/ROS2CameraSensorComponent.h>
 #include <GNSS/ROS2GNSSSensorComponent.h>
+#include <Gripper/GripperActionServerComponent.h>
+#include <Gripper/VacuumGripperComponent.h>
 #include <Imu/ROS2ImuSensorComponent.h>
 #include <Lidar/LidarRegistrarSystemComponent.h>
 #include <Lidar/ROS2Lidar2DSensorComponent.h>
@@ -86,6 +88,8 @@ namespace ROS2
                     JointsTrajectoryComponent::CreateDescriptor(),
                     PidMotorControllerComponent::CreateDescriptor(),
                     ROS2ProximitySensor::CreateDescriptor(),
+                    GripperActionServerComponent::CreateDescriptor(),
+                    VacuumGripperComponent::CreateDescriptor(),
                 });
         }
 

Gems/ROS2/Code/ros2_files.cmake

@@ -30,6 +30,12 @@ set(FILES
         Source/Frame/NamespaceConfiguration.cpp
         Source/Frame/ROS2FrameComponent.cpp
         Source/Frame/ROS2Transform.cpp
+        Source/Gripper/GripperActionServer.cpp
+        Source/Gripper/GripperActionServer.h
+        Source/Gripper/GripperActionServerComponent.cpp
+        Source/Gripper/GripperActionServerComponent.h
+        Source/Gripper/VacuumGripperComponent.h
+        Source/Gripper/VacuumGripperComponent.cpp
         Source/GNSS/GNSSFormatConversions.cpp
         Source/GNSS/GNSSFormatConversions.h
         Source/GNSS/ROS2GNSSSensorComponent.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -14,6 +14,7 @@ set(FILES
         Include/ROS2/Frame/NamespaceConfiguration.h
         Include/ROS2/Frame/ROS2FrameComponent.h
         Include/ROS2/Frame/ROS2Transform.h
+        Include/ROS2/Gripper/GripperRequestBus.h
         Include/ROS2/Manipulation/Controllers/JointsPositionControllerRequests.h
         Include/ROS2/Manipulation/JointInfo.h
         Include/ROS2/Manipulation/JointsManipulationRequests.h