udpates regarding framed box loactions

source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt

@@ -2,6 +2,7 @@ add_subdirectory(core)
 
 add_subdirectory(util/LocationLoader)
 add_subdirectory(util/AffordanceLoader)
+add_subdirectory(util/CommonPlaceLoader)
 
 add_subdirectory(motions)
 add_subdirectory(objects)

source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt

+set(LIB_NAME       ${PROJECT_NAME}PriorKnowledgeCommonPlaceLoaderUtil)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS
+        SimoxUtility
+        RobotAPI::Core
+        RobotAPI::Aron::Common
+    SOURCES
+        datatypes/CommonPlace.cpp
+        CommonPlaceLoader.cpp
+    HEADERS
+        datatypes/CommonPlace.h
+        CommonPlaceLoader.h
+)
+
+add_library(${PROJECT_NAME}::PriorKnowledge::util::CommonPlaceLoader ALIAS ${PROJECT_NAME}PriorKnowledgeCommonPlaceLoaderUtil)
+
+# add unit tests
+#add_subdirectory(test)

source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp

+#include "CommonPlaceLoader.h"
+
+#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+namespace armarx::priorknowledge::util
+{
+
+    std::vector<CommonPlace>
+    CommonPlaceLoader::LoadCommonPlaces(const std::string& source, const nlohmann::json& js)
+    {
+        std::vector<CommonPlace> ret;
+        if (not js.contains("common_places"))
+        {
+            ARMARX_WARNING << "The common_places file has the wrong structure. Missing key "
+                              "'common_places'.";
+            return ret;
+        }
+
+        for (const auto& [locationName, priority] :
+             js["common_places"].get<std::map<std::string, int>>())
+        {
+            CommonPlace a{source, locationName, priority};
+            ret.push_back(a);
+        }
+        return ret;
+    }
+} // namespace armarx::priorknowledge::util

source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h

+#pragma once
+
+#include <fstream>
+#include <string>
+#include <vector>
+
+#include <SimoxUtility/json.h>
+
+#include "datatypes/CommonPlace.h"
+
+namespace armarx::priorknowledge::util
+{
+    class CommonPlaceLoader
+    {
+    public:
+        static const constexpr auto DEFAULT_FILE_NAME = "common_places.json";
+
+        CommonPlaceLoader() = delete;
+
+        static std::vector<CommonPlace> LoadCommonPlaces(const std::string& source,
+                                                         const nlohmann::json&);
+    };
+} // namespace armarx::priorknowledge::util

source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp

+#include "CommonPlace.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+
+} // namespace armarx::priorknowledge::util

source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h

+#pragma once
+
+#include <string>
+
+namespace armarx::priorknowledge::util
+{
+    struct CommonPlace
+    {
+        std::string source;
+        std::string locationName;
+        int priority;
+    };
+
+} // namespace armarx::priorknowledge::util

source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp

@@ -53,8 +53,8 @@ namespace armarx::priorknowledge::util
             pose,
             framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
 
-        FramedLocationPtr loc(new FramedLocation{
-            {{source, locationName}, LocationType::FRAMED_LOCATION}, frame, agent, pose});
+        FramedLocationPtr loc(new FramedLocation(
+            LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
         return loc;
     }
 
@@ -71,7 +71,7 @@ namespace armarx::priorknowledge::util
         std::string frame = framedOrientedBox.at("frame");
         std::string agent = framedOrientedBox.at("agent");
         Eigen::Matrix4f pose;
-        Eigen::Vector3f extend;
+        Eigen::Vector3f extents;
 
         // sanitize frame if not set
         if (frame.empty())
@@ -108,12 +108,15 @@ namespace armarx::priorknowledge::util
 
         // Utilize ice structure of eigen
         armarx::core::eigen::fromIce(
-            extend,
-            framedOrientedBox.at("extend").get<std::vector<float>>()); // load the 4x4 matrix
-
-        FramedBoxedLocationPtr loc(new FramedBoxedLocation{
-            {{{source, locationName}, LocationType::FRAMED_BOXED_LOCATION}, frame, agent, pose},
-            extend});
+            extents,
+            framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
+
+        FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
+                                                           LocationType::FRAMED_BOXED_LOCATION,
+                                                           frame,
+                                                           agent,
+                                                           pose,
+                                                           extents));
         return loc;
     }
 

