RobotecAI · michalpelka · Jul 5, 2023
diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt
@@ -43,6 +43,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)

diff --git a/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.cpp b/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.cpp
@@ -0,0 +1,200 @@
+/*
+ * 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 "VelocitySplinePublisher.h"
+#include <AzCore/Asset/AssetSerializer.h>
+#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/Serialization/EditContext.h>
+#include <LmbrCentral/Shape/SplineComponentBus.h>
+
+namespace ROS2
+{
+
+    void VelocitySplinePublisher::Reflect(AZ::ReflectContext* context)
+    {
+        if (AZ::SerializeContext* serialize = azrtti_cast<AZ::SerializeContext*>(context))
+        {
+            serialize->Class<VelocitySplinePublisher>()
+                ->Version(1)
+
+                ->Field("LinearSpeedFactor", &VelocitySplinePublisher::m_linearSpeedFactor)
+                ->Field("AngularSpeedFactor", &VelocitySplinePublisher::m_angularSpeedFactor)
+                ->Field("CrossTrackFactor", &VelocitySplinePublisher::m_crossTrackFactor)
+                ->Field("LookAheadDistance", &VelocitySplinePublisher::m_lookAheadDistance)
+                ->Field("cmdTopic", &VelocitySplinePublisher::m_cmdTopicConfiguration)
+                ->Field("RobotBaselink", &VelocitySplinePublisher::m_baselinkEntityId)
+                ->Field("DrawInGame", &VelocitySplinePublisher::m_drawInGame);
+
+            if (AZ::EditContext* ec = serialize->GetEditContext())
+            {
+                ec->Class<VelocitySplinePublisher>("VelocitySplinePublisher", "VelocitySplinePublisher")
+                    ->ClassElement(AZ::Edit::ClassElements::EditorData, "")
+                    ->Attribute(AZ::Edit::Attributes::Category, "ROS2")
+                    ->Attribute(AZ::Edit::Attributes::AppearsInAddComponentMenu, AZ_CRC_CE("Game"))
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &VelocitySplinePublisher::m_linearSpeedFactor,
+                        "LinearSpeedFactor",
+                        "LinearSpeedFactor")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &VelocitySplinePublisher::m_angularSpeedFactor,
+                        "AngularSpeedFactor",
+                        "AngularSpeedFactor")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &VelocitySplinePublisher::m_cmdTopicConfiguration,
+                        "Velocity Topic publisher",
+                        "Velocity Topic publisher")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default, &VelocitySplinePublisher::m_crossTrackFactor, "CrossTrackFactor", "CrossTrackFactor")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &VelocitySplinePublisher::m_lookAheadDistance,
+                        "LookAheadDistance",
+                        "LookAheadDistance")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::EntityId, &VelocitySplinePublisher::m_baselinkEntityId, "RobotBaselink", "RobotBaselink")
+                    ->DataElement(
+                        AZ::Edit::UIHandlers::Default,
+                        &VelocitySplinePublisher::m_drawInGame,
+                        "Draw in Game",
+                        "Draw track and goal in game.");
+            }
+        }
+    }
+
+    void VelocitySplinePublisher::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC_CE("SplineService"));
+    }
+
+    void VelocitySplinePublisher::Activate()
+    {
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+        const auto& topicName = m_cmdTopicConfiguration.m_topic;
+        const auto& qos = m_cmdTopicConfiguration.GetQoS();
+        m_cmdPublisher = ros2Node->create_publisher<geometry_msgs::msg::Twist>(topicName.data(), qos);
+        AZ::EntityBus::Handler::BusConnect(m_baselinkEntityId);
+        if (m_drawInGame)
+        {
+            AzFramework::EntityDebugDisplayEventBus::Handler::BusConnect(m_entity->GetId());
+        }
+    }
+
+    void VelocitySplinePublisher::Deactivate()
+    {
+        if (m_drawInGame)
+        {
+            AzFramework::EntityDebugDisplayEventBus::Handler::BusDisconnect(m_entity->GetId());
+        }
+        AZ::EntityBus::Handler::BusDisconnect();
+        AZ::TickBus::Handler::BusDisconnect();
+    }
+
+    float VelocitySplinePublisher::GetAngle(const AZ::Vector3& v1, const AZ::Vector3& v2)
+    {
+        return atan2(v1.Cross(v2).Dot(AZ::Vector3::CreateAxisZ(1.0f)), v1.Dot(v2));
+    }
+
+    void VelocitySplinePublisher::OnTick(float deltaTime, AZ::ScriptTimePoint time)
+    {
+        AZ::ConstSplinePtr splinePtr{ nullptr };
+        LmbrCentral::SplineComponentRequestBus::EventResult(splinePtr, m_entity->GetId(), &LmbrCentral::SplineComponentRequests::GetSpline);
+        AZ_Assert(splinePtr, "Spline pointer is null");
+
+        AZ::Transform splineTransform{ AZ::Transform::CreateIdentity() };
+        AZ::TransformBus::EventResult(splineTransform, m_entity->GetId(), &AZ::TransformBus::Events::GetWorldTM);
+
+        // get robot location in spline's frame
+        AZ::Transform robotLocationWorld{ AZ::Transform::CreateIdentity() };
+        AZ::TransformBus::EventResult(robotLocationWorld, m_baselinkEntityId, &AZ::TransformBus::Events::GetWorldTM);
+        AZ::Transform robotLocationSpline = splineTransform.GetInverse() * robotLocationWorld;
+
+        // query spline for nearest address
+        const AZ::PositionSplineQueryResult splineQuery =
+            splinePtr->GetNearestAddressPosition(robotLocationSpline.TransformPoint(AZ::Vector3::CreateAxisX(m_lookAheadDistance)));
+        const AZ::Vector3 position = splinePtr->GetPosition(splineQuery.m_splineAddress);
+        const AZ::Vector3 tangent = splinePtr->GetTangent(splineQuery.m_splineAddress);
+        const AZ::Vector3 normal = splinePtr->GetNormal(splineQuery.m_splineAddress);
+
+        // construct ideal pose as SE(3) - in spline space
+        const AZ::Matrix3x3 rot = AZ::Matrix3x3::CreateFromColumns(tangent, normal, tangent.Cross(normal));
+        const AZ::Transform goalTransform = AZ::Transform::CreateFromMatrix3x3AndTranslation(rot, position);
+        m_idealGoal = splineTransform * goalTransform;
+
+        // calculate robot location in goal space
+        AZ::Vector3 robotLocationInGoalSpace = m_idealGoal.GetInverse().TransformPoint(robotLocationWorld.GetTranslation());
+
+        // calculate linear velocity
+        float linearVelocity = 0;
+        if (splineQuery.m_splineAddress.m_segmentIndex != splinePtr->GetSegmentCount())
+        {
+            linearVelocity = m_linearSpeedFactor;
+        }
+
+        // cross track error
+        const float crossTrackError = robotLocationInGoalSpace.GetY();
+
+        // calculate bearing error
+        const AZ::Vector3 robotDirectionSpline = robotLocationSpline.GetBasisX();
+        const float bearingError = GetAngle(robotDirectionSpline, tangent);
+
+        AZ_Printf(
+            "ROS2",
+            " %d %f %f %f",
+            splineQuery.m_splineAddress.m_segmentIndex,
+            robotLocationInGoalSpace.GetX(),
+            robotLocationInGoalSpace.GetY(),
+            robotLocationInGoalSpace.GetZ());
+
+        geometry_msgs::msg::Twist cmd;
+        cmd.angular.z = bearingError * m_angularSpeedFactor - crossTrackError * m_crossTrackFactor;
+        cmd.linear.x = linearVelocity;
+        m_cmdPublisher->publish(cmd);
+    }
+
+    void VelocitySplinePublisher::DisplayEntityViewport(
+        [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay)
+    {
+        AZ::ConstSplinePtr splinePtr{ nullptr };
+        LmbrCentral::SplineComponentRequestBus::EventResult(splinePtr, m_entity->GetId(), &LmbrCentral::SplineComponentRequests::GetSpline);
+        AZ_Assert(splinePtr, "Spline pointer is null");
+
+        AZ::Transform splineTransform{ AZ::Transform::CreateIdentity() };
+        AZ::TransformBus::EventResult(splineTransform, m_entity->GetId(), &AZ::TransformBus::Events::GetWorldTM);
+
+        constexpr float step = 0.01f;
+        for (float f = step; f < 1.0f; f += step)
+        {
+            auto pos1 = splinePtr->GetAddressByFraction(f);
+            auto pos2 = splinePtr->GetAddressByFraction(f - step);
+
+            debugDisplay.SetColor(AZ::Colors::Red);
+            debugDisplay.DrawLine(
+                splineTransform.TransformPoint(splinePtr->GetPosition(pos1)), splineTransform.TransformPoint(splinePtr->GetPosition(pos2)));
+        }
+        debugDisplay.SetColor(AZ::Colors::White);
+        debugDisplay.DrawPoint(m_idealGoal.GetTranslation(), 4);
+        debugDisplay.SetColor(AZ::Colors::Red);
+        debugDisplay.DrawLine(m_idealGoal.GetTranslation(), m_idealGoal.TransformPoint(AZ::Vector3::CreateAxisX(1.0f)));
+        debugDisplay.SetColor(AZ::Colors::Green);
+        debugDisplay.DrawLine(m_idealGoal.GetTranslation(), m_idealGoal.TransformPoint(AZ::Vector3::CreateAxisY(1.0f)));
+        debugDisplay.SetColor(AZ::Colors::Blue);
+        debugDisplay.DrawLine(m_idealGoal.GetTranslation(), m_idealGoal.TransformPoint(AZ::Vector3::CreateAxisZ(1.0f)));
+    }
+
+    void VelocitySplinePublisher::OnEntityActivated(const AZ::EntityId& entityId)
+    {
+        if (entityId == m_baselinkEntityId)
+        {
+            AZ::TickBus::Handler::BusConnect();
+        }
+    }
+
+} // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.h b/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.h
@@ -0,0 +1,67 @@
+/*
+ * 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/Entity.h>
+#include <AzCore/Component/TickBus.h>
+
+#include <AzCore/Math/Spline.h>
+#include <AzCore/Math/Transform.h>
+#include <AzCore/Serialization/SerializeContext.h>
+#include <AzCore/std/containers/deque.h>
+
+#include <AzCore/Component/EntityBus.h>
+#include <AzFramework/Entity/EntityDebugDisplayBus.h>
+#include <geometry_msgs/msg/twist.hpp>
+
+namespace ROS2
+{
+    //! Component that send velocity commands to ROS2 system according to entity position and spline trajectory.
+    class VelocitySplinePublisher
+        : public AZ::Component
+        , private AZ::TickBus::Handler
+        , private AZ::EntityBus::Handler
+        , private AzFramework::EntityDebugDisplayEventBus::Handler
+    {
+    public:
+        AZ_COMPONENT(VelocitySplinePublisher, "{28b8b025-b499-4876-bc06-1b9112ff62d3}");
+        VelocitySplinePublisher() = default;
+        ~VelocitySplinePublisher() override = default;
+
+        static void Reflect(AZ::ReflectContext* context);
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+
+        // AZ::Component overrides ...
+        void Activate() override;
+        void Deactivate() override;
+
+        // AZ::TickBus::Handler overrides ...
+        void OnTick(float deltaTime, AZ::ScriptTimePoint time) override;
+
+        // AZ::EntityBus::Handler overrides ...
+        void OnEntityActivated(const AZ::EntityId& entityId) override;
+
+
+        // AzFramework::EntityDebugDisplayEventBus::Handler overrides
+        void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override;
+
+    private:
+        float GetAngle(const AZ::Vector3 &v1, const AZ::Vector3 &v2);
+        float m_lookAheadDistance = 1.f;
+        float m_linearSpeedFactor = 0.5f;
+        float m_angularSpeedFactor = 5.f;
+        float m_crossTrackFactor = 0.3f;
+        bool m_drawInGame = true;
+
+        TopicConfiguration m_cmdTopicConfiguration;
+        AZ::EntityId m_baselinkEntityId;
+        std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> m_cmdPublisher;
+        AZ::Transform m_idealGoal{ AZ::Transform::CreateIdentity() };
+    };
+} // namespace ROS2
diff --git a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h
@@ -16,6 +16,7 @@
 #include <Lidar/LidarRegistrarSystemComponent.h>
 #include <Lidar/ROS2Lidar2DSensorComponent.h>
 #include <Lidar/ROS2LidarSensorComponent.h>
+#include <NPC/VelocitySplinePublisher.h>
 #include <Odometry/ROS2OdometrySensorComponent.h>
 #include <Odometry/ROS2WheelOdometry.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
@@ -82,6 +83,7 @@ namespace ROS2
                     ManipulatorControllerComponent::CreateDescriptor(),
                     PidMotorControllerComponent::CreateDescriptor(),
                     FollowingCameraComponent::CreateDescriptor(),
+                    VelocitySplinePublisher::CreateDescriptor(),
                 });
         }
 

diff --git a/Gems/ROS2/Code/ros2_files.cmake b/Gems/ROS2/Code/ros2_files.cmake
@@ -59,6 +59,8 @@ set(FILES
         Source/Odometry/ROS2OdometrySensorComponent.h
         Source/Odometry/ROS2WheelOdometry.cpp
         Source/Odometry/ROS2WheelOdometry.h
+        Source/NPC/VelocitySplinePublisher.cpp
+        Source/NPC/VelocitySplinePublisher.h
         Source/Utilities/ROS2OdometryCovariance.cpp
         Source/RobotControl/Ackermann/AckermannSubscriptionHandler.cpp
         Source/RobotControl/Ackermann/AckermannSubscriptionHandler.h