Robot control

- A component and some abstractions
- Generic class - implement control for your message type
- Default implementation for Twist, operating on Entity's rigidbody (for tests and as an example)
- Utility conversion class (vector3 only for now)

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

Gems/ROS2/Code/Source/ROS2LidarSensorComponent.cpp

@@ -27,7 +27,6 @@ namespace ROS2
         m_frameTime = m_hz == 0 ? 1 : 1 / m_hz;
         AZ::TickBus::Handler::BusConnect();
         m_entityTransform = GetEntity()->FindComponent<AzFramework::TransformComponent>();
-
     }
 
     void ROS2LidarSensorComponent::Deactivate()
@@ -50,7 +49,7 @@ namespace ROS2
             {
                 ec->Class<ROS2LidarSensorComponent>("Lidar Sensor", "[Simple Lidar component]")
                     ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
-                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game"))
+                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) // TODO - "Simulation"?
                     ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2LidarSensorComponent::m_hz, "Hz", "Lidar data acquisition and publish frequency")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2LidarSensorComponent::m_frameName, "Frame Name", "Lidar ros2 message frame")
                     ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2LidarSensorComponent::m_lidarModel, "Lidar Model", "Lidar model")

Gems/ROS2/Code/Source/ROS2ModuleInterface.h

@@ -8,6 +8,8 @@
 #include <AzCore/Memory/SystemAllocator.h>
 #include <AzCore/Module/Module.h>
 #include <ROS2SystemComponent.h>
+#include <Lidar/ROS2LidarSensorComponent.h>
+#include <RobotControl/ROS2RobotControlComponent.h>
 
 namespace ROS2
 {
@@ -25,7 +27,9 @@ namespace ROS2
             // This will associate the AzTypeInfo information for the components with the the SerializeContext, BehaviorContext and EditContext.
             // This happens through the [MyComponent]::Reflect() function.
             m_descriptors.insert(m_descriptors.end(), {
-                ROS2SystemComponent::CreateDescriptor()
+                ROS2SystemComponent::CreateDescriptor(),
+                ROS2LidarSensorComponent::CreateDescriptor(),
+                ROS2RobotControlComponent::CreateDescriptor(),
                 });
         }
 

Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.cpp

@@ -0,0 +1,62 @@
+/*
+ * 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/Entity.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <AzCore/Serialization/EditContextConstants.inl>
+
+#include "ROS2RobotControlComponent.h"
+#include "TwistControl.h"
+
+namespace ROS2
+{
+    void ROS2RobotControlComponent::Init()
+    {
+        // TODO - instead, create/reset robot control in Activate based on selected implementation (in the component)
+        m_robotControl = std::make_unique<TwistControl>();
+    }
+
+    void ROS2RobotControlComponent::Activate()
+    {
+        m_robotControl->Activate(GetEntity(), m_topic);
+    }
+
+    void ROS2RobotControlComponent::Deactivate()
+    {
+        m_robotControl->Deactivate();
+    }
+
+    void ROS2RobotControlComponent::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<ROS2RobotControlComponent, AZ::Component>()
+                ->Version(1)
+                ->Field("topic", &ROS2RobotControlComponent::m_topic)
+                ;
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<ROS2RobotControlComponent>("Robot control", "[Customizable robot control component]")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                        ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC("Game")) // TODO - "Simulation"?
+                    ->DataElement(AZ::Edit::UIHandlers::Default, &ROS2RobotControlComponent::m_topic, "Topic", "ROS2 topic to subscribe to")
+                    ;
+            }
+        }
+    }
+
+    /*
+    void ROS2RobotControlComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        // TODO - query current/selected RobotControl implementation for what components are required
+    }
+    */
+}  // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/ROS2RobotControlComponent.h

