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/README b/source/RobotAPI/libraries/PriorKnowledge/README
new file mode 100644
index 0000000000000000000000000000000000000000..bf66dd168ff03dd38799072d053c7b96b7b270a3
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/README
@@ -0,0 +1 @@
+This one should be moved to prior knowledge data itself, shouldnt it? (as optional dependency for robotapi)
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
index 7283ac4abae3f8a64d659c9b92430c9b610715f7..2e9db269c52a2e418e609ed0e0f79f95734f50bf 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.cpp
@@ -6,42 +6,41 @@
namespace armarx::priorknowledge::util
{
std::vector<StaticAffordance>
- AffordanceLoader::LoadStaticAffordances(const std::string& sourceId, const nlohmann::json& js)
+ AffordanceLoader::LoadAffordances(const std::string& source, const nlohmann::json& js)
{
std::vector<StaticAffordance> ret;
- if (not js.contains("static_affordances"))
+ if (not js.contains("affordances"))
{
ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
- "'static_affordances'.";
+ "'affordances'.";
return ret;
}
- for (const auto& affordance : js["static_affordances"].get<std::vector<std::string>>())
+ for (const auto& affordance : js["affordances"].get<std::vector<std::string>>())
{
- StaticAffordance a(sourceId, affordance);
+ StaticAffordance a{{source, affordance}};
ret.push_back(a);
}
return ret;
}
- std::vector<LocatableAffordance>
- AffordanceLoader::LoadLocatableAffordances(const std::string& sourceId,
- const nlohmann::json& js)
+ std::vector<LocationAffordance>
+ AffordanceLoader::LoadLocationAffordances(const std::string& source, const nlohmann::json& js)
{
- std::vector<LocatableAffordance> ret;
- if (not js.contains("locatable_affordances"))
+ std::vector<LocationAffordance> ret;
+ if (not js.contains("location_affordances"))
{
ARMARX_WARNING << "The affordances file has the wrong structure. Missing key "
- "'locatable_affordances'.";
+ "'location_affordances'.";
return ret;
}
- for (const auto& [affordance, locationIds] :
- js["locatable_affordances"].get<std::map<std::string, std::vector<std::string>>>())
+ for (const auto& [locationName, affordances] :
+ js["location_affordances"].get<std::map<std::string, std::vector<std::string>>>())
{
- for (const auto& locationId : locationIds)
+ for (const auto& affordance : affordances)
{
- LocatableAffordance a(sourceId, affordance, locationId);
+ LocationAffordance a{{source, affordance}, locationName};
ret.push_back(a);
}
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
index 71696abc97db46be35c9475d9ccc7506ed0933dd..b8f0b9f6e0cf7a4ec7d64eab6a650c7afb90ad1b 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/AffordanceLoader.h
@@ -14,13 +14,14 @@ namespace armarx::priorknowledge::util
{
public:
static const constexpr auto DEFAULT_FILE_NAME = "affordances.json";
+ static const constexpr auto DEFAULT_LOCATION_FILE_NAME = "location_affordances.json";
AffordanceLoader() = delete;
- static std::vector<StaticAffordance> LoadStaticAffordances(const std::string& sourceId,
- const nlohmann::json&);
+ static std::vector<StaticAffordance> LoadAffordances(const std::string& sourceId,
+ const nlohmann::json&);
- static std::vector<LocatableAffordance>
- LoadLocatableAffordances(const std::string& sourceId, const nlohmann::json&);
+ static std::vector<LocationAffordance> LoadLocationAffordances(const std::string& sourceId,
+ const nlohmann::json&);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
index 621ce74f0e41668382bf65ecc992e92890e8d3f5..f28e60c80c3932c083feb25db5b0f93b40266148 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.cpp
@@ -6,28 +6,10 @@
namespace armarx::priorknowledge::util
{
-
- AffordanceId::AffordanceId(const std::string& dataset, const std::string& name) :
- sourceId(dataset), name(name)
- {
- }
-
std::string
AffordanceId::toString() const
{
- return sourceId + "/" + name;
- }
-
- StaticAffordance::StaticAffordance(const std::string& dataset, const std::string& name) :
- id(dataset, name)
- {
- }
-
- LocatableAffordance::LocatableAffordance(const std::string& dataset,
- const std::string& name,
- const std::string& locationId) :
- id(dataset, name), locationId(locationId)
- {
+ return source + "/" + name;
}
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
index ce030b534a4471c1ff33d89478972b3477fe71e0..39eaedc0c86c1fe7a8f3993264c2bd9987ca1601 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/AffordanceLoader/datatypes/Affordance.h
@@ -6,28 +6,21 @@ namespace armarx::priorknowledge::util
{
struct AffordanceId
{
- std::string sourceId;
+ std::string source;
std::string name;
- AffordanceId(const std::string& dataset, const std::string& name);
-
std::string toString() const;
};
struct StaticAffordance
{
AffordanceId id;
-
- StaticAffordance(const std::string& dataset, const std::string& name);
};
- struct LocatableAffordance
+ struct LocationAffordance
{
AffordanceId id;
- std::string locationId;
-
- LocatableAffordance(const std::string& dataset,
- const std::string& name,
- const std::string& locationId);
+ std::string locationName;
};
+
} // namespace armarx::priorknowledge::util
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..ce15a37df98b729bd0a0dc875ccbac13aae2108f
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/CMakeLists.txt
@@ -0,0 +1,27 @@
+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
+
+ ArViz
+ RobotAPIArmarXObjects
+ SOURCES
+ datatypes/CommonPlace.cpp
+ CommonPlaceLoader.cpp
+ Visu.cpp
+ HEADERS
+ datatypes/CommonPlace.h
+ CommonPlaceLoader.h
+ Visu.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/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfd68864ff87c6490af33be5d40645051b0e4fe7
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.cpp
@@ -0,0 +1,24 @@
+#include "Visu.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+
+namespace armarx::priorknowledge::util::common_place
+{
+ viz::Layer
+ Visu::commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& commonPlaceData) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, data] : commonPlaceData)
+ {
+ auto o = armarx::viz::Object("");
+ o = o.fileByObjectFinder(armarx::ObjectID(data.objectId));
+ o.pose(data.globalPose).alpha(0.5);
+ layer.add(o);
+ }
+ return layer;
+ }
+
+} // namespace armarx::priorknowledge::util::common_place
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..e4df667ca056b92db65053834a1fea75b75d8eb9
--- /dev/null
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/CommonPlaceLoader/Visu.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "datatypes/CommonPlace.h"
+
+namespace armarx::priorknowledge::util::common_place
+{
+
+ class Visu
+ {
+ public:
+ struct CommonPlaceData
+ {
+ Eigen::Matrix4f globalPose;
+ std::string objectId;
+ };
+
+ Visu(viz::Client& arviz) : arviz(arviz)
+
+ {
+ }
+
+ ~Visu() = default;
+
+ viz::Layer commonPlaceToLayer(const std::string& layerName,
+ const std::map<std::string, CommonPlaceData>& data) const;
+
+
+ public:
+ struct Settings
+ {
+
+ } settings;
+
+ protected:
+ viz::Client& arviz;
+ };
+} // namespace armarx::priorknowledge::util::common_place
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 47b8601280e5191e99a7f1a4d56fec5aeaf75ab5..cfeb43db57231ff85188667c60773c6d0e30390c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -3,79 +3,154 @@
#include <SimoxUtility/algorithm/string.h>
#include <ArmarXCore/core/eigen/ice_conversions.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
namespace armarx::priorknowledge::util
{
- std::vector<Location>
- LocationLoader::LoadLocations(const std::string& dataset, const nlohmann::json& js)
+ FramedLocationPtr
+ LocationLoader::LoadFramedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j)
{
- std::vector<Location> ret;
- if (not js.contains("locations"))
- {
- ARMARX_WARNING
- << "The locations file has the wrong structure. Missing key 'locations'.";
- return ret;
- }
+ ARMARX_CHECK(j.find("framedPose") != j.end());
- for (const auto& [locationIdStr, j] :
- js["locations"].get<std::map<std::string, nlohmann::json>>())
+ const auto framedPose = j.at("framedPose").get<std::map<std::string, nlohmann::json>>();
+
+ std::string frame = framedPose.at("frame");
+ std::string agent = framedPose.at("agent");
+ Eigen::Matrix4f pose;
+
+ // sanitize frame if not set
+ if (frame.empty())
{
- if (j.find("framedPose") == j.end())
+ if (agent.empty())
{
- ARMARX_WARNING << "The element '" << locationIdStr
- << "' has no 'framedPose' member. Skipping "
- "this entity.";
- continue;
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to '" + GlobalFrame + "'.";
+ frame = GlobalFrame;
+ }
+ else
+ {
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to 'root' because "
+ "an agent name '" +
+ agent + "' was set.";
+ frame = "root";
}
+ }
+
+ if (frame != GlobalFrame && agent.empty())
+ {
+ ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
+ "' for location '" + locationName +
+ "'. This may lead to problems...";
+ // TODO: What to do in that case?
+ }
- const auto framedPose = j.at("framedPose").get<std::map<std::string, nlohmann::json>>();
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- std::string frame = framedPose.at("frame");
- std::string agent = framedPose.at("agent");
- Eigen::Matrix4f pose;
+ FramedLocationPtr loc(new FramedLocation(
+ LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
+ return loc;
+ }
- // sanitize frame if not set
- if (frame.empty())
+ FramedBoxedLocationPtr
+ LocationLoader::LoadBoxedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j)
+ {
+ ARMARX_CHECK(j.find("framedOrientedBox") != j.end());
+
+ const auto framedOrientedBox =
+ j.at("framedOrientedBox").get<std::map<std::string, nlohmann::json>>();
+
+ std::string frame = framedOrientedBox.at("frame");
+ std::string agent = framedOrientedBox.at("agent");
+ Eigen::Matrix4f pose;
+ Eigen::Vector3f extents;
+
+ // sanitize frame if not set
+ if (frame.empty())
+ {
+ if (agent.empty())
{
- if (agent.empty())
- {
- ARMARX_WARNING << "Got empty frame for location '" + locationIdStr +
- "'. Sanitizing it to '" + GlobalFrame + "'.";
- frame = GlobalFrame;
- }
- else
- {
- ARMARX_WARNING << "Got empty frame for location '" + locationIdStr +
- "'. Sanitizing it to 'root' because "
- "an agent name '" +
- agent + "' was set.";
- frame = "root";
- }
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to '" + GlobalFrame + "'.";
+ frame = GlobalFrame;
}
-
- if (frame != GlobalFrame && agent.empty())
+ else
{
- ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
- "' for location '" + locationIdStr +
- "'. This may lead to problems...";
- // TODO: What to do in that case?
+ ARMARX_WARNING << "Got empty frame for location '" + locationName +
+ "'. Sanitizing it to 'root' because "
+ "an agent name '" +
+ agent + "' was set.";
+ frame = "root";
}
+ }
+
+ if (frame != GlobalFrame && agent.empty())
+ {
+ ARMARX_WARNING << "Got an empty agent name but a set frame '" + frame +
+ "' for location '" + locationName +
+ "'. This may lead to problems...";
+ // TODO: What to do in that case?
+ }
+
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ pose,
+ framedOrientedBox.at("pose")
+ .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- // Utilize ice structure of eigen
- armarx::core::eigen::fromIce(
- pose,
- framedPose.at("pose")
- .get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
+ // Utilize ice structure of eigen
+ armarx::core::eigen::fromIce(
+ extents,
+ framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
- armarx::FramedPose fp(pose, frame, agent);
+ FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
+ LocationType::FRAMED_BOXED_LOCATION,
+ frame,
+ agent,
+ pose,
+ extents));
+ return loc;
+ }
- // escape locationIdStr
- auto locationIdStrSplitted = simox::alg::split(locationIdStr, "#");
- std::string locationIdStrEscaped = simox::alg::trim_copy(locationIdStrSplitted[0]);
+ std::vector<LocationPtr>
+ LocationLoader::LoadLocations(const std::string& source /* e.g. dataset/class or graph */,
+ const nlohmann::json& js)
+ {
+ std::vector<LocationPtr> ret;
+ if (not js.contains("locations"))
+ {
+ ARMARX_WARNING
+ << "The locations file has the wrong structure. Missing key 'locations'.";
+ return ret;
+ }
- Location loc{{dataset, fp.agent, locationIdStrEscaped}, fp};
- ret.push_back(loc);
+ for (const auto& [locationName, j] :
+ js["locations"].get<std::map<std::string, nlohmann::json>>())
+ {
+ if (j.find("framedPose") != j.end())
+ {
+ ret.push_back(LocationLoader::LoadFramedLocation(source, locationName, j));
+ }
+ else if (j.find("framedOrientedBox") != j.end())
+ {
+ ret.push_back(LocationLoader::LoadBoxedLocation(source, locationName, j));
+ }
+ else
+ {
+ ARMARX_WARNING
+ << "The element '" << locationName
+ << "' has no 'framedPose' member or no 'framedOrientedBox' member. Skipping "
+ "this entity.";
+ continue;
+ }
}
return ret;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
index 70f66236bc4d8c4f286a669c6199caf64cf2ae97..b480f98922f78127aa395a34439ec13f8fb2279d 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.h
@@ -17,7 +17,20 @@ namespace armarx::priorknowledge::util
LocationLoader() = delete;
- static std::vector<Location> LoadLocations(const std::string& dataset,
- const nlohmann::json&);
+ // static std::vector<FramedLocation> LoadFramedLocations(const std::string& dataset,
+ // const nlohmann::json&);
+ // static std::vector<FramedBoxedLocation> LoadBoxedLocations(const std::string& dataset,
+ // const nlohmann::json&);
+ static std::vector<LocationPtr> LoadLocations(const std::string& dataset,
+ const nlohmann::json&);
+
+ private:
+ static FramedLocationPtr LoadFramedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j);
+
+ static FramedBoxedLocationPtr LoadBoxedLocation(const std::string& source,
+ const std::string& locationName,
+ const nlohmann::json& j);
};
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
index 6935d58403c62e1b74dd9361da955f6236519554..80dfd636de50f94003f673cb47114c86afdcb844 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.cpp
@@ -1,23 +1,83 @@
#include "Visu.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
namespace armarx::priorknowledge::util::location
{
- armarx::viz::Pose
- Visu::getLocationVertex(const std::string& id, const Eigen::Matrix4f& locationGlobalPose) const
+ void
+ Visu::addFramedLocationToLayer(viz::Layer& layer,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose) const
+ {
+ // Add global location to layer
+ layer.add(armarx::viz::Pose(id).pose(locationGlobalPose));
+ layer.add(armarx::viz::Arrow(id + "_dir")
+ .pose(locationGlobalPose)
+ .length(110)
+ .width(7.5)
+ .color(this->settings.framedLocationArrowColor));
+ }
+
+ void
+ Visu::addFramedBoxedLocationToLayer(viz::Layer& layer,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose,
+ const Eigen::Vector3f& extends) const
{
// Add global location to layer
- return armarx::viz::Pose(id).pose(locationGlobalPose).scale(settings.vertexScale);
+ layer.add(armarx::viz::Box(id)
+ .pose(locationGlobalPose)
+ .size(extends)
+ .color(this->settings.framedBoxedLocationColor));
}
viz::Layer
Visu::locationsToLayer(const std::string& layerName,
- const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
+ 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,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const
{
auto layer = arviz.layer(layerName);
for (auto& [id, pose] : locationGlobalPoses)
{
// Add global location to layer
- layer.add(getLocationVertex(id, pose));
+ addFramedLocationToLayer(layer, id, pose);
+ }
+ return layer;
+ }
+
+ viz::Layer
+ Visu::framedBoxedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
+ locationGlobalPosesAndExtends) const
+ {
+ auto layer = arviz.layer(layerName);
+ for (auto& [id, pair] : locationGlobalPosesAndExtends)
+ {
+ const auto& pose = pair.first;
+ const auto& extends = pair.second;
+
+ // Add global location to layer
+ addFramedBoxedLocationToLayer(layer, id, pose, extends);
}
return layer;
}
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
index 33abc90e7d3e8560f93b96f226f8542692b33a7c..a237c48b3ee1ae78bce651ab887f37c3aa2569df 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/Visu.h
@@ -7,25 +7,55 @@
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;
- armarx::viz::Pose getLocationVertex(const std::string& id,
- const Eigen::Matrix4f& locationGlobalPose) const;
-
viz::Layer
locationsToLayer(const std::string& layerName,
- const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
+ const std::map<std::string, FramedLocationData>& locationData) const;
+
+ viz::Layer framedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, Eigen::Matrix4f>& locationGlobalPoses) const;
+
+ viz::Layer framedBoxedLocationsToLayer(
+ const std::string& layerName,
+ const std::map<std::string, std::pair<Eigen::Matrix4f, Eigen::Vector3f>>&
+ locationGlobalPosesAndExtends) const;
+ protected:
+ void addFramedLocationToLayer(viz::Layer&,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose) const;
+
+ void addFramedBoxedLocationToLayer(viz::Layer&,
+ const std::string& id,
+ const Eigen::Matrix4f& locationGlobalPose,
+ const Eigen::Vector3f& extends) const;
+
+ public:
struct Settings
{
- float vertexScale = 1.0;
+ // FramedLocation
+ viz::Color framedLocationArrowColor = viz::Color::green();
+
+ // FramedBoxedLocation
+ viz::Color framedBoxedLocationColor = viz::Color(0, 150, 130, 40);
+
} settings;
protected:
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
index ea08e20229bb09989f2b4239a529d9b107a3e0f2..56617061dfc387e3bc74d6247134a00dfae799fa 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.cpp
@@ -9,17 +9,7 @@ namespace armarx::priorknowledge::util
std::string
LocationId::toString() const
{
- return sourceId + "/" + toStringWithoutSourceId();
- }
-
- std::string
- LocationId::toStringWithoutSourceId() const
- {
- if (! prefix.empty())
- {
- return prefix + "/" + name;
- }
- return name;
+ return source + "/" + name;
}
} // namespace armarx::priorknowledge::util
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index 202b7734a2b7dea90e37d1df1f4a6886ba162b18..2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -1,24 +1,85 @@
#pragma once
+#include <memory>
#include <string>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::priorknowledge::util
{
+ enum class LocationType
+ {
+ FRAMED_LOCATION,
+ FRAMED_BOXED_LOCATION
+ };
+
struct LocationId
{
- std::string sourceId;
- std::string prefix;
+ std::string source;
std::string name;
+ LocationId(const std::string& s, const std::string& n) : source(s), name(n)
+ {
+ }
+
std::string toString() const;
- std::string toStringWithoutSourceId() const;
};
struct Location
{
LocationId id;
- armarx::FramedPose pose;
+ LocationType type;
+
+ Location(const LocationId& i, const LocationType t) : id(i), type(t)
+ {
+ }
+
+ virtual ~Location() = default;
};
+
+ struct FramedLocation : public Location
+ {
+ std::string frame;
+ 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();
+ };
+
+ struct FramedBoxedLocation : public FramedLocation
+ {
+ 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;
+
+ simox::OrientedBox<float> toOrientedBox();
+ };
+
+ using FramedBoxedLocationPtr = std::unique_ptr<FramedBoxedLocation>;
+ using FramedLocationPtr = std::unique_ptr<FramedLocation>;
+ using LocationPtr = std::unique_ptr<Location>;
+
} // namespace armarx::priorknowledge::util