From 1e903f7bf4fc0d5843d97222045cbedaa3267d0f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Wed, 5 Jul 2023 15:35:18 +0200 Subject: [PATCH] Controller that publish cmd_vel twist message w.r.t the spline. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- Gems/ROS2/Code/CMakeLists.txt | 1 + .../Source/NPC/VelocitySplinePublisher.cpp | 200 ++++++++++++++++++ .../Code/Source/NPC/VelocitySplinePublisher.h | 67 ++++++ Gems/ROS2/Code/Source/ROS2ModuleInterface.h | 2 + Gems/ROS2/Code/ros2_files.cmake | 2 + 5 files changed, 272 insertions(+) create mode 100644 Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.cpp create mode 100644 Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.h diff --git a/Gems/ROS2/Code/CMakeLists.txt b/Gems/ROS2/Code/CMakeLists.txt index 980bfb0cbd..9b5931d703 100644 --- 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 new file mode 100644 index 0000000000..d332991f63 --- /dev/null +++ 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 +#include +#include +#include +#include + +namespace ROS2 +{ + + void VelocitySplinePublisher::Reflect(AZ::ReflectContext* context) + { + if (AZ::SerializeContext* serialize = azrtti_cast(context)) + { + serialize->Class() + ->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") + ->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(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 \ No newline at end of file diff --git a/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.h b/Gems/ROS2/Code/Source/NPC/VelocitySplinePublisher.h new file mode 100644 index 0000000000..f0f72891ea --- /dev/null +++ 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +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> 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 index f27a78ed7f..9238163c24 100644 --- a/Gems/ROS2/Code/Source/ROS2ModuleInterface.h +++ b/Gems/ROS2/Code/Source/ROS2ModuleInterface.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -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 index 5f9e458653..d2d75ec820 100644 --- 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