From 1dbc8494f02c1ccf30e75d25e5a780e96211cbe0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 20:36:21 +0200
Subject: [PATCH] Add aron conversion
---
source/armarx/navigation/human/CMakeLists.txt | 8 ++-
source/armarx/navigation/human/aron/Human.xml | 34 +++++++++
.../navigation/human/aron_conversions.cpp | 69 +++++++++++++++++++
.../navigation/human/aron_conversions.h | 46 +++++++++++++
source/armarx/navigation/human/shapes.h | 48 +++++++++++++
source/armarx/navigation/human/types.h | 4 +-
6 files changed, 207 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/human/aron/Human.xml
create mode 100644 source/armarx/navigation/human/aron_conversions.cpp
create mode 100644 source/armarx/navigation/human/aron_conversions.h
create mode 100644 source/armarx/navigation/human/shapes.h
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index c173b591..670c1869 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -1,14 +1,20 @@
armarx_add_aron_library(human_aron
ARON_FILES
+ aron/Human.xml
)
armarx_add_library(human
DEPENDENCIES_PUBLIC
ArmarXCore
armarx_navigation::core
+ armarx_navigation::conversions
DEPENDENCIES_PRIVATE
+ range-v3::range-v3
SOURCES
- #types.cpp
+ types.cpp
+ aron_conversions.cpp
HEADERS
types.h
+ aron_conversions.h
+ shapes.h
)
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
new file mode 100644
index 00000000..57d104d8
--- /dev/null
+++ b/source/armarx/navigation/human/aron/Human.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::human::arondto::Human'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='linearVelocity'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::human::arondto::HumanGroup'>
+ <ObjectChild key='shape'>
+ <List>
+ <Position />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='humans'>
+ <List>
+ <armarx::navigation::human::arondto::Human />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
new file mode 100644
index 00000000..e09a3ca3
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::human
+{
+ void
+ toAron(arondto::Human& dto, const Human& bo)
+ {
+ dto.pose = conv::to3D(bo.pose).matrix();
+ dto.linearVelocity = conv::to3D(bo.linearVelocity);
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::Human& dto, Human& bo)
+ {
+ bo.pose = conv::to2D(core::Pose(dto.pose));
+ bo.linearVelocity = conv::to2D(dto.linearVelocity);
+ bo.detectionTime = dto.detectionTime;
+ }
+
+
+ void
+ toAron(arondto::HumanGroup& dto, const HumanGroup& bo)
+ {
+ dto.shape = bo.shape.vertices |
+ ranges::views::transform([](const Eigen::Vector2f& boVer) -> Eigen::Vector3f
+ { return conv::to3D(boVer); }) |
+ ranges::to_vector;
+
+ dto.humans = bo.humans |
+ ranges::views::transform(
+ [](const Human& boHuman) -> arondto::Human
+ {
+ arondto::Human dtoHuman;
+ toAron(dtoHuman, boHuman);
+ return dtoHuman;
+ }) |
+ ranges::to_vector;
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::HumanGroup& dto, HumanGroup& bo)
+ {
+ bo.shape.vertices =
+ dto.shape |
+ ranges::views::transform([](const Eigen::Vector3f& dtoVer) -> Eigen::Vector2f
+ { return conv::to2D(dtoVer); }) |
+ ranges::to_vector;
+
+ bo.humans = dto.humans |
+ ranges::views::transform(
+ [](const arondto::Human& dtoHuman) -> Human
+ {
+ Human boHuman;
+ fromAron(dtoHuman, boHuman);
+ return boHuman;
+ }) |
+ ranges::to_vector;
+ bo.detectionTime = dto.detectionTime;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
new file mode 100644
index 00000000..71159748
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.h
@@ -0,0 +1,46 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ struct Human;
+ struct HumanGroup;
+
+ namespace arondto
+ {
+ struct Human;
+ struct HumanGroup;
+
+ } // namespace arondto
+
+} // namespace armarx::navigation::human
+
+namespace armarx::navigation::human
+{
+ void toAron(arondto::Human& dto, const Human& bo);
+ void fromAron(const arondto::Human& dto, Human& bo);
+
+ void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
+ void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/shapes.h b/source/armarx/navigation/human/shapes.h
new file mode 100644
index 00000000..f6fd25a5
--- /dev/null
+++ b/source/armarx/navigation/human/shapes.h
@@ -0,0 +1,48 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * 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 )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human::shapes
+{
+
+ /**
+ * @brief An axis oriented ellipse with half-axes a and b along the x- and y-axis respectively.
+ */
+ struct Ellipse
+ {
+ float a;
+ float b;
+ };
+
+ /**
+ * @brief A polygon with arbitrarily many vertices. The polygon will always be closed automatically.
+ */
+ struct Polygon
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ };
+
+} // namespace armarx::navigation::human::shapes
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 867849a1..6b66e7c6 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/time.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/shapes.h>
namespace armarx::navigation::human
{
@@ -38,7 +39,7 @@ namespace armarx::navigation::human
struct HumanGroup
{
- std::vector<Eigen::Vector2f> vertices;
+ shapes::Polygon shape;
std::vector<Human> humans;
DateTime detectionTime;
};
@@ -46,6 +47,7 @@ namespace armarx::navigation::human
struct ProxemicZone
{
core::Pose2D pose;
+ shapes::Ellipse shape;
};
} // namespace armarx::navigation::human
--
GitLab