source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp

 #include "Visu.h"
 
+#include <ArmarXCore/core/logging/Logging.h>
+
 namespace armarx::priorknowledge::util::location
 {
     void
@@ -25,6 +27,25 @@ namespace armarx::priorknowledge::util::location
                       .color(this->settings.framedBoxedLocationColor));
     }
 
+    viz::Layer
+    Visu::locationsToLayer(const std::string& layerName,
+                           const std::map<std::string, FramedLocationData>& locationData) const
+    {
+        auto layer = arviz.layer(layerName);
+        for (auto& [id, data] : locationData)
+        {
+            if (data.extents.has_value())
+            {
+                addFramedBoxedLocationToLayer(layer, id, data.globalPose, data.extents.value());
+            }
+            else
+            {
+                addFramedLocationToLayer(layer, id, data.globalPose);
+            }
+        }
+        return layer;
+    }
+
     viz::Layer
     Visu::framedLocationsToLayer(
         const std::string& layerName,
@@ -43,10 +64,10 @@ namespace armarx::priorknowledge::util::location
     Visu::framedBoxedLocationsToLayer(
         const std::string& layerName,
         const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
-            locationGlobalPoses) const
+            locationGlobalPosesAndExtends) const
     {
         auto layer = arviz.layer(layerName);
-        for (auto& [id, pair] : locationGlobalPoses)
+        for (auto& [id, pair] : locationGlobalPosesAndExtends)
         {
             const auto& pose = pair.first;
             const auto& extends = pair.second;

source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h

@@ -7,15 +7,27 @@
 
 namespace armarx::priorknowledge::util::location
 {
+
     class Visu
     {
     public:
+        struct FramedLocationData
+        {
+            Eigen::Matrix4f globalPose;
+            std::optional<Eigen::Vector3f> extents;
+        };
+
         Visu(viz::Client& arviz) : arviz(arviz)
+
         {
         }
 
         ~Visu() = default;
 
+        viz::Layer
+        locationsToLayer(const std::string& layerName,
+                         const std::map<std::string, FramedLocationData>& locationData) const;
+
         viz::Layer framedLocationsToLayer(
             const std::string& layerName,
             const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
@@ -23,7 +35,7 @@ namespace armarx::priorknowledge::util::location
         viz::Layer framedBoxedLocationsToLayer(
             const std::string& layerName,
             const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
-                locationGlobalPoses) const;
+                locationGlobalPosesAndExtends) const;
 
     protected:
         void addFramedLocationToLayer(viz::Layer&,

source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h

@@ -20,15 +20,22 @@ namespace armarx::priorknowledge::util
         std::string source;
         std::string name;
 
+        LocationId(const std::string& s, const std::string& n) : source(s), name(n)
+        {
+        }
+
         std::string toString() const;
     };
 
     struct Location
     {
         LocationId id;
-
         LocationType type;
 
+        Location(const LocationId& i, const LocationType t) : id(i), type(t)
+        {
+        }
+
         virtual ~Location() = default;
     };
 
@@ -38,6 +45,15 @@ namespace armarx::priorknowledge::util
         std::string agent;
         Eigen::Matrix4f pose;
 
+        FramedLocation(const LocationId& i,
+                       const LocationType t,
+                       const std::string& f,
+                       const std::string& a,
+                       const Eigen::Matrix4f& p) :
+            Location(i, t), frame(f), agent(a), pose(p)
+        {
+        }
+
         virtual ~FramedLocation() = default;
 
         armarx::FramedPose toFramedPose();
@@ -45,7 +61,17 @@ namespace armarx::priorknowledge::util
 
     struct FramedBoxedLocation : public FramedLocation
     {
-        Eigen::Vector3f extends;
+        Eigen::Vector3f extents;
+
+        FramedBoxedLocation(const LocationId& i,
+                            const LocationType t,
+                            const std::string& f,
+                            const std::string& a,
+                            const Eigen::Matrix4f& p,
+                            const Eigen::Vector3f& e) :
+            FramedLocation(i, t, f, a, p), extents(e)
+        {
+        }
 
         virtual ~FramedBoxedLocation() = default;