adding HumanPoseReader and native types for human pose

source/VisionX/libraries/armem_human/CMakeLists.txt

@@ -21,11 +21,14 @@ armarx_add_library(
     SOURCES
         aron_conversions.cpp
         json_conversions.cpp
+        client/HumanPoseReader.cpp
 
     HEADERS
+        types.h
         aron_conversions.h
         forward_declarations.h
         json_conversions.h
+        client/HumanPoseReader.h
 
     ARON_FILES
         aron/BODY_25Pose.xml

source/VisionX/libraries/armem_human/aron_conversions.cpp

@@ -21,8 +21,69 @@
  */
 
 #include "aron_conversions.h"
+#include "VisionX/libraries/armem_human/types.h"
 
 namespace armarx::armem::server::human
 {
 
 }
+
+
+
+namespace armarx::armem::human
+{
+    void fromAron(const ::armarx::human::arondto::Body25Pose3D& dto, HumanPose& bo)
+    {
+      fromAron(dto.keypoints2D, bo.keypoint2dMap);
+      fromAron(dto.keypoints3D, bo.keypoint3dMap);
+    }
+
+    void toAron(::armarx::human::arondto::Body25Pose3D& dto, const HumanPose& bo)
+    {
+      // FIXME implement
+    }
+
+    void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint3D>& dto, armarx::armem::human::Keypoint3DIdMap& bo)
+    {
+        for(const auto& [k, v]: dto)
+        {
+          Keypoint3D kp;
+          fromAron(v, kp);
+          bo[k] = kp;
+        }
+    }
+
+    void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint2D>& dto, armarx::armem::human::Keypoint2DIdMap& bo)
+    {
+      for(const auto& [k, v]: dto)
+        {
+          Keypoint2D kp;
+          fromAron(v, kp);
+          bo[k] = kp;
+        }
+    }
+
+    void fromAron(const armarx::human::arondto::Keypoint2D& dto, Keypoint2D& bo)
+    {
+       bo = Keypoint2D
+       {
+          .label = dto.label,
+          .position = Eigen::Vector2f{dto.x, dto.y},
+          .confidence = dto.confidence,
+          .dominantColor = simox::color::Color{dto.dominantColor.r, dto.dominantColor.g, dto.dominantColor.b}
+       };
+    }
+
+    void fromAron(const armarx::human::arondto::Keypoint3D& dto, Keypoint3D& bo)
+    {
+      bo = Keypoint3D
+       { 
+          .label = dto.label,
+          .positionRobot = dto.positionRobot.position,
+          .positionGlobal = dto.positionGlobal.position,
+          .confidence = dto.confidence,
+          .dominantColor = simox::color::Color{dto.dominantColor.r, dto.dominantColor.g, dto.dominantColor.b}
+       };
+    }
+
+}

source/VisionX/libraries/armem_human/aron_conversions.h

@@ -22,6 +22,7 @@
 
 #pragma once
 
+#include "VisionX/libraries/armem_human/types.h"
 #include <RobotAPI/libraries/aron/core/data/variant/forward_declarations.h>
 #include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
 #include <VisionX/libraries/armem_human/aron/PersonProfile.aron.generated.h>
@@ -31,3 +32,16 @@ namespace armarx::armem::server::human
 {
 
 }
+
+namespace armarx::armem::human
+{
+    void fromAron(const ::armarx::human::arondto::Body25Pose3D& dto, HumanPose& bo);
+    void toAron(::armarx::human::arondto::Body25Pose3D& dto, const HumanPose& bo);
+
+    void fromAron(const armarx::human::arondto::Keypoint2D& dto, Keypoint2D& bo);
+    void fromAron(const armarx::human::arondto::Keypoint3D& dto, Keypoint3D& bo);
+
+    void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint3D>& dto, armarx::armem::human::Keypoint3DIdMap& bo);
+    void fromAron(const std::map<std::string, armarx::human::arondto::Keypoint2D>& dto, armarx::armem::human::Keypoint2DIdMap& bo);
+    
+}

source/VisionX/libraries/armem_human/client/HumanPoseReader.cpp

