diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
index cfeb43db57231ff85188667c60773c6d0e30390c..9a667a933b40e6da8f24810862a79dd934205e6c 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/LocationLoader.cpp
@@ -6,6 +6,8 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/core/json_conversions.h>
+
namespace armarx::priorknowledge::util
{
FramedLocationPtr
@@ -53,8 +55,18 @@ namespace armarx::priorknowledge::util
pose,
framedPose.at("pose").get<std::vector<std::vector<float>>>()); // load the 4x4 matrix
- FramedLocationPtr loc(new FramedLocation(
- LocationId(source, locationName), LocationType::FRAMED_LOCATION, frame, agent, pose));
+ std::optional<Names> names;
+ if (auto it = j.find("names"); it != j.end())
+ {
+ it->get_to(names.emplace());
+ }
+
+ FramedLocationPtr loc(new FramedLocation(LocationId(source, locationName),
+ LocationType::FRAMED_LOCATION,
+ frame,
+ agent,
+ pose,
+ names));
return loc;
}
@@ -111,12 +123,19 @@ namespace armarx::priorknowledge::util
extents,
framedOrientedBox.at("extents").get<std::vector<float>>()); // load the 4x4 matrix
+ std::optional<Names> names;
+ if (auto it = j.find("names"); it != j.end())
+ {
+ it->get_to(names.emplace());
+ }
+
FramedBoxedLocationPtr loc(new FramedBoxedLocation(LocationId(source, locationName),
LocationType::FRAMED_BOXED_LOCATION,
frame,
agent,
pose,
- extents));
+ extents,
+ names));
return loc;
}
@@ -133,7 +152,7 @@ namespace armarx::priorknowledge::util
}
for (const auto& [locationName, j] :
- js["locations"].get<std::map<std::string, nlohmann::json>>())
+ js.at("locations").get<std::map<std::string, nlohmann::json>>())
{
if (j.find("framedPose") != j.end())
{
diff --git a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
index 2a1eccb4cbe72f30ac0da1582c294eb6508ef0f2..f4a05204d706c1598751b34459c7cbdae1a5ca91 100644
--- a/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
+++ b/source/RobotAPI/libraries/PriorKnowledge/util/LocationLoader/datatypes/Location.h
@@ -1,11 +1,13 @@
#pragma once
#include <memory>
+#include <optional>
#include <string>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Names.h>
namespace armarx::priorknowledge::util
{
@@ -31,8 +33,12 @@ namespace armarx::priorknowledge::util
{
LocationId id;
LocationType type;
+ std::optional<Names> names;
- Location(const LocationId& i, const LocationType t) : id(i), type(t)
+ Location(const LocationId& i,
+ const LocationType t,
+ const std::optional<Names>& names = std::nullopt) :
+ id(i), type(t), names(names)
{
}
@@ -45,12 +51,13 @@ 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)
+ FramedLocation(const LocationId& id,
+ const LocationType type,
+ const std::string& frame,
+ const std::string& agent,
+ const Eigen::Matrix4f& pose,
+ const std::optional<Names>& names = std::nullopt) :
+ Location(id, type, names), frame(frame), agent(agent), pose(pose)
{
}
@@ -63,13 +70,14 @@ namespace armarx::priorknowledge::util
{
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)
+ FramedBoxedLocation(const LocationId& id,
+ const LocationType type,
+ const std::string& frame,
+ const std::string& agent,
+ const Eigen::Matrix4f& pose,
+ const Eigen::Vector3f& extents,
+ const std::optional<Names>& names = std::nullopt) :
+ FramedLocation(id, type, frame, agent, pose, names), extents(extents)
{
}
diff --git a/source/RobotAPI/libraries/armem_locations/aron/Location.xml b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
index d53f51c2c8f75bacae2868ae9bc155aad717379e..6b5bebc692a9898e63f508ae07939afd02085e22 100644
--- a/source/RobotAPI/libraries/armem_locations/aron/Location.xml
+++ b/source/RobotAPI/libraries/armem_locations/aron/Location.xml
@@ -1,14 +1,27 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
+ <AronIncludes>
+ <Include include="RobotAPI/libraries/aron/common/aron/Names.xml" />
+ </AronIncludes>
<GenerateTypes>
+ <!--
+ ToDo: Model regions. Ideas:
+ - Polygon (convex, non-convex)
+ -
+ -->
+
<Object name='armarx::navigation::location::arondto::Location'>
<ObjectChild key='framedPose'>
<FramedPose />
</ObjectChild>
+ <ObjectChild key='names'>
+ <armarx::arondto::Names optional="true" />
+ </ObjectChild>
+
</Object>
</GenerateTypes>
diff --git a/source/RobotAPI/libraries/aron/common/CMakeLists.txt b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
index 36cbeb1f837f96ea05b131a843654481b5195bb9..011f3d196f33eb1caba3640c6dce3688bb3b7483 100644
--- a/source/RobotAPI/libraries/aron/common/CMakeLists.txt
+++ b/source/RobotAPI/libraries/aron/common/CMakeLists.txt
@@ -18,14 +18,15 @@ armarx_add_library(
forward_declarations.h
json_conversions.h
+ aron_conversions/armarx.h
aron_conversions/core.h
- aron_conversions/stl.h
- aron_conversions/packagepath.h
+ aron_conversions/eigen.h
aron_conversions/framed.h
- aron_conversions/time.h
- aron_conversions/armarx.h
+ aron_conversions/names.h
+ aron_conversions/packagepath.h
aron_conversions/simox.h
- aron_conversions/eigen.h
+ aron_conversions/stl.h
+ aron_conversions/time.h
json_conversions/armarx.h
json_conversions/framed.h
@@ -37,15 +38,15 @@ armarx_add_library(
util/object_finders.h
SOURCES
-
+ aron_conversions/armarx.cpp
aron_conversions/core.cpp
- aron_conversions/stl.cpp
- aron_conversions/packagepath.cpp
+ aron_conversions/eigen.cpp
aron_conversions/framed.cpp
- aron_conversions/time.cpp
- aron_conversions/armarx.cpp
+ aron_conversions/names.cpp
+ aron_conversions/packagepath.cpp
aron_conversions/simox.cpp
- aron_conversions/eigen.cpp
+ aron_conversions/stl.cpp
+ aron_conversions/time.cpp
json_conversions/armarx.cpp
json_conversions/framed.cpp
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions.h b/source/RobotAPI/libraries/aron/common/aron_conversions.h
index 95177ce57e2b6ab67e0e0e5e4f761646d2775302..c753c3c964ec032276b7a0492cf380ad356329fb 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions.h
@@ -1,6 +1,11 @@
#pragma once
+#include <RobotAPI/libraries/aron/common/aron_conversions/armarx.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/eigen.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/framed.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/names.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/packagepath.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/time.h>
#include <RobotAPI/libraries/aron/core/aron_conversions.h>
-#include "aron_conversions/armarx.h"
-#include "aron_conversions/simox.h"
-#include "aron_conversions/eigen.h"
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0871f1f68a89bea5142f7d17634068e9ac02026d
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp
@@ -0,0 +1,18 @@
+#include "names.h"
+
+#include <RobotAPI/libraries/aron/common/aron/Names.aron.generated.h>
+#include <RobotAPI/libraries/core/Names.h>
+
+void
+armarx::fromAron(const arondto::Names& dto, Names& bo)
+{
+ bo.recognized = dto.recognized;
+ bo.spoken = dto.spoken;
+}
+
+void
+armarx::toAron(arondto::Names& dto, const Names& bo)
+{
+ dto.recognized = bo.recognized;
+ dto.spoken = bo.spoken;
+}
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/names.h b/source/RobotAPI/libraries/aron/common/aron_conversions/names.h
new file mode 100644
index 0000000000000000000000000000000000000000..237e2b20c1e076d55270cd08b756303dfb49d374
--- /dev/null
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/names.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include <SimoxUtility/json/json.h>
+
+#include <RobotAPI/libraries/aron/common/forward_declarations.h>
+#include <RobotAPI/libraries/core/forward_declarations.h>
+
+namespace armarx
+{
+ void fromAron(const arondto::Names& dto, armarx::Names& bo);
+ void toAron(arondto::Names& dto, const armarx::Names& bo);
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/common/forward_declarations.h b/source/RobotAPI/libraries/aron/common/forward_declarations.h
index d3c7df9f82d251032ed5f1e49180f74f30cf223f..b6a0cf94f11c3fa43352d8c4cca3d29237ce3c51 100644
--- a/source/RobotAPI/libraries/aron/common/forward_declarations.h
+++ b/source/RobotAPI/libraries/aron/common/forward_declarations.h
@@ -5,10 +5,12 @@ namespace simox::arondto
class AxisAlignedBoundingBox;
class Color;
class OrientedBox;
+
} // namespace simox::arondto
namespace armarx::arondto
{
class Names;
class FrameID;
+
} // namespace armarx::arondto
diff --git a/source/RobotAPI/libraries/core/CMakeLists.txt b/source/RobotAPI/libraries/core/CMakeLists.txt
index 1e4b08820998f187015b03173f5e38c6fee21a14..7756a2cd5be8edad7ee10e8fdcdc25b4501cb008 100644
--- a/source/RobotAPI/libraries/core/CMakeLists.txt
+++ b/source/RobotAPI/libraries/core/CMakeLists.txt
@@ -20,6 +20,8 @@ set(LIBS RobotAPIInterfaces ArmarXCoreObservers ArmarXCoreStatechart ArmarXCoreE
)
set(LIB_FILES
+ json_conversions.cpp
+
PIDController.cpp
Trajectory.cpp
TrajectoryController.cpp
@@ -51,6 +53,7 @@ set(LIB_FILES
CartesianVelocityControllerWithRamp.cpp
CartesianNaturalPositionController.cpp
#CartesianNaturalVelocityController.cpp
+ Names.cpp
visualization/DebugDrawerTopic.cpp
visualization/GlasbeyLUT.cpp
@@ -60,6 +63,9 @@ set(LIB_FILES
)
set(LIB_HEADERS
+ forward_declarations.h
+ json_conversions.h
+
PIDController.h
MultiDimPIDController.h
Trajectory.h
@@ -103,14 +109,13 @@ set(LIB_HEADERS
CartesianNaturalPositionController.h
#CartesianNaturalVelocityController.h
EigenHelpers.h
+ Names.h
visualization/DebugDrawerTopic.h
visualization/GlasbeyLUT.h
#diffik/NaturalDiffIK.h
#diffik/SimpleDiffIK.h
-
- json_conversions.h
)
add_subdirectory(test)
diff --git a/source/RobotAPI/libraries/core/Names.cpp b/source/RobotAPI/libraries/core/Names.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e30ec9a3fb6abedb2bd7fb6f12de6e2aba967d1
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Names.cpp
@@ -0,0 +1,7 @@
+#include "Names.h"
+
+namespace armarx
+{
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/Names.h b/source/RobotAPI/libraries/core/Names.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f3ec69588d538c284bcee016db37af5808daed3
--- /dev/null
+++ b/source/RobotAPI/libraries/core/Names.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace armarx
+{
+
+ struct Names
+ {
+ std::vector<std::string> recognized;
+ std::vector<std::string> spoken;
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/forward_declarations.h b/source/RobotAPI/libraries/core/forward_declarations.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3030f5921b332a881d7dffdb715ee076c1979e0
--- /dev/null
+++ b/source/RobotAPI/libraries/core/forward_declarations.h
@@ -0,0 +1,7 @@
+#pragma once
+
+namespace armarx
+{
+ class Names;
+ class FramedPose;
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/core/json_conversions.cpp b/source/RobotAPI/libraries/core/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be883c3db8bb45f624ed5b87e489a73666c28b95
--- /dev/null
+++ b/source/RobotAPI/libraries/core/json_conversions.cpp
@@ -0,0 +1,46 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/Names.h>
+
+void
+armarx::to_json(nlohmann::json& j, const FramedPose& fp)
+{
+ j = nlohmann::json{{"agent", fp.agent},
+ {"frame", fp.frame},
+ {"qw", fp.orientation->qw},
+ {"qx", fp.orientation->qx},
+ {"qy", fp.orientation->qy},
+ {"qz", fp.orientation->qz},
+ {"x", fp.position->x},
+ {"y", fp.position->y},
+ {"z", fp.position->z}};
+}
+
+void
+armarx::from_json(const nlohmann::json& j, FramedPose& fp)
+{
+ j.at("agent").get_to(fp.agent);
+ j.at("frame").get_to(fp.frame);
+ j.at("qw").get_to(fp.orientation->qw);
+ j.at("qx").get_to(fp.orientation->qx);
+ j.at("qy").get_to(fp.orientation->qy);
+ j.at("qz").get_to(fp.orientation->qz);
+ j.at("x").get_to(fp.position->x);
+ j.at("y").get_to(fp.position->y);
+ j.at("z").get_to(fp.position->z);
+}
+
+void
+armarx::to_json(simox::json::json& j, const Names& value)
+{
+ j["recognized"] = value.recognized;
+ j["spoken"] = value.spoken;
+}
+
+void
+armarx::from_json(const simox::json::json& j, Names& value)
+{
+ j.at("recognized").get_to(value.recognized);
+ j.at("spoken").get_to(value.spoken);
+}
diff --git a/source/RobotAPI/libraries/core/json_conversions.h b/source/RobotAPI/libraries/core/json_conversions.h
index bccd85c6884cb75930932cec64a4480742191b20..0d45d0f05ead70ca1ea58c877b3ec0f65f8293f5 100644
--- a/source/RobotAPI/libraries/core/json_conversions.h
+++ b/source/RobotAPI/libraries/core/json_conversions.h
@@ -20,45 +20,18 @@
* GNU General Public License
*/
-
#pragma once
-
-// Simox
#include <SimoxUtility/json.h>
-// RobotAPI
-#include <RobotAPI/libraries/core/FramedPose.h>
-
+#include <RobotAPI/libraries/core/forward_declarations.h>
namespace armarx
{
- void to_json(nlohmann::json& j, const FramedPose& fp)
- {
- j = nlohmann::json
- {
- {"agent", fp.agent},
- {"frame", fp.frame},
- {"qw", fp.orientation->qw},
- {"qx", fp.orientation->qx},
- {"qy", fp.orientation->qy},
- {"qz", fp.orientation->qz},
- {"x", fp.position->x},
- {"y", fp.position->y},
- {"z", fp.position->z}
- };
- }
+ void to_json(nlohmann::json& j, const FramedPose& fp);
+ void from_json(const nlohmann::json& j, FramedPose& fp);
+
+ void to_json(simox::json::json& j, const Names& value);
+ void from_json(const simox::json::json& j, Names& value);
- void from_json(const nlohmann::json& j, FramedPose& fp)
- {
- j.at("agent").get_to(fp.agent);
- j.at("frame").get_to(fp.frame);
- j.at("qw").get_to(fp.orientation->qw);
- j.at("qx").get_to(fp.orientation->qx);
- j.at("qy").get_to(fp.orientation->qy);
- j.at("qz").get_to(fp.orientation->qz);
- j.at("x").get_to(fp.position->x);
- j.at("y").get_to(fp.position->y);
- j.at("z").get_to(fp.position->z);
- }
-}
+} // namespace armarx