Pose consideration

Signed-off-by: Krzysztof Rymski <[email protected]> formating add include try/catch tryc catch part2 Resolving conversations Update Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp Co-authored-by: Piotr Jaroszek <[email protected]> Fixed bad movement Pose consideration Signed-off-by: Krzysztof Rymski <[email protected]> formating add include try/catch tryc catch part2 Resolving conversations Update Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp Co-authored-by: Piotr Jaroszek <[email protected]> Fixed bad movement
RobotecAI · Aug 6, 2024 · 37c9a1b · 37c9a1b
1 parent 0d71487
commit 37c9a1b
Show file tree

Hide file tree

Showing 6 changed files with 103 additions and 34 deletions.
diff --git a/Gems/ROS2PoseControl/Code/CMakeLists.txt b/Gems/ROS2PoseControl/Code/CMakeLists.txt
@@ -40,23 +40,25 @@ ly_add_target(
         NAME ${gem_name}.Private.Object STATIC
         NAMESPACE Gem
         FILES_CMAKE
-        ros2posecontrol_private_files.cmake
-        ${pal_dir}/ros2posecontrol_private_files.cmake
+            ros2posecontrol_private_files.cmake
+            ${pal_dir}/ros2posecontrol_private_files.cmake
+        PLATFORM_INCLUDE_FILES
+            ${CMAKE_CURRENT_LIST_DIR}/Platform/Common/${PAL_TRAIT_COMPILER_ID}/ros2scriptintegration_${PAL_TRAIT_COMPILER_ID_LOWERCASE}.cmake
         TARGET_PROPERTIES
         O3DE_PRIVATE_TARGET TRUE
         INCLUDE_DIRECTORIES
-        PUBLIC
-        Include
-        PRIVATE
-        Include
-        Source
+            PUBLIC
+                Include
+            PRIVATE
+                Include
+                Source
         BUILD_DEPENDENCIES
-        PUBLIC
-        AZ::AzCore
-        AZ::AzFramework
-        Gem::ImGui.Static
-        Gem::ROS2.Static
-        Gem::ROS2.API
+            PUBLIC
+                AZ::AzCore
+                AZ::AzFramework
+                Gem::ImGui.Static
+                Gem::ROS2.Static
+                Gem::ROS2.API
 )
 
 target_depends_on_ros2_packages(${gem_name}.Private.Object geometry_msgs)

diff --git a/Gems/ROS2PoseControl/Code/Platform/Common/Clang/ros2scriptintegration_clang.cmake b/Gems/ROS2PoseControl/Code/Platform/Common/Clang/ros2scriptintegration_clang.cmake
@@ -0,0 +1,4 @@
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            -fexceptions
+)
diff --git a/Gems/ROS2PoseControl/Code/Platform/Common/GCC/ros2scriptintegration_gcc.cmake b/Gems/ROS2PoseControl/Code/Platform/Common/GCC/ros2scriptintegration_gcc.cmake
@@ -0,0 +1,4 @@
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            -fexceptions
+)
diff --git a/Gems/ROS2PoseControl/Code/Platform/Common/msvc/ros2scriptintegration_msvc.cmake b/Gems/ROS2PoseControl/Code/Platform/Common/msvc/ros2scriptintegration_msvc.cmake
@@ -0,0 +1,4 @@
+set(LY_COMPILE_OPTIONS
+        PRIVATE
+            /EHsc
+)
diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.cpp
@@ -43,10 +43,11 @@ namespace ROS2PoseControl
     void ROS2PoseControl::Activate()
     {
         auto ros2Node = ROS2::ROS2Interface::Get()->GetNode();
+        AZ_Assert(ros2Node, "ROS2PoseControl requires a valid ROS 2 node.")
+        m_tf_buffer = std::make_unique<tf2_ros::Buffer>(ros2Node->get_clock());
+        m_tf_listener = std::make_shared<tf2_ros::TransformListener>(*m_tf_buffer);
         if (m_configuration.m_tracking_mode == ROS2PoseControlConfiguration::TrackingMode::TF2)
         {
-            m_tf_buffer = std::make_unique<tf2_ros::Buffer>(ros2Node->get_clock());
-            m_tf_listener = std::make_shared<tf2_ros::TransformListener>(*m_tf_buffer);
             AZ::TickBus::Handler::BusConnect();
         }
         else if (m_configuration.m_tracking_mode == ROS2PoseControlConfiguration::TrackingMode::PoseMessages)
@@ -90,27 +91,44 @@ namespace ROS2PoseControl
         }
     }
 
