vision messages

Signed-off-by: Adam Dabrowski <[email protected]>

Project/Gem/CMakeLists.txt

@@ -49,7 +49,7 @@ ly_add_target(
             Gem::Atom_AtomBridge.Static
 )
 
-target_depends_on_ros2_packages(ROSConDemo.Static std_srvs)
+target_depends_on_ros2_packages(ROSConDemo.Static std_srvs vision_msgs)
 
 ly_add_target(
     NAME ROSConDemo ${PAL_TRAIT_MONOLITHIC_DRIVEN_MODULE_TYPE}

Project/Gem/Source/ApplePicker/AppleDetectionGroundTruth.cpp

@@ -0,0 +1,101 @@
+/*
+ * 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 "AppleDetectionGroundTruth.h"
+#include <ROS2/Frame/ROS2FrameComponent.h>
+#include <ROS2/ROS2Bus.h>
+#include "ROS2/Utilities/ROS2Conversions.h"
+#include <ROS2/Utilities/ROS2Names.h>
+#include <rclcpp/qos.hpp>
+
+using namespace ROS2;
+
+namespace AppleKraken
+{
+    AppleDetectionGroundTruth::AppleDetectionGroundTruth(const AZStd::string& rosNamespace, const AZStd::string& namespacedFrameId)
+        : m_frameId(namespacedFrameId)
+    {
+        auto defaultQoS = rclcpp::QoS(10);
+        auto ros2Node = ROS2Interface::Get()->GetNode();
+
+        m_appleDetections3DMessage.header.frame_id = m_frameId.c_str();
+        AZStd::string full3DTopic = ROS2Names::GetNamespacedName(rosNamespace, "ground_truth_3D_detection");
+        m_detection3DPublisher = ros2Node->create_publisher<vision_msgs::msg::Detection3DArray>(full3DTopic.data(), defaultQoS);
+
+        m_appleDetections2DMessage.header.frame_id = m_frameId.c_str();
+        AZStd::string full2DTopic = ROS2Names::GetNamespacedName(rosNamespace, "ground_truth_2D_detection");
+        m_detection2DPublisher = ros2Node->create_publisher<vision_msgs::msg::Detection2DArray>(full2DTopic.data(), defaultQoS);
+    }
+
+    void AppleDetectionGroundTruth::UpdateGroundTruth(const AZStd::vector<PickAppleTask>& apples)
+    {
+        ConstructROS2Detection2DMessage(apples);
+        ConstructROS2Detection3DMessage(apples);
+    }
+
+    void AppleDetectionGroundTruth::Publish()
+    {
+        auto timestamp = ROS2Interface::Get()->GetROSTimestamp();
+        m_appleDetections2DMessage.header.stamp = timestamp;
+        for (auto& detection : m_appleDetections2DMessage.detections)
+        {
+            detection.header.stamp = timestamp;
+        }
+        m_detection2DPublisher->publish(m_appleDetections2DMessage);
+
+        m_appleDetections3DMessage.header.stamp = timestamp;
+        for (auto& detection : m_appleDetections3DMessage.detections)
+        {
+            detection.header.stamp = timestamp;
+        }
+        m_detection3DPublisher->publish(m_appleDetections3DMessage);
+    }
+
+    vision_msgs::msg::Detection3D AppleDetectionGroundTruth::Construct3DDetection(const PickAppleTask& apple)
+    {
+        vision_msgs::msg::Detection3D detection;
+        detection.header.frame_id = m_frameId.c_str();
+        detection.id = "Apple"; // duh
+        auto middleVector = ROS2Conversions::ToROS2Vector3(apple.m_middle);
+        detection.bbox.center.position.x = middleVector.x;
+        detection.bbox.center.position.y = middleVector.y;
+        detection.bbox.center.position.z = middleVector.z;
+        detection.bbox.size = ROS2Conversions::ToROS2Vector3(apple.m_appleBoundingBox.GetExtents());
+        return detection;
+    }
+
+    void AppleDetectionGroundTruth::ConstructROS2Detection3DMessage(const AZStd::vector<PickAppleTask>& apples)
+    {
+        m_appleDetections3DMessage.detections.clear();
+        for (const auto& apple : apples)
+        {
+            m_appleDetections3DMessage.detections.push_back(Construct3DDetection(apple));
+        }
+    }
+
+    vision_msgs::msg::Detection2D AppleDetectionGroundTruth::Construct2DDetection(const PickAppleTask& apple)
+    {
+        vision_msgs::msg::Detection2D detection;
+        detection.header.frame_id = m_frameId.c_str();
+        detection.id = "Apple";
+        detection.bbox.center.position.x = apple.m_middle.GetX();
+        detection.bbox.center.position.y = apple.m_middle.GetY();
+        detection.bbox.size_x = apple.m_appleBoundingBox.GetExtents().GetX();
+        detection.bbox.size_y = apple.m_appleBoundingBox.GetExtents().GetY();
+        return detection;
+    }
+
+    void AppleDetectionGroundTruth::ConstructROS2Detection2DMessage(const AZStd::vector<PickAppleTask>& apples)
+    {
+        m_appleDetections2DMessage.detections.clear();
+        for (const auto& apple : apples)
+        {
+            m_appleDetections2DMessage.detections.push_back(Construct2DDetection(apple));
+        }
+    }
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/AppleDetectionGroundTruth.h

@@ -0,0 +1,40 @@
+/*
+ * 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 "PickingStructs.h"
+#include <rclcpp/rclcpp.hpp>
+#include <vision_msgs/msg/detection3_d_array.hpp>
+#include <vision_msgs/msg/detection2_d_array.hpp>
+#include <AzCore/std/containers/vector.h>
+
+namespace AppleKraken
+{
+    //! A class which provides ground truth for apples. For now it is only for debug.
+    class AppleDetectionGroundTruth
+    {
+    public:
+        AppleDetectionGroundTruth(const AZStd::string& rosNamespace, const AZStd::string& namespacedFrameId);
+        void UpdateGroundTruth(const AZStd::vector<PickAppleTask>& apples);
+        void Publish();
+
+    private:
+        // TODO - templates would improve it here (code is similar)
+        vision_msgs::msg::Detection3D Construct3DDetection(const PickAppleTask& apple);
+        void ConstructROS2Detection3DMessage(const AZStd::vector<PickAppleTask>& apples);
+
+        vision_msgs::msg::Detection2D Construct2DDetection(const PickAppleTask& apple);
+        void ConstructROS2Detection2DMessage(const AZStd::vector<PickAppleTask>& apples);
+
+        AZStd::string m_frameId; //!< includes namespace
+        std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::Detection3DArray>> m_detection3DPublisher;
+        std::shared_ptr<rclcpp::Publisher<vision_msgs::msg::Detection2DArray>> m_detection2DPublisher;
+        vision_msgs::msg::Detection3DArray m_appleDetections3DMessage;
+        vision_msgs::msg::Detection2DArray m_appleDetections2DMessage;
+    };
+} // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickerComponent.cpp

@@ -106,6 +106,7 @@ namespace AppleKraken
     void ApplePickerComponent::OnTick([[maybe_unused]] float deltaTime, [[maybe_unused]] AZ::ScriptTimePoint time)
     {
         // TODO handle timeouts and incoming commands
+        m_appleGroundTruthDetector->Publish();
     }
 
     float ApplePickerComponent::ReportProgress()
@@ -131,7 +132,8 @@ namespace AppleKraken
         AZ::TickBus::Handler::BusConnect();
 
         auto ros2Node = ROS2Interface::Get()->GetNode();
-        auto robotNamespace = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity())->GetNamespace();
+        auto frame = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
+        auto robotNamespace = frame->GetNamespace();
         auto topic = ROS2Names::GetNamespacedName(robotNamespace, m_triggerServiceTopic);
         m_triggerService = ros2Node->create_service<std_srvs::srv::Trigger>(
             topic.c_str(),
@@ -139,10 +141,13 @@ namespace AppleKraken
             {
                 this->ProcessTriggerServiceCall(request, response);
             });
+
+        m_appleGroundTruthDetector = AZStd::make_unique<AppleDetectionGroundTruth>(robotNamespace, frame->GetFrameID());
     }
 
     void ApplePickerComponent::Deactivate()
     {
+        m_appleGroundTruthDetector.reset();
         m_triggerService.reset();
         AZ::TickBus::Handler::BusDisconnect();
         ApplePickingNotificationBus::Handler::BusDisconnect();
@@ -266,6 +271,7 @@ namespace AppleKraken
         // apples need to be unique
         AZStd::unordered_set<AZ::EntityId> found_apples;
 
+        AZStd::vector<PickAppleTask> appleTasks;
         for (auto& physicScene : physicsSystem->GetAllScenes())
         {
             if (!physicScene)
@@ -304,12 +310,26 @@ namespace AppleKraken
                 t.m_appleEntityId = r.m_entityId;
                 t.m_appleBoundingBox = r.m_shape->GetAabb(targetTM);
                 t.m_middle = targetTM.GetTranslation(); /// TODO consider `r.m_position` here
-                m_currentAppleTasks.push(t);
+                appleTasks.push_back(t);
                 found_apples.emplace(r.m_entityId);
             }
         }
+
+        std::sort(
+            appleTasks.begin(),
+            appleTasks.end(),
+            [globalBox](const PickAppleTask& a, const PickAppleTask& b) -> bool
+            {   // a is further than b from the globalBox
+                return globalBox.GetDistance(a.m_middle) > globalBox.GetDistance(b.m_middle);
+            });
+
+        m_appleGroundTruthDetector->UpdateGroundTruth(appleTasks);
+        for (const auto& appleTask : appleTasks)
+        {
+            m_currentAppleTasks.emplace(appleTask);
+        }
+
         m_initialTasksSize = m_currentAppleTasks.size();
         AZ_Printf("ApplePickerComponent", "There are %d apples in reach box \n", m_currentAppleTasks.size());
     }
-
 } // namespace AppleKraken

Project/Gem/Source/ApplePicker/ApplePickerComponent.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include "ApplePickingNotifications.h"
+#include "AppleDetectionGroundTruth.h"
 #include "ApplePickingRequests.h"
 #include <AzCore/Component/Component.h>
 #include <AzCore/Component/TickBus.h>
@@ -62,5 +63,7 @@ namespace AppleKraken
         rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_triggerService;
         size_t m_initialTasksSize = 0;
         AZStd::queue<PickAppleTask> m_currentAppleTasks;
+
+        AZStd::unique_ptr<AppleDetectionGroundTruth> m_appleGroundTruthDetector;
     };
 } // namespace AppleKraken

Project/Gem/roscondemo_files.cmake

@@ -1,6 +1,8 @@
 
 set(FILES
         Include/ROSConDemo/ROSConDemoBus.h
+        Source/ApplePicker/AppleDetectionGroundTruth.cpp
+        Source/ApplePicker/AppleDetectionGroundTruth.h
         Source/ApplePicker/ApplePickerComponent.cpp
         Source/ApplePicker/ApplePickerComponent.h
         Source/ApplePicker/ApplePickingNotifications.h