Implement contact sensor component.

Signed-off-by: Anna Faferek <anna.faferek@robotec.ai>

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

@@ -0,0 +1,198 @@
+/*
+ * 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 "ROS2ContactSensorComponent.h"
+#include "ArticulationLinkComponent.h"
+#include "AzCore/Component/ComponentApplicationBus.h"
+#include "AzCore/Component/TickBus.h"
+#include "AzCore/Debug/Trace.h"
+#include "AzCore/Math/Vector3.h"
+#include "AzCore/Serialization/EditContextConstants.inl"
+#include "AzCore/std/parallel/lock.h"
+#include "AzCore/std/parallel/mutex.h"
+#include "AzCore/std/utility/move.h"
+#include "AzFramework/Physics/Collision/CollisionEvents.h"
+#include "AzFramework/Physics/Common/PhysicsSimulatedBody.h"
+#include "AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h"
+#include "AzFramework/Physics/Common/PhysicsTypes.h"
+#include "AzFramework/Physics/PhysicsSystem.h"
+#include "ROS2/Frame/ROS2FrameComponent.h"
+#include "ROS2/ROS2Bus.h"
+#include "ROS2/Sensor/ROS2SensorComponent.h"
+#include "ROS2/Utilities/ROS2Names.h"
+#include <cstddef>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/wrench.hpp>
+
+namespace ROS2
+{
+    ROS2ContactSensorComponent::ROS2ContactSensorComponent()
+    {
+        TopicConfiguration tc;
+        AZStd::string type = "gazebo_msgs::msg::ContactsState";
+        tc.m_type = type;
+        tc.m_topic = "contact_sensor";
+        m_sensorConfiguration.m_frequency = 15;
+        m_sensorConfiguration.m_publishersConfigurations.insert(AZStd::make_pair(AZStd::move(type), AZStd::move(tc)));
+    }
+
+    void ROS2ContactSensorComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ROS2ContactSensorComponent, ROS2SensorComponent>()->Version(1);
+
+            if (AZ::EditContext* editContext = serialize->GetEditContext())
+            {
+                editContext->Class<ROS2ContactSensorComponent>("ROS2 Contact Sensor", "Contact detection controller")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"));
+            }
+        }
+    }
+
+    void ROS2ContactSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("PhysicsColliderService"));
+        required.push_back(AZ_CRC_CE("ROS2Frame"));
+    }
+
+    void ROS2ContactSensorComponent::Activate()
+    {
+        m_entityId = GetEntityId();
+        AZ::Entity* entity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId);
+        m_entityName = entity->GetName();
+
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        AZ_Assert(m_sensorConfiguration.m_publishersConfigurations.size() == 1, "Invalid configuration of publishers for Contact sensor");
+        const auto publisherConfig = m_sensorConfiguration.m_publishersConfigurations["gazebo_msgs::msg::ContactsState"];
+        const auto fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig.m_topic);
+        m_contactsPublisher = ros2Node->create_publisher<gazebo_msgs::msg::ContactsState>(fullTopic.data(), publisherConfig.GetQoS());
+
+        m_onCollisionBeginHandler = AzPhysics::SimulatedBodyEvents::OnCollisionBegin::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                AddNewContact(event);
+            });
+
+        m_onCollisionPersistHandler = AzPhysics::SimulatedBodyEvents::OnCollisionPersist::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                AddNewContact(event);
+            });
+
+        m_onCollisionEndHandler = AzPhysics::SimulatedBodyEvents::OnCollisionEnd::Handler(
+            [this]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, const AzPhysics::CollisionEvent& event)
+            {
+                m_activeContacts.erase(event.m_body2->GetEntityId());
+            });
+
+        ROS2SensorComponent::Activate();
+    }
+
+    void ROS2ContactSensorComponent::Deactivate()
+    {
+        m_activeContacts.clear();
+        m_contactsPublisher.reset();
+        m_onCollisionBeginHandler.Disconnect();
+        m_onCollisionPersistHandler.Disconnect();
+        m_onCollisionEndHandler.Disconnect();
+        ROS2SensorComponent::Deactivate();
+    }
+
+    void ROS2ContactSensorComponent::FrequencyTick()
+    {
+        // Connects the collision handlers if not already connected
+        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 interface.");
+
+        AzPhysics::SceneHandle defaultSceneHandle = sceneInterface->GetSceneHandle(AzPhysics::DefaultPhysicsSceneName);
+        AZ_Assert(defaultSceneHandle != AzPhysics::InvalidSceneHandle, "Invalid default physics scene handle.");
+
+        if (!m_onCollisionBeginHandler.IsConnected() || !m_onCollisionPersistHandler.IsConnected() ||
+            !m_onCollisionEndHandler.IsConnected())
+        {
+            if (auto* physicsSystem = AZ::Interface<AzPhysics::SystemInterface>::Get())
+            {
+                AZStd::pair<AzPhysics::SceneHandle, AzPhysics::SimulatedBodyHandle> foundBody =
+                    physicsSystem->FindAttachedBodyHandleFromEntityId(GetEntityId());
+                AZ_Warning("Contact Sensor", foundBody.first != AzPhysics::InvalidSceneHandle, "Invalid scene handle");
+                if (foundBody.first != AzPhysics::InvalidSceneHandle)
+                {
+                    AzPhysics::SimulatedBodyEvents::RegisterOnCollisionBeginHandler(
+                        foundBody.first, foundBody.second, m_onCollisionBeginHandler);
+                    AzPhysics::SimulatedBodyEvents::RegisterOnCollisionPersistHandler(
+                        foundBody.first, foundBody.second, m_onCollisionPersistHandler);
+                    AzPhysics::SimulatedBodyEvents::RegisterOnCollisionEndHandler(
+                        foundBody.first, foundBody.second, m_onCollisionEndHandler);
+                }
+            }
+        }
+
+        // Publishes all contacts
+        gazebo_msgs::msg::ContactsState msg;
+        auto* ros2Frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        msg.header.frame_id = ros2Frame->GetFrameID().data();
+        msg.header.stamp = ROS2Interface::Get()->GetROSTimestamp();
+
+        AZStd::lock_guard<AZStd::mutex> lock(m_contactsContainerMutex);
+        for (auto& [id, contact] : m_activeContacts)
+        {
+            msg.states.push_back(contact);
+        }
+
+        m_contactsPublisher->publish(AZStd::move(msg));
+    }
+
+    void ROS2ContactSensorComponent::AddNewContact(const AzPhysics::CollisionEvent& event)
+    {
+        AZ::Entity* contactedEntity = nullptr;
+        AZ::ComponentApplicationBus::BroadcastResult(
+            contactedEntity, &AZ::ComponentApplicationRequests::FindEntity, event.m_body2->GetEntityId());
+        gazebo_msgs::msg::ContactState state;
+        AZ_Assert(contactedEntity != nullptr, "Invalid entity pointer value") state.collision1_name =
+            ("ID: " + m_entityId.ToString() + " Name:" + m_entityName).c_str();
+        state.collision2_name = ("ID: " + event.m_body2->GetEntityId().ToString() + " Name:" + contactedEntity->GetName()).c_str();
+
+        geometry_msgs::msg::Wrench totalWrench;
+        for (auto& contact : event.m_contacts)
+        {
+            geometry_msgs::msg::Vector3 contactPosition;
+            contactPosition.x = contact.m_position.GetX();
+            contactPosition.y = contact.m_position.GetY();
+            contactPosition.z = contact.m_position.GetZ();
+            state.contact_positions.push_back(AZStd::move(contactPosition));
+
+            geometry_msgs::msg::Vector3 contactNormal;
+            contactNormal.x = contact.m_normal.GetX();
+            contactNormal.y = contact.m_normal.GetY();
+            contactNormal.z = contact.m_normal.GetZ();
+            state.contact_normals.push_back(AZStd::move(contactNormal));
+
+            geometry_msgs::msg::Wrench contactWrench;
+            contactWrench.force.x = contact.m_impulse.GetX();
+            contactWrench.force.y = contact.m_impulse.GetY();
+            contactWrench.force.z = contact.m_impulse.GetZ();
+            state.wrenches.push_back(AZStd::move(contactWrench));
+
+            totalWrench.force.x += contact.m_impulse.GetX();
+            totalWrench.force.y += contact.m_impulse.GetY();
+            totalWrench.force.z += contact.m_impulse.GetZ();
+        }
+
+        state.total_wrench = AZStd::move(totalWrench);
+
+        AZStd::lock_guard<AZStd::mutex> lock(m_contactsContainerMutex);
+        m_activeContacts[event.m_body2->GetEntityId()] = AZStd::move(state);
+    }
+} // namespace ROS2

Gems/ROS2/Code/Source/ContactSensor/ROS2ContactSensorComponent.h

@@ -0,0 +1,66 @@
+/*
+ * 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/Math/Vector3.h"
+#include "AzCore/RTTI/ReflectContext.h"
+#include "AzCore/std/containers/unordered_map.h"
+#include "AzCore/std/parallel/mutex.h"
+#include "AzCore/std/string/string.h"
+#include "AzFramework/Physics/Common/PhysicsSimulatedBodyEvents.h"
+#include "ROS2/Sensor/ROS2SensorComponent.h"
+#include <gazebo_msgs/msg/contact_state.hpp>
+#include <gazebo_msgs/msg/contacts_state.hpp>
+#include <memory>
+#include <rclcpp/publisher.hpp>
+#include <unordered_map>
+
+namespace ROS2
+{
+    //! Contact sensor detects collisions between two objects.
+    //! It reports the location of the contact associated forces.
+    //! This component publishes a contact_sensor topic.
+    //! Doesn't measure torque
+    class ROS2ContactSensorComponent : public ROS2SensorComponent
+    {
+    public:
+        AZ_COMPONENT(ROS2ContactSensorComponent, "{91272e66-c9f1-4aa2-a9d5-98eaa4ef4e9a}", ROS2SensorComponent);
+        ROS2ContactSensorComponent();
+        ~ROS2ContactSensorComponent() = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        //////////////////////////////////////////////////////////////////////////
+        // Component overrides
+        void Activate() override;
+        void Deactivate() override;
+        //////////////////////////////////////////////////////////////////////////
+
+    private:
+        //////////////////////////////////////////////////////////////////////////
+        // ROS2SensorComponent overrides
+        void FrequencyTick() override;
+        //////////////////////////////////////////////////////////////////////////
+
+        void AddNewContact(const AzPhysics::CollisionEvent& event);
+
+        AZ::EntityId m_entityId;
+        AZStd::string m_entityName = "";
+
+        AzPhysics::SimulatedBodyEvents::OnCollisionBegin::Handler m_onCollisionBeginHandler;
+        AzPhysics::SimulatedBodyEvents::OnCollisionPersist::Handler m_onCollisionPersistHandler;
+        AzPhysics::SimulatedBodyEvents::OnCollisionEnd::Handler m_onCollisionEndHandler;
+
+        std::shared_ptr<rclcpp::Publisher<gazebo_msgs::msg::ContactsState>> m_contactsPublisher;
+
+        AZStd::unordered_map<AZ::EntityId, gazebo_msgs::msg::ContactState> m_activeContacts;
+        AZStd::mutex m_contactsContainerMutex;
+    };
+} // namespace ROS2

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -11,6 +11,7 @@
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Module/Module.h>
 #include <Camera/ROS2CameraSensorComponent.h>
+#include <ContactSensor/ROS2ContactSensorComponent.h>
 #include <GNSS/ROS2GNSSSensorComponent.h>
 #include <Gripper/GripperActionServerComponent.h>
 #include <Gripper/VacuumGripperComponent.h>
@@ -90,6 +91,7 @@ namespace ROS2
                     ROS2ProximitySensor::CreateDescriptor(),
                     GripperActionServerComponent::CreateDescriptor(),
                     VacuumGripperComponent::CreateDescriptor(),
+                    ROS2ContactSensorComponent::CreateDescriptor(),
                 });
         }
 

Gems/ROS2/Code/ros2_files.cmake

@@ -27,6 +27,8 @@ set(FILES
         Source/Communication/QoS.cpp
         Source/Communication/PublisherConfiguration.cpp
         Source/Communication/TopicConfiguration.cpp
+        Source/ContactSensor/ROS2ContactSensorComponent.cpp
+        Source/ContactSensor/ROS2ContactSensorComponent.h
         Source/Frame/NamespaceConfiguration.cpp
         Source/Frame/ROS2FrameComponent.cpp
         Source/Frame/ROS2Transform.cpp