-    AZ::Outcome<AZ::Transform, const char*> ROS2PoseControl::GetCurrentTransformViaTF2()
+    AZ::Outcome<AZ::Transform, AZStd::string> ROS2PoseControl::GetCurrentTransformViaTF2(
+        const AZStd::string& targetFrame,
+        const AZStd::string& sourceFrame)
     {
         geometry_msgs::msg::TransformStamped transformStamped;
         std::string errorString;
+        bool exceptionThrown = false;
+        const auto targetFrameCStr = targetFrame.c_str();
+        const auto sourceFrameCStr = sourceFrame.c_str();
         if (m_tf_buffer->canTransform(
-                m_configuration.m_referenceFrame.c_str(), m_configuration.m_targetFrame.c_str(), tf2::TimePointZero, &errorString))
+            targetFrameCStr,
+            sourceFrameCStr,
+            tf2::TimePointZero,
+            &errorString))
         {
-            transformStamped = m_tf_buffer->lookupTransform(
-                m_configuration.m_referenceFrame.c_str(), m_configuration.m_targetFrame.c_str(), tf2::TimePointZero);
-            m_tf2WarningShown = false;
+            try
+            {
+                transformStamped = m_tf_buffer->lookupTransform(
+                    targetFrameCStr,
+                    sourceFrameCStr,
+                    tf2::TimePointZero);
+                m_tf2WarningShown = false;
+            } catch (const tf2::TransformException& ex)
+            {
+                errorString = ex.what();
+                exceptionThrown = true;
+            }
         }