+#include "HumanPoseReader.h"
+
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "VisionX/libraries/armem_human/aron_conversions.h"
+#include <VisionX/libraries/armem_human/aron/BODY_25Pose.aron.generated.h>
+
+
+namespace armarx::armem::human::client
+{
+    Reader::~Reader() = default;
+
+    armarx::armem::client::query::Builder
+    Reader::buildQuery(const Query& query) const
+    {
+        armarx::armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties().coreSegmentName)
+        .providerSegments().withName(query.providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(query.timestamp);
+        // clang-format on
+
+        return qb;
+    }
+
+    std::string
+    Reader::propertyPrefix() const
+    {
+        return "mem.vision.occupancy_grid.";
+    }
+
+    armarx::armem::client::util::SimpleReaderBase::Properties
+    Reader::defaultProperties() const
+    {
+        return {.memoryName = "Vision", .coreSegmentName = "OccupancyGrid"};
+    }
+
+    std::vector<HumanPose>
+    asHumanPoses(const wm::ProviderSegment& providerSegment)
+    {
+        ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+        ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+        std::vector<HumanPose> humanPoses;
+        providerSegment.forEachEntity(
+            [&](const wm::Entity& entity)
+            {
+                const auto& entitySnapshot = entity.getLatestSnapshot();
+                ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+                const auto* entityInstance = &entitySnapshot.getInstance(0);
+
+                ARMARX_CHECK_NOT_NULL(entityInstance);
+
+                const auto aronDto = tryCast<armarx::human::arondto::Body25Pose3D>(*entityInstance);
+                ARMARX_CHECK(aronDto) << "Failed casting to Body25Pose3D";
+
+                HumanPose humanPose;
+                fromAron(*aronDto, humanPose);
+
+                humanPoses.push_back(humanPose);
+            });
+
+        return humanPoses;
+    }
+
+    Reader::Result
+    Reader::query(const Query& query) const
+    {
+        const auto qb = buildQuery(query);
+
+        ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+        const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+        if (not qResult.success)
+        {
+            ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+            return {.humanPoses = {},
+                    .status = Result::Status::Error,
+                    .errorMessage = qResult.errorMessage};
+        }
+
+        const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+        if (not coreSegment.hasProviderSegment(query.providerName))
+        {
+            ARMARX_WARNING << "Provider segment `" << query.providerName
+                           << "` does not exist (yet).";
+            return {.humanPoses = {}, .status = Result::Status::NoData};
+        }
+
+        const wm::ProviderSegment& providerSegment =
+            coreSegment.getProviderSegment(query.providerName);
+
+        if (providerSegment.empty())
+        {
+            ARMARX_WARNING << "No entities.";
+            return {
+                .humanPoses = {}, .status = Result::Status::NoData, .errorMessage = "No entities"};
+        }
+
+        try
+        {
+            const auto humanPoses = asHumanPoses(providerSegment);
+            return Result{.humanPoses = humanPoses, .status = Result::Status::Success};
+        }
+        catch (...)
+        {
+            return Result{.status = Result::Status::Error,
+                          .errorMessage = GetHandledExceptionString()};
+        }
+    }
+
+} // namespace armarx::armem::human::client

source/VisionX/libraries/armem_human/client/HumanPoseReader.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <VisionX/libraries/armem_human/types.h>
+
+namespace armarx::armem::human::client
+{
+
+    class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+        ~Reader() override;
+
+        struct Query
+        {
+            std::string providerName;
+            armem::Time timestamp;
+        };
+
+        struct Result
+        {
+            std::vector<HumanPose> humanPoses{};
+
+            enum Status
+            {
+                Success,
+                NoData,
+                Error
+            } status;
+
+            std::string errorMessage = "";
+
+            operator bool() const noexcept
+            {
+                return status == Status::Success;
+            }
+        };
+
+        Result query(const Query& query) const;
+
+        ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+    protected:
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+} // namespace armarx::armem::human::client

source/VisionX/libraries/armem_human/types.h

+#pragma once
+
+#include <Eigen/Core>
+#include <SimoxUtility/color/Color.h>
+#include <map>
+
+namespace armarx::armem::human {
+
+struct Keypoint2D {
+  std::string label;
+
+  Eigen::Vector2f position; // which frame?
+
+  float confidence;
+
+  simox::color::Color dominantColor;
+};
+
+struct Keypoint3D {
+  std::string label;
+
+  Eigen::Vector3f positionRobot; 
+  Eigen::Vector3f positionGlobal;
+
+  float confidence;
+
+  simox::color::Color dominantColor;
+};
+
+using Keypoint2DIdMap = std::map<std::string, Keypoint2D>;
+using Keypoint3DIdMap = std::map<std::string, Keypoint3D>;
+
+struct HumanPose {
+  Keypoint2DIdMap keypoint2dMap;
+  Keypoint3DIdMap keypoint3dMap;
+};
+
+} // namespace armarx::armem::human