Add aron conversions for proxemics as part of human

source/armarx/navigation/human/aron/Human.xml

@@ -2,6 +2,40 @@
 <AronTypeDefinition>
     <GenerateTypes>
 
+        <Object name='armarx::navigation::human::arondto::ExponentialPenaltyModel'>
+            <ObjectChild key='minDistance'>
+                <Float />
+            </ObjectChild>
+            <ObjectChild key='epsilon'>
+                <Float />
+            </ObjectChild>
+            <ObjectChild key='exponent'>
+                <Float />
+            </ObjectChild>
+        </Object>
+
+        <Object name='armarx::navigation::human::arondto::ProxemicZone'>
+            <ObjectChild key='pose'>
+                <Pose />
+            </ObjectChild>
+            <ObjectChild key='ellipseA'>
+                <Float />
+            </ObjectChild>
+            <ObjectChild key='ellipseB'>
+                <Float />
+            </ObjectChild>
+            <ObjectChild key='penalty'>
+                <armarx::navigation::human::arondto::ExponentialPenaltyModel />
+            </ObjectChild>
+            <ObjectChild key='weight'>
+                <Float />
+            </ObjectChild>
+            <ObjectChild key='homotopicRelevance'>
+                <Bool />
+            </ObjectChild>
+        </Object>
+
+
         <Object name='armarx::navigation::human::arondto::Human'>
             <ObjectChild key='pose'>
                 <Pose />
@@ -12,6 +46,11 @@
             <ObjectChild key='detectionTime'>
                 <Time />
             </ObjectChild>
+            <ObjectChild key='proxemicZones'>
+                <List>
+                    <armarx::navigation::human::arondto::ProxemicZone />
+                </List>
+            </ObjectChild>
         </Object>
 
         <Object name='armarx::navigation::human::arondto::HumanGroup'>

source/armarx/navigation/human/aron_conversions.cpp

@@ -14,6 +14,15 @@ namespace armarx::navigation::human
         dto.pose = conv::to3D(bo.pose).matrix();
         dto.linearVelocity = conv::to3D(bo.linearVelocity);
         dto.detectionTime = bo.detectionTime;
+        dto.proxemicZones = bo.proxemicZones |
+                            ranges::views::transform(
+                                [](const ProxemicZone& boZone) -> arondto::ProxemicZone
+                                {
+                                    arondto::ProxemicZone dtoZone;
+                                    toAron(dtoZone, boZone);
+                                    return dtoZone;
+                                }) |
+                            ranges::to_vector;
     }
 
     void
@@ -22,6 +31,15 @@ namespace armarx::navigation::human
         bo.pose = conv::to2D(core::Pose(dto.pose));
         bo.linearVelocity = conv::to2D(dto.linearVelocity);
         bo.detectionTime = dto.detectionTime;
+        bo.proxemicZones = dto.proxemicZones |
+                           ranges::views::transform(
+                               [](const arondto::ProxemicZone& dtoZone) -> ProxemicZone
+                               {
+                                   ProxemicZone boZone;
+                                   fromAron(dtoZone, boZone);
+                                   return boZone;
+                               }) |
+                           ranges::to_vector;
     }
 
 
@@ -66,4 +84,42 @@ namespace armarx::navigation::human
         bo.detectionTime = dto.detectionTime;
     }
 
+    void
+    toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo)
+    {
+        dto.minDistance = bo.minDistance;
+        dto.epsilon = bo.epsilon;
+        dto.exponent = bo.exponent;
+    }
+
+    void
+    fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo)
+    {
+        bo.minDistance = dto.minDistance;
+        bo.epsilon = dto.epsilon;
+        bo.exponent = dto.exponent;
+    }
+
+    void
+    toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo)
+    {
+        dto.pose = conv::to3D(bo.pose).matrix();
+        dto.ellipseA = bo.shape.a;
+        dto.ellipseB = bo.shape.b;
+        toAron(dto.penalty, bo.penalty);
+        dto.weight = bo.weight;
+        dto.homotopicRelevance = bo.homotopicRelevance;
+    }
+
+    void
+    fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo)
+    {
+        bo.pose = conv::to2D(core::Pose(dto.pose));
+        bo.shape = {.a = dto.ellipseA, .b = dto.ellipseB};
+        fromAron(dto.penalty, bo.penalty);
+        bo.weight = dto.weight;
+        bo.homotopicRelevance = dto.homotopicRelevance;
+    }
+
+
 } // namespace armarx::navigation::human