-        else
+        if (exceptionThrown || !errorString.empty())
         {
             if (!m_tf2WarningShown)
             {
                 AZ_Warning(
                     "ROS2PositionControl",
                     false,
                     "Could not transform %s to %s, error: %s",
-                    m_configuration.m_targetFrame.c_str(),
-                    m_configuration.m_referenceFrame.c_str(),
+                    targetFrameCStr,
+                    sourceFrameCStr,
                     errorString.c_str());
                 m_tf2WarningShown = true;
             }
@@ -125,7 +143,7 @@ namespace ROS2PoseControl
     {
         if (m_configuration.m_tracking_mode == ROS2PoseControlConfiguration::TrackingMode::TF2)
         {
-            const AZ::Outcome<AZ::Transform, const char*> transform_outcome = GetCurrentTransformViaTF2();
+            const AZ::Outcome<AZ::Transform, AZStd::string> transform_outcome = GetCurrentTransformViaTF2(m_configuration.m_referenceFrame,m_configuration.m_targetFrame);
             if (!transform_outcome.IsSuccess())
             {
                 return;
@@ -147,18 +165,51 @@ namespace ROS2PoseControl
         const auto* ros2_frame_component = m_entity->FindComponent<ROS2::ROS2FrameComponent>();
         auto namespaced_topic_name =
             ROS2::ROS2Names::GetNamespacedName(ros2_frame_component->GetNamespace(), m_configuration.m_poseTopicConfiguration.m_topic);
+        auto frameId = ros2_frame_component->GetFrameID();
         m_poseSubscription = ros2Node->create_subscription<geometry_msgs::msg::PoseStamped>(
             namespaced_topic_name.data(),
             m_configuration.m_poseTopicConfiguration.GetQoS(),
-            [this](const geometry_msgs::msg::PoseStamped::SharedPtr msg)
+            [frameId, this](const geometry_msgs::msg::PoseStamped::SharedPtr msg)
             {
                 if (m_configuration.m_tracking_mode != ROS2PoseControlConfiguration::TrackingMode::PoseMessages || !m_isTracking)
                 {
                     return;
                 }
-                const AZ::Transform transform = ROS2::ROS2Conversions::FromROS2Pose(msg->pose);
-                //                AZ::TransformBus::Event(GetEntityId(), &AZ::TransformBus::Events::SetWorldTM, transform);
-                ApplyTransform(transform);
+                const AZ::Transform requestedTransform = ROS2::ROS2Conversions::FromROS2Pose(msg->pose);
+                AZ::Transform finalTransform;
+                if (msg->header.frame_id.empty())
+                {
+                    finalTransform = requestedTransform;
+                }
+                else if (frameId.compare(msg->header.frame_id.c_str()) == 0)
+                {
+                    auto offsetTransformOptional = GetOffsetTransform(m_configuration.m_startOffsetTag);
+                    AZ::Transform offsetTransform = offsetTransformOptional.has_value() ? offsetTransformOptional.value() : AZ::Transform::CreateIdentity();
+                    offsetTransform.Invert();
+
+                    auto entityTransform = GetEntity()->GetTransform()->GetWorldTM();
+
+                    finalTransform = offsetTransform * entityTransform * requestedTransform;
+                }
+                else
+                {
+                    AZStd::string headerFrameId(msg->header.frame_id.c_str());
+
+                    const AZ::Outcome<AZ::Transform, AZStd::string> transform_outcome = GetCurrentTransformViaTF2(
+                        m_configuration.m_referenceFrame,
+                        headerFrameId);
+                    if (transform_outcome.IsSuccess())
+                    {
+                        finalTransform = transform_outcome.GetValue() * requestedTransform;
+                    }
+                    else
+                    {
+                        AZ_Warning("ROS2PoseControl", true, "No transform found from refrence frame (%s) to requested frame (%s)\n",m_configuration.m_referenceFrame.c_str(), headerFrameId.c_str());
+                        return;
+                    }
+                }
+
+                ApplyTransform(finalTransform);
             });
     }
 
@@ -168,7 +219,10 @@ namespace ROS2PoseControl
         {
             if (m_isTracking)
             {
-                AZ::TickBus::Handler::BusConnect();
+                if (!AZ::TickBus::Handler::BusIsConnected())
+                {
+                    AZ::TickBus::Handler::BusConnect();
+                }
             }
             else
             {
@@ -277,7 +331,7 @@ namespace ROS2PoseControl
         AZ::EBusAggregateResults<AZ::EntityId> aggregator;
         const LmbrCentral::Tag tag = AZ::Crc32(tagName);
         LmbrCentral::TagGlobalRequestBus::EventResult(aggregator, tag, &LmbrCentral::TagGlobalRequests::RequestTaggedEntities);
-        if (!m_groundNotFoundWarningShown)
+        if (!m_startingOffsetNotFoundWarningShown)
         {
             AZ_Warning(
                 "ROS2PoseControl",
@@ -286,11 +340,11 @@ namespace ROS2PoseControl
                 tagName.c_str());
 
             AZ_Warning("ROS2PoseControl", !aggregator.values.empty(), "No entity with tag found %s.", tagName.c_str());
-            m_groundNotFoundWarningShown = aggregator.values.size() != 1;
+            m_startingOffsetNotFoundWarningShown = aggregator.values.size() != 1;
         }
         if (aggregator.values.size() == 1)
         {
-            m_groundNotFoundWarningShown = false;
+            m_startingOffsetNotFoundWarningShown = false;
         }
         if (!aggregator.values.empty())
         {

diff --git a/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h b/Gems/ROS2PoseControl/Code/Source/Clients/ROS2PoseControl.h
@@ -59,8 +59,9 @@ namespace ROS2PoseControl
         // ImGui::ImGuiUpdateListenerBus::Handler overrides
         void OnImGuiUpdate() override;
 
-        [[nodiscard]] AZ::Outcome<AZ::Transform, const char*> GetCurrentTransformViaTF2();
-
+        [[nodiscard]]  AZ::Outcome<AZ::Transform, AZStd::string>
+        GetCurrentTransformViaTF2(const AZStd::string &targetFrame, const AZStd::string &sourceFrame);
+
         void OnTopicConfigurationChanged();
 
         void OnIsTrackingChanged();