diff --git a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
index 73984b1d1c3f6539e384e02668e76e264f30565c..03ff54827ffe615760598e6e4d76bfb0e44b7d30 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/CMakeLists.txt
+++ b/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)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e3467eb9a250af792c5c53d578b0555b49a95efa
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
@@ -0,0 +1,22 @@
+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)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8c05ffe2e56883568f00621c8661a7a631d722f
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.cpp
@@ -0,0 +1,28 @@
+#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
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6ee9021210c5698008f578deb6f54d232eb4385
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CommonPlaceLoader.h
@@ -0,0 +1,23 @@
+#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
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de43258cf4040b1aa95fdd3ae9a930d767f66a06
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.cpp
@@ -0,0 +1,10 @@
+#include "CommonPlace.h"
+
+#include <SimoxUtility/algorithm/string.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx::priorknowledge::util
+{
+
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h
new file mode 100644
index 0000000000000000000000000000000000000000..b95e5a236be3647eebc5b5c70868d27b21c1cccd
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/datatypes/CommonPlace.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <string>
+
+namespace armarx::priorknowledge::util
+{
+ struct CommonPlace
+ {
+ std::string source;
+ std::string locationName;
+ int priority;
+ };
+
+} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
index 012ba1eda7dfd2664df7e3697f9f59fdfe3fcaf9..cfeb43db57231ff85188667c60773c6d0e30390c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/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;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
index 7503e7f79c32c5f07243f6c3966d84fde7f37d7c..07901817bebccb45ed4f74fb9aa21cb4234b88f0 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -1,5 +1,7 @@
#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;
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
index d9bcace63886e802000a0053db81fce13b9fea4e..7922e307b3644c00eeb6c984969b0d70c451e5bc 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
+++ b/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&,
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index a34b30990a1ab89af02274d1bd568608fc658e64..2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/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;