@@ -0,0 +1,34 @@
+/*
+ * 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 <memory>
+#include <AzCore/Component/Component.h>
+#include "RobotControl.h"
+
+namespace ROS2
+{
+    class ROS2RobotControlComponent
+        : public AZ::Component
+    {
+    public:
+        AZ_COMPONENT(ROS2RobotControlComponent, "{CBFB0764-99F9-40EE-9FEE-F5F5A66E59D2}", AZ::Component);
+
+        // AZ::Component interface implementation.
+        void Init() override;
+        void Activate() override;
+        void Deactivate() override;
+
+        // Required Reflect function.
+        static void Reflect(AZ::ReflectContext* context);
+
+    private:
+        AZStd::string m_topic = "/o3de_robot_control";
+        std::unique_ptr<IRobotControl> m_robotControl;
+    };
+}  // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/RobotControl.h

@@ -0,0 +1,68 @@
+/*
+ * 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 <memory>
+#include <rclcpp/rclcpp.hpp>
+#include <ROS2/ROS2Bus.h>
+
+namespace ROS2
+{
+    // Component extension enabling polymorphic use of generics
+    // TODO - naming (this is a component activation interface capturing component and entity state)
+    class IRobotControl
+    {
+    public:
+        virtual void Activate(const AZ::Entity* entity, const AZStd::string& topicName) = 0;
+        virtual void Deactivate() = 0;
+        virtual ~IRobotControl() = default;
+    };
+
+    template <typename T>
+    class RobotControl : public IRobotControl
+    {
+    public:
+        // TODO - pass component args (qos, topic name, etc) as editor-enabled serializable struct
+        void Activate(const AZ::Entity* entity, const AZStd::string& topicName) final
+        {
+            SetTargetComponent(entity);
+            m_active = true;
+            if (!m_controlSubscription)
+            {
+                auto ros2Node = ROS2Interface::Get()->GetNode();
+                rclcpp::QoS qos(10); // TODO - expose QoS
+                m_controlSubscription = ros2Node->create_subscription<T>(topicName.data(), qos,
+                    std::bind(&RobotControl<T>::OnControlMessage, this, std::placeholders::_1));
+            }
+        };
+
+        void Deactivate() final
+        {
+            m_active = false;
+            m_controlSubscription.reset(); // Note: topic and qos can change, need to re-subscribe
+        };
+
+    private:
+        void OnControlMessage(const T& message)
+        {
+            if (m_active)
+            {
+                ApplyControl(message);
+            }
+        };
+
+        virtual void ApplyControl(const T& message) = 0;
+
+        // Subclass implementations will extract necessary services or components to manipulate
+        virtual void SetTargetComponent(const AZ::Entity* entity) = 0;
+
+        bool m_active = false;
+        std::string m_topicName;
+
+        typename rclcpp::Subscription<T>::SharedPtr m_controlSubscription;
+    };
+}  // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/TwistControl.cpp

@@ -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
+ *
+ */
+
+#include <AzFramework/Physics/RigidBodyBus.h>
+#include "TwistControl.h"
+#include "Utilities/ROS2Conversions.h"
+
+namespace ROS2
+{
+    void TwistControl::SetTargetComponent(const AZ::Entity* entity)
+    {
+        m_entityID = entity->GetId();
+    }
+
+    void TwistControl::ApplyControl(const geometry_msgs::msg::Twist& message)
+    {
+        const AZ::Vector3 linearVelocity = ROS2Conversions::FromROS2Vector3(message.linear);
+        const AZ::Vector3 angularVelocity = ROS2Conversions::FromROS2Vector3(message.angular);
+        Physics::RigidBodyRequestBus::Event(m_entityID, &Physics::RigidBodyRequests::SetLinearVelocity, linearVelocity);
+        Physics::RigidBodyRequestBus::Event(m_entityID, &Physics::RigidBodyRequests::SetAngularVelocity, angularVelocity);
+    }
+}  // namespace ROS2

Gems/ROS2/Code/Source/RobotControl/TwistControl.h

@@ -0,0 +1,23 @@
+/*
+ * 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 "RobotControl.h"
+#include "geometry_msgs/msg/twist.hpp"
+
+namespace ROS2
+{
+    class TwistControl : public RobotControl<geometry_msgs::msg::Twist>
+    {
+    private:
+        void ApplyControl(const geometry_msgs::msg::Twist& message) override;
+        void SetTargetComponent(const AZ::Entity* entity) override;
+
+        AZ::EntityId m_entityID;
+    };
+}  // namespace ROS2

Gems/ROS2/Code/Source/Utilities/ROS2Conversions.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 "ROS2Conversions.h"
+
+namespace ROS2
+{   // Same coordinate systems - just translate types
+    AZ::Vector3 ROS2Conversions::FromROS2Vector3(const geometry_msgs::msg::Vector3& ros2vector)
+    {
+        return AZ::Vector3(ros2vector.x, ros2vector.y, ros2vector.z);
+    }
+
+    geometry_msgs::msg::Vector3 ROS2Conversions::ToROS2Vector3(const AZ::Vector3& azvector)
+    {
+        geometry_msgs::msg::Vector3 ros2vector;
+        ros2vector.x = azvector.GetX();
+        ros2vector.y = azvector.GetY();
+        ros2vector.z = azvector.GetZ();
+        return ros2vector;
+    }
+}  // namespace ROS2

Gems/ROS2/Code/Source/Utilities/ROS2Conversions.h

@@ -0,0 +1,22 @@
+/*
+ * 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/Math/Vector3.h>
+#include "geometry_msgs/msg/vector3.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+
+namespace ROS2
+{
+    class ROS2Conversions
+    {
+    public:
+        static AZ::Vector3 FromROS2Vector3(const geometry_msgs::msg::Vector3& ros2vector);
+        static geometry_msgs::msg::Vector3 ToROS2Vector3(const AZ::Vector3& azvector);
+    };
+}  // namespace ROS2

Gems/ROS2/Code/ros2_files.cmake

@@ -6,14 +6,19 @@
 set(FILES
     Source/Clock/SimulationClock.cpp
     Source/Clock/SimulationClock.h
-    Source/LidarRaycaster.cpp
-    Source/LidarRaycaster.h
-    Source/LidarTemplate.h
-    Source/ROS2LidarSensorComponent.cpp
-    Source/ROS2LidarSensorComponent.h
+    Source/Lidar/LidarRaycaster.cpp
+    Source/Lidar/LidarRaycaster.h
+    Source/Lidar/LidarTemplate.h
+    Source/Lidar/ROS2LidarSensorComponent.cpp
+    Source/Lidar/ROS2LidarSensorComponent.h
+    Source/RobotControl/RobotControl.h
+    Source/RobotControl/ROS2RobotControlComponent.cpp
+    Source/RobotControl/ROS2RobotControlComponent.h
+    Source/RobotControl/TwistControl.cpp
+    Source/RobotControl/TwistControl.h
     Source/ROS2ModuleInterface.h
     Source/ROS2SystemComponent.cpp
     Source/ROS2SystemComponent.h
-    Source/SimulationClock.cpp
-    Source/SimulationClock.h
+    Source/Utilities/ROS2Conversions.cpp
+    Source/Utilities/ROS2Conversions.h
 )