Merge branch 'names' into 'master'

Add Names to Locations

See merge request !402

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())
             {

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

 #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)
         {
         }
 

source/RobotAPI/libraries/armem_locations/aron/Location.xml

 <?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>

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

source/RobotAPI/libraries/aron/common/aron_conversions.h

 #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"

source/RobotAPI/libraries/aron/common/aron_conversions/names.cpp

+#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;
+}

source/RobotAPI/libraries/aron/common/aron_conversions/names.h

+#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

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

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)
 

source/RobotAPI/libraries/core/Names.cpp

+#include "Names.h"
+
+namespace armarx
+{
+
+
+} // namespace armarx

source/RobotAPI/libraries/core/Names.h

+#pragma once
+
+#include <string>
+#include <vector>
+
+namespace armarx
+{
+
+    struct Names
+    {
+        std::vector<std::string> recognized;
+        std::vector<std::string> spoken;
+    };
+
+} // namespace armarx

source/RobotAPI/libraries/core/forward_declarations.h

+#pragma once
+
+namespace armarx
+{
+    class Names;
+    class FramedPose;
+} // namespace armarx

source/RobotAPI/libraries/core/json_conversions.cpp

+#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);
+}

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