source/armarx/navigation/human/aron_conversions.h

@@ -14,6 +14,7 @@
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
  * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author     Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
  * @date       2022
  * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
  *             GNU General Public License
@@ -25,11 +26,15 @@ namespace armarx::navigation::human
 {
     struct Human;
     struct HumanGroup;
+    struct ExponentialPenaltyModel;
+    struct ProxemicZone;
 
     namespace arondto
     {
         struct Human;
         struct HumanGroup;
+        struct ExponentialPenaltyModel;
+        struct ProxemicZone;
 
     } // namespace arondto
 
@@ -43,4 +48,10 @@ namespace armarx::navigation::human
     void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
     void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
 
+    void toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo);
+    void fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo);
+
+    void toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo);
+    void fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo);
+
 } // namespace armarx::navigation::human

source/armarx/navigation/human/types.cpp

@@ -22,6 +22,7 @@ namespace armarx::navigation::human
                other.detectionTime == this->detectionTime;
     }
 
+
     aron::data::DictPtr
     Human::toAron() const
     {
@@ -42,6 +43,46 @@ namespace armarx::navigation::human
         return bo;
     }
 
+    aron::data::DictPtr
+    ExponentialPenaltyModel::toAron() const
+    {
+        arondto::ExponentialPenaltyModel dto;
+        human::toAron(dto, *this);
+        return dto.toAron();
+    }
+
+    ExponentialPenaltyModel
+    ExponentialPenaltyModel::FromAron(const aron::data::DictPtr& dict)
+    {
+        ARMARX_CHECK_NOT_NULL(dict);
+        arondto::ExponentialPenaltyModel dto;
+        dto.fromAron(dict);
+
+        ExponentialPenaltyModel bo;
+        fromAron(dto, bo);
+        return bo;
+    }
+
+    aron::data::DictPtr
+    ProxemicZone::toAron() const
+    {
+        arondto::ProxemicZone dto;
+        human::toAron(dto, *this);
+        return dto.toAron();
+    }
+
+    ProxemicZone
+    ProxemicZone::FromAron(const aron::data::DictPtr& dict)
+    {
+        ARMARX_CHECK_NOT_NULL(dict);
+        arondto::ProxemicZone dto;
+        dto.fromAron(dict);
+
+        ProxemicZone bo;
+        fromAron(dto, bo);
+        return bo;
+    }
+
 
     aron::data::DictPtr
     HumanGroup::toAron() const

source/armarx/navigation/human/types.h

@@ -30,11 +30,42 @@
 
 namespace armarx::navigation::human
 {
+    struct LinearPenaltyModel
+    {
+        float minDistance; // [m]
+        float epsilon; // [m]
+    };
+
+    struct ExponentialPenaltyModel
+    {
+        float minDistance; // [m]
+        float epsilon; // [m]
+        float exponent;
+
+        aron::data::DictPtr toAron() const;
+        static ExponentialPenaltyModel FromAron(const aron::data::DictPtr& dict);
+    };
+
+    struct ProxemicZone
+    {
+        core::Pose2D pose;
+        shapes::Ellipse shape;
+        ExponentialPenaltyModel penalty;
+        float weight;
+        bool homotopicRelevance = true;
+
+        aron::data::DictPtr toAron() const;
+        static ProxemicZone FromAron(const aron::data::DictPtr& dict);
+    };
+
+    using ProxemicZones = std::vector<ProxemicZone>;
+
     struct Human
     {
         core::Pose2D pose;
         Eigen::Vector2f linearVelocity;
         DateTime detectionTime;
+        ProxemicZones proxemicZones = {};
 
         core::Pose2D estimateAt(const DateTime& time) const;
 
@@ -59,28 +90,4 @@ namespace armarx::navigation::human
 
     using HumanGroups = std::vector<HumanGroup>;
 
-    struct LinearPenaltyModel
-    {
-        float minDistance; // [m]
-        float epsilon; // [m]
-    };
-
-    struct ExponentialPenaltyModel
-    {
-        float minDistance; // [m]
-        float epsilon; // [m]
-        float exponent;
-    };
-
-    struct ProxemicZone
-    {
-        core::Pose2D pose;
-        shapes::Ellipse shape;
-        ExponentialPenaltyModel penalty;
-        float weight;
-        bool homotopicRelevance = true;
-    };
-
-    using ProxemicZones = std::vector<ProxemicZone>;
-
 } // namespace armarx::navigation::human