From f1edd920358670d2e73dffc00996737ce037567e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 29 Aug 2022 17:03:34 +0200
Subject: [PATCH 01/62] Create HumanGrouper class
---
.../dynamic_scene_provider/CMakeLists.txt | 2 ++
.../dynamic_scene_provider/HumanGrouper.cpp | 9 +++++
.../dynamic_scene_provider/HumanGrouper.h | 35 +++++++++++++++++++
3 files changed, 46 insertions(+)
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index e061ea23..f7223696 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -10,10 +10,12 @@ armarx_add_component(dynamic_scene_provider
Component.cpp
ArVizDrawer.cpp
HumanTracker.cpp
+ HumanGrouper.cpp
HEADERS
Component.h
ArVizDrawer.h
HumanTracker.h
+ HumanGrouper.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
new file mode 100644
index 00000000..4e80e320
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
@@ -0,0 +1,9 @@
+#include "HumanGrouper.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ HumanGrouper::HumanGrouper()
+ {
+
+ }
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
new file mode 100644
index 00000000..f5363b8c
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
@@ -0,0 +1,35 @@
+/**
+ * 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 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
+ */
+
+#pragma once
+
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ class HumanGrouper
+ {
+ public:
+ HumanGrouper();
+ };
+} // namespace armarx::navigation::components::dynamic_scene_provider
+
--
GitLab
From a550747b8971eee22a680530f3b92185c946625c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 29 Aug 2022 17:36:44 +0200
Subject: [PATCH 02/62] Create methods and documentation
---
.../dynamic_scene_provider/HumanGrouper.cpp | 12 ++++++++
.../dynamic_scene_provider/HumanGrouper.h | 28 ++++++++++++++++++-
2 files changed, 39 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
index 4e80e320..c38cccec 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
@@ -6,4 +6,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
}
+
+ void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
+ {
+
+ }
+
+ std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
+ {
+
+ }
+
+
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
index f5363b8c..1c6abe10 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
@@ -26,10 +26,36 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
+ /**
+ * Can identify social interaction groups in a collection of humans. The currently detected
+ * humans can be set by calling updateHumans.
+ *
+ * @brief Identifies social interaction groups in a collection of detected humans.
+ */
class HumanGrouper
{
public:
+ /**
+ * @brief Creates a new HumanGrouper
+ */
HumanGrouper();
+
+ /**
+ * Sets the currently detected humans. Only these humans will be assumed to currently exist
+ * when getCurrentGroups() is called the next time.
+ *
+ * @brief Sets the currently detected humans.
+ * @param newHumans a vector containing the most recently detected humans
+ */
+ void updateHumans(std::vector<human::Human>& newHumans);
+
+ /**
+ * Identifies and returns social groups in the humans given to this instance by the last
+ * call to updateHumans.
+ *
+ * @brief Recognizes groups in the current humans.
+ * @return a vector containing the recognized human groups
+ */
+ std::vector<human::HumanGroup> getCurrentGroups();
};
} // namespace armarx::navigation::components::dynamic_scene_provider
-
--
GitLab
From 85571bbdc753c7cbd22933336cddafa8a8b9372d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 29 Aug 2022 20:28:42 +0200
Subject: [PATCH 03/62] Add distance function and start impl (WIP!)
---
.../dynamic_scene_provider/CMakeLists.txt | 7 ++++++
.../dynamic_scene_provider/DistanceFunction.h | 10 ++++++++
.../DistanceFunctionDecorator.cpp | 16 +++++++++++++
.../DistanceFunctionDecorator.h | 16 +++++++++++++
.../EuclideanDistance.cpp | 15 ++++++++++++
.../EuclideanDistance.h | 17 ++++++++++++++
.../dynamic_scene_provider/HumanGrouper.cpp | 2 +-
.../dynamic_scene_provider/HumanGrouper.h | 20 +++++++++++-----
.../OrientationDecorator.cpp | 18 +++++++++++++++
.../OrientationDecorator.h | 23 +++++++++++++++++++
10 files changed, 137 insertions(+), 7 deletions(-)
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index f7223696..05d9c3d9 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -11,11 +11,18 @@ armarx_add_component(dynamic_scene_provider
ArVizDrawer.cpp
HumanTracker.cpp
HumanGrouper.cpp
+ DistanceFunctionDecorator.cpp
+ OrientationDecorator.cpp
+ EuclideanDistance.cpp
HEADERS
Component.h
ArVizDrawer.h
HumanTracker.h
HumanGrouper.h
+ DistanceFunction.h
+ DistanceFunctionDecorator.h
+ OrientationDecorator.h
+ EuclideanDistance.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
new file mode 100644
index 00000000..24f9842b
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
@@ -0,0 +1,10 @@
+#pragma once
+
+template <typename T>
+class DistanceFunction
+{
+public:
+ DistanceFunction();
+ virtual double computeDistance(T& t1, T& t2) = 0;
+};
+
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
new file mode 100644
index 00000000..adf1430a
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
@@ -0,0 +1,16 @@
+#include "DistanceFunctionDecorator.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ template<typename T>
+ DistanceFunctionDecorator<T>::DistanceFunctionDecorator(DistanceFunction<T> &subject)
+ {
+ this->subject = subject;
+ }
+
+ template<typename T>
+ double DistanceFunctionDecorator<T>::computeDistance(T &t1, T &t2)
+ {
+ return subject.computeDistance(t1, t2);
+ }
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
new file mode 100644
index 00000000..1b7bb8fc
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
@@ -0,0 +1,16 @@
+#include "DistanceFunction.h"
+
+#pragma once
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ template <typename T>
+ class DistanceFunctionDecorator : DistanceFunction<T>
+ {
+ public:
+ DistanceFunctionDecorator(DistanceFunction<T>& subject);
+ virtual double computeDistance(T& t1, T& t2);
+ private:
+ DistanceFunction<T> subject;
+ };
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
new file mode 100644
index 00000000..999d6bac
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
@@ -0,0 +1,15 @@
+#include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ using Human = armarx::navigation::human::Human;
+
+ double EuclideanDistance::computeDistance(Human &h1, Human &h2)
+ {
+ return (h1.pose.translation() - h2.pose.translation()).norm();
+ }
+}
+
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
new file mode 100644
index 00000000..07a993a1
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
@@ -0,0 +1,17 @@
+#include "DistanceFunction.h"
+
+#include "armarx/navigation/human/types.h"
+
+#pragma once
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ using Human = armarx::navigation::human::Human;
+
+ class EuclideanDistance : DistanceFunction<Human>
+ {
+ public:
+ EuclideanDistance();
+ double computeDistance(Human &h1, Human &h2);
+ };
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
index c38cccec..6544782d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
@@ -9,7 +9,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
{
-
+ currentHumans = newHumans;
}
std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
index 1c6abe10..bec7c4a0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
@@ -27,10 +27,10 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
/**
+ * @brief Identifies social interaction groups in a collection of detected humans.
+ *
* Can identify social interaction groups in a collection of humans. The currently detected
* humans can be set by calling updateHumans.
- *
- * @brief Identifies social interaction groups in a collection of detected humans.
*/
class HumanGrouper
{
@@ -41,21 +41,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
HumanGrouper();
/**
+ * @brief Sets the currently detected humans.
+ *
* Sets the currently detected humans. Only these humans will be assumed to currently exist
* when getCurrentGroups() is called the next time.
- *
- * @brief Sets the currently detected humans.
* @param newHumans a vector containing the most recently detected humans
*/
void updateHumans(std::vector<human::Human>& newHumans);
/**
+ * @brief Recognizes groups in the current humans.
+ *
* Identifies and returns social groups in the humans given to this instance by the last
* call to updateHumans.
- *
- * @brief Recognizes groups in the current humans.
* @return a vector containing the recognized human groups
*/
std::vector<human::HumanGroup> getCurrentGroups();
+
+ struct DistanceFunction
+ {
+
+ };
+
+ private:
+ std::vector<human::Human> currentHumans;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
new file mode 100644
index 00000000..c363ff9c
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
@@ -0,0 +1,18 @@
+#include "OrientationDecorator.h"
+
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ OrientationDecorator::OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor)
+ : DistanceFunctionDecorator<Human>(subject)
+ {
+ this->influenceFactor = influenceFactor;
+ }
+
+ double OrientationDecorator::computeDistance(Human &t1, Human &t2)
+ {
+ double orientationFactor = 1; // TODO: implement
+ return 0;
+ }
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
new file mode 100644
index 00000000..f14d132a
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "DistanceFunctionDecorator.h"
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ using Human = armarx::navigation::human::Human;
+
+ class OrientationDecorator : DistanceFunctionDecorator<Human>
+ {
+ public:
+ OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor);
+
+ virtual double computeDistance(Human &t1, Human &t2);
+
+ private:
+ double influenceFactor;
+ };
+}
+
--
GitLab
From 8bff4cb6fcc934452b7301bc16b3e0fe3b293fa3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 15:50:50 +0200
Subject: [PATCH 04/62] Simplify design
---
.../dynamic_scene_provider/CMakeLists.txt | 6 ++---
.../dynamic_scene_provider/DistanceFunction.h | 20 +++++++++++-----
.../DistanceFunctionDecorator.cpp | 16 -------------
.../DistanceFunctionDecorator.h | 16 -------------
.../EuclideanDistance.cpp | 5 ----
.../EuclideanDistance.h | 11 ++++-----
.../OrientationDecorator.cpp | 18 ---------------
.../OrientationDecorator.h | 23 -------------------
.../OrientationDistance.cpp | 17 ++++++++++++++
.../OrientationDistance.h | 14 +++++++++++
10 files changed, 51 insertions(+), 95 deletions(-)
delete mode 100644 source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
delete mode 100644 source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
delete mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
delete mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 05d9c3d9..a7b57ea1 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -11,18 +11,16 @@ armarx_add_component(dynamic_scene_provider
ArVizDrawer.cpp
HumanTracker.cpp
HumanGrouper.cpp
- DistanceFunctionDecorator.cpp
- OrientationDecorator.cpp
EuclideanDistance.cpp
+ OrientationDistance.cpp
HEADERS
Component.h
ArVizDrawer.h
HumanTracker.h
HumanGrouper.h
DistanceFunction.h
- DistanceFunctionDecorator.h
- OrientationDecorator.h
EuclideanDistance.h
+ OrientationDistance.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
index 24f9842b..5978366f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
@@ -1,10 +1,18 @@
#pragma once
-template <typename T>
-class DistanceFunction
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
{
-public:
- DistanceFunction();
- virtual double computeDistance(T& t1, T& t2) = 0;
-};
+ using Human = armarx::navigation::human::Human;
+
+ class DistanceFunction
+ {
+ public:
+ DistanceFunction();
+ virtual ~DistanceFunction();
+ virtual double computeDistance(Human &h1, Human &h2) = 0;
+ };
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
deleted file mode 100644
index adf1430a..00000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "DistanceFunctionDecorator.h"
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
- template<typename T>
- DistanceFunctionDecorator<T>::DistanceFunctionDecorator(DistanceFunction<T> &subject)
- {
- this->subject = subject;
- }
-
- template<typename T>
- double DistanceFunctionDecorator<T>::computeDistance(T &t1, T &t2)
- {
- return subject.computeDistance(t1, t2);
- }
-}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h b/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
deleted file mode 100644
index 1b7bb8fc..00000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunctionDecorator.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "DistanceFunction.h"
-
-#pragma once
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
- template <typename T>
- class DistanceFunctionDecorator : DistanceFunction<T>
- {
- public:
- DistanceFunctionDecorator(DistanceFunction<T>& subject);
- virtual double computeDistance(T& t1, T& t2);
- private:
- DistanceFunction<T> subject;
- };
-}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
index 999d6bac..db8a7ebc 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
@@ -1,12 +1,7 @@
-#include "DistanceFunction.h"
#include "EuclideanDistance.h"
-#include <armarx/navigation/human/types.h>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
- using Human = armarx::navigation::human::Human;
-
double EuclideanDistance::computeDistance(Human &h1, Human &h2)
{
return (h1.pose.translation() - h2.pose.translation()).norm();
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
index 07a993a1..0885b12d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
@@ -1,17 +1,14 @@
+#pragma once
+
#include "DistanceFunction.h"
-#include "armarx/navigation/human/types.h"
-
-#pragma once
namespace armarx::navigation::components::dynamic_scene_provider
{
- using Human = armarx::navigation::human::Human;
-
- class EuclideanDistance : DistanceFunction<Human>
+ class EuclideanDistance : DistanceFunction
{
public:
- EuclideanDistance();
+ EuclideanDistance(double min = 0);
double computeDistance(Human &h1, Human &h2);
};
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
deleted file mode 100644
index c363ff9c..00000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-#include "OrientationDecorator.h"
-
-
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
- OrientationDecorator::OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor)
- : DistanceFunctionDecorator<Human>(subject)
- {
- this->influenceFactor = influenceFactor;
- }
-
- double OrientationDecorator::computeDistance(Human &t1, Human &t2)
- {
- double orientationFactor = 1; // TODO: implement
- return 0;
- }
-}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
deleted file mode 100644
index f14d132a..00000000
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDecorator.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#pragma once
-
-#include "DistanceFunctionDecorator.h"
-
-#include <armarx/navigation/human/types.h>
-
-
-namespace armarx::navigation::components::dynamic_scene_provider
-{
- using Human = armarx::navigation::human::Human;
-
- class OrientationDecorator : DistanceFunctionDecorator<Human>
- {
- public:
- OrientationDecorator(DistanceFunction<Human> &subject, double influenceFactor);
-
- virtual double computeDistance(Human &t1, Human &t2);
-
- private:
- double influenceFactor;
- };
-}
-
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
new file mode 100644
index 00000000..9e52cb44
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
@@ -0,0 +1,17 @@
+#include "OrientationDistance.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+
+ OrientationDistance::OrientationDistance(double max, double min)
+ {
+
+ }
+
+double OrientationDistance::computeDistance(Human &h1, Human &h2)
+ {
+
+ }
+}
+
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
new file mode 100644
index 00000000..8c9cfaa1
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ class OrientationDistance : DistanceFunction
+ {
+ public:
+ OrientationDistance(double max = 1, double min = 0);
+ virtual double computeDistance(Human &h1, Human &h2);
+ };
+}
--
GitLab
From 1af26b60f7c07ea2f32b5d95b5bd78a0be756925 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 15:56:01 +0200
Subject: [PATCH 05/62] Add combined distance
---
.../dynamic_scene_provider/CMakeLists.txt | 2 ++
.../dynamic_scene_provider/CombinedDistance.cpp | 15 +++++++++++++++
.../dynamic_scene_provider/CombinedDistance.h | 14 ++++++++++++++
3 files changed, 31 insertions(+)
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index a7b57ea1..9f6ace73 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -13,6 +13,7 @@ armarx_add_component(dynamic_scene_provider
HumanGrouper.cpp
EuclideanDistance.cpp
OrientationDistance.cpp
+ CombinedDistance.cpp
HEADERS
Component.h
ArVizDrawer.h
@@ -21,6 +22,7 @@ armarx_add_component(dynamic_scene_provider
DistanceFunction.h
EuclideanDistance.h
OrientationDistance.h
+ CombinedDistance.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
new file mode 100644
index 00000000..a396ef5c
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
@@ -0,0 +1,15 @@
+#include "CombinedDistance.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ CombinedDistance::CombinedDistance()
+ {
+
+ }
+
+ double CombinedDistance::computeDistance(Human &h1, Human &h2)
+ {
+
+ }
+}
+
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
new file mode 100644
index 00000000..a5119c6f
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
@@ -0,0 +1,14 @@
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+ class CombinedDistance : DistanceFunction
+ {
+ public:
+ CombinedDistance();
+ virtual double computeDistance(Human &h1, Human &h2);
+ };
+}
--
GitLab
From e6c6e2f8f03246225310441e46410de57044a941 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 16:06:14 +0200
Subject: [PATCH 06/62] Implement combined distance
---
.../dynamic_scene_provider/CombinedDistance.cpp | 7 ++++---
.../components/dynamic_scene_provider/CombinedDistance.h | 9 ++++++++-
2 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
index a396ef5c..183e8bc9 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
@@ -2,14 +2,15 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
- CombinedDistance::CombinedDistance()
+ CombinedDistance::CombinedDistance(double maxOrientationInfluence)
{
-
+ this->euclidean = EuclideanDistance();
+ this->orientation = OrientationDistance(maxOrientationInfluence, 1);
}
double CombinedDistance::computeDistance(Human &h1, Human &h2)
{
-
+ return orientation.computeDistance(h1, h2) * euclidean.computeDistance(h1, h2);
}
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
index a5119c6f..0288b118 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
@@ -1,6 +1,8 @@
#pragma once
#include "DistanceFunction.h"
+#include "EuclideanDistance.h"
+#include "OrientationDistance.h"
namespace armarx::navigation::components::dynamic_scene_provider
@@ -8,7 +10,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
class CombinedDistance : DistanceFunction
{
public:
- CombinedDistance();
+ CombinedDistance(double maxOrientationInfluence);
virtual double computeDistance(Human &h1, Human &h2);
+
+ private:
+ EuclideanDistance euclidean;
+ OrientationDistance orientation;
+ double maxOrientationInfluence;
};
}
--
GitLab
From 5798708536fd1b26a151f4eeb6520fd3be88439c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 17:19:58 +0200
Subject: [PATCH 07/62] Implement OrientationDistance
---
.../OrientationDistance.cpp | 41 ++++++++++++++++++-
.../OrientationDistance.h | 5 +++
2 files changed, 44 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
index 9e52cb44..32fb7f9d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
@@ -2,16 +2,53 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
+ OrientationDistance::OrientationDistance(double max, double min)
+ {
+ this->max = max;
+ this->min = min;
+ }
+ double OrientationDistance::computeDistance(Human &h1, Human &h2)
+ {
+ // ranges from 0 to 1
+ double factor = getOrientationFactor(h1, h2);
- OrientationDistance::OrientationDistance(double max, double min)
+ return min + factor * (max - min);
+ }
+
+
+ double OrientationDistance::getOrientationFactor(Human &h1, Human &h2)
{
+ double lineOrientation = getLineOrientation(h2, h1);
+ double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
+ double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
+
+ // assuming the angles to be in the interval [0, 2pi]
+ double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
+ double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
+ // ranges from 0 (looking directly at each other) to 1 (back to back)
+ return (deviationFirst + deviationSecond) / 2;
}
-double OrientationDistance::computeDistance(Human &h1, Human &h2)
+ double OrientationDistance::getLineOrientation(Human &h2, Human &h1)
{
+ double dx = (h1.pose.translation() - h2.pose.translation()).x();
+ double dy = (h1.pose.translation() - h2.pose.translation()).y();
+ double lineOrientation = atan2(dy, dx);
+ return lineOrientation;
+ }
+
+ // scales orientation to (-1, 1]
+ double OrientationDistance::normOrientation(double orientation)
+ {
+ double normedOrientation = orientation / 3.1416;
+ // brings orientation to [0, 2)
+ normedOrientation = std::fmod(((std::fmod(normedOrientation, 2)) + 2), 2);
+ // brings orientation to [-1, 1)
+ normedOrientation -= (normedOrientation > 1) * 2;
+ return normedOrientation;
}
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
index 8c9cfaa1..c849b217 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
@@ -10,5 +10,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
public:
OrientationDistance(double max = 1, double min = 0);
virtual double computeDistance(Human &h1, Human &h2);
+ private:
+ double getOrientationFactor(Human &h1, Human &h2);
+ double normOrientation(double orientation);
+ double max;
+ double min;
};
}
--
GitLab
From a8a0189fe691ae54319e1d310f1f4081611edac0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 19:23:30 +0200
Subject: [PATCH 08/62] Implement grouping algorithm
---
.../dynamic_scene_provider/CombinedDistance.h | 2 +-
.../EuclideanDistance.h | 2 +-
.../dynamic_scene_provider/HumanGrouper.cpp | 42 ++++++++++++++++++-
.../dynamic_scene_provider/HumanGrouper.h | 33 +++++++++++----
.../OrientationDistance.h | 3 +-
5 files changed, 69 insertions(+), 13 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
index 0288b118..a3b01c20 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
@@ -7,7 +7,7 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
- class CombinedDistance : DistanceFunction
+ class CombinedDistance : public DistanceFunction
{
public:
CombinedDistance(double maxOrientationInfluence);
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
index 0885b12d..b935e074 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
@@ -5,7 +5,7 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
- class EuclideanDistance : DistanceFunction
+ class EuclideanDistance : public DistanceFunction
{
public:
EuclideanDistance(double min = 0);
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
index 6544782d..53407187 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
@@ -1,10 +1,16 @@
+#include "CombinedDistance.h"
#include "HumanGrouper.h"
namespace armarx::navigation::components::dynamic_scene_provider
{
- HumanGrouper::HumanGrouper()
+ HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
{
+ this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
+ }
+ HumanGrouper::~HumanGrouper()
+ {
+ delete this->distanceFunction;
}
void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
@@ -14,8 +20,40 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
{
+ DateTime now = DateTime::Now();
- }
+ std::vector<human::HumanGroup> groups = {};
+
+ // maps index of human in currentHumans to index of group in groups
+ std::unordered_map<int, int> humanGroupMap;
+
+ for (int parentIdx = 0; parentIdx < currentHumans.size(); parentIdx++)
+ {
+ Human parent = currentHumans.at(parentIdx);
+ // if no group exists yet for this human, create one just for them
+ if (humanGroupMap.find(parentIdx) == humanGroupMap.end())
+ {
+ human::HumanGroup group = {{{}}, {parent}, now};
+ groups.push_back(group);
+ humanGroupMap[parentIdx] = groups.size() - 1;
+ }
+
+ for (int childIdx = 0; childIdx < currentHumans.size(); childIdx++)
+ {
+ Human child = currentHumans.at(childIdx);
+
+ if (this->distanceFunction->computeDistance(parent, child) < settings.groupingThreshold)
+ {
+ humanGroupMap[childIdx] = humanGroupMap[parentIdx];
+ groups.at(humanGroupMap[childIdx]).humans.push_back(currentHumans.at(childIdx));
+ }
+ }
+ }
+
+ // TODO: shape generation
+
+ return groups;
+ }
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
index bec7c4a0..29b5f640 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
@@ -21,6 +21,7 @@
#pragma once
+#include "DistanceFunction.h"
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
@@ -34,11 +35,28 @@ namespace armarx::navigation::components::dynamic_scene_provider
*/
class HumanGrouper
{
+
+ using Human = armarx::navigation::human::Human;
+
public:
+ /**
+ * @brief The GroupingSettings struct contains settings for group generation.
+ * This includes the influence of aligned orientation and how aggressively groups
+ * are generated.
+ */
+ struct GroupingSettings
+ {
+ double groupingThreshold;
+ double maxOrientationInfluence;
+ };
+
/**
* @brief Creates a new HumanGrouper
+ * @param settings different parameters that will affect group generation
*/
- HumanGrouper();
+ HumanGrouper(GroupingSettings settings);
+
+ ~HumanGrouper();
/**
* @brief Sets the currently detected humans.
@@ -47,7 +65,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
* when getCurrentGroups() is called the next time.
* @param newHumans a vector containing the most recently detected humans
*/
- void updateHumans(std::vector<human::Human>& newHumans);
+ void updateHumans(std::vector<Human> &newHumans);
/**
* @brief Recognizes groups in the current humans.
@@ -58,12 +76,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
*/
std::vector<human::HumanGroup> getCurrentGroups();
- struct DistanceFunction
- {
-
- };
private:
- std::vector<human::Human> currentHumans;
+ void generateShapes(std::vector<human::HumanGroup> &groups);
+ std::vector<Human> currentHumans;
+ DistanceFunction *distanceFunction;
+ GroupingSettings settings;
};
-} // namespace armarx::navigation::components::dynamic_scene_provider
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
index c849b217..6bc66c3c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
@@ -5,13 +5,14 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
- class OrientationDistance : DistanceFunction
+ class OrientationDistance : public DistanceFunction
{
public:
OrientationDistance(double max = 1, double min = 0);
virtual double computeDistance(Human &h1, Human &h2);
private:
double getOrientationFactor(Human &h1, Human &h2);
+ double getLineOrientation(Human &h2, Human &h1);
double normOrientation(double orientation);
double max;
double min;
--
GitLab
From b03d45a209a83c4ae70f3317bd7325711874f962 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 31 Aug 2022 19:54:23 +0200
Subject: [PATCH 09/62] Move everything to library (away from component)
---
.../components/dynamic_scene_provider/CMakeLists.txt | 9 ---------
source/armarx/navigation/human/CMakeLists.txt | 9 +++++++++
.../CombinedDistance.cpp | 2 +-
.../dynamic_scene_provider => human}/CombinedDistance.h | 2 +-
.../dynamic_scene_provider => human}/DistanceFunction.h | 4 +---
.../EuclideanDistance.cpp | 2 +-
.../dynamic_scene_provider => human}/EuclideanDistance.h | 2 +-
.../dynamic_scene_provider => human}/HumanGrouper.cpp | 2 +-
.../dynamic_scene_provider => human}/HumanGrouper.h | 3 +--
.../OrientationDistance.cpp | 2 +-
.../OrientationDistance.h | 2 +-
11 files changed, 18 insertions(+), 21 deletions(-)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/CombinedDistance.cpp (86%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/CombinedDistance.h (87%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/DistanceFunction.h (67%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/EuclideanDistance.cpp (74%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/EuclideanDistance.h (77%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/HumanGrouper.cpp (96%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/HumanGrouper.h (95%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/OrientationDistance.cpp (96%)
rename source/armarx/navigation/{components/dynamic_scene_provider => human}/OrientationDistance.h (87%)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 9f6ace73..e061ea23 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -10,19 +10,10 @@ armarx_add_component(dynamic_scene_provider
Component.cpp
ArVizDrawer.cpp
HumanTracker.cpp
- HumanGrouper.cpp
- EuclideanDistance.cpp
- OrientationDistance.cpp
- CombinedDistance.cpp
HEADERS
Component.h
ArVizDrawer.h
HumanTracker.h
- HumanGrouper.h
- DistanceFunction.h
- EuclideanDistance.h
- OrientationDistance.h
- CombinedDistance.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index c0a1b74e..025a0df2 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -17,9 +17,18 @@ armarx_add_library(teb_human
types.cpp
aron_conversions.cpp
ProxemicZoneCreator.cpp
+ HumanGrouper.cpp
+ EuclideanDistance.cpp
+ OrientationDistance.cpp
+ CombinedDistance.cpp
HEADERS
types.h
aron_conversions.h
shapes.h
ProxemicZoneCreator.h
+ HumanGrouper.h
+ DistanceFunction.h
+ EuclideanDistance.h
+ OrientationDistance.h
+ CombinedDistance.h
)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
similarity index 86%
rename from source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
rename to source/armarx/navigation/human/CombinedDistance.cpp
index 183e8bc9..a7957a4d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.cpp
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -1,6 +1,6 @@
#include "CombinedDistance.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
CombinedDistance::CombinedDistance(double maxOrientationInfluence)
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
similarity index 87%
rename from source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
rename to source/armarx/navigation/human/CombinedDistance.h
index a3b01c20..5e322969 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -5,7 +5,7 @@
#include "OrientationDistance.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
class CombinedDistance : public DistanceFunction
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
similarity index 67%
rename from source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
rename to source/armarx/navigation/human/DistanceFunction.h
index 5978366f..bdb5c329 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/DistanceFunction.h
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -3,10 +3,8 @@
#include <armarx/navigation/human/types.h>
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
- using Human = armarx::navigation::human::Human;
-
class DistanceFunction
{
public:
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
similarity index 74%
rename from source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
rename to source/armarx/navigation/human/EuclideanDistance.cpp
index db8a7ebc..664ca172 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.cpp
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -1,6 +1,6 @@
#include "EuclideanDistance.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
double EuclideanDistance::computeDistance(Human &h1, Human &h2)
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
similarity index 77%
rename from source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
rename to source/armarx/navigation/human/EuclideanDistance.h
index b935e074..27dea9a6 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -3,7 +3,7 @@
#include "DistanceFunction.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
class EuclideanDistance : public DistanceFunction
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
similarity index 96%
rename from source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
rename to source/armarx/navigation/human/HumanGrouper.cpp
index 53407187..bfe30ba9 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -1,7 +1,7 @@
#include "CombinedDistance.h"
#include "HumanGrouper.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
similarity index 95%
rename from source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
rename to source/armarx/navigation/human/HumanGrouper.h
index 29b5f640..97fae335 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -25,7 +25,7 @@
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
/**
* @brief Identifies social interaction groups in a collection of detected humans.
@@ -78,7 +78,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
- void generateShapes(std::vector<human::HumanGroup> &groups);
std::vector<Human> currentHumans;
DistanceFunction *distanceFunction;
GroupingSettings settings;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
similarity index 96%
rename from source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
rename to source/armarx/navigation/human/OrientationDistance.cpp
index 32fb7f9d..d33a0593 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.cpp
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -1,6 +1,6 @@
#include "OrientationDistance.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
OrientationDistance::OrientationDistance(double max, double min)
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
similarity index 87%
rename from source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
rename to source/armarx/navigation/human/OrientationDistance.h
index 6bc66c3c..8b74b987 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/OrientationDistance.h
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -3,7 +3,7 @@
#include "DistanceFunction.h"
-namespace armarx::navigation::components::dynamic_scene_provider
+namespace armarx::navigation::human
{
class OrientationDistance : public DistanceFunction
{
--
GitLab
From 1f282b2dcdaed15c07f3d38fae248a0728979679 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 15:49:46 +0200
Subject: [PATCH 10/62] Add documentation
---
.../navigation/human/CombinedDistance.h | 34 +++++++++++++++++
.../navigation/human/DistanceFunction.h | 34 +++++++++++++++++
.../navigation/human/EuclideanDistance.h | 25 ++++++++++++
.../navigation/human/OrientationDistance.h | 38 +++++++++++++++++++
4 files changed, 131 insertions(+)
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
index 5e322969..f644f863 100644
--- a/source/armarx/navigation/human/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -1,3 +1,25 @@
+/**
+ * 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 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
+ */
+
+
#pragma once
#include "DistanceFunction.h"
@@ -7,9 +29,21 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief The CombinedDistance class combines the standard euclidean distance multiplicatively
+ * with an orientation distance in such a way that the distance is always at least the euclidean
+ * distance but will be scaled by up to the maximum influence factor if the people face away
+ * from each other.
+ */
class CombinedDistance : public DistanceFunction
{
public:
+
+ /**
+ * @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
+ * @param maxOrientationInfluence the euclidean distance between the humans will be scaled
+ * by up to this factor if they face away from each other
+ */
CombinedDistance(double maxOrientationInfluence);
virtual double computeDistance(Human &h1, Human &h2);
diff --git a/source/armarx/navigation/human/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
index bdb5c329..4f3a39e7 100644
--- a/source/armarx/navigation/human/DistanceFunction.h
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -1,3 +1,24 @@
+/**
+ * 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 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
+ */
+
#pragma once
#include <armarx/navigation/human/types.h>
@@ -5,11 +26,24 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief A distance function is able to compute a distance between two humans.
+ * The exact semantics of this distance depends on the implementation. For example,
+ * a HumanGrouper uses distances two group humans together (the smaller the distance between
+ * two humans, the more likely it is for them to belong to the same social group).
+ */
class DistanceFunction
{
public:
DistanceFunction();
virtual ~DistanceFunction();
+
+ /**
+ * @brief computeDistance computes the (semantic or literal, depending on the implementation) distance between the given humans
+ * @param h1 the first human
+ * @param h2 the second human
+ * @return the distance between the humans (greater values correspond to humans that are further away)
+ */
virtual double computeDistance(Human &h1, Human &h2) = 0;
};
}
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
index 27dea9a6..205d3007 100644
--- a/source/armarx/navigation/human/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -1,3 +1,24 @@
+/**
+ * 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 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
+ */
+
#pragma once
#include "DistanceFunction.h"
@@ -5,6 +26,10 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief The EuclideanDistance class is an implementation of a distance function that computes
+ * the literal, euclidean distance between two human. (i.e. sqrt(dx²+dy²)).
+ */
class EuclideanDistance : public DistanceFunction
{
public:
diff --git a/source/armarx/navigation/human/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
index 8b74b987..135d871f 100644
--- a/source/armarx/navigation/human/OrientationDistance.h
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -1,3 +1,24 @@
+/**
+ * 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 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
+ */
+
#pragma once
#include "DistanceFunction.h"
@@ -5,9 +26,26 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief The OrientationDistance class is a DistanceFunction that computes the distance between
+ * the two humans based upon their orientation towards or away from each other.
+ *
+ * This distance function will return
+ * - the specified minimum distance (0 by default) if the humans are exactly facing each other
+ * - the specified maximum distance (1 by default) if the humans are oriented back to back
+ * - a linear interpolation between the two for anything inbetween (for instance, two people
+ * looking in the same direction will have a distance equal to the arithmetic mean of the
+ * minimum and maximum distance, 0.5 by default)
+ */
class OrientationDistance : public DistanceFunction
{
public:
+
+ /**
+ * @brief OrientationDistance Creates a new OrientationDistance with the specified range
+ * @param max the distance two people will have when faced back to back
+ * @param min the distance two people will have when oriented face to face
+ */
OrientationDistance(double max = 1, double min = 0);
virtual double computeDistance(Human &h1, Human &h2);
private:
--
GitLab
From 47a81593efe327d5888f0428b4cffb686da13434 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:01:43 +0200
Subject: [PATCH 11/62] Add GroupShapeStrategy
---
source/armarx/navigation/human/CMakeLists.txt | 1 +
.../navigation/human/EuclideanDistance.h | 2 +-
.../navigation/human/GroupShapeStrategy.h | 47 +++++++++++++++++++
3 files changed, 49 insertions(+), 1 deletion(-)
create mode 100644 source/armarx/navigation/human/GroupShapeStrategy.h
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 025a0df2..46d3110c 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -31,4 +31,5 @@ armarx_add_library(teb_human
EuclideanDistance.h
OrientationDistance.h
CombinedDistance.h
+ GroupShapeStrategy.h
)
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
index 205d3007..00c8509a 100644
--- a/source/armarx/navigation/human/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -34,6 +34,6 @@ namespace armarx::navigation::human
{
public:
EuclideanDistance(double min = 0);
- double computeDistance(Human &h1, Human &h2);
+ virtual double computeDistance(Human &h1, Human &h2);
};
}
diff --git a/source/armarx/navigation/human/GroupShapeStrategy.h b/source/armarx/navigation/human/GroupShapeStrategy.h
new file mode 100644
index 00000000..c3bc8d4c
--- /dev/null
+++ b/source/armarx/navigation/human/GroupShapeStrategy.h
@@ -0,0 +1,47 @@
+/**
+ * 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 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
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief Encapsules a way to create a shape describing the special area occupied by a social
+ * human group
+ */
+ class GroupShapeStrategy
+ {
+ public:
+ GroupShapeStrategy();
+ virtual ~GroupShapeStrategy();
+
+ /**
+ * @brief createShape creates and returns the shape describing the given group
+ * @param group a social grouping of at least one human
+ * @return a polygon describing the proxemic zone occupied by the given group (not including
+ * the zones of the individual members of the group)
+ */
+ virtual shapes::Polygon createShape(const HumanGroup &group) = 0;
+ };
+}
--
GitLab
From 887a053f8cf3b821e2960e838ee127fb23c3917b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:05:22 +0200
Subject: [PATCH 12/62] Use GroupShapeStrategy in HumanGrouper
---
source/armarx/navigation/human/HumanGrouper.cpp | 6 +++++-
source/armarx/navigation/human/HumanGrouper.h | 2 ++
2 files changed, 7 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index bfe30ba9..8a653267 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -6,6 +6,7 @@ namespace armarx::navigation::human
HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
{
this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
+ this->shapeGenerator = NULL; // TODO: implement and use interface
}
HumanGrouper::~HumanGrouper()
@@ -51,7 +52,10 @@ namespace armarx::navigation::human
}
}
- // TODO: shape generation
+ for (human::HumanGroup group : groups)
+ {
+ group.shape = shapeGenerator->createShape(group);
+ }
return groups;
}
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index 97fae335..fde0f732 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -22,6 +22,7 @@
#pragma once
#include "DistanceFunction.h"
+#include "GroupShapeStrategy.h"
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
@@ -80,6 +81,7 @@ namespace armarx::navigation::human
private:
std::vector<Human> currentHumans;
DistanceFunction *distanceFunction;
+ GroupShapeStrategy *shapeGenerator;
GroupingSettings settings;
};
}
--
GitLab
From ba75689877237cd2ce4812baed1c3c25b6479e9d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:09:37 +0200
Subject: [PATCH 13/62] Rename GroupShapeStrategy->GroupShapeGenerator
---
source/armarx/navigation/human/CMakeLists.txt | 2 +-
.../{GroupShapeStrategy.h => GroupShapeGenerator.h} | 10 +++++-----
source/armarx/navigation/human/HumanGrouper.h | 2 +-
3 files changed, 7 insertions(+), 7 deletions(-)
rename source/armarx/navigation/human/{GroupShapeStrategy.h => GroupShapeGenerator.h} (85%)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 46d3110c..e5940f2b 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -31,5 +31,5 @@ armarx_add_library(teb_human
EuclideanDistance.h
OrientationDistance.h
CombinedDistance.h
- GroupShapeStrategy.h
+ GroupShapeGenerator.h
)
diff --git a/source/armarx/navigation/human/GroupShapeStrategy.h b/source/armarx/navigation/human/GroupShapeGenerator.h
similarity index 85%
rename from source/armarx/navigation/human/GroupShapeStrategy.h
rename to source/armarx/navigation/human/GroupShapeGenerator.h
index c3bc8d4c..f5dc1430 100644
--- a/source/armarx/navigation/human/GroupShapeStrategy.h
+++ b/source/armarx/navigation/human/GroupShapeGenerator.h
@@ -27,14 +27,14 @@
namespace armarx::navigation::human
{
/**
- * @brief Encapsules a way to create a shape describing the special area occupied by a social
- * human group
+ * @brief A strategy encapsuling a way to create a shape describing the special area occupied
+ * by a social human group
*/
- class GroupShapeStrategy
+ class GroupShapeGenerator
{
public:
- GroupShapeStrategy();
- virtual ~GroupShapeStrategy();
+ GroupShapeGenerator();
+ virtual ~GroupShapeGenerator();
/**
* @brief createShape creates and returns the shape describing the given group
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index fde0f732..1b517907 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -81,7 +81,7 @@ namespace armarx::navigation::human
private:
std::vector<Human> currentHumans;
DistanceFunction *distanceFunction;
- GroupShapeStrategy *shapeGenerator;
+ GroupShapeGenerator *shapeGenerator;
GroupingSettings settings;
};
}
--
GitLab
From ac6d045e0cd2d80744c10a0744613982a2686785 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:13:54 +0200
Subject: [PATCH 14/62] Add ConvexHullGenerator
---
source/armarx/navigation/human/CMakeLists.txt | 2 +
.../navigation/human/ConvexHullGenerator.cpp | 6 +++
.../navigation/human/ConvexHullGenerator.h | 40 +++++++++++++++++++
3 files changed, 48 insertions(+)
create mode 100644 source/armarx/navigation/human/ConvexHullGenerator.cpp
create mode 100644 source/armarx/navigation/human/ConvexHullGenerator.h
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index e5940f2b..33feba9f 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -21,6 +21,7 @@ armarx_add_library(teb_human
EuclideanDistance.cpp
OrientationDistance.cpp
CombinedDistance.cpp
+ ConvexHullGenerator.cpp
HEADERS
types.h
aron_conversions.h
@@ -32,4 +33,5 @@ armarx_add_library(teb_human
OrientationDistance.h
CombinedDistance.h
GroupShapeGenerator.h
+ ConvexHullGenerator.h
)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
new file mode 100644
index 00000000..66ae05a0
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -0,0 +1,6 @@
+#include "ConvexHullGenerator.h"
+
+ConvexHullGenerator::ConvexHullGenerator()
+{
+
+}
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
new file mode 100644
index 00000000..2e34fbd6
--- /dev/null
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -0,0 +1,40 @@
+/**
+ * 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 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
+ */
+
+#pragma once
+
+#include <armarx/navigation/human/types.h>
+
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ /**
+ * @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
+ * convex hull of their positions in 2D space.
+ */
+ class ConvexHullGenerator
+ {
+ public:
+ ConvexHullGenerator();
+ };
+}
--
GitLab
From 7c6a613530ef49d2d8b2f572c67da3b6e4d15f7f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:49:19 +0200
Subject: [PATCH 15/62] Implement ConvexHullGenerator
---
.../navigation/human/ConvexHullGenerator.cpp | 42 ++++++++++++++++++-
.../navigation/human/ConvexHullGenerator.h | 6 ++-
2 files changed, 46 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
index 66ae05a0..d86cadc2 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.cpp
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -1,6 +1,46 @@
#include "ConvexHullGenerator.h"
-ConvexHullGenerator::ConvexHullGenerator()
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/multi/geometries/register/multi_point.hpp>
+
+using Point = std::pair<double, double>;
+BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian, first, second)
+
+namespace armarx::navigation::human
{
+ ConvexHullGenerator::ConvexHullGenerator()
+ {
+
+ }
+
+ shapes::Polygon ConvexHullGenerator::createShape(const HumanGroup &group)
+ {
+ using BoostPoly = boost::geometry::model::polygon<Point>;
+ using GroupPoly = shapes::Polygon;
+
+ std::vector<Point> coordinates;
+ for (Human human : group.humans)
+ {
+ coordinates.push_back({human.pose.translation().x(), human.pose.translation().y()});
+ }
+
+ BoostPoly poly, hull;
+ poly.outer().assign(coordinates.begin(), coordinates.end());
+ boost::geometry::convex_hull(poly, hull);
+
+ std::vector<Eigen::Vector2f> vertices;
+
+ for (auto it = boost::begin(boost::geometry::exterior_ring(hull)); it != boost::end(boost::geometry::exterior_ring(hull)); ++it)
+ {
+ double x = boost::geometry::get<0>(*it);
+ double y = boost::geometry::get<1>(*it);
+
+ vertices.push_back(Eigen::Vector2f(x, y));
+ }
+
+ return {.vertices = vertices};
+ }
}
+
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
index 2e34fbd6..44489050 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.h
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -21,6 +21,8 @@
#pragma once
+#include "GroupShapeGenerator.h"
+
#include <armarx/navigation/human/types.h>
@@ -32,9 +34,11 @@ namespace armarx::navigation::human
* @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
* convex hull of their positions in 2D space.
*/
- class ConvexHullGenerator
+ class ConvexHullGenerator : GroupShapeGenerator
{
public:
ConvexHullGenerator();
+
+ virtual shapes::Polygon createShape(const HumanGroup &group);
};
}
--
GitLab
From 3576ed73612be609a829b983c047c0487edf9099 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:52:01 +0200
Subject: [PATCH 16/62] Use ConvexHullGenerator in HumanGrouper
---
source/armarx/navigation/human/ConvexHullGenerator.h | 2 +-
source/armarx/navigation/human/HumanGrouper.cpp | 4 +++-
2 files changed, 4 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
index 44489050..3ed1df62 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.h
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::human
* @brief A GroupShapeGenerator that defines the group shape of a set of humans as the
* convex hull of their positions in 2D space.
*/
- class ConvexHullGenerator : GroupShapeGenerator
+ class ConvexHullGenerator : public GroupShapeGenerator
{
public:
ConvexHullGenerator();
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index 8a653267..28e7825a 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -1,4 +1,5 @@
#include "CombinedDistance.h"
+#include "ConvexHullGenerator.h"
#include "HumanGrouper.h"
namespace armarx::navigation::human
@@ -6,12 +7,13 @@ namespace armarx::navigation::human
HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
{
this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
- this->shapeGenerator = NULL; // TODO: implement and use interface
+ this->shapeGenerator = new ConvexHullGenerator();
}
HumanGrouper::~HumanGrouper()
{
delete this->distanceFunction;
+ delete this->shapeGenerator;
}
void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
--
GitLab
From 833e130b3ce11424482baacc4a70e5ce38001542 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 16:54:58 +0200
Subject: [PATCH 17/62] Throw exception when human group size is 0
---
source/armarx/navigation/human/ConvexHullGenerator.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
index d86cadc2..b6cfd45d 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.cpp
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -17,6 +17,11 @@ namespace armarx::navigation::human
shapes::Polygon ConvexHullGenerator::createShape(const HumanGroup &group)
{
+ if (group.humans.size() == 0)
+ {
+ throw InvalidArgumentException("Human group size has to be at least 1");
+ }
+
using BoostPoly = boost::geometry::model::polygon<Point>;
using GroupPoly = shapes::Polygon;
--
GitLab
From fea33c075ea23368035579748f2fb8b6f7ddc388 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 17:22:05 +0200
Subject: [PATCH 18/62] Add MovementDistance
---
source/armarx/navigation/human/CMakeLists.txt | 2 +
.../navigation/human/MovementDistance.cpp | 15 ++++++++
.../navigation/human/MovementDistance.h | 38 +++++++++++++++++++
3 files changed, 55 insertions(+)
create mode 100644 source/armarx/navigation/human/MovementDistance.cpp
create mode 100644 source/armarx/navigation/human/MovementDistance.h
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 33feba9f..6f8399b2 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -22,6 +22,7 @@ armarx_add_library(teb_human
OrientationDistance.cpp
CombinedDistance.cpp
ConvexHullGenerator.cpp
+ MovementDistance.cpp
HEADERS
types.h
aron_conversions.h
@@ -34,4 +35,5 @@ armarx_add_library(teb_human
CombinedDistance.h
GroupShapeGenerator.h
ConvexHullGenerator.h
+ MovementDistance.h
)
diff --git a/source/armarx/navigation/human/MovementDistance.cpp b/source/armarx/navigation/human/MovementDistance.cpp
new file mode 100644
index 00000000..90c91ff3
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.cpp
@@ -0,0 +1,15 @@
+#include "MovementDistance.h"
+
+namespace armarx::navigation::human
+{
+ MovementDistance::MovementDistance()
+ {
+
+ }
+
+ double armarx::navigation::human::MovementDistance::computeDistance(Human &h1, Human &h2)
+ {
+
+ }
+}
+
diff --git a/source/armarx/navigation/human/MovementDistance.h b/source/armarx/navigation/human/MovementDistance.h
new file mode 100644
index 00000000..eaff1748
--- /dev/null
+++ b/source/armarx/navigation/human/MovementDistance.h
@@ -0,0 +1,38 @@
+/**
+ * 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 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
+ */
+
+#pragma once
+
+#include "DistanceFunction.h"
+
+
+
+namespace armarx::navigation::human
+{
+ class MovementDistance : public DistanceFunction
+ {
+ public:
+ MovementDistance();
+
+ virtual double computeDistance(Human &h1, Human &h2);
+ };
+}
+
--
GitLab
From 1ca0e184708cb0c97ef324729c919ad5f5f8ed50 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 17:33:53 +0200
Subject: [PATCH 19/62] Implement MovementDistance
---
source/armarx/navigation/human/EuclideanDistance.h | 2 +-
source/armarx/navigation/human/MovementDistance.cpp | 4 ++--
source/armarx/navigation/human/MovementDistance.h | 4 ++++
3 files changed, 7 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
index 00c8509a..31523e43 100644
--- a/source/armarx/navigation/human/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -33,7 +33,7 @@ namespace armarx::navigation::human
class EuclideanDistance : public DistanceFunction
{
public:
- EuclideanDistance(double min = 0);
+ EuclideanDistance();
virtual double computeDistance(Human &h1, Human &h2);
};
}
diff --git a/source/armarx/navigation/human/MovementDistance.cpp b/source/armarx/navigation/human/MovementDistance.cpp
index 90c91ff3..77c44322 100644
--- a/source/armarx/navigation/human/MovementDistance.cpp
+++ b/source/armarx/navigation/human/MovementDistance.cpp
@@ -7,9 +7,9 @@ namespace armarx::navigation::human
}
- double armarx::navigation::human::MovementDistance::computeDistance(Human &h1, Human &h2)
+ double MovementDistance::computeDistance(Human &h1, Human &h2)
{
-
+ return (h1.linearVelocity - h2.linearVelocity).norm();
}
}
diff --git a/source/armarx/navigation/human/MovementDistance.h b/source/armarx/navigation/human/MovementDistance.h
index eaff1748..a7a1d87c 100644
--- a/source/armarx/navigation/human/MovementDistance.h
+++ b/source/armarx/navigation/human/MovementDistance.h
@@ -27,6 +27,10 @@
namespace armarx::navigation::human
{
+ /**
+ * @brief A DistanceFunction that defines the distance between two humans as the difference
+ * in velocity (that is, the norm of the linear velocity difference vector).
+ */
class MovementDistance : public DistanceFunction
{
public:
--
GitLab
From e51f23d73b78c0f1c9d954731bb1afdc3e7e5410 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 17:46:24 +0200
Subject: [PATCH 20/62] Use MovementDistance in CombinedDistance
---
source/armarx/navigation/human/CombinedDistance.cpp | 12 +++++++++---
source/armarx/navigation/human/CombinedDistance.h | 7 ++++++-
2 files changed, 15 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/human/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
index a7957a4d..d66a0ba4 100644
--- a/source/armarx/navigation/human/CombinedDistance.cpp
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -2,15 +2,21 @@
namespace armarx::navigation::human
{
- CombinedDistance::CombinedDistance(double maxOrientationInfluence)
+ CombinedDistance::CombinedDistance(double maxOrientationInfluence, double movementInfluence)
+ : maxOrientationInfluence(maxOrientationInfluence), movementInfluence(movementInfluence)
{
this->euclidean = EuclideanDistance();
- this->orientation = OrientationDistance(maxOrientationInfluence, 1);
+ this->orientation = OrientationDistance();
+ this->movement = MovementDistance();
}
double CombinedDistance::computeDistance(Human &h1, Human &h2)
{
- return orientation.computeDistance(h1, h2) * euclidean.computeDistance(h1, h2);
+ return euclidean.computeDistance(h1, h2)
+ // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+ * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
+ // scales the euclidean distance by a factor in [1, inf) depending on influence
+ * (1 + movementInfluence * movement.computeDistance(h1, h2));
}
}
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
index f644f863..bcb76289 100644
--- a/source/armarx/navigation/human/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -24,6 +24,7 @@
#include "DistanceFunction.h"
#include "EuclideanDistance.h"
+#include "MovementDistance.h"
#include "OrientationDistance.h"
@@ -43,13 +44,17 @@ namespace armarx::navigation::human
* @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
* @param maxOrientationInfluence the euclidean distance between the humans will be scaled
* by up to this factor if they face away from each other
+ * @param movementInfluence scales the difference in velocity of the humans before applying
+ * it to the total distance
*/
- CombinedDistance(double maxOrientationInfluence);
+ CombinedDistance(double maxOrientationInfluence, double movementInfluence);
virtual double computeDistance(Human &h1, Human &h2);
private:
EuclideanDistance euclidean;
OrientationDistance orientation;
+ MovementDistance movement;
double maxOrientationInfluence;
+ double movementInfluence;
};
}
--
GitLab
From 54df41422a40dab968ec40755034a77ddd58416a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 18:52:57 +0200
Subject: [PATCH 21/62] Add test (copy paste)
---
source/armarx/navigation/human/CMakeLists.txt | 14 ++
.../navigation/human/test/human_test.cpp | 157 ++++++++++++++++++
2 files changed, 171 insertions(+)
create mode 100644 source/armarx/navigation/human/test/human_test.cpp
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 6f8399b2..109d426b 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -37,3 +37,17 @@ armarx_add_library(teb_human
ConvexHullGenerator.h
MovementDistance.h
)
+
+
+
+armarx_add_test(human_test
+ TEST_FILES
+ test/human_test.cpp
+ DEPENDENCIES
+ PUBLIC
+ ArmarXCore
+ armarx_navigation::core
+
+ PRIVATE
+ range-v3::range-v3
+)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
new file mode 100644
index 00000000..a176e269
--- /dev/null
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -0,0 +1,157 @@
+/**
+ * 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/>.
+ *
+ * @package Navigation::ArmarXObjects::core
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <algorithm>
+#include <vector>
+
+#include <range/v3/view/zip.hpp>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+#include <armarx/navigation/core/Graph.h>
+#include <armarx/navigation/core/Trajectory.h>
+#include <armarx/navigation/core/types.h>
+
+// test includes and other stuff
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+BOOST_AUTO_TEST_CASE(testPathLength)
+{
+ armarx::navigation::core::Path path{
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
+
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+ BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
+}
+
+BOOST_AUTO_TEST_CASE(testResampleAlongLine)
+{
+ armarx::navigation::core::Path path{
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
+
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+ BOOST_CHECK_EQUAL(traj.points().size(), 2);
+
+ const auto resampledTraj = traj.resample(500);
+
+ for (const auto& pt : resampledTraj.positions())
+ {
+ ARMARX_DEBUG << VAROUT(pt);
+ }
+
+ BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
+}
+
+BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
+{
+ armarx::navigation::core::Path path{
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
+ armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
+
+ const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+ BOOST_CHECK_EQUAL(traj.points().size(), 3);
+
+ const auto resampledTraj = traj.resample(500);
+
+ for (const auto& pt : resampledTraj.positions())
+ {
+ ARMARX_DEBUG << VAROUT(pt);
+ }
+
+ BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
+}
+
+
+BOOST_AUTO_TEST_CASE(testGraphRoutes)
+{
+ using namespace armarx::navigation;
+
+ /*
+ *
+ * (0) -> (2) -> (3)
+ * ^
+ * |
+ * (1)
+ */
+
+ core::Graph graph;
+ graph.addVertex(semrel::ShapeID(0));
+ graph.addVertex(semrel::ShapeID(1));
+ graph.addVertex(semrel::ShapeID(2));
+ graph.addVertex(semrel::ShapeID(3));
+
+ graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
+ graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
+ graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
+
+ const auto paths =
+ armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
+
+ // GT: paths
+ // - {3}
+ // - {2,3}
+ // - {0,2,3}
+ // - {1,2,3}
+
+ BOOST_CHECK_EQUAL(paths.size(), 4);
+
+ std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
+
+ const auto isMatchingPath = [](const auto& path, const auto& gtPath)
+ {
+ if (path.size() != gtPath.size())
+ {
+ return false;
+ }
+
+ // check that sequences match
+ for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
+ {
+ if (v.objectID().t != gtId)
+ {
+ return false;
+ }
+ }
+
+ ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
+ return true;
+ };
+
+ // check that all required paths are available
+ for (const auto& gtPath : gtPaths)
+ {
+ BOOST_CHECK(std::any_of(paths.begin(),
+ paths.end(),
+ [>Path, &isMatchingPath](const auto& path)
+ { return isMatchingPath(path, gtPath); }));
+ }
+}
--
GitLab
From 3753019f69b2c87b1746a9b7a30cc842dfc7632e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 20:15:03 +0200
Subject: [PATCH 22/62] Clean up everything
This includes:
- using the autoformatter
- consts everywhere
- no more raw pointers
---
.../navigation/human/CombinedDistance.cpp | 26 +++++-----
.../navigation/human/CombinedDistance.h | 16 +++----
.../navigation/human/ConvexHullGenerator.cpp | 32 ++++++-------
.../navigation/human/ConvexHullGenerator.h | 5 +-
.../navigation/human/DistanceFunction.h | 5 +-
.../navigation/human/EuclideanDistance.cpp | 6 +--
.../navigation/human/EuclideanDistance.h | 4 +-
.../navigation/human/GroupShapeGenerator.h | 4 +-
.../armarx/navigation/human/HumanGrouper.cpp | 47 +++++++++---------
source/armarx/navigation/human/HumanGrouper.h | 28 ++++++-----
.../navigation/human/MovementDistance.cpp | 11 ++---
.../navigation/human/MovementDistance.h | 6 +--
.../navigation/human/OrientationDistance.cpp | 48 ++++++++++---------
.../navigation/human/OrientationDistance.h | 19 ++++----
.../navigation/human/test/human_test.cpp | 3 +-
15 files changed, 131 insertions(+), 129 deletions(-)
diff --git a/source/armarx/navigation/human/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
index d66a0ba4..b44ba1ad 100644
--- a/source/armarx/navigation/human/CombinedDistance.cpp
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -2,21 +2,23 @@
namespace armarx::navigation::human
{
- CombinedDistance::CombinedDistance(double maxOrientationInfluence, double movementInfluence)
- : maxOrientationInfluence(maxOrientationInfluence), movementInfluence(movementInfluence)
+ CombinedDistance::CombinedDistance(const double maxOrientationInfluence,
+ const double movementInfluence) :
+ euclidean(EuclideanDistance()),
+ orientation(OrientationDistance()),
+ movement(MovementDistance()),
+ maxOrientationInfluence(maxOrientationInfluence),
+ movementInfluence(movementInfluence)
{
- this->euclidean = EuclideanDistance();
- this->orientation = OrientationDistance();
- this->movement = MovementDistance();
}
- double CombinedDistance::computeDistance(Human &h1, Human &h2)
+ double
+ CombinedDistance::computeDistance(const Human& h1, const Human& h2) const
{
return euclidean.computeDistance(h1, h2)
- // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
- * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
- // scales the euclidean distance by a factor in [1, inf) depending on influence
- * (1 + movementInfluence * movement.computeDistance(h1, h2));
+ // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+ * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
+ // scales the euclidean distance by a factor in [1, inf) depending on influence
+ * (1 + movementInfluence * movement.computeDistance(h1, h2));
}
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
index bcb76289..5633a69f 100644
--- a/source/armarx/navigation/human/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -19,7 +19,6 @@
* GNU General Public License
*/
-
#pragma once
#include "DistanceFunction.h"
@@ -39,7 +38,6 @@ namespace armarx::navigation::human
class CombinedDistance : public DistanceFunction
{
public:
-
/**
* @brief CombinedDistance Creates a new combined distance with the specified orientation scale factor
* @param maxOrientationInfluence the euclidean distance between the humans will be scaled
@@ -48,13 +46,13 @@ namespace armarx::navigation::human
* it to the total distance
*/
CombinedDistance(double maxOrientationInfluence, double movementInfluence);
- virtual double computeDistance(Human &h1, Human &h2);
+ double computeDistance(const Human& h1, const Human& h2) const override;
private:
- EuclideanDistance euclidean;
- OrientationDistance orientation;
- MovementDistance movement;
- double maxOrientationInfluence;
- double movementInfluence;
+ const EuclideanDistance euclidean;
+ const OrientationDistance orientation;
+ const MovementDistance movement;
+ const double maxOrientationInfluence;
+ const double movementInfluence;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
index b6cfd45d..f6ed6f42 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.cpp
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -10,42 +10,42 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian,
namespace armarx::navigation::human
{
- ConvexHullGenerator::ConvexHullGenerator()
- {
-
- }
+ ConvexHullGenerator::ConvexHullGenerator() = default;
- shapes::Polygon ConvexHullGenerator::createShape(const HumanGroup &group)
+ shapes::Polygon
+ ConvexHullGenerator::createShape(const HumanGroup& group) const
{
- if (group.humans.size() == 0)
+ if (group.humans.empty())
{
throw InvalidArgumentException("Human group size has to be at least 1");
}
using BoostPoly = boost::geometry::model::polygon<Point>;
- using GroupPoly = shapes::Polygon;
std::vector<Point> coordinates;
- for (Human human : group.humans)
+ for (const Human& human : group.humans)
{
- coordinates.push_back({human.pose.translation().x(), human.pose.translation().y()});
+ coordinates.emplace_back(human.pose.translation().x(), human.pose.translation().y());
}
- BoostPoly poly, hull;
+ BoostPoly poly;
+ const BoostPoly hull;
+
poly.outer().assign(coordinates.begin(), coordinates.end());
boost::geometry::convex_hull(poly, hull);
std::vector<Eigen::Vector2f> vertices;
- for (auto it = boost::begin(boost::geometry::exterior_ring(hull)); it != boost::end(boost::geometry::exterior_ring(hull)); ++it)
+ for (auto it = boost::begin(boost::geometry::exterior_ring(hull));
+ it != boost::end(boost::geometry::exterior_ring(hull));
+ ++it)
{
- double x = boost::geometry::get<0>(*it);
- double y = boost::geometry::get<1>(*it);
+ const double x = boost::geometry::get<0>(*it);
+ const double y = boost::geometry::get<1>(*it);
- vertices.push_back(Eigen::Vector2f(x, y));
+ vertices.emplace_back(x, y);
}
return {.vertices = vertices};
}
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
index 3ed1df62..b7f1ad57 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.h
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -22,7 +22,6 @@
#pragma once
#include "GroupShapeGenerator.h"
-
#include <armarx/navigation/human/types.h>
@@ -39,6 +38,6 @@ namespace armarx::navigation::human
public:
ConvexHullGenerator();
- virtual shapes::Polygon createShape(const HumanGroup &group);
+ shapes::Polygon createShape(const HumanGroup& group) const override;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
index 4f3a39e7..ecbc6155 100644
--- a/source/armarx/navigation/human/DistanceFunction.h
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -44,7 +44,6 @@ namespace armarx::navigation::human
* @param h2 the second human
* @return the distance between the humans (greater values correspond to humans that are further away)
*/
- virtual double computeDistance(Human &h1, Human &h2) = 0;
+ virtual double computeDistance(const Human& h1, const Human& h2) const = 0;
};
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
index 664ca172..911d59a9 100644
--- a/source/armarx/navigation/human/EuclideanDistance.cpp
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -2,9 +2,9 @@
namespace armarx::navigation::human
{
- double EuclideanDistance::computeDistance(Human &h1, Human &h2)
+ double
+ EuclideanDistance::computeDistance(const Human& h1, const Human& h2) const
{
return (h1.pose.translation() - h2.pose.translation()).norm();
}
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
index 31523e43..5c9bcb9a 100644
--- a/source/armarx/navigation/human/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -34,6 +34,6 @@ namespace armarx::navigation::human
{
public:
EuclideanDistance();
- virtual double computeDistance(Human &h1, Human &h2);
+ double computeDistance(const Human& h1, const Human& h2) const override;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/GroupShapeGenerator.h b/source/armarx/navigation/human/GroupShapeGenerator.h
index f5dc1430..4d42238e 100644
--- a/source/armarx/navigation/human/GroupShapeGenerator.h
+++ b/source/armarx/navigation/human/GroupShapeGenerator.h
@@ -42,6 +42,6 @@ namespace armarx::navigation::human
* @return a polygon describing the proxemic zone occupied by the given group (not including
* the zones of the individual members of the group)
*/
- virtual shapes::Polygon createShape(const HumanGroup &group) = 0;
+ virtual shapes::Polygon createShape(const HumanGroup& group) const = 0;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index 28e7825a..fd218353 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -1,55 +1,56 @@
+#include "HumanGrouper.h"
+
#include "CombinedDistance.h"
#include "ConvexHullGenerator.h"
-#include "HumanGrouper.h"
namespace armarx::navigation::human
{
- HumanGrouper::HumanGrouper(GroupingSettings settings) : settings(settings)
- {
- this->distanceFunction = new CombinedDistance(settings.maxOrientationInfluence);
- this->shapeGenerator = new ConvexHullGenerator();
- }
-
- HumanGrouper::~HumanGrouper()
+ HumanGrouper::HumanGrouper(const GroupingSettings& settings) :
+ distanceFunction(std::make_unique<CombinedDistance>(settings.maxOrientationInfluence,
+ settings.movementInfluence)),
+ shapeGenerator(std::make_unique<ConvexHullGenerator>()),
+ settings(settings)
{
- delete this->distanceFunction;
- delete this->shapeGenerator;
}
- void HumanGrouper::updateHumans(std::vector<human::Human> &newHumans)
+ void
+ HumanGrouper::updateHumans(const std::vector<human::Human>& newHumans)
{
- currentHumans = newHumans;
+ currentHumans_ = newHumans;
}
- std::vector<human::HumanGroup> HumanGrouper::getCurrentGroups()
+ std::vector<human::HumanGroup>
+ HumanGrouper::getCurrentGroups()
{
- DateTime now = DateTime::Now();
+ const DateTime now = DateTime::Now();
std::vector<human::HumanGroup> groups = {};
// maps index of human in currentHumans to index of group in groups
- std::unordered_map<int, int> humanGroupMap;
+ std::unordered_map<uint64_t, uint64_t> humanGroupMap;
- for (int parentIdx = 0; parentIdx < currentHumans.size(); parentIdx++)
+ for (uint64_t parentIdx = 0; parentIdx < currentHumans_.size(); parentIdx++)
{
- Human parent = currentHumans.at(parentIdx);
+ const Human parent = currentHumans_.at(parentIdx);
// if no group exists yet for this human, create one just for them
if (humanGroupMap.find(parentIdx) == humanGroupMap.end())
{
- human::HumanGroup group = {{{}}, {parent}, now};
+ const human::HumanGroup group = {{{}}, {parent}, now};
groups.push_back(group);
humanGroupMap[parentIdx] = groups.size() - 1;
}
- for (int childIdx = 0; childIdx < currentHumans.size(); childIdx++)
+ for (uint64_t childIdx = 0; childIdx < currentHumans_.size(); childIdx++)
{
- Human child = currentHumans.at(childIdx);
+ const Human child = currentHumans_.at(childIdx);
- if (this->distanceFunction->computeDistance(parent, child) < settings.groupingThreshold)
+ if (this->distanceFunction->computeDistance(parent, child) <
+ settings.groupingThreshold)
{
humanGroupMap[childIdx] = humanGroupMap[parentIdx];
- groups.at(humanGroupMap[childIdx]).humans.push_back(currentHumans.at(childIdx));
+ groups.at(humanGroupMap[childIdx])
+ .humans.push_back(currentHumans_.at(childIdx));
}
}
}
@@ -62,4 +63,4 @@ namespace armarx::navigation::human
return groups;
}
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index 1b517907..04123911 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -22,7 +22,7 @@
#pragma once
#include "DistanceFunction.h"
-#include "GroupShapeStrategy.h"
+#include "GroupShapeGenerator.h"
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
@@ -37,7 +37,7 @@ namespace armarx::navigation::human
class HumanGrouper
{
- using Human = armarx::navigation::human::Human;
+ using Human = armarx::navigation::human::Human;
public:
/**
@@ -47,17 +47,16 @@ namespace armarx::navigation::human
*/
struct GroupingSettings
{
- double groupingThreshold;
- double maxOrientationInfluence;
+ const double groupingThreshold;
+ const double maxOrientationInfluence;
+ const double movementInfluence;
};
/**
* @brief Creates a new HumanGrouper
* @param settings different parameters that will affect group generation
*/
- HumanGrouper(GroupingSettings settings);
-
- ~HumanGrouper();
+ HumanGrouper(const GroupingSettings& settings);
/**
* @brief Sets the currently detected humans.
@@ -66,7 +65,7 @@ namespace armarx::navigation::human
* when getCurrentGroups() is called the next time.
* @param newHumans a vector containing the most recently detected humans
*/
- void updateHumans(std::vector<Human> &newHumans);
+ void updateHumans(const std::vector<Human>& newHumans);
/**
* @brief Recognizes groups in the current humans.
@@ -78,10 +77,13 @@ namespace armarx::navigation::human
std::vector<human::HumanGroup> getCurrentGroups();
+ void setCurrentHumans(const std::vector<Human>& newCurrentHumans);
+
private:
- std::vector<Human> currentHumans;
- DistanceFunction *distanceFunction;
- GroupShapeGenerator *shapeGenerator;
- GroupingSettings settings;
+ const std::unique_ptr<DistanceFunction> distanceFunction;
+ const std::unique_ptr<GroupShapeGenerator> shapeGenerator;
+ const GroupingSettings settings;
+
+ std::vector<Human> currentHumans_;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.cpp b/source/armarx/navigation/human/MovementDistance.cpp
index 77c44322..a71bda4d 100644
--- a/source/armarx/navigation/human/MovementDistance.cpp
+++ b/source/armarx/navigation/human/MovementDistance.cpp
@@ -2,14 +2,11 @@
namespace armarx::navigation::human
{
- MovementDistance::MovementDistance()
- {
-
- }
+ MovementDistance::MovementDistance() = default;
- double MovementDistance::computeDistance(Human &h1, Human &h2)
+ double
+ MovementDistance::computeDistance(const Human& h1, const Human& h2) const
{
return (h1.linearVelocity - h2.linearVelocity).norm();
}
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/MovementDistance.h b/source/armarx/navigation/human/MovementDistance.h
index a7a1d87c..57fb065f 100644
--- a/source/armarx/navigation/human/MovementDistance.h
+++ b/source/armarx/navigation/human/MovementDistance.h
@@ -24,7 +24,6 @@
#include "DistanceFunction.h"
-
namespace armarx::navigation::human
{
/**
@@ -36,7 +35,6 @@ namespace armarx::navigation::human
public:
MovementDistance();
- virtual double computeDistance(Human &h1, Human &h2);
+ double computeDistance(const Human& h1, const Human& h2) const override;
};
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
index d33a0593..81f51f62 100644
--- a/source/armarx/navigation/human/OrientationDistance.cpp
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -1,54 +1,58 @@
#include "OrientationDistance.h"
+#include <boost/math/constants/constants.hpp>
+
namespace armarx::navigation::human
{
- OrientationDistance::OrientationDistance(double max, double min)
+ OrientationDistance::OrientationDistance(const double max, const double min) :
+ max(max), min(min)
{
- this->max = max;
- this->min = min;
}
- double OrientationDistance::computeDistance(Human &h1, Human &h2)
+ double
+ OrientationDistance::computeDistance(const Human& h1, const Human& h2) const
{
// ranges from 0 to 1
- double factor = getOrientationFactor(h1, h2);
+ const double factor = getOrientationFactor(h1, h2);
return min + factor * (max - min);
}
- double OrientationDistance::getOrientationFactor(Human &h1, Human &h2)
+ double
+ OrientationDistance::getOrientationFactor(const Human& h1, const Human& h2)
{
- double lineOrientation = getLineOrientation(h2, h1);
- double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
- double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
+ const double lineOrientation = getLineOrientation(h2, h1);
+ const double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
+ const double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
// assuming the angles to be in the interval [0, 2pi]
- double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
- double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
+ const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
+ const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
// ranges from 0 (looking directly at each other) to 1 (back to back)
return (deviationFirst + deviationSecond) / 2;
}
- double OrientationDistance::getLineOrientation(Human &h2, Human &h1)
+ double
+ OrientationDistance::getLineOrientation(const Human& h2, const Human& h1)
{
- double dx = (h1.pose.translation() - h2.pose.translation()).x();
- double dy = (h1.pose.translation() - h2.pose.translation()).y();
- double lineOrientation = atan2(dy, dx);
+ const double dx = (h1.pose.translation() - h2.pose.translation()).x();
+ const double dy = (h1.pose.translation() - h2.pose.translation()).y();
+ const double lineOrientation = atan2(dy, dx);
return lineOrientation;
}
// scales orientation to (-1, 1]
- double OrientationDistance::normOrientation(double orientation)
+ double
+ OrientationDistance::normOrientation(const double orientation)
{
- double normedOrientation = orientation / 3.1416;
+ double convertedOrientation = orientation / pi;
// brings orientation to [0, 2)
- normedOrientation = std::fmod(((std::fmod(normedOrientation, 2)) + 2), 2);
+ convertedOrientation = std::fmod(((std::fmod(convertedOrientation, 2)) + 2), 2);
// brings orientation to [-1, 1)
- normedOrientation -= (normedOrientation > 1) * 2;
- return normedOrientation;
+ convertedOrientation -= static_cast<int>(convertedOrientation > 1) * 2;
+ return convertedOrientation;
}
-}
-
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
index 135d871f..7684666f 100644
--- a/source/armarx/navigation/human/OrientationDistance.h
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -40,19 +40,22 @@ namespace armarx::navigation::human
class OrientationDistance : public DistanceFunction
{
public:
-
/**
* @brief OrientationDistance Creates a new OrientationDistance with the specified range
* @param max the distance two people will have when faced back to back
* @param min the distance two people will have when oriented face to face
*/
OrientationDistance(double max = 1, double min = 0);
- virtual double computeDistance(Human &h1, Human &h2);
+ double computeDistance(const Human& h1, const Human& h2) const override;
+
private:
- double getOrientationFactor(Human &h1, Human &h2);
- double getLineOrientation(Human &h2, Human &h1);
- double normOrientation(double orientation);
- double max;
- double min;
+ const double max;
+ const double min;
+
+ // helper methods
+ static double getOrientationFactor(const Human& h1, const Human& h2);
+ static double getLineOrientation(const Human& h2, const Human& h1);
+ static double normOrientation(double orientation);
+ static constexpr double pi = 3.1415926535898;
};
-}
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index a176e269..e5a7d2ac 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -24,8 +24,6 @@
#include <algorithm>
#include <vector>
-#include <range/v3/view/zip.hpp>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
@@ -33,6 +31,7 @@
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
+#include <range/v3/view/zip.hpp>
// test includes and other stuff
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
--
GitLab
From 2979fef6326769f7e8b4c5ec83d417e1caf15d29 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 1 Sep 2022 20:47:46 +0200
Subject: [PATCH 23/62] Start implementing tests
---
.../navigation/human/test/human_test.cpp | 250 ++++++++++--------
1 file changed, 137 insertions(+), 113 deletions(-)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index e5a7d2ac..9b7b9b7b 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -28,9 +28,8 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <SemanticObjectRelations/Shapes/Shape.h>
-#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/Trajectory.h>
-#include <armarx/navigation/core/types.h>
+#include <armarx/navigation/human/EuclideanDistance.h>
#include <range/v3/view/zip.hpp>
// test includes and other stuff
@@ -38,119 +37,144 @@
#define ARMARX_BOOST_TEST
#include <armarx/navigation/Test.h>
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/types.h>
-BOOST_AUTO_TEST_CASE(testPathLength)
-{
- armarx::navigation::core::Path path{
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
-
- const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
- BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
-}
+using armarx::navigation::core::Pose2D;
+using armarx::navigation::human::EuclideanDistance;
+using armarx::navigation::human::Human;
-BOOST_AUTO_TEST_CASE(testResampleAlongLine)
+BOOST_AUTO_TEST_CASE(testEuclideanDistance)
{
- armarx::navigation::core::Path path{
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
-
- const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
- BOOST_CHECK_EQUAL(traj.points().size(), 2);
-
- const auto resampledTraj = traj.resample(500);
-
- for (const auto& pt : resampledTraj.positions())
- {
- ARMARX_DEBUG << VAROUT(pt);
- }
-
- BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
+ EuclideanDistance distance = EuclideanDistance();
+
+ Pose2D pose1 = Pose2D::Identity();
+ // pose1.translation() = Eigen::Vector2f(3, 4);
+ // pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
+ // Human h1 = {.pose = pose1,
+ // .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ // .detectionTime = armarx::DateTime::Now()};
+
+ // Pose2D pose2 = Pose2D::Identity();
+ // pose2.translation() = Eigen::Vector2f(6, 8);
+ // pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+ // Human h2 = {.pose = pose2,
+ // .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ // .detectionTime = armarx::DateTime::Now()};
}
-BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
-{
- armarx::navigation::core::Path path{
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
- armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
-
- const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
- BOOST_CHECK_EQUAL(traj.points().size(), 3);
-
- const auto resampledTraj = traj.resample(500);
-
- for (const auto& pt : resampledTraj.positions())
- {
- ARMARX_DEBUG << VAROUT(pt);
- }
-
- BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
-}
-
-
-BOOST_AUTO_TEST_CASE(testGraphRoutes)
-{
- using namespace armarx::navigation;
-
- /*
- *
- * (0) -> (2) -> (3)
- * ^
- * |
- * (1)
- */
-
- core::Graph graph;
- graph.addVertex(semrel::ShapeID(0));
- graph.addVertex(semrel::ShapeID(1));
- graph.addVertex(semrel::ShapeID(2));
- graph.addVertex(semrel::ShapeID(3));
-
- graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
- graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
- graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
-
- const auto paths =
- armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
-
- // GT: paths
- // - {3}
- // - {2,3}
- // - {0,2,3}
- // - {1,2,3}
-
- BOOST_CHECK_EQUAL(paths.size(), 4);
-
- std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
-
- const auto isMatchingPath = [](const auto& path, const auto& gtPath)
- {
- if (path.size() != gtPath.size())
- {
- return false;
- }
-
- // check that sequences match
- for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
- {
- if (v.objectID().t != gtId)
- {
- return false;
- }
- }
-
- ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
- return true;
- };
-
- // check that all required paths are available
- for (const auto& gtPath : gtPaths)
- {
- BOOST_CHECK(std::any_of(paths.begin(),
- paths.end(),
- [>Path, &isMatchingPath](const auto& path)
- { return isMatchingPath(path, gtPath); }));
- }
-}
+//BOOST_AUTO_TEST_CASE(testPathLength)
+//{
+// armarx::navigation::core::Path path{
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
+
+// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+// BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
+//}
+
+//BOOST_AUTO_TEST_CASE(testResampleAlongLine)
+//{
+// armarx::navigation::core::Path path{
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
+
+// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+// BOOST_CHECK_EQUAL(traj.points().size(), 2);
+
+// const auto resampledTraj = traj.resample(500);
+
+// for (const auto& pt : resampledTraj.positions())
+// {
+// ARMARX_DEBUG << VAROUT(pt);
+// }
+
+// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
+//}
+
+//BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
+//{
+// armarx::navigation::core::Path path{
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
+// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
+
+// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
+// BOOST_CHECK_EQUAL(traj.points().size(), 3);
+
+// const auto resampledTraj = traj.resample(500);
+
+// for (const auto& pt : resampledTraj.positions())
+// {
+// ARMARX_DEBUG << VAROUT(pt);
+// }
+
+// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
+//}
+
+
+//BOOST_AUTO_TEST_CASE(testGraphRoutes)
+//{
+// using namespace armarx::navigation;
+
+// /*
+// *
+// * (0) -> (2) -> (3)
+// * ^
+// * |
+// * (1)
+// */
+
+// core::Graph graph;
+// graph.addVertex(semrel::ShapeID(0));
+// graph.addVertex(semrel::ShapeID(1));
+// graph.addVertex(semrel::ShapeID(2));
+// graph.addVertex(semrel::ShapeID(3));
+
+// graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
+// graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
+// graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
+
+// const auto paths =
+// armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
+
+// // GT: paths
+// // - {3}
+// // - {2,3}
+// // - {0,2,3}
+// // - {1,2,3}
+
+// BOOST_CHECK_EQUAL(paths.size(), 4);
+
+// std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
+
+// const auto isMatchingPath = [](const auto& path, const auto& gtPath)
+// {
+// if (path.size() != gtPath.size())
+// {
+// return false;
+// }
+
+// // check that sequences match
+// for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
+// {
+// if (v.objectID().t != gtId)
+// {
+// return false;
+// }
+// }
+
+// ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
+// return true;
+// };
+
+// // check that all required paths are available
+// for (const auto& gtPath : gtPaths)
+// {
+// BOOST_CHECK(std::any_of(paths.begin(),
+// paths.end(),
+// [>Path, &isMatchingPath](const auto& path)
+// { return isMatchingPath(path, gtPath); }));
+// }
+//}
--
GitLab
From 9ce933929986d2e8a85d0f735f5c7d02c11c0f43 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 12 Sep 2022 19:17:41 +0200
Subject: [PATCH 24/62] Fix destructors
---
source/armarx/navigation/human/ConvexHullGenerator.h | 3 ++-
source/armarx/navigation/human/DistanceFunction.h | 4 ++--
source/armarx/navigation/human/EuclideanDistance.cpp | 2 ++
source/armarx/navigation/human/GroupShapeGenerator.h | 4 ++--
4 files changed, 8 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.h b/source/armarx/navigation/human/ConvexHullGenerator.h
index b7f1ad57..d99d845d 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.h
+++ b/source/armarx/navigation/human/ConvexHullGenerator.h
@@ -36,7 +36,8 @@ namespace armarx::navigation::human
class ConvexHullGenerator : public GroupShapeGenerator
{
public:
- ConvexHullGenerator();
+ ConvexHullGenerator() = default;
+ virtual ~ConvexHullGenerator() = default;
shapes::Polygon createShape(const HumanGroup& group) const override;
};
diff --git a/source/armarx/navigation/human/DistanceFunction.h b/source/armarx/navigation/human/DistanceFunction.h
index ecbc6155..070d8974 100644
--- a/source/armarx/navigation/human/DistanceFunction.h
+++ b/source/armarx/navigation/human/DistanceFunction.h
@@ -35,8 +35,8 @@ namespace armarx::navigation::human
class DistanceFunction
{
public:
- DistanceFunction();
- virtual ~DistanceFunction();
+ DistanceFunction() = default;
+ virtual ~DistanceFunction() = default;
/**
* @brief computeDistance computes the (semantic or literal, depending on the implementation) distance between the given humans
diff --git a/source/armarx/navigation/human/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
index 911d59a9..5788a2f1 100644
--- a/source/armarx/navigation/human/EuclideanDistance.cpp
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -2,6 +2,8 @@
namespace armarx::navigation::human
{
+ EuclideanDistance::EuclideanDistance() = default;
+
double
EuclideanDistance::computeDistance(const Human& h1, const Human& h2) const
{
diff --git a/source/armarx/navigation/human/GroupShapeGenerator.h b/source/armarx/navigation/human/GroupShapeGenerator.h
index 4d42238e..34f777d9 100644
--- a/source/armarx/navigation/human/GroupShapeGenerator.h
+++ b/source/armarx/navigation/human/GroupShapeGenerator.h
@@ -33,8 +33,8 @@ namespace armarx::navigation::human
class GroupShapeGenerator
{
public:
- GroupShapeGenerator();
- virtual ~GroupShapeGenerator();
+ GroupShapeGenerator() = default;
+ virtual ~GroupShapeGenerator() = default;
/**
* @brief createShape creates and returns the shape describing the given group
--
GitLab
From 008820cad5d7ccfe901b4a614a908f0dffe024df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 14:56:09 +0200
Subject: [PATCH 25/62] Fix stuff for buildable state
---
source/armarx/navigation/human/ConvexHullGenerator.cpp | 5 ++---
source/armarx/navigation/human/EuclideanDistance.cpp | 2 --
source/armarx/navigation/human/EuclideanDistance.h | 2 +-
source/armarx/navigation/human/test/human_test.cpp | 4 ++--
4 files changed, 5 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
index f6ed6f42..56a933e0 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.cpp
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -10,8 +10,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D(Point, double, boost::geometry::cs::cartesian,
namespace armarx::navigation::human
{
- ConvexHullGenerator::ConvexHullGenerator() = default;
-
shapes::Polygon
ConvexHullGenerator::createShape(const HumanGroup& group) const
{
@@ -29,7 +27,7 @@ namespace armarx::navigation::human
}
BoostPoly poly;
- const BoostPoly hull;
+ BoostPoly hull;
poly.outer().assign(coordinates.begin(), coordinates.end());
boost::geometry::convex_hull(poly, hull);
@@ -47,5 +45,6 @@ namespace armarx::navigation::human
}
return {.vertices = vertices};
+
}
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/EuclideanDistance.cpp b/source/armarx/navigation/human/EuclideanDistance.cpp
index 5788a2f1..911d59a9 100644
--- a/source/armarx/navigation/human/EuclideanDistance.cpp
+++ b/source/armarx/navigation/human/EuclideanDistance.cpp
@@ -2,8 +2,6 @@
namespace armarx::navigation::human
{
- EuclideanDistance::EuclideanDistance() = default;
-
double
EuclideanDistance::computeDistance(const Human& h1, const Human& h2) const
{
diff --git a/source/armarx/navigation/human/EuclideanDistance.h b/source/armarx/navigation/human/EuclideanDistance.h
index 5c9bcb9a..c1d39ff1 100644
--- a/source/armarx/navigation/human/EuclideanDistance.h
+++ b/source/armarx/navigation/human/EuclideanDistance.h
@@ -33,7 +33,7 @@ namespace armarx::navigation::human
class EuclideanDistance : public DistanceFunction
{
public:
- EuclideanDistance();
+ EuclideanDistance() = default;
double computeDistance(const Human& h1, const Human& h2) const override;
};
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 9b7b9b7b..5b85e543 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -46,9 +46,9 @@ using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testEuclideanDistance)
{
- EuclideanDistance distance = EuclideanDistance();
+ // EuclideanDistance distance = EuclideanDistance();
- Pose2D pose1 = Pose2D::Identity();
+ // Pose2D pose1 = Pose2D::Identity();
// pose1.translation() = Eigen::Vector2f(3, 4);
// pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
// Human h1 = {.pose = pose1,
--
GitLab
From 89a5ac045fc81143f536b0a3eca2b35599fa84c0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 17:44:31 +0200
Subject: [PATCH 26/62] Fix unit test dependency
---
source/armarx/navigation/human/CMakeLists.txt | 2 ++
.../navigation/human/test/human_test.cpp | 34 +++++++++++--------
2 files changed, 21 insertions(+), 15 deletions(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 109d426b..c9fdd675 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -23,6 +23,7 @@ armarx_add_library(teb_human
CombinedDistance.cpp
ConvexHullGenerator.cpp
MovementDistance.cpp
+
HEADERS
types.h
aron_conversions.h
@@ -47,6 +48,7 @@ armarx_add_test(human_test
PUBLIC
ArmarXCore
armarx_navigation::core
+ armarx_navigation::human
PRIVATE
range-v3::range-v3
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 5b85e543..6d685d66 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -38,6 +38,7 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/Test.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
@@ -46,21 +47,24 @@ using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testEuclideanDistance)
{
- // EuclideanDistance distance = EuclideanDistance();
-
- // Pose2D pose1 = Pose2D::Identity();
- // pose1.translation() = Eigen::Vector2f(3, 4);
- // pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
- // Human h1 = {.pose = pose1,
- // .linearVelocity = Eigen::Vector2f(0.2, 0.1),
- // .detectionTime = armarx::DateTime::Now()};
-
- // Pose2D pose2 = Pose2D::Identity();
- // pose2.translation() = Eigen::Vector2f(6, 8);
- // pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
- // Human h2 = {.pose = pose2,
- // .linearVelocity = Eigen::Vector2f(0.5, 0.8),
- // .detectionTime = armarx::DateTime::Now()};
+ const EuclideanDistance distance = EuclideanDistance();
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(3, 4);
+ pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(6, 8);
+ pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ ARMARX_INFO << "Distance is" + std::to_string(d);
+ BOOST_CHECK_CLOSE(d, 4, 0.001);
}
//BOOST_AUTO_TEST_CASE(testPathLength)
--
GitLab
From f685e9df6f15c5288d265e8151ff26cd824066a4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 18:53:10 +0200
Subject: [PATCH 27/62] Remove leftover include from testing
---
source/armarx/navigation/human/test/human_test.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 6d685d66..eff91139 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -38,7 +38,6 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
-#include <armarx/navigation/human/Test.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
--
GitLab
From fb1e9ab87f63ffdf2639b04c7cb741b5bb009b9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 19:10:10 +0200
Subject: [PATCH 28/62] Fix test dependency
---
source/armarx/navigation/human/CMakeLists.txt | 2 +-
.../navigation/human/test/human_test.cpp | 24 +++++++++++++++++--
2 files changed, 23 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index c9fdd675..4a3035b4 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -48,7 +48,7 @@ armarx_add_test(human_test
PUBLIC
ArmarXCore
armarx_navigation::core
- armarx_navigation::human
+ armarx_navigation::teb_human
PRIVATE
range-v3::range-v3
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index eff91139..e7156bd7 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -38,13 +38,14 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/OrientationDistance.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::Human;
-BOOST_AUTO_TEST_CASE(testEuclideanDistance)
+BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
{
const EuclideanDistance distance = EuclideanDistance();
Pose2D pose1 = Pose2D::Identity();
@@ -63,9 +64,28 @@ BOOST_AUTO_TEST_CASE(testEuclideanDistance)
double d = distance.computeDistance(h1, h2);
ARMARX_INFO << "Distance is" + std::to_string(d);
- BOOST_CHECK_CLOSE(d, 4, 0.001);
+ BOOST_CHECK_CLOSE(d, 5, 0.001);
}
+//BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
+//{
+// const OrientationDistance distance = OrientationDistance();
+// Pose2D pose1 = Pose2D::Identity();
+// pose1.translation() = Eigen::Vector2f(0, 0);
+// pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+// const Human h1 = {.pose = pose1,
+// .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+// .detectionTime = armarx::DateTime::Now()};
+
+// Pose2D pose2 = Pose2D::Identity();
+// pose2.translation() = Eigen::Vector2f(1, 0);
+// pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+// const Human h2 = {.pose = pose2,
+// .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+// .detectionTime = armarx::DateTime::Now()};
+
+//}
+
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
// armarx::navigation::core::Path path{
--
GitLab
From 7bdddca100d3a634ca96b61be7831dc65186c5d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 19:12:45 +0200
Subject: [PATCH 29/62] Change map syntax for key availability check
---
source/armarx/navigation/human/HumanGrouper.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index fd218353..db4865e0 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::human
const Human parent = currentHumans_.at(parentIdx);
// if no group exists yet for this human, create one just for them
- if (humanGroupMap.find(parentIdx) == humanGroupMap.end())
+ if (humanGroupMap.count(parentIdx) == 0)
{
const human::HumanGroup group = {{{}}, {parent}, now};
groups.push_back(group);
--
GitLab
From e029009e625d193c85118dc4def4dab5d9444feb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 20:15:55 +0200
Subject: [PATCH 30/62] Change pi source from hardcoded to math.h
---
source/armarx/navigation/human/OrientationDistance.cpp | 8 +++++++-
1 file changed, 7 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
index 81f51f62..8eda0c3c 100644
--- a/source/armarx/navigation/human/OrientationDistance.cpp
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -1,6 +1,7 @@
#include "OrientationDistance.h"
#include <boost/math/constants/constants.hpp>
+#include <math.h>
namespace armarx::navigation::human
{
@@ -26,10 +27,15 @@ namespace armarx::navigation::human
const double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
const double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
+ printf("line orientation = %.2f\n", lineOrientation);
+ printf("angle h1 = %.2f, angle h2 = %.2f\n", angleH1, angleH2);
+
// assuming the angles to be in the interval [0, 2pi]
const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
+ printf("deviation first = %.2f, second = %.2f\n", deviationFirst, deviationSecond);
+
// ranges from 0 (looking directly at each other) to 1 (back to back)
return (deviationFirst + deviationSecond) / 2;
}
@@ -48,7 +54,7 @@ namespace armarx::navigation::human
double
OrientationDistance::normOrientation(const double orientation)
{
- double convertedOrientation = orientation / pi;
+ double convertedOrientation = orientation / M_PI;
// brings orientation to [0, 2)
convertedOrientation = std::fmod(((std::fmod(convertedOrientation, 2)) + 2), 2);
// brings orientation to [-1, 1)
--
GitLab
From 5f84402e45367a875dcbd228bf29a60403eef603 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 20:17:18 +0200
Subject: [PATCH 31/62] Remove unused constant
---
source/armarx/navigation/human/OrientationDistance.h | 1 -
1 file changed, 1 deletion(-)
diff --git a/source/armarx/navigation/human/OrientationDistance.h b/source/armarx/navigation/human/OrientationDistance.h
index 7684666f..3b8175e6 100644
--- a/source/armarx/navigation/human/OrientationDistance.h
+++ b/source/armarx/navigation/human/OrientationDistance.h
@@ -56,6 +56,5 @@ namespace armarx::navigation::human
static double getOrientationFactor(const Human& h1, const Human& h2);
static double getLineOrientation(const Human& h2, const Human& h1);
static double normOrientation(double orientation);
- static constexpr double pi = 3.1415926535898;
};
} // namespace armarx::navigation::human
--
GitLab
From c022189fcec8b1c40c7357e273d6cd6d5b14a470 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Wed, 21 Sep 2022 20:34:55 +0200
Subject: [PATCH 32/62] Fix bugs in OrientationDistance
---
.../navigation/human/OrientationDistance.cpp | 13 +++---
.../navigation/human/test/human_test.cpp | 43 +++++++++++--------
2 files changed, 30 insertions(+), 26 deletions(-)
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
index 8eda0c3c..2b70248b 100644
--- a/source/armarx/navigation/human/OrientationDistance.cpp
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -24,8 +24,8 @@ namespace armarx::navigation::human
OrientationDistance::getOrientationFactor(const Human& h1, const Human& h2)
{
const double lineOrientation = getLineOrientation(h2, h1);
- const double angleH1 = Eigen::Rotation2Dd(h1.pose.linear()).angle();
- const double angleH2 = Eigen::Rotation2Dd(h2.pose.linear()).angle();
+ const double angleH1 = normOrientation(Eigen::Rotation2Dd(h1.pose.linear()).angle() / M_PI);
+ const double angleH2 = normOrientation(Eigen::Rotation2Dd(h2.pose.linear()).angle() / M_PI);
printf("line orientation = %.2f\n", lineOrientation);
printf("angle h1 = %.2f, angle h2 = %.2f\n", angleH1, angleH2);
@@ -41,22 +41,21 @@ namespace armarx::navigation::human
}
double
- OrientationDistance::getLineOrientation(const Human& h2, const Human& h1)
+ OrientationDistance::getLineOrientation(const Human& h1, const Human& h2)
{
const double dx = (h1.pose.translation() - h2.pose.translation()).x();
const double dy = (h1.pose.translation() - h2.pose.translation()).y();
- const double lineOrientation = atan2(dy, dx);
+ const double lineOrientation = atan2(dy, dx) / M_PI;
return lineOrientation;
}
- // scales orientation to (-1, 1]
+ // brings orientation to (-1, 1]
double
OrientationDistance::normOrientation(const double orientation)
{
- double convertedOrientation = orientation / M_PI;
// brings orientation to [0, 2)
- convertedOrientation = std::fmod(((std::fmod(convertedOrientation, 2)) + 2), 2);
+ double convertedOrientation = std::fmod(((std::fmod(orientation, 2)) + 2), 2);
// brings orientation to [-1, 1)
convertedOrientation -= static_cast<int>(convertedOrientation > 1) * 2;
return convertedOrientation;
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index e7156bd7..64d3126b 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -30,7 +30,9 @@
#include <SemanticObjectRelations/Shapes/Shape.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/human/EuclideanDistance.h>
+#include <armarx/navigation/human/OrientationDistance.h>
#include <range/v3/view/zip.hpp>
+#include <math.h>
// test includes and other stuff
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
@@ -38,11 +40,11 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
-#include <armarx/navigation/human/OrientationDistance.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
using armarx::navigation::human::EuclideanDistance;
+using armarx::navigation::human::OrientationDistance;
using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -63,28 +65,31 @@ BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
.detectionTime = armarx::DateTime::Now()};
double d = distance.computeDistance(h1, h2);
- ARMARX_INFO << "Distance is" + std::to_string(d);
+ printf("Distance is %.2f\n", d);
BOOST_CHECK_CLOSE(d, 5, 0.001);
}
-//BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
-//{
-// const OrientationDistance distance = OrientationDistance();
-// Pose2D pose1 = Pose2D::Identity();
-// pose1.translation() = Eigen::Vector2f(0, 0);
-// pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
-// const Human h1 = {.pose = pose1,
-// .linearVelocity = Eigen::Vector2f(0.2, 0.1),
-// .detectionTime = armarx::DateTime::Now()};
-
-// Pose2D pose2 = Pose2D::Identity();
-// pose2.translation() = Eigen::Vector2f(1, 0);
-// pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
-// const Human h2 = {.pose = pose2,
-// .linearVelocity = Eigen::Vector2f(0.5, 0.8),
-// .detectionTime = armarx::DateTime::Now()};
+BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
+{
+ const OrientationDistance distance = OrientationDistance(1, 0);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(0, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
-//}
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(1, 0);
+ pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ printf("Distance is %.2f\n", d);
+ BOOST_CHECK(d < 0.001);
+}
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
--
GitLab
From 5ce512544eafff19032de74eebd7c2ee090246a3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 11:30:08 +0200
Subject: [PATCH 33/62] Remove debug prints
---
source/armarx/navigation/human/OrientationDistance.cpp | 5 -----
1 file changed, 5 deletions(-)
diff --git a/source/armarx/navigation/human/OrientationDistance.cpp b/source/armarx/navigation/human/OrientationDistance.cpp
index 2b70248b..07f98cf2 100644
--- a/source/armarx/navigation/human/OrientationDistance.cpp
+++ b/source/armarx/navigation/human/OrientationDistance.cpp
@@ -27,15 +27,10 @@ namespace armarx::navigation::human
const double angleH1 = normOrientation(Eigen::Rotation2Dd(h1.pose.linear()).angle() / M_PI);
const double angleH2 = normOrientation(Eigen::Rotation2Dd(h2.pose.linear()).angle() / M_PI);
- printf("line orientation = %.2f\n", lineOrientation);
- printf("angle h1 = %.2f, angle h2 = %.2f\n", angleH1, angleH2);
-
// assuming the angles to be in the interval [0, 2pi]
const double deviationFirst = std::abs(normOrientation(angleH1 - lineOrientation));
const double deviationSecond = std::abs(normOrientation(angleH2 - (lineOrientation + 1)));
- printf("deviation first = %.2f, second = %.2f\n", deviationFirst, deviationSecond);
-
// ranges from 0 (looking directly at each other) to 1 (back to back)
return (deviationFirst + deviationSecond) / 2;
}
--
GitLab
From 7ae36466a27f2b200cfb03db0817aac3a440aa07 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 11:31:39 +0200
Subject: [PATCH 34/62] Add tests
---
.../navigation/human/test/human_test.cpp | 46 +++++++++++++++++++
1 file changed, 46 insertions(+)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 64d3126b..6225a118 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -40,11 +40,13 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/MovementDistance.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::OrientationDistance;
+using armarx::navigation::human::MovementDistance;
using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -91,6 +93,50 @@ BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
BOOST_CHECK(d < 0.001);
}
+BOOST_AUTO_TEST_CASE(testOrientationDistance2_backToBack)
+{
+ const OrientationDistance distance = OrientationDistance(7, 0);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(2, 2);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 3);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ printf("Distance is %.2f\n", d);
+ BOOST_CHECK_CLOSE(d, 7, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
+{
+ const MovementDistance distance = MovementDistance();
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 3);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 5);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = distance.computeDistance(h1, h2);
+ printf("Distance is %.2f\n", d);
+ BOOST_CHECK_CLOSE(d, 0.5, 0.001);
+}
+
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
// armarx::navigation::core::Path path{
--
GitLab
From 3a5e3413b8405a690744c2e49ad7bedab501fc00 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 11:57:36 +0200
Subject: [PATCH 35/62] Add test for combined distance
---
.../navigation/human/CombinedDistance.cpp | 14 +++++++----
.../navigation/human/CombinedDistance.h | 3 ++-
.../navigation/human/test/human_test.cpp | 24 +++++++++++++++++++
3 files changed, 35 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/human/CombinedDistance.cpp b/source/armarx/navigation/human/CombinedDistance.cpp
index b44ba1ad..18330577 100644
--- a/source/armarx/navigation/human/CombinedDistance.cpp
+++ b/source/armarx/navigation/human/CombinedDistance.cpp
@@ -15,10 +15,14 @@ namespace armarx::navigation::human
double
CombinedDistance::computeDistance(const Human& h1, const Human& h2) const
{
- return euclidean.computeDistance(h1, h2)
- // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
- * (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2))
- // scales the euclidean distance by a factor in [1, inf) depending on influence
- * (1 + movementInfluence * movement.computeDistance(h1, h2));
+ double d = euclidean.computeDistance(h1, h2);
+
+ // scales the euclidean distance by a factor in [1, maxOrientationInfluence]
+ d *= (1 + (maxOrientationInfluence - 1) * orientation.computeDistance(h1, h2));
+
+ // scales the euclidean distance by a factor in [1, inf) depending on influence
+ d *= (1 + movementInfluence * movement.computeDistance(h1, h2));
+
+ return d;
}
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
index 5633a69f..33cb7bdb 100644
--- a/source/armarx/navigation/human/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -33,7 +33,8 @@ namespace armarx::navigation::human
* @brief The CombinedDistance class combines the standard euclidean distance multiplicatively
* with an orientation distance in such a way that the distance is always at least the euclidean
* distance but will be scaled by up to the maximum influence factor if the people face away
- * from each other.
+ * from each other. The distance will then again be increased by the scaled difference in movement
+ * between the two people. The distance will always be at least the euclidean distance.
*/
class CombinedDistance : public DistanceFunction
{
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 6225a118..85138163 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -40,6 +40,7 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/CombinedDistance.h>
#include <armarx/navigation/human/MovementDistance.h>
#include <armarx/navigation/human/types.h>
@@ -47,6 +48,7 @@ using armarx::navigation::core::Pose2D;
using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::OrientationDistance;
using armarx::navigation::human::MovementDistance;
+using armarx::navigation::human::CombinedDistance;
using armarx::navigation::human::Human;
BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -137,6 +139,28 @@ BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
BOOST_CHECK_CLOSE(d, 0.5, 0.001);
}
+BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
+{
+ const CombinedDistance combined = CombinedDistance(2, 3);
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(0, 0);
+ pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+ .detectionTime = armarx::DateTime::Now()};
+
+ double d = combined.computeDistance(h1, h2);
+ printf("Distance is %.2f\n", d);
+ BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
+}
+
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
// armarx::navigation::core::Path path{
--
GitLab
From 22047644ab945846bf3dbf27963623c5eb4682b9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 12:42:50 +0200
Subject: [PATCH 36/62] Add unit test for convex hull generation
---
.../navigation/human/ConvexHullGenerator.cpp | 1 -
.../navigation/human/test/human_test.cpp | 68 +++++++++++++++++--
2 files changed, 64 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/human/ConvexHullGenerator.cpp b/source/armarx/navigation/human/ConvexHullGenerator.cpp
index 56a933e0..2aff0df1 100644
--- a/source/armarx/navigation/human/ConvexHullGenerator.cpp
+++ b/source/armarx/navigation/human/ConvexHullGenerator.cpp
@@ -45,6 +45,5 @@ namespace armarx::navigation::human
}
return {.vertices = vertices};
-
}
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 85138163..9ea7ef96 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -21,6 +21,8 @@
* GNU General Public License
*/
+#include <math.h>
+
#include <algorithm>
#include <vector>
@@ -32,7 +34,6 @@
#include <armarx/navigation/human/EuclideanDistance.h>
#include <armarx/navigation/human/OrientationDistance.h>
#include <range/v3/view/zip.hpp>
-#include <math.h>
// test includes and other stuff
#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::core
@@ -41,15 +42,19 @@
#include <armarx/navigation/Test.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/CombinedDistance.h>
+#include <armarx/navigation/human/ConvexHullGenerator.h>
#include <armarx/navigation/human/MovementDistance.h>
#include <armarx/navigation/human/types.h>
using armarx::navigation::core::Pose2D;
-using armarx::navigation::human::EuclideanDistance;
-using armarx::navigation::human::OrientationDistance;
-using armarx::navigation::human::MovementDistance;
using armarx::navigation::human::CombinedDistance;
+using armarx::navigation::human::ConvexHullGenerator;
+using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::Human;
+using armarx::navigation::human::HumanGroup;
+using armarx::navigation::human::MovementDistance;
+using armarx::navigation::human::OrientationDistance;
+using armarx::navigation::human::shapes::Polygon;
BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
{
@@ -161,6 +166,61 @@ BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
}
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator_triangle)
+{
+ ConvexHullGenerator generator = ConvexHullGenerator();
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose3 = Pose2D::Identity();
+ pose3.translation() = Eigen::Vector2f(0, 2);
+ pose3.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h3 = {.pose = pose3,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose4 = Pose2D::Identity();
+ pose4.translation() = Eigen::Vector2f(0, 1);
+ pose4.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h4 = {.pose = pose4,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+
+ std::vector<Human> humans = {h1, h2, h3, h4};
+
+ HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+ Polygon hull = generator.createShape(group);
+
+ // Check that the vertices of the polygon contain all 3 positions from the outer 3 people
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose3.translation()) !=
+ hull.vertices.end());
+
+
+ // Check that the hull doesn't contain the inner person
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
+ hull.vertices.end());
+}
+
//BOOST_AUTO_TEST_CASE(testPathLength)
//{
// armarx::navigation::core::Path path{
--
GitLab
From 505fb3a551b03d0d8e33d2e0997e3b471351edce Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 12:43:50 +0200
Subject: [PATCH 37/62] Remove old commented out copy/paste code
---
.../navigation/human/test/human_test.cpp | 118 +-----------------
1 file changed, 1 insertion(+), 117 deletions(-)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 9ea7ef96..6af86894 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -166,7 +166,7 @@ BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
}
-BOOST_AUTO_TEST_CASE(testConvexHullGenerator_triangle)
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
{
ConvexHullGenerator generator = ConvexHullGenerator();
@@ -220,119 +220,3 @@ BOOST_AUTO_TEST_CASE(testConvexHullGenerator_triangle)
BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
hull.vertices.end());
}
-
-//BOOST_AUTO_TEST_CASE(testPathLength)
-//{
-// armarx::navigation::core::Path path{
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0)),
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 4000, 0))};
-
-// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
-// BOOST_CHECK_CLOSE(traj.length(), 4000, 0.01);
-//}
-
-//BOOST_AUTO_TEST_CASE(testResampleAlongLine)
-//{
-// armarx::navigation::core::Path path{
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2000, 0))};
-
-// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
-// BOOST_CHECK_EQUAL(traj.points().size(), 2);
-
-// const auto resampledTraj = traj.resample(500);
-
-// for (const auto& pt : resampledTraj.positions())
-// {
-// ARMARX_DEBUG << VAROUT(pt);
-// }
-
-// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 4);
-//}
-
-//BOOST_AUTO_TEST_CASE(testResampleAlongLineWithWaypoint)
-//{
-// armarx::navigation::core::Path path{
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 0, 0)),
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 1050, 0)),
-// armarx::navigation::core::Pose(Eigen::Translation3f(0, 2100, 0))};
-
-// const auto traj = armarx::navigation::core::GlobalTrajectory::FromPath(path, 100);
-// BOOST_CHECK_EQUAL(traj.points().size(), 3);
-
-// const auto resampledTraj = traj.resample(500);
-
-// for (const auto& pt : resampledTraj.positions())
-// {
-// ARMARX_DEBUG << VAROUT(pt);
-// }
-
-// BOOST_CHECK_EQUAL(resampledTraj.points().size(), 5);
-//}
-
-
-//BOOST_AUTO_TEST_CASE(testGraphRoutes)
-//{
-// using namespace armarx::navigation;
-
-// /*
-// *
-// * (0) -> (2) -> (3)
-// * ^
-// * |
-// * (1)
-// */
-
-// core::Graph graph;
-// graph.addVertex(semrel::ShapeID(0));
-// graph.addVertex(semrel::ShapeID(1));
-// graph.addVertex(semrel::ShapeID(2));
-// graph.addVertex(semrel::ShapeID(3));
-
-// graph.addEdge(semrel::ShapeID(0), semrel::ShapeID(2));
-// graph.addEdge(semrel::ShapeID(1), semrel::ShapeID(2));
-// graph.addEdge(semrel::ShapeID(2), semrel::ShapeID(3));
-
-// const auto paths =
-// armarx::navigation::core::findPathsTo(graph.vertex(semrel::ShapeID(3)), graph);
-
-// // GT: paths
-// // - {3}
-// // - {2,3}
-// // - {0,2,3}
-// // - {1,2,3}
-
-// BOOST_CHECK_EQUAL(paths.size(), 4);
-
-// std::vector<std::vector<int>> gtPaths{{3}, {2, 3}, {0, 2, 3}, {1, 2, 3}};
-
-// const auto isMatchingPath = [](const auto& path, const auto& gtPath)
-// {
-// if (path.size() != gtPath.size())
-// {
-// return false;
-// }
-
-// // check that sequences match
-// for (const auto& [v, gtId] : ranges::views::zip(path, gtPath))
-// {
-// if (v.objectID().t != gtId)
-// {
-// return false;
-// }
-// }
-
-// ARMARX_INFO << "Found matching path: " << VAROUT(path) << ", " << VAROUT(gtPath);
-// return true;
-// };
-
-// // check that all required paths are available
-// for (const auto& gtPath : gtPaths)
-// {
-// BOOST_CHECK(std::any_of(paths.begin(),
-// paths.end(),
-// [>Path, &isMatchingPath](const auto& path)
-// { return isMatchingPath(path, gtPath); }));
-// }
-//}
--
GitLab
From 63d4838a0ffa440d19eef1cf20777cf807ea9ea3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 15:15:40 +0200
Subject: [PATCH 38/62] Add grouping test and fix bugs
---
.../armarx/navigation/human/HumanGrouper.cpp | 8 ++-
source/armarx/navigation/human/HumanGrouper.h | 5 +-
.../navigation/human/test/human_test.cpp | 58 +++++++++++++++++++
source/armarx/navigation/human/types.cpp | 8 +++
source/armarx/navigation/human/types.h | 2 +
5 files changed, 75 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index db4865e0..96a436c3 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -1,5 +1,7 @@
#include "HumanGrouper.h"
+#include <unordered_set>
+
#include "CombinedDistance.h"
#include "ConvexHullGenerator.h"
@@ -45,8 +47,10 @@ namespace armarx::navigation::human
{
const Human child = currentHumans_.at(childIdx);
- if (this->distanceFunction->computeDistance(parent, child) <
- settings.groupingThreshold)
+ if ((humanGroupMap.count(childIdx) == 0 ||
+ humanGroupMap[childIdx] != humanGroupMap[parentIdx]) &&
+ this->distanceFunction->computeDistance(parent, child) <
+ settings.groupingThreshold)
{
humanGroupMap[childIdx] = humanGroupMap[parentIdx];
groups.at(humanGroupMap[childIdx])
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index 04123911..2318443d 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -71,14 +71,11 @@ namespace armarx::navigation::human
* @brief Recognizes groups in the current humans.
*
* Identifies and returns social groups in the humans given to this instance by the last
- * call to updateHumans.
+ * call to updateHumans. Will also return groups of size one for single people.
* @return a vector containing the recognized human groups
*/
std::vector<human::HumanGroup> getCurrentGroups();
-
- void setCurrentHumans(const std::vector<Human>& newCurrentHumans);
-
private:
const std::unique_ptr<DistanceFunction> distanceFunction;
const std::unique_ptr<GroupShapeGenerator> shapeGenerator;
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 6af86894..a29de484 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -43,6 +43,7 @@
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/CombinedDistance.h>
#include <armarx/navigation/human/ConvexHullGenerator.h>
+#include <armarx/navigation/human/HumanGrouper.h>
#include <armarx/navigation/human/MovementDistance.h>
#include <armarx/navigation/human/types.h>
@@ -52,6 +53,7 @@ using armarx::navigation::human::ConvexHullGenerator;
using armarx::navigation::human::EuclideanDistance;
using armarx::navigation::human::Human;
using armarx::navigation::human::HumanGroup;
+using armarx::navigation::human::HumanGrouper;
using armarx::navigation::human::MovementDistance;
using armarx::navigation::human::OrientationDistance;
using armarx::navigation::human::shapes::Polygon;
@@ -220,3 +222,59 @@ BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose4.translation()) ==
hull.vertices.end());
}
+
+BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
+{
+ /* ^ ^
+ * | |
+ * h1> <h2
+ *
+ * h3
+ * V
+ */
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(2, 1);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(3, 0.9);
+ pose2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 1.1),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose3 = Pose2D::Identity();
+ pose3.translation() = Eigen::Vector2f(2.5, 0);
+ pose3.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+ const Human h3 = {.pose = pose3,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ HumanGrouper::GroupingSettings settings = {
+ .groupingThreshold = 2, .maxOrientationInfluence = 3, .movementInfluence = 2};
+
+ HumanGrouper grouper = HumanGrouper(settings);
+
+ std::vector<Human> humans = {h1, h2, h3};
+ grouper.updateHumans(humans);
+
+ std::vector<HumanGroup> groups = grouper.getCurrentGroups();
+
+ BOOST_CHECK_EQUAL(groups.size(), 2);
+ HumanGroup g1 = groups.at(0);
+ HumanGroup g2 = groups.at(1);
+
+ BOOST_CHECK((g1.humans.size() == 2) != (g2.humans.size() == 2));
+ HumanGroup& groupOfTwo = g1.humans.size() == 2 ? g1 : g2;
+
+
+ // Check that the group of two contains h1 and h2
+ BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h1) !=
+ groupOfTwo.humans.end());
+ BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h2) !=
+ groupOfTwo.humans.end());
+}
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
index 854b0d15..b2ec8b13 100644
--- a/source/armarx/navigation/human/types.cpp
+++ b/source/armarx/navigation/human/types.cpp
@@ -14,6 +14,14 @@ namespace armarx::navigation::human
return estimation;
}
+ bool
+ Human::operator==(const Human& other) const
+ {
+ return other.pose.matrix() == this->pose.matrix() &&
+ other.linearVelocity == this->linearVelocity &&
+ other.detectionTime == this->detectionTime;
+ }
+
aron::data::DictPtr
Human::toAron() const
{
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index fe5490d5..ca768298 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -38,6 +38,8 @@ namespace armarx::navigation::human
core::Pose2D estimateAt(const DateTime& time) const;
+ bool operator==(const Human& other) const;
+
aron::data::DictPtr toAron() const;
static Human FromAron(const aron::data::DictPtr& dict);
};
--
GitLab
From 41bcab03f324be6ae039d450bf935134c6674049 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 16:10:08 +0200
Subject: [PATCH 39/62] Fix bugs
---
.../armarx/navigation/human/HumanGrouper.cpp | 2 +-
.../navigation/human/test/human_test.cpp | 40 ++++++++++++++++---
2 files changed, 36 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/human/HumanGrouper.cpp b/source/armarx/navigation/human/HumanGrouper.cpp
index 96a436c3..281a77d4 100644
--- a/source/armarx/navigation/human/HumanGrouper.cpp
+++ b/source/armarx/navigation/human/HumanGrouper.cpp
@@ -59,7 +59,7 @@ namespace armarx::navigation::human
}
}
- for (human::HumanGroup group : groups)
+ for (human::HumanGroup& group : groups)
{
group.shape = shapeGenerator->createShape(group);
}
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index a29de484..55032523 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -76,7 +76,6 @@ BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
.detectionTime = armarx::DateTime::Now()};
double d = distance.computeDistance(h1, h2);
- printf("Distance is %.2f\n", d);
BOOST_CHECK_CLOSE(d, 5, 0.001);
}
@@ -98,7 +97,6 @@ BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
.detectionTime = armarx::DateTime::Now()};
double d = distance.computeDistance(h1, h2);
- printf("Distance is %.2f\n", d);
BOOST_CHECK(d < 0.001);
}
@@ -120,7 +118,6 @@ BOOST_AUTO_TEST_CASE(testOrientationDistance2_backToBack)
.detectionTime = armarx::DateTime::Now()};
double d = distance.computeDistance(h1, h2);
- printf("Distance is %.2f\n", d);
BOOST_CHECK_CLOSE(d, 7, 0.001);
}
@@ -142,7 +139,6 @@ BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
.detectionTime = armarx::DateTime::Now()};
double d = distance.computeDistance(h1, h2);
- printf("Distance is %.2f\n", d);
BOOST_CHECK_CLOSE(d, 0.5, 0.001);
}
@@ -164,7 +160,6 @@ BOOST_AUTO_TEST_CASE(testCombinedDistance1_threeDistances)
.detectionTime = armarx::DateTime::Now()};
double d = combined.computeDistance(h1, h2);
- printf("Distance is %.2f\n", d);
BOOST_CHECK_CLOSE(d, 2 * 1.5 * 2.5, 0.001);
}
@@ -223,6 +218,38 @@ BOOST_AUTO_TEST_CASE(testConvexHullGenerator1_triangle)
hull.vertices.end());
}
+BOOST_AUTO_TEST_CASE(testConvexHullGenerator2_line)
+{
+ ConvexHullGenerator generator = ConvexHullGenerator();
+
+ Pose2D pose1 = Pose2D::Identity();
+ pose1.translation() = Eigen::Vector2f(-2, 0);
+ pose1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h1 = {.pose = pose1,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ Pose2D pose2 = Pose2D::Identity();
+ pose2.translation() = Eigen::Vector2f(2, 0);
+ pose2.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ const Human h2 = {.pose = pose2,
+ .linearVelocity = Eigen::Vector2f(0, 0),
+ .detectionTime = armarx::DateTime::Now()};
+
+ std::vector<Human> humans = {h1, h2};
+
+ HumanGroup group = {.shape = {{}}, .humans = humans, .detectionTime = armarx::DateTime::Now()};
+
+ Polygon hull = generator.createShape(group);
+
+ // Check that the vertices of the polygon contains both positions
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose1.translation()) !=
+ hull.vertices.end());
+
+ BOOST_CHECK(std::find(hull.vertices.begin(), hull.vertices.end(), pose2.translation()) !=
+ hull.vertices.end());
+}
+
BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
{
/* ^ ^
@@ -277,4 +304,7 @@ BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
groupOfTwo.humans.end());
BOOST_CHECK(std::find(groupOfTwo.humans.begin(), groupOfTwo.humans.end(), h2) !=
groupOfTwo.humans.end());
+
+ // Check that a shape was set (the detailed convex hull generation is tested seperatly)
+ BOOST_CHECK(groupOfTwo.shape.vertices.size() > 0);
}
--
GitLab
From 2c47c97c08abf19405a0a71838e0ec00d2946843 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Thu, 22 Sep 2022 16:28:24 +0200
Subject: [PATCH 40/62] Add documentation
---
source/armarx/navigation/human/CombinedDistance.h | 8 ++++++++
source/armarx/navigation/human/HumanGrouper.h | 9 +++++++--
source/armarx/navigation/human/test/human_test.cpp | 5 ++---
3 files changed, 17 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/human/CombinedDistance.h b/source/armarx/navigation/human/CombinedDistance.h
index 33cb7bdb..4cacc38d 100644
--- a/source/armarx/navigation/human/CombinedDistance.h
+++ b/source/armarx/navigation/human/CombinedDistance.h
@@ -47,6 +47,14 @@ namespace armarx::navigation::human
* it to the total distance
*/
CombinedDistance(double maxOrientationInfluence, double movementInfluence);
+
+ /**
+ * @brief computeDistance computes the distance value for the two given people. Should
+ * always be commutative, i.e. computeDistance(h1, h2) = computeDistance(h2, h1)
+ * @param h1 the first human
+ * @param h2 the second human
+ * @return a value specifying a distance between the two people
+ */
double computeDistance(const Human& h1, const Human& h2) const override;
private:
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index 2318443d..0a3e4e92 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -32,11 +32,11 @@ namespace armarx::navigation::human
* @brief Identifies social interaction groups in a collection of detected humans.
*
* Can identify social interaction groups in a collection of humans. The currently detected
- * humans can be set by calling updateHumans.
+ * humans can be set by calling updateHumans. The grouper will also return groups of size 1,
+ * and every detected human will be in exactly one group.
*/
class HumanGrouper
{
-
using Human = armarx::navigation::human::Human;
public:
@@ -47,8 +47,13 @@ namespace armarx::navigation::human
*/
struct GroupingSettings
{
+ // people strictly closer than this distance will be grouped
const double groupingThreshold;
+
+ // distances between humans will be scaled by up to this factor based on their orientation
const double maxOrientationInfluence;
+
+ // distances between humans will be scaled by their movement difference depending on this factor
const double movementInfluence;
};
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index 55032523..b7b92a9f 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -13,9 +13,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package Navigation::ArmarXObjects::core
- * @author Fabian Reister ( fabian dot reister at kit dot edu )
- * @author Christian R. G. Dreher ( c dot dreher at kit dot edu )
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -307,4 +305,5 @@ BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
// Check that a shape was set (the detailed convex hull generation is tested seperatly)
BOOST_CHECK(groupOfTwo.shape.vertices.size() > 0);
+ u
}
--
GitLab
From 7f19fa53c869132988d43539e1a4b7ddb44ec6a5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 24 Oct 2022 15:31:00 +0200
Subject: [PATCH 41/62] Use HumanGrouper in dynamic scene provider
still missing a way to write the groups like the human writer plugin
---
.../dynamic_scene_provider/Component.cpp | 53 ++++++++++++-------
.../dynamic_scene_provider/Component.h | 3 ++
2 files changed, 38 insertions(+), 18 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 23a58b26..1a56743f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -36,6 +36,7 @@
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
@@ -76,18 +77,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
// def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
// Add an optionalproperty.
- def->optional(properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
-
- def->optional(properties.laserScannerFeatures.providerName, "p.laserScannerFeatures.providerName", "");
+ def->optional(
+ properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+
+ def->optional(properties.laserScannerFeatures.providerName,
+ "p.laserScannerFeatures.providerName",
+ "");
def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
-
+
def->optional(properties.robot.name, "p.robot.name", "");
-
+
def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
- def->optional(properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
- def->optional(properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
-
+ def->optional(
+ properties.occupancyGrid.freespaceThreshold, "p.occupancyGrid.freespaceThreshold", "");
+ def->optional(
+ properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
+
def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
return def;
@@ -137,10 +143,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
*/
- while(true)
+ while (true)
{
robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
- if(robot != nullptr)
+ if (robot != nullptr)
{
break;
}
@@ -205,12 +211,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Querying humans";
- const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
- .timestamp = timestamp, .maxAge = Duration::MilliSeconds(500)};
+ const armem::human::client::Reader::Query humanPoseQuery{
+ .providerName = properties.humanPoseProvider,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
const armem::human::client::Reader::Result humanPoseResult =
humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success) << humanPoseResult.errorMessage;
+ ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success)
+ << humanPoseResult.errorMessage;
ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
@@ -265,10 +274,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Querying costmap";
- const memory::client::costmap::Reader::Query costmapQuery{.providerName =
- "distance_to_obstacle_costmap_provider", // TODO check
- .name = "distance_to_obstacles",
- .timestamp = timestamp};
+ const memory::client::costmap::Reader::Query costmapQuery{
+ .providerName = "distance_to_obstacle_costmap_provider", // TODO check
+ .name = "distance_to_obstacles",
+ .timestamp = timestamp};
const memory::client::costmap::Reader::Result costmapResult =
costmapReaderPlugin->get().query(costmapQuery);
@@ -345,9 +354,17 @@ namespace armarx::navigation::components::dynamic_scene_provider
humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
.humanPoses = humanPoseResult.humanPoses});
+ using Human = armarx::navigation::human::Human;
+ std::vector<Human> humans = humanTracker.getTrackedHumans();
+
+ humanGrouper.updateHumans(humans);
+
+ using HumanGroup = armarx::navigation::human::HumanGroup;
+ std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
+ // TODO: store groups somewhere
- humanWriterPlugin->get().store(humanTracker.getTrackedHumans(),getName(), timestamp);
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index bcc3ad7d..26326fb5 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -51,6 +51,7 @@
#include "armarx/navigation/memory/client/costmap/Reader.h"
#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
+#include <armarx/navigation/human/HumanGrouper.h>
namespace armarx::navigation::components::dynamic_scene_provider
@@ -206,6 +207,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
HumanTracker humanTracker;
+ using HumanGrouper = armarx::navigation::human::HumanGrouper;
+ HumanGrouper humanGrouper;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From c41f3cd8e0a4f86cc4fd91b2044c21425522cc49 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 24 Oct 2022 15:40:54 +0200
Subject: [PATCH 42/62] Add human grouper info prints in component
---
.../navigation/components/dynamic_scene_provider/Component.cpp | 3 +++
1 file changed, 3 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 1a56743f..4e311ea6 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -357,8 +357,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
using Human = armarx::navigation::human::Human;
std::vector<Human> humans = humanTracker.getTrackedHumans();
+ ARMARX_INFO << "Providing grouper with new humans";
+
humanGrouper.updateHumans(humans);
+ ARMARX_INFO << "Running human grouper";
using HumanGroup = armarx::navigation::human::HumanGroup;
std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
--
GitLab
From 213b53ae89ad49ad1fc8a792c4cbf08d8354f9bf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 25 Nov 2022 12:21:53 +0100
Subject: [PATCH 43/62] Store human groups using humanWriterPlugin
---
.../components/dynamic_scene_provider/Component.cpp | 5 ++---
1 file changed, 2 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 4e311ea6..f4e05a7d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -356,6 +356,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
.humanPoses = humanPoseResult.humanPoses});
using Human = armarx::navigation::human::Human;
std::vector<Human> humans = humanTracker.getTrackedHumans();
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
ARMARX_INFO << "Providing grouper with new humans";
@@ -365,9 +366,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
using HumanGroup = armarx::navigation::human::HumanGroup;
std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
- // TODO: store groups somewhere
-
- humanWriterPlugin->get().store(humans, getName(), timestamp);
+ humanWriterPlugin->get().store(groups, getName(), timestamp);
}
--
GitLab
From ab40a345a806b9a9a992fc3a037ba8e02e98c8be Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 25 Nov 2022 12:44:19 +0100
Subject: [PATCH 44/62] Start implementing group visualization
---
.../navigation_memory/Component.cpp | 18 ++--
.../components/navigation_memory/Visu.cpp | 85 +++++++++++++++++--
.../components/navigation_memory/Visu.h | 5 +-
source/armarx/navigation/memory/constants.h | 1 +
4 files changed, 94 insertions(+), 15 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index 13f1fe8c..646445ea 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -41,16 +41,16 @@
#include "Visu.h"
#include <armarx/navigation/algorithms/Costmap.h>
-#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
#include <armarx/navigation/core/aron/Twist.aron.generated.h>
-#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::components::navigation_memory
@@ -440,11 +440,13 @@ namespace armarx::navigation::components::navigation_memory
void
Component::visuRun()
{
- memory::Visu visu{arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
- workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
+ memory::Visu visu{
+ arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanGroupCoreSegmentName)};
Properties::LocationGraph p;
@@ -484,4 +486,4 @@ namespace armarx::navigation::components::navigation_memory
}
-} // namespace armarx::navigation::components::navigation_memory
+} // namespace armarx::navigation::components::navigation_memory
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 2b5e1aa0..17fd6b73 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -21,6 +21,7 @@
*/
#include "Visu.h"
+
#include <Eigen/src/Geometry/Translation.h>
#include <SimoxUtility/color/Color.h>
@@ -52,12 +53,14 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment) :
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
humanSegment(humanSegment),
+ humanGroupSegment(humanGroupSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -180,7 +183,7 @@ namespace armarx::navigation::memory
visualize(const human::Humans& humans, viz::Layer& layer)
{
- const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0,0, 1000});
+ const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
ARMARX_INFO << "Visualizing " << humans.size() << " humans";
for (const auto& human : humans)
@@ -198,10 +201,38 @@ namespace armarx::navigation::memory
viz::Robot mmm(std::to_string(layer.size()));
mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
mmm.pose(conv::to3D(human.pose) * human_T_mmm);
- mmm.scale(1.7); // 1.7m
+ mmm.scale(1.7); // 1.7m
mmm.overrideColor(viz::Color::orange(255, 100));
layer.add(mmm);
+ }
+ }
+
+ void
+ visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
+ {
+
+ const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
+
+ for (const auto& humanGroup : humanGroups)
+ {
+ viz::Polygon polygon();
+ polygon.addPoints(humanGroup.shape.vertices);
+ // viz::Cylinder cylinder(std::to_string(layer.size()));
+ // cylinder.fromTo(conv::to3D(human.pose.translation()),
+ // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+
+ // cylinder.color(simox::Color::orange());
+ // cylinder.radius(300);
+ // layer.add(cylinder);
+
+
+ viz::Robot mmm(std::to_string(layer.size()));
+ mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
+ mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.scale(1.7); // 1.7m
+ mmm.overrideColor(viz::Color::orange(255, 100));
+ layer.add(mmm);
}
}
@@ -266,7 +297,8 @@ namespace armarx::navigation::memory
[&](const wm::Entity& entity)
{
entity.getLatestSnapshot().forEachInstance(
- [&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
+ [&namedProviderHumans](
+ const armarx::armem::wm::EntityInstance& instance)
{
const auto dto =
navigation::human::arondto::Human::FromAron(instance.data());
@@ -274,8 +306,8 @@ namespace armarx::navigation::memory
navigation::human::Human human;
fromAron(dto, human);
- namedProviderHumans[instance.id().providerSegmentName]
- .emplace_back(std::move(human));
+ namedProviderHumans[instance.id().providerSegmentName].emplace_back(
+ std::move(human));
});
});
});
@@ -287,5 +319,46 @@ namespace armarx::navigation::memory
}
}
+ void
+ Visu::drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::HumanGroups> namedProviderHumanGroups;
+
+ humanGroupSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanGroupSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumanGroups](
+ const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+ instance.data());
+
+ navigation::human::HumanGroup humanGroup;
+ fromAron(dto, humanGroup);
+
+ namedProviderHumanGroups[instance.id().providerSegmentName]
+ .emplace_back(std::move(humanGroup));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humanGroups] : namedProviderHumanGroups)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ visualize(humanGroups, layer);
+ }
+ }
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 71c057df..d8f6002b 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -48,7 +48,8 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment);
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment);
~Visu();
@@ -56,6 +57,7 @@ namespace armarx::navigation::memory
void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
+ void drawHumanGroups(std::vector<viz::Layer>& layers, bool enabled);
viz::ScopedClient arviz;
@@ -64,6 +66,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
const armem::server::wm::CoreSegment& humanSegment;
+ const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 2b3364a8..7f3b3287 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,5 +33,6 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
+ inline const std::string HumanGroupCoreSegmentName = "HumanGroup";
} // namespace armarx::navigation::memory::constants
--
GitLab
From 8c3cb06fcabf7197ba3a7459050143472ec8a9e2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 29 Nov 2022 17:55:47 +0100
Subject: [PATCH 45/62] Implement polygon drawing
---
.../dynamic_scene_provider/Component.cpp | 2 ++
.../components/navigation_memory/Visu.cpp | 29 +++++++------------
2 files changed, 13 insertions(+), 18 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index f4e05a7d..9b15b960 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -366,6 +366,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
using HumanGroup = armarx::navigation::human::HumanGroup;
std::vector<HumanGroup> groups = humanGrouper.getCurrentGroups();
+ ARMARX_INFO << "Detected " << groups.size() << " human groups";
+
humanWriterPlugin->get().store(groups, getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index b0078f58..0b7beb92 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -215,24 +215,17 @@ namespace armarx::navigation::memory
for (const auto& humanGroup : humanGroups)
{
- viz::Polygon polygon();
- polygon.addPoints(humanGroup.shape.vertices);
- // viz::Cylinder cylinder(std::to_string(layer.size()));
- // cylinder.fromTo(conv::to3D(human.pose.translation()),
- // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
-
-
- // cylinder.color(simox::Color::orange());
- // cylinder.radius(300);
- // layer.add(cylinder);
-
+ viz::Polygon polygon(std::to_string(layer.size()));
- viz::Robot mmm(std::to_string(layer.size()));
- mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
- mmm.pose(conv::to3D(human.pose) * human_T_mmm);
- mmm.scale(1.7); // 1.7m
- mmm.overrideColor(viz::Color::orange(255, 100));
- layer.add(mmm);
+ const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
+ std::vector<Eigen::Vector3f> verts3D;
+ for (const auto& v2d : verts2D)
+ {
+ verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 0));
+ }
+ polygon.points(verts3D);
+ polygon.color(simox::Color::yellow());
+ layer.add(polygon);
}
}
@@ -357,7 +350,7 @@ namespace armarx::navigation::memory
for (const auto& [providerName, humanGroups] : namedProviderHumanGroups)
{
- viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humanGroups_" + providerName));
visualize(humanGroups, layer);
}
}
--
GitLab
From 1717f82cb52f6841282f0ffe8b3f434f82afa8f0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 29 Nov 2022 18:23:52 +0100
Subject: [PATCH 46/62] Initialize human grouper with default settings
---
.../components/dynamic_scene_provider/Component.cpp | 2 +-
source/armarx/navigation/human/HumanGrouper.h | 6 +++---
2 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 49ab3da1..bb815477 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -47,7 +47,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
const std::string Component::defaultName = "dynamic_scene_provider";
- Component::Component()
+ Component::Component() : humanGrouper(HumanGrouper::GroupingSettings())
{
addPlugin(humanPoseReaderPlugin);
addPlugin(laserScannerFeaturesReaderPlugin);
diff --git a/source/armarx/navigation/human/HumanGrouper.h b/source/armarx/navigation/human/HumanGrouper.h
index 0a3e4e92..fc3fe60d 100644
--- a/source/armarx/navigation/human/HumanGrouper.h
+++ b/source/armarx/navigation/human/HumanGrouper.h
@@ -48,13 +48,13 @@ namespace armarx::navigation::human
struct GroupingSettings
{
// people strictly closer than this distance will be grouped
- const double groupingThreshold;
+ const double groupingThreshold = 2000;
// distances between humans will be scaled by up to this factor based on their orientation
- const double maxOrientationInfluence;
+ const double maxOrientationInfluence = 1.5;
// distances between humans will be scaled by their movement difference depending on this factor
- const double movementInfluence;
+ const double movementInfluence = 1.5;
};
/**
--
GitLab
From bab5d1584126e55f7e240261fafafc6f4a6686d0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 29 Nov 2022 18:25:00 +0100
Subject: [PATCH 47/62] Fix redundant parantheses that got in by merge
---
source/armarx/navigation/human/CMakeLists.txt | 3 ---
1 file changed, 3 deletions(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 60d27e33..aa8f69ee 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -46,9 +46,6 @@ armarx_add_library(teb_human
HumanFilter.h
)
-
-)
-
find_package(sciplot)
armarx_add_test(so2_kalman_test
--
GitLab
From 8c4f4df381da7bac35eab77747559ee210a875ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 29 Nov 2022 18:38:15 +0100
Subject: [PATCH 48/62] Fix random 'u'
---
source/armarx/navigation/human/test/human_test.cpp | 1 -
1 file changed, 1 deletion(-)
diff --git a/source/armarx/navigation/human/test/human_test.cpp b/source/armarx/navigation/human/test/human_test.cpp
index b7b92a9f..862c7e3b 100644
--- a/source/armarx/navigation/human/test/human_test.cpp
+++ b/source/armarx/navigation/human/test/human_test.cpp
@@ -305,5 +305,4 @@ BOOST_AUTO_TEST_CASE(testHumanGrouper1_twoOfThree)
// Check that a shape was set (the detailed convex hull generation is tested seperatly)
BOOST_CHECK(groupOfTwo.shape.vertices.size() > 0);
- u
}
--
GitLab
From 7017dcab8b4b4ed75d9f8a0ae378be8154dd72ba Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 5 Dec 2022 16:16:59 +0100
Subject: [PATCH 49/62] Use human segment instead of special human group
as a human group segment does not exist and the writer writes it in the
human segment
---
.../components/navigation_memory/Component.cpp | 15 ++++++++-------
.../components/navigation_memory/Component.h | 1 +
.../components/navigation_memory/Visu.cpp | 11 +++--------
.../components/navigation_memory/Visu.h | 4 +---
source/armarx/navigation/memory/constants.h | 1 -
5 files changed, 13 insertions(+), 19 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index c6519b81..c75d9b2b 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -421,13 +421,11 @@ namespace armarx::navigation::components::navigation_memory
void
Component::visuRun()
{
- memory::Visu visu{
- arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
- workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
- workingMemory().getCoreSegment(memory::constants::HumanGroupCoreSegmentName)};
+ memory::Visu visu{arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
Properties::LocationGraph p;
@@ -460,6 +458,9 @@ namespace armarx::navigation::components::navigation_memory
// Humans
visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
+ // Groups
+ visu.drawHumanGroups(layers, p.visuHumanGroups);
+
arviz.commit(layers);
metronome.waitForNextTick();
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index b1b1a1b6..619be519 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -105,6 +105,7 @@ namespace armarx::navigation::components::navigation_memory
bool visuGraphEdges = true;
bool visuCostmaps = true;
bool visuHumans = true;
+ bool visuHumanGroups = true;
bool visuTransparent = false;
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index f8d8e576..3706262c 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -53,14 +53,12 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment,
- const armem::server::wm::CoreSegment& humanGroupSegment) :
+ const armem::server::wm::CoreSegment& humanSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
humanSegment(humanSegment),
- humanGroupSegment(humanGroupSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -218,9 +216,6 @@ namespace armarx::navigation::memory
void
visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
{
-
- const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
-
for (const auto& humanGroup : humanGroups)
{
viz::Polygon polygon(std::to_string(layer.size()));
@@ -332,12 +327,12 @@ namespace armarx::navigation::memory
std::map<std::string, navigation::human::HumanGroups> namedProviderHumanGroups;
- humanGroupSegment.doLocked(
+ humanSegment.doLocked(
[&]()
{
using namespace armem::server;
- humanGroupSegment.forEachEntity(
+ humanSegment.forEachEntity(
[&](const wm::Entity& entity)
{
entity.getLatestSnapshot().forEachInstance(
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 55b0e35f..86666011 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -48,8 +48,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment,
- const armem::server::wm::CoreSegment& humanGroupSegment);
+ const armem::server::wm::CoreSegment& humanSegment);
~Visu();
@@ -66,7 +65,6 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
const armem::server::wm::CoreSegment& humanSegment;
- const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 23c01a1c..f0932218 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,7 +33,6 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
- inline const std::string HumanGroupCoreSegmentName = "HumanGroup";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
--
GitLab
From 7e2413c56f4158d4ce84216e59e3b6f55043222d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 9 Dec 2022 12:32:07 +0100
Subject: [PATCH 50/62] Split human/human group reader/writer
Since human groups are now stored in their own core segment (as
discussed with Fabian) for clearer distinction between humans and human
groups and a cleaner structure.
---
.../dynamic_scene_provider/Component.cpp | 36 ++++-
.../dynamic_scene_provider/Component.h | 3 +
.../navigation_memory/Component.cpp | 4 +-
.../armarx/navigation/memory/CMakeLists.txt | 4 +
.../navigation/memory/client/human/Reader.cpp | 106 -------------
.../navigation/memory/client/human/Reader.h | 21 ---
.../navigation/memory/client/human/Writer.cpp | 62 +-------
.../navigation/memory/client/human/Writer.h | 7 +-
.../memory/client/human_group/Reader.cpp | 146 ++++++++++++++++++
.../memory/client/human_group/Reader.h | 77 +++++++++
.../memory/client/human_group/Writer.cpp | 83 ++++++++++
.../memory/client/human_group/Writer.h | 64 ++++++++
source/armarx/navigation/memory/constants.h | 1 +
13 files changed, 416 insertions(+), 198 deletions(-)
create mode 100644 source/armarx/navigation/memory/client/human_group/Reader.cpp
create mode 100644 source/armarx/navigation/memory/client/human_group/Reader.h
create mode 100644 source/armarx/navigation/memory/client/human_group/Writer.cpp
create mode 100644 source/armarx/navigation/memory/client/human_group/Writer.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 82738c5a..2bd77ba3 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -56,6 +56,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
addPlugin(humanWriterPlugin);
+ addPlugin(humanGroupWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -243,6 +244,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
+ ARMARX_INFO << "Not querying laser scanner features for testing purposes (no "
+ "robot available)";
+
/*
TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
@@ -252,13 +256,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
.name = properties.laserScannerFeatures.name,
.timestamp = timestamp};
- const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
- laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
- ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success);
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+ armem::vision::laser_scanner_features::client::Reader::Result::Success);
- ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+ ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
TIMING_END_COMMENT_STREAM(
READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
@@ -395,6 +399,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE);
+ ARMARX_INFO << "Not running tracker with lasersensors because they are not available for "
+ "the current tests.";
+
/*
ARMARX_INFO << "Running human tracker with lasersensor measurements";
@@ -432,6 +439,21 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<Human> humans = humanTracker.getTrackedHumans();
+ ARMARX_INFO << "For debugging purposes, we will add another 2 humans!";
+ core::Pose2D p1 = core::Pose2D::Identity();
+ p1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
+ p1.translation() = Eigen::Vector2f(0, 500);
+ Human h1 = {
+ .pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+ core::Pose2D p2 = core::Pose2D::Identity();
+ p2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
+ p2.translation() = Eigen::Vector2f(0, -500);
+ Human h2 = {
+ .pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+
+ humans.emplace_back(h1);
+ humans.emplace_back(h2);
+
humanWriterPlugin->get().store(humans, getName(), timestamp);
if (not humans.empty())
@@ -450,10 +472,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Detected " << groups.size() << " human groups";
- humanWriterPlugin->get().store(groups, getName(), timestamp);
+ humanGroupWriterPlugin->get().store(groups, getName(), timestamp);
TIMING_END_COMMENT_STREAM(
- WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
+ WRITE_BACK_HUMANS, "Timer: write humans and human groups to memory", ARMARX_VERBOSE);
TIMING_END_COMMENT_STREAM(
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 9a075597..63723a7a 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -52,6 +52,7 @@
#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
#include <armarx/navigation/human/HumanGrouper.h>
+#include <armarx/navigation/memory/client/human_group/Writer.h>
namespace armarx::navigation::components::dynamic_scene_provider
@@ -205,6 +206,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::human_group::Writer>* humanGroupWriterPlugin = nullptr;
+
human::HumanTracker humanTracker;
using HumanGrouper = armarx::navigation::human::HumanGrouper;
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index c75d9b2b..b6a365a5 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -133,8 +133,8 @@ namespace armarx::navigation::components::navigation_memory
workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
navigation::human::arondto::Human::ToAronType());
- // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
- // navigation::human::arondto::Human::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ navigation::human::arondto::HumanGroup::ToAronType());
if (not properties.snapshotToLoad.empty())
{
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 267142f9..9059612c 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -11,6 +11,8 @@ armarx_add_library(memory
client/costmap/Reader.cpp
client/human/Reader.cpp
client/human/Writer.cpp
+ client/human_group/Reader.cpp
+ client/human_group/Writer.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
@@ -24,6 +26,8 @@ armarx_add_library(memory
client/costmap/Reader.h
client/human/Reader.h
client/human/Writer.h
+ client/human_group/Reader.h
+ client/human_group/Writer.h
# ./client/events/Reader.h
DEPENDENCIES
ArmarXCoreInterfaces
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
index f1ad2e2b..905eb69e 100644
--- a/source/armarx/navigation/memory/client/human/Reader.cpp
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -41,23 +41,6 @@ namespace armarx::navigation::memory::client::human
return qb;
}
- armarx::armem::client::query::Builder
- Reader::buildHumanGroupsQuery(const Query& query) const
- {
- armarx::armem::client::query::Builder qb;
-
- // clang-format off
- qb
- .coreSegments().withName(properties().coreSegmentName)
- .providerSegments().withName(query.providerName)
- .entities().withName("groups")
- .snapshots().beforeOrAtTime(query.timestamp);
- // clang-format on
-
- return qb;
- }
-
-
std::string
Reader::propertyPrefix() const
{
@@ -107,95 +90,6 @@ namespace armarx::navigation::memory::client::human
return humans;
}
- navigation::human::HumanGroups
- asGroups(const armem::wm::ProviderSegment& providerSegment,
- const DateTime& timestamp,
- const Duration& maxAge)
- {
- navigation::human::HumanGroups humans;
-
- ARMARX_CHECK(not providerSegment.empty()) << "No entities";
- ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
-
- providerSegment.forEachEntity(
- [&humans, ×tamp, &maxAge](const armem::wm::Entity& entity)
- {
- const auto& entitySnapshot = entity.getLatestSnapshot();
- ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
-
- entitySnapshot.forEachInstance(
- [&](const armem::wm::EntityInstance& entityInstance)
- {
- const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
-
- if (dtToNow < maxAge and dtToNow.isPositive())
- {
- const auto dto = navigation::human::arondto::HumanGroup::FromAron(
- entityInstance.data());
-
- navigation::human::HumanGroup human;
- fromAron(dto, human);
- humans.push_back(human);
- }
- });
- });
-
- return humans;
- }
-
- Reader::HumanGroupResult
- Reader::queryHumanGroups(const Query& query) const
- {
- const auto qb = buildHumansQuery(query);
-
- ARMARX_DEBUG << "[MappingDataReader] query ... ";
-
- const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
-
- ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
-
- if (not qResult.success)
- {
- ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
- return {.groups = {},
- .status = HumanGroupResult::Status::Error,
- .errorMessage = qResult.errorMessage};
- }
-
- const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
-
- if (not coreSegment.hasProviderSegment(query.providerName))
- {
- ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
- return {.groups = {}, .status = HumanGroupResult::Status::NoData};
- }
-
- const armem::wm::ProviderSegment& providerSegment =
- coreSegment.getProviderSegment(query.providerName);
-
- if (providerSegment.empty())
- {
- ARMARX_DEBUG << "No entities.";
- return {.groups = {},
- .status = HumanGroupResult::Status::NoData,
- .errorMessage = "No entities"};
- }
-
- try
- {
- return HumanGroupResult{.groups =
- asGroups(providerSegment, query.timestamp, query.maxAge),
- .status = HumanGroupResult::Status::Success};
- }
- catch (...)
- {
- return HumanGroupResult{.groups = {},
- .status = HumanGroupResult::Status::Error,
- .errorMessage = GetHandledExceptionString()};
- }
- }
-
-
Reader::HumanResult
Reader::queryHumans(const Query& query) const
{
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
index 91a17afa..3a47556a 100644
--- a/source/armarx/navigation/memory/client/human/Reader.h
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -65,31 +65,10 @@ namespace armarx::navigation::memory::client::human
}
};
- struct HumanGroupResult
- {
- armarx::navigation::human::HumanGroups groups;
-
- enum Status
- {
- Success,
- NoData,
- Error
- } status;
-
- std::string errorMessage = "";
-
- operator bool() const noexcept
- {
- return status == Status::Success;
- }
- };
-
HumanResult queryHumans(const Query& query) const;
- HumanGroupResult queryHumanGroups(const Query& query) const;
protected:
::armarx::armem::client::query::Builder buildHumansQuery(const Query& query) const;
- ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
std::string propertyPrefix() const override;
Properties defaultProperties() const override;
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
index c38c2ac7..97767d14 100644
--- a/source/armarx/navigation/memory/client/human/Writer.cpp
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -6,8 +6,8 @@
#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
#include <armarx/navigation/algorithms/aron_conversions.h>
#include <armarx/navigation/human/aron/Human.aron.generated.h>
-#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::human
@@ -42,62 +42,12 @@ namespace armarx::navigation::memory::client::human
std::transform(humans.begin(),
humans.end(),
std::back_inserter(update.instancesData),
- [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr {
- navigation::human::arondto::Human dto;
- toAron(dto, human);
-
- return dto.toAron();
- });
-
-
- update.timeCreated = timestamp;
-
- ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
- armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
-
- ARMARX_DEBUG << updateResult;
-
- if (not updateResult.success)
- {
- ARMARX_ERROR << updateResult.errorMessage;
- }
-
- return updateResult.success;
- }
-
- bool
- Writer::store(const armarx::navigation::human::HumanGroups& groups,
- // const std::string& name,
- const std::string& providerName,
- const armem::Time& timestamp)
- {
- std::lock_guard g{memoryWriterMutex()};
-
- const auto result =
- memoryWriter().addSegment(memory::constants::HumanCoreSegmentName, providerName);
-
- if (not result.success)
- {
- ARMARX_ERROR << result.errorMessage;
-
- // TODO(fabian.reister): message
- return false;
- }
-
- const auto providerId = armem::MemoryID(result.segmentID);
- const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
-
- armem::EntityUpdate update;
- update.entityID = entityID;
-
- std::transform(groups.begin(),
- groups.end(),
- std::back_inserter(update.instancesData),
- [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr {
- navigation::human::arondto::HumanGroup dto;
- toAron(dto, group);
+ [](const navigation::human::Human& human) -> armarx::aron::data::DictPtr
+ {
+ navigation::human::arondto::Human dto;
+ toAron(dto, human);
- return dto.toAron();
+ return dto.toAron();
});
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
index b224149c..9afb179a 100644
--- a/source/armarx/navigation/memory/client/human/Writer.h
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -51,12 +51,7 @@ namespace armarx::navigation::memory::client::human
~Writer() override;
bool store(const armarx::navigation::human::Humans& humans,
- // const std::string& name,
- const std::string& providerName,
- const armem::Time& timestamp);
-
- bool store(const armarx::navigation::human::HumanGroups& groups,
- // const std::string& name,
+ // const std::string& name,
const std::string& providerName,
const armem::Time& timestamp);
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.cpp b/source/armarx/navigation/memory/client/human_group/Reader.cpp
new file mode 100644
index 00000000..0f6c8ad5
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+ Reader::~Reader() = default;
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumanGroupsQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("groups")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ return qb;
+ }
+
+
+ std::string
+ Reader::propertyPrefix() const
+ {
+ return "mem.nav.human_group.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::HumanGroupCoreSegmentName};
+ }
+
+ navigation::human::HumanGroups
+ asGroups(const armem::wm::ProviderSegment& providerSegment,
+ const DateTime& timestamp,
+ const Duration& maxAge)
+ {
+ navigation::human::HumanGroups humanGroups;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humanGroups, ×tamp, &maxAge](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&](const armem::wm::EntityInstance& entityInstance)
+ {
+ const Duration dtToNow = timestamp - entityInstance.metadata().timeCreated;
+
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto = navigation::human::arondto::HumanGroup::FromAron(
+ entityInstance.data());
+
+ navigation::human::HumanGroup humanGroup;
+ fromAron(dto, humanGroup);
+ humanGroups.push_back(humanGroup);
+ }
+ });
+ });
+
+ return humanGroups;
+ }
+
+ Reader::HumanGroupResult
+ Reader::queryHumanGroups(const Query& query) const
+ {
+ const auto qb = buildHumanGroupsQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_DEBUG << "Provider segment `" << query.providerName << "` does not exist (yet).";
+ return {.groups = {}, .status = HumanGroupResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_DEBUG << "No entities.";
+ return {.groups = {},
+ .status = HumanGroupResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanGroupResult{.groups =
+ asGroups(providerSegment, query.timestamp, query.maxAge),
+ .status = HumanGroupResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanGroupResult{.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Reader.h b/source/armarx/navigation/memory/client/human_group/Reader.h
new file mode 100644
index 00000000..537ac74d
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Reader.h
@@ -0,0 +1,77 @@
+/*
+ * 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 Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/Costmap.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+ ~Reader() override;
+
+ struct Query
+ {
+ std::string providerName;
+ armem::Time timestamp;
+ Duration maxAge;
+ };
+
+ struct HumanGroupResult
+ {
+ armarx::navigation::human::HumanGroups groups;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ HumanGroupResult queryHumanGroups(const Query& query) const;
+
+ protected:
+ ::armarx::armem::client::query::Builder buildHumanGroupsQuery(const Query& query) const;
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.cpp b/source/armarx/navigation/memory/client/human_group/Writer.cpp
new file mode 100644
index 00000000..2d38c45d
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.cpp
@@ -0,0 +1,83 @@
+#include "Writer.h"
+
+#include <iterator>
+
+#include "armarx/navigation/human/types.h"
+#include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
+#include <armarx/navigation/algorithms/aron_conversions.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
+
+
+namespace armarx::navigation::memory::client::human_group
+{
+ Writer::~Writer() = default;
+
+ bool
+ Writer::store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex()};
+
+ const auto result =
+ memoryWriter().addSegment(memory::constants::HumanGroupCoreSegmentName, providerName);
+
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+
+ // TODO(fabian.reister): message
+ return false;
+ }
+
+ const auto providerId = armem::MemoryID(result.segmentID);
+ const auto entityID = providerId.withEntityName("groups").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ std::transform(groups.begin(),
+ groups.end(),
+ std::back_inserter(update.instancesData),
+ [](const navigation::human::HumanGroup& group) -> armarx::aron::data::DictPtr
+ {
+ navigation::human::arondto::HumanGroup dto;
+ toAron(dto, group);
+
+ return dto.toAron();
+ });
+
+
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.human_group.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::HumanGroupCoreSegmentName};
+ }
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/client/human_group/Writer.h b/source/armarx/navigation/memory/client/human_group/Writer.h
new file mode 100644
index 00000000..c3de9d34
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human_group/Writer.h
@@ -0,0 +1,64 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+#include <armarx/navigation/algorithms/Costmap.h>
+#include <armarx/navigation/human/types.h>
+
+namespace armarx::navigation::memory::client::human_group
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+ ~Writer() override;
+
+ bool store(const armarx::navigation::human::HumanGroups& groups,
+ // const std::string& name,
+ const std::string& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+} // namespace armarx::navigation::memory::client::human_group
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index f0932218..23c01a1c 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,6 +33,7 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
+ inline const std::string HumanGroupCoreSegmentName = "HumanGroup";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
--
GitLab
From 34726922ff4acc030aa41442b04d78143f48c9d4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 9 Dec 2022 13:13:30 +0100
Subject: [PATCH 51/62] Use human group segment in visu
---
.../components/navigation_memory/Component.cpp | 12 +++++++-----
.../navigation/components/navigation_memory/Visu.cpp | 8 +++++---
.../navigation/components/navigation_memory/Visu.h | 4 +++-
3 files changed, 15 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index b6a365a5..e2cc5737 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -421,11 +421,13 @@ namespace armarx::navigation::components::navigation_memory
void
Component::visuRun()
{
- memory::Visu visu{arviz,
- workingMemory().getCoreSegment(navigation::location::coreSegmentID),
- workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
- workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
+ memory::Visu visu{
+ arviz,
+ workingMemory().getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanGroupCoreSegmentName)};
Properties::LocationGraph p;
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 60f5db09..a3faedff 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -53,12 +53,14 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment) :
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
humanSegment(humanSegment),
+ humanGroupSegment(humanGroupSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -339,12 +341,12 @@ namespace armarx::navigation::memory
std::map<std::string, navigation::human::HumanGroups> namedProviderHumanGroups;
- humanSegment.doLocked(
+ humanGroupSegment.doLocked(
[&]()
{
using namespace armem::server;
- humanSegment.forEachEntity(
+ humanGroupSegment.forEachEntity(
[&](const wm::Entity& entity)
{
entity.getLatestSnapshot().forEachInstance(
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 86666011..55b0e35f 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -48,7 +48,8 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
const armem::server::wm::CoreSegment& costmapSegment,
- const armem::server::wm::CoreSegment& humanSegment);
+ const armem::server::wm::CoreSegment& humanSegment,
+ const armem::server::wm::CoreSegment& humanGroupSegment);
~Visu();
@@ -65,6 +66,7 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& graphSegment;
const armem::server::wm::CoreSegment& costmapSegment;
const armem::server::wm::CoreSegment& humanSegment;
+ const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
--
GitLab
From c2d0c078cad997711247049326a8d9a01791384a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 9 Dec 2022 16:30:41 +0100
Subject: [PATCH 52/62] Fix missing initializer in human test
---
source/armarx/navigation/human/test/human_tracker_test.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/test/human_tracker_test.cpp b/source/armarx/navigation/human/test/human_tracker_test.cpp
index 32202149..0e45d1a8 100644
--- a/source/armarx/navigation/human/test/human_tracker_test.cpp
+++ b/source/armarx/navigation/human/test/human_tracker_test.cpp
@@ -239,7 +239,7 @@ namespace armarx::navigation::human
BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 1);
- CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000))};
+ CamMm late = {.detectionTime = DateTime(Duration::MilliSeconds(1000)), .humanPoses = {}};
tracker.update(late);
BOOST_CHECK_EQUAL(tracker.getTrackedHumans().size(), 0);
--
GitLab
From 3047f220988cea66f20008f1b8935e37e12b1215 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 9 Dec 2022 17:26:13 +0100
Subject: [PATCH 53/62] Use third demo human and other tweaks
---
.../dynamic_scene_provider/Component.cpp | 21 ++++++++++++++-----
.../components/navigation_memory/Visu.cpp | 6 ++++--
2 files changed, 20 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 2bd77ba3..90868b31 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -439,20 +439,31 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<Human> humans = humanTracker.getTrackedHumans();
- ARMARX_INFO << "For debugging purposes, we will add another 2 humans!";
+ ARMARX_INFO << "For testing purposes, we will add another 3 humans!";
core::Pose2D p1 = core::Pose2D::Identity();
- p1.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
- p1.translation() = Eigen::Vector2f(0, 500);
+ p1.linear() = Eigen::Rotation2Df(1.25 * M_PI).toRotationMatrix();
+ p1.translation() = Eigen::Vector2f(1000, 500);
Human h1 = {
.pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
core::Pose2D p2 = core::Pose2D::Identity();
- p2.linear() = Eigen::Rotation2Df(M_PI).toRotationMatrix();
- p2.translation() = Eigen::Vector2f(0, -500);
+ p2.linear() = Eigen::Rotation2Df(-0.25 * M_PI).toRotationMatrix();
+ p2.translation() = Eigen::Vector2f(1000, -500);
Human h2 = {
.pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+ core::Pose2D p3 = core::Pose2D::Identity();
+ p3.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ p3.translation() = Eigen::Vector2f(1500, 0);
+ Human h3 = {
+ .pose = p3, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
+
humans.emplace_back(h1);
humans.emplace_back(h2);
+ // periodically toggle third human for showing group of 2 vs group of 3
+ if ((timestamp.toSecondsSinceEpoch() / 5) % 2 == 0)
+ {
+ humans.emplace_back(h3);
+ }
humanWriterPlugin->get().store(humans, getName(), timestamp);
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index a3faedff..d506f16e 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -236,10 +236,11 @@ namespace armarx::navigation::memory
std::vector<Eigen::Vector3f> verts3D;
for (const auto& v2d : verts2D)
{
- verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 0));
+ verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50));
}
polygon.points(verts3D);
- polygon.color(simox::Color::yellow());
+ polygon.color(simox::Color::kit_yellow());
+ polygon.lineWidth(10);
layer.add(polygon);
}
}
@@ -349,6 +350,7 @@ namespace armarx::navigation::memory
humanGroupSegment.forEachEntity(
[&](const wm::Entity& entity)
{
+ namedProviderHumanGroups[entity.id().providerSegmentName];
entity.getLatestSnapshot().forEachInstance(
[&namedProviderHumanGroups](
const armarx::armem::wm::EntityInstance& instance)
--
GitLab
From 49a6aff983d23cc8e86470400cdebd8e3742e8ca Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 16 Dec 2022 11:07:38 +0100
Subject: [PATCH 54/62] Add TEB usage to other example client modes
---
.../components/example_client/Component.cpp | 3 +
.../config/dynamic_scene_provider.cfg | 24 ++
.../config/example_client.cfg | 163 +++++----
.../config/human_simulator.cfg | 314 +++++++++++++++++-
.../local_planning/TebObstacleManager.cpp | 3 +
5 files changed, 437 insertions(+), 70 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 1110c342..8d776c19 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -189,6 +189,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -289,6 +290,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -366,6 +368,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
index 30889783..2d2cc8c6 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
@@ -266,6 +266,30 @@
# ArmarX.dynamic_scene_provider.mem.nav.human.Provider = ""
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.CoreSegment:
+# Attributes:
+# - Default: HumanGroup
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.CoreSegment = HumanGroup
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human_group.Provider = ""
+
+
# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
diff --git a/scenarios/HumanAwareNavigation/config/example_client.cfg b/scenarios/HumanAwareNavigation/config/example_client.cfg
index 680e39ca..5b947d42 100644
--- a/scenarios/HumanAwareNavigation/config/example_client.cfg
+++ b/scenarios/HumanAwareNavigation/config/example_client.cfg
@@ -76,179 +76,204 @@
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleClient.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleClient.nav.NavigatorName: No Description
# Attributes:
-# - Default: false
-# - Case sensitivity: yes
+# - Default: navigator
+# - Case sensitivity: no
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.EnableProfiling = false
+ArmarX.ExampleClient.nav.NavigatorName = navigator
-# ArmarX.ExampleClient.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
-# - Default: Undefined
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.ExampleClient.MinimumLoggingLevel = Undefined
+# ArmarX.LoadLibraries = ""
-# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.ObjectName = ""
+# ArmarX.LoggingGroup = ""
-# ArmarX.ExampleClient.mem.robot_state.Memory:
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: RobotState
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
-# - Default: Localization
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: true
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled = true
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
-# - Default: navigator
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-ArmarX.ExampleClient.nav.NavigatorName = navigator
+# ArmarX.TopicSuffix = ""
-# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: 200
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.relativeMovement = 200
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.ExampleClient.robotName:
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: Armar6
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleClient.robotName = Armar6
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# ArmarX.example_client.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: ""
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoadLibraries = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.example_client.EnableProfiling = false
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# ArmarX.example_client.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.example_client.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.example_client.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.LoggingGroup = ""
+# ArmarX.example_client.ObjectName = ""
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# ArmarX.example_client.mem.nav.costmap.CoreSegment:
# Attributes:
-# - Default: true
+# - Default: Costmap
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.example_client.mem.nav.costmap.CoreSegment = Costmap
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# ArmarX.example_client.mem.nav.costmap.Memory:
# Attributes:
-# - Default: 3000
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# ArmarX.example_client.mem.nav.costmap.Memory = Navigation
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.example_client.mem.robot_state.Memory:
# Attributes:
-# - Default: 0
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.example_client.mem.robot_state.Memory = RobotState
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.example_client.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: false
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.example_client.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.example_client.mns.MemoryNameSystemEnabled = true
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.example_client.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: 1
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# ArmarX.example_client.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# ArmarX.example_client.mode: Which example to run
# Attributes:
-# - Default: ""
+# - Default: standard
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# - Possible values: {complex, point_to_point, standard, update_navigator, wander_around}
+ArmarX.example_client.mode = wander_around
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.example_client.nav.NavigatorName: Name of the Navigator
# Attributes:
-# - Default: false
+# - Default: navigator
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.example_client.nav.NavigatorName = navigator
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.example_client.relativeMovement: The distance between two target poses [mm]
# Attributes:
-# - Default: Info
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+# ArmarX.example_client.relativeMovement = 200
-# ArmarX.example_client.mode: No Description
+# ArmarX.example_client.robotName:
# Attributes:
-# - Default: wander_around
-# - Case sensitivity: no
+# - Default: Armar6
+# - Case sensitivity: yes
# - Required: no
-ArmarX.example_client.mode = wander_around
+# ArmarX.example_client.robotName = Armar6
diff --git a/scenarios/HumanAwareNavigation/config/human_simulator.cfg b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
index b628b0a4..7149a004 100644
--- a/scenarios/HumanAwareNavigation/config/human_simulator.cfg
+++ b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
@@ -2,13 +2,325 @@
# human_simulator properties
# ==================================================================
-# ArmarX.human_simulator.p.nHumans:
+# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.AdditionalPackages = Default value not mapped.
+
+
+# ArmarX.ApplicationName: Application name
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ApplicationName = ""
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
+# Attributes:
+# - Default: mongo/.cache
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.CachePath = mongo/.cache
+
+
+# ArmarX.Config: Comma-separated list of configuration files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.Config = ""
+
+
+# ArmarX.DataPath: Semicolon-separated search list for data files
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DataPath = ""
+
+
+# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
+# Attributes:
+# - Default: Default value not mapped.
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DefaultPackages = Default value not mapped.
+
+
+# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
+# Attributes:
+# - Default: ./config/dependencies.cfg
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.DependenciesConfig = ./config/dependencies.cfg
+
+
+# ArmarX.DisableLogging: Turn logging off in whole application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.DisableLogging = false
+
+
+# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.EnableProfiling = false
+
+
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoadLibraries = ""
+
+
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LoggingGroup = ""
+
+
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
+
+
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
+
+
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.SecondsStartupDelay = 0
+
+
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.StartDebuggerOnCrash = false
+
+
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ThreadPoolSize = 1
+
+
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.TopicSuffix = ""
+
+
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
+
+
+# ArmarX.Verbosity: Global logging level for whole application
+# Attributes:
+# - Default: Info
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
+
+
+# ArmarX.human_simulator.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.human_simulator.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.human_simulator.EnableProfiling: enable profiler which is used for logging performance events
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_simulator.EnableProfiling = false
+
+
+# ArmarX.human_simulator.MinimumLoggingLevel: Local logging level only for this component
+# Attributes:
+# - Default: Undefined
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.human_simulator.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.human_simulator.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.human_simulator.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ObjectName = ""
+
+
+# ArmarX.human_simulator.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.human_simulator.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.human_simulator.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.human_simulator.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.human_simulator.mem.nav.human.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.nav.human.Provider = ""
+
+
+# ArmarX.human_simulator.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.human_simulator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.human_simulator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.human_simulator.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.human_simulator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.human_simulator.p.human.goalDistanceThreshold:
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.p.human.goalDistanceThreshold = 100
+
+
+# ArmarX.human_simulator.p.human.maxLinearVelocity:
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.p.human.maxLinearVelocity = 200
+
+
+# ArmarX.human_simulator.p.nHumans: Number of humans to spawn.
+# Attributes:
+# - Default: 4
+# - Case sensitivity: yes
+# - Required: no
ArmarX.human_simulator.p.nHumans = 3
+# ArmarX.human_simulator.p.objectPoseProviderName:
+# Attributes:
+# - Default: R003_grasping_challenge
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.p.objectPoseProviderName = R003_grasping_challenge
+
+
# ArmarX.human_simulator.p.taskPeriodMs:
# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
ArmarX.human_simulator.p.taskPeriodMs = 100
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index e16fffd3..4dce66d7 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -83,9 +83,12 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
+ ARMARX_INFO << deactivateSpam() << "Added human obstacle";
+
// visualize proxemic zone if layer is available
if (visLayer != nullptr)
{
+ ARMARX_INFO << deactivateSpam() << "Drawing proxemic zones...";
const Eigen::Vector3f axisLength(
proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
const core::Pose pose3d = conv::to3D(proxemicZone.pose);
--
GitLab
From 0a95064bff4a07594b345ddfac09061aee603df8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 16 Dec 2022 11:17:51 +0100
Subject: [PATCH 55/62] Revert "Add TEB usage to other example client modes"
This reverts commit 49a6aff983d23cc8e86470400cdebd8e3742e8ca.
---
.../components/example_client/Component.cpp | 3 -
.../config/dynamic_scene_provider.cfg | 24 --
.../config/example_client.cfg | 163 ++++-----
.../config/human_simulator.cfg | 314 +-----------------
.../local_planning/TebObstacleManager.cpp | 3 -
5 files changed, 70 insertions(+), 437 deletions(-)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 8d776c19..1110c342 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -189,7 +189,6 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
- .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -290,7 +289,6 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
- .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -368,7 +366,6 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
- .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
index 2d2cc8c6..30889783 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
@@ -266,30 +266,6 @@
# ArmarX.dynamic_scene_provider.mem.nav.human.Provider = ""
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.CoreSegment:
-# Attributes:
-# - Default: HumanGroup
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.CoreSegment = HumanGroup
-
-
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.Memory:
-# Attributes:
-# - Default: Navigation
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.Memory = Navigation
-
-
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.Provider: Name of this provider
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.dynamic_scene_provider.mem.nav.human_group.Provider = ""
-
-
# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
diff --git a/scenarios/HumanAwareNavigation/config/example_client.cfg b/scenarios/HumanAwareNavigation/config/example_client.cfg
index 5b947d42..680e39ca 100644
--- a/scenarios/HumanAwareNavigation/config/example_client.cfg
+++ b/scenarios/HumanAwareNavigation/config/example_client.cfg
@@ -76,204 +76,179 @@
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleClient.nav.NavigatorName: No Description
+# ArmarX.ExampleClient.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
-# - Default: navigator
-# - Case sensitivity: no
-# - Required: no
-ArmarX.ExampleClient.nav.NavigatorName = navigator
-
-
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoadLibraries = ""
-
-
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoggingGroup = ""
-
-
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
-# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
+# ArmarX.ExampleClient.EnableProfiling = false
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
+# ArmarX.ExampleClient.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
-# - Default: 3000
+# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.ExampleClient.MinimumLoggingLevel = Undefined
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
+# ArmarX.ExampleClient.ObjectName: Name of IceGrid well-known object
# Attributes:
-# - Default: 0
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.SecondsStartupDelay = 0
+# ArmarX.ExampleClient.ObjectName = ""
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
+# ArmarX.ExampleClient.mem.robot_state.Memory:
# Attributes:
-# - Default: false
+# - Default: RobotState
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
+# ArmarX.ExampleClient.mem.robot_state.Memory = RobotState
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
# Attributes:
-# - Default: 1
+# - Default: Localization
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ThreadPoolSize = 1
+# ArmarX.ExampleClient.mem.robot_state.localizationSegment = Localization
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
+# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
+# Set to false to use this memory as a stand-alone.
# Attributes:
-# - Default: ""
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.TopicSuffix = ""
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ExampleClient.mns.MemoryNameSystemEnabled = true
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
+# ArmarX.ExampleClient.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
-# - Default: false
+# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
+# ArmarX.ExampleClient.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.Verbosity: Global logging level for whole application
+# ArmarX.ExampleClient.nav.NavigatorName: Name of the Navigator
# Attributes:
-# - Default: Info
+# - Default: navigator
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.ExampleClient.nav.NavigatorName = navigator
-# ArmarX.example_client.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleClient.relativeMovement: The distance between two target poses [mm]
# Attributes:
-# - Default: false
+# - Default: 200
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.example_client.EnableProfiling = false
+# ArmarX.ExampleClient.relativeMovement = 200
-# ArmarX.example_client.MinimumLoggingLevel: Local logging level only for this component
+# ArmarX.ExampleClient.robotName:
# Attributes:
-# - Default: Undefined
+# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.example_client.MinimumLoggingLevel = Undefined
+# ArmarX.ExampleClient.robotName = Armar6
-# ArmarX.example_client.ObjectName: Name of IceGrid well-known object
+# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.ObjectName = ""
+# ArmarX.LoadLibraries = ""
-# ArmarX.example_client.mem.nav.costmap.CoreSegment:
+# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
# Attributes:
-# - Default: Costmap
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.mem.nav.costmap.CoreSegment = Costmap
+# ArmarX.LoggingGroup = ""
-# ArmarX.example_client.mem.nav.costmap.Memory:
+# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
-# - Default: Navigation
+# - Default: true
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.mem.nav.costmap.Memory = Navigation
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RedirectStdout = true
-# ArmarX.example_client.mem.robot_state.Memory:
+# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
-# - Default: RobotState
+# - Default: 3000
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.mem.robot_state.Memory = RobotState
+# ArmarX.RemoteHandlesDeletionTimeout = 3000
-# ArmarX.example_client.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
-# - Default: Localization
+# - Default: 0
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.mem.robot_state.localizationSegment = Localization
+# ArmarX.SecondsStartupDelay = 0
-# ArmarX.example_client.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
+# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
-# - Default: true
+# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.example_client.mns.MemoryNameSystemEnabled = true
+# ArmarX.StartDebuggerOnCrash = false
-# ArmarX.example_client.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
-# - Default: MemoryNameSystem
+# - Default: 1
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.ThreadPoolSize = 1
-# ArmarX.example_client.mode: Which example to run
+# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
-# - Default: standard
+# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {complex, point_to_point, standard, update_navigator, wander_around}
-ArmarX.example_client.mode = wander_around
+# ArmarX.TopicSuffix = ""
-# ArmarX.example_client.nav.NavigatorName: Name of the Navigator
+# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
-# - Default: navigator
+# - Default: false
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.nav.NavigatorName = navigator
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.UseTimeServer = false
-# ArmarX.example_client.relativeMovement: The distance between two target poses [mm]
+# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
-# - Default: 200
+# - Default: Info
# - Case sensitivity: yes
# - Required: no
-# ArmarX.example_client.relativeMovement = 200
+# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
+# ArmarX.Verbosity = Info
-# ArmarX.example_client.robotName:
+# ArmarX.example_client.mode: No Description
# Attributes:
-# - Default: Armar6
-# - Case sensitivity: yes
+# - Default: wander_around
+# - Case sensitivity: no
# - Required: no
-# ArmarX.example_client.robotName = Armar6
+ArmarX.example_client.mode = wander_around
diff --git a/scenarios/HumanAwareNavigation/config/human_simulator.cfg b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
index 7149a004..b628b0a4 100644
--- a/scenarios/HumanAwareNavigation/config/human_simulator.cfg
+++ b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
@@ -2,325 +2,13 @@
# human_simulator properties
# ==================================================================
-# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
+# ArmarX.human_simulator.p.nHumans:
# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
-
-
-# ArmarX.ApplicationName: Application name
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ApplicationName = ""
-
-
-# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
-# Attributes:
-# - Default: mongo/.cache
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.CachePath = mongo/.cache
-
-
-# ArmarX.Config: Comma-separated list of configuration files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Config = ""
-
-
-# ArmarX.DataPath: Semicolon-separated search list for data files
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DataPath = ""
-
-
-# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DefaultPackages = Default value not mapped.
-
-
-# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
-# Attributes:
-# - Default: ./config/dependencies.cfg
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.DependenciesConfig = ./config/dependencies.cfg
-
-
-# ArmarX.DisableLogging: Turn logging off in whole application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.DisableLogging = false
-
-
-# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.EnableProfiling = false
-
-
-# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoadLibraries = ""
-
-
-# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.LoggingGroup = ""
-
-
-# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.RedirectStdout = true
-
-
-# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
-# Attributes:
-# - Default: 3000
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.RemoteHandlesDeletionTimeout = 3000
-
-
-# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
-# Attributes:
-# - Default: 0
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.SecondsStartupDelay = 0
-
-
-# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.StartDebuggerOnCrash = false
-
-
-# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
-# Attributes:
-# - Default: 1
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ThreadPoolSize = 1
-
-
-# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.TopicSuffix = ""
-
-
-# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.UseTimeServer = false
-
-
-# ArmarX.Verbosity: Global logging level for whole application
-# Attributes:
-# - Default: Info
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
-
-
-# ArmarX.human_simulator.ArVizStorageName: Name of the ArViz storage
-# Attributes:
-# - Default: ArVizStorage
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.ArVizStorageName = ArVizStorage
-
-
-# ArmarX.human_simulator.ArVizTopicName: Name of the ArViz topic
-# Attributes:
-# - Default: ArVizTopic
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.ArVizTopicName = ArVizTopic
-
-
-# ArmarX.human_simulator.EnableProfiling: enable profiler which is used for logging performance events
-# Attributes:
-# - Default: false
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.human_simulator.EnableProfiling = false
-
-
-# ArmarX.human_simulator.MinimumLoggingLevel: Local logging level only for this component
-# Attributes:
-# - Default: Undefined
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.human_simulator.MinimumLoggingLevel = Undefined
-
-
-# ArmarX.human_simulator.ObjectMemoryName: Name of the object memory.
-# Attributes:
-# - Default: ObjectMemory
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.ObjectMemoryName = ObjectMemory
-
-
-# ArmarX.human_simulator.ObjectName: Name of IceGrid well-known object
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.ObjectName = ""
-
-
-# ArmarX.human_simulator.mem.nav.costmap.CoreSegment:
-# Attributes:
-# - Default: Costmap
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.nav.costmap.CoreSegment = Costmap
-
-
-# ArmarX.human_simulator.mem.nav.costmap.Memory:
-# Attributes:
-# - Default: Navigation
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.nav.costmap.Memory = Navigation
-
-
-# ArmarX.human_simulator.mem.nav.human.CoreSegment:
-# Attributes:
-# - Default: Human
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.nav.human.CoreSegment = Human
-
-
-# ArmarX.human_simulator.mem.nav.human.Memory:
-# Attributes:
-# - Default: Navigation
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.nav.human.Memory = Navigation
-
-
-# ArmarX.human_simulator.mem.nav.human.Provider: Name of this provider
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.nav.human.Provider = ""
-
-
-# ArmarX.human_simulator.mem.robot_state.Memory:
-# Attributes:
-# - Default: RobotState
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.robot_state.Memory = RobotState
-
-
-# ArmarX.human_simulator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
-# Attributes:
-# - Default: Localization
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mem.robot_state.localizationSegment = Localization
-
-
-# ArmarX.human_simulator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
-# Set to false to use this memory as a stand-alone.
-# Attributes:
-# - Default: true
-# - Case sensitivity: yes
-# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.human_simulator.mns.MemoryNameSystemEnabled = true
-
-
-# ArmarX.human_simulator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
-# Attributes:
-# - Default: MemoryNameSystem
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.mns.MemoryNameSystemName = MemoryNameSystem
-
-
-# ArmarX.human_simulator.p.human.goalDistanceThreshold:
-# Attributes:
-# - Default: 100
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.p.human.goalDistanceThreshold = 100
-
-
-# ArmarX.human_simulator.p.human.maxLinearVelocity:
-# Attributes:
-# - Default: 200
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.p.human.maxLinearVelocity = 200
-
-
-# ArmarX.human_simulator.p.nHumans: Number of humans to spawn.
-# Attributes:
-# - Default: 4
-# - Case sensitivity: yes
-# - Required: no
ArmarX.human_simulator.p.nHumans = 3
-# ArmarX.human_simulator.p.objectPoseProviderName:
-# Attributes:
-# - Default: R003_grasping_challenge
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.human_simulator.p.objectPoseProviderName = R003_grasping_challenge
-
-
# ArmarX.human_simulator.p.taskPeriodMs:
# Attributes:
-# - Default: 100
-# - Case sensitivity: yes
-# - Required: no
ArmarX.human_simulator.p.taskPeriodMs = 100
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 4dce66d7..e16fffd3 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -83,12 +83,9 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
- ARMARX_INFO << deactivateSpam() << "Added human obstacle";
-
// visualize proxemic zone if layer is available
if (visLayer != nullptr)
{
- ARMARX_INFO << deactivateSpam() << "Drawing proxemic zones...";
const Eigen::Vector3f axisLength(
proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
const core::Pose pose3d = conv::to3D(proxemicZone.pose);
--
GitLab
From fac615fbeb78d65d5a40135dfb330b99178b3152 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Fri, 16 Dec 2022 11:20:43 +0100
Subject: [PATCH 56/62] Add TEB to other example client modes
---
examples/components/example_client/Component.cpp | 3 +++
1 file changed, 3 insertions(+)
diff --git a/examples/components/example_client/Component.cpp b/examples/components/example_client/Component.cpp
index 1110c342..8d776c19 100644
--- a/examples/components/example_client/Component.cpp
+++ b/examples/components/example_client/Component.cpp
@@ -189,6 +189,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -289,6 +290,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::SPFAParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
@@ -366,6 +368,7 @@ namespace armarx::navigation::components::example_client
client::NavigationStackConfig()
.general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
.globalPlanner(global_planning::Point2PointParams())
+ .localPlanner(local_planning::TimedElasticBandsParams())
.trajectoryController(
traj_ctrl::local::TrajectoryFollowingControllerParams())); // FIXME
--
GitLab
From e8373e731f6c6ac8c827f2331823ab3638f533df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 19 Dec 2022 12:52:09 +0100
Subject: [PATCH 57/62] Add aron conversions for proxemics as part of human
---
source/armarx/navigation/human/aron/Human.xml | 39 +++++++++++++
.../navigation/human/aron_conversions.cpp | 56 +++++++++++++++++++
.../navigation/human/aron_conversions.h | 11 ++++
source/armarx/navigation/human/types.cpp | 41 ++++++++++++++
source/armarx/navigation/human/types.h | 55 ++++++++++--------
5 files changed, 178 insertions(+), 24 deletions(-)
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
index 57d104d8..2f2b64b9 100644
--- a/source/armarx/navigation/human/aron/Human.xml
+++ b/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'>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
index e09a3ca3..9afef2fe 100644
--- a/source/armarx/navigation/human/aron_conversions.cpp
+++ b/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
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
index 71159748..230a9403 100644
--- a/source/armarx/navigation/human/aron_conversions.h
+++ b/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
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
index b2ec8b13..8bf473be 100644
--- a/source/armarx/navigation/human/types.cpp
+++ b/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
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index cbb1905d..f519375e 100644
--- a/source/armarx/navigation/human/types.h
+++ b/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
--
GitLab
From 07361bfa3367129de1181b4b75915f446b9c8322 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 19 Dec 2022 12:55:11 +0100
Subject: [PATCH 58/62] Create and store proxemic zones in dyn scene prov
---
.../components/dynamic_scene_provider/Component.cpp | 4 ++++
.../navigation/components/dynamic_scene_provider/Component.h | 4 +++-
2 files changed, 7 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 90868b31..126aad0a 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -470,6 +470,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
if (not humans.empty())
{
ARMARX_INFO << "Detected " << humans.size() << " humans";
+ for (Human& human : humans)
+ {
+ human.proxemicZones = proxemics.createProxemicZones(human);
+ }
humanWriterPlugin->get().store(humans, getName(), timestamp);
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 63723a7a..c57b5e33 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -52,6 +52,7 @@
#include "armarx/navigation/memory/client/human/Writer.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
#include <armarx/navigation/human/HumanGrouper.h>
+#include <armarx/navigation/human/ProxemicZoneCreator.h>
#include <armarx/navigation/memory/client/human_group/Writer.h>
@@ -210,7 +211,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
human::HumanTracker humanTracker;
- using HumanGrouper = armarx::navigation::human::HumanGrouper;
+ human::ProxemicZoneCreator proxemics;
+ using HumanGrouper = human::HumanGrouper;
HumanGrouper humanGrouper;
};
--
GitLab
From 3ca26629d0034a4c51df1c8027e2cd7ec6c1c340 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Mon, 19 Dec 2022 12:59:57 +0100
Subject: [PATCH 59/62] Visualize proxemics zone in nav mem visu
---
.../components/navigation_memory/Visu.cpp | 166 ++++++++++--------
.../components/navigation_memory/Visu.h | 12 ++
2 files changed, 101 insertions(+), 77 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index d506f16e..1418f8da 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -15,6 +15,7 @@
*
* @package Navigation::ArmarXObjects::NavigationMemory
* @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -139,113 +140,124 @@ namespace armarx::navigation::memory
}
}
- namespace
+
+ void
+ Visu::visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
{
+ const auto cmap = simox::color::cmaps::viridis();
+ const float vmax = costmap.getGrid().array().maxCoeff();
- void
- visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name)
+ const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
{
- const auto cmap = simox::color::cmaps::viridis();
- const float vmax = costmap.getGrid().array().maxCoeff();
+ const auto color = cmap.at(distance, 0.F, vmax);
+ return {color.a, color.r, color.g, color.b};
+ };
- const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
- {
- const auto color = cmap.at(distance, 0.F, vmax);
- return {color.a, color.r, color.g, color.b};
- };
+ const std::int64_t cols = costmap.getGrid().cols();
+ const std::int64_t rows = costmap.getGrid().rows();
- const std::int64_t cols = costmap.getGrid().cols();
- const std::int64_t rows = costmap.getGrid().rows();
+ auto mesh = viz::Mesh(name);
- auto mesh = viz::Mesh(name);
+ std::vector<std::vector<Eigen::Vector3f>> vertices;
+ std::vector<std::vector<viz::data::Color>> colors;
- std::vector<std::vector<Eigen::Vector3f>> vertices;
- std::vector<std::vector<viz::data::Color>> colors;
+ for (int r = 0; r < rows; r++)
+ {
+ auto& verticesRow = vertices.emplace_back(cols);
+ auto& colorsRow = colors.emplace_back(cols);
- for (int r = 0; r < rows; r++)
+ for (int c = 0; c < cols; c++)
{
- auto& verticesRow = vertices.emplace_back(cols);
- auto& colorsRow = colors.emplace_back(cols);
-
- for (int c = 0; c < cols; c++)
- {
- verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
- colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
- }
+ verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
+ colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
}
+ }
- mesh.grid2D(vertices, colors);
+ mesh.grid2D(vertices, colors);
- layer.add(mesh);
- }
+ layer.add(mesh);
+ }
- void
- visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent)
- {
+ void
+ Visu::visualize(const human::Humans& humans, viz::Layer& layer, const bool visuTransparent)
+ {
- const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
+ const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0, 0, 1000});
- ARMARX_INFO << "Visualizing " << humans.size() << " humans";
- for (const auto& human : humans)
- {
- // viz::Cylinder cylinder(std::to_string(layer.size()));
- // cylinder.fromTo(conv::to3D(human.pose.translation()),
- // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+ ARMARX_INFO << "Visualizing " << humans.size() << " humans";
+ for (const auto& human : humans)
+ {
+ // viz::Cylinder cylinder(std::to_string(layer.size()));
+ // cylinder.fromTo(conv::to3D(human.pose.translation()),
+ // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
- // cylinder.color(simox::Color::orange());
- // cylinder.radius(300);
- // layer.add(cylinder);
+ // cylinder.color(simox::Color::orange());
+ // cylinder.radius(300);
+ // layer.add(cylinder);
- viz::Robot mmm(std::to_string(layer.size()));
- mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
- mmm.pose(conv::to3D(human.pose) * human_T_mmm);
- mmm.scale(1.7); // 1.7m
- mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
- layer.add(mmm);
+ viz::Robot mmm(std::to_string(layer.size()));
+ mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
+ mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.scale(1.7); // 1.7m
+ mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
+ layer.add(mmm);
- core::Pose pose3d = conv::to3D(human.pose);
- pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
+ core::Pose pose3d = conv::to3D(human.pose);
+ pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .pose(pose3d)
+ .length(200)
+ .color(simox::Color::red());
+ layer.add(arrow);
+
+ {
auto arrow = viz::Arrow(std::to_string(layer.size()))
- .pose(pose3d)
- .length(200)
- .color(simox::Color::red());
+ .fromTo(pose3d.translation(),
+ pose3d.translation() + conv::to3D(human.linearVelocity))
+ .color(simox::Color::blue());
+ arrow.width(10);
layer.add(arrow);
+ }
- {
- auto arrow =
- viz::Arrow(std::to_string(layer.size()))
- .fromTo(pose3d.translation(),
- pose3d.translation() + conv::to3D(human.linearVelocity))
- .color(simox::Color::blue());
- arrow.width(10);
- layer.add(arrow);
- }
+ auto proxemicZones = human.proxemicZones;
+
+ int i = 0;
+ for (const auto& proxemicZone : proxemicZones)
+ {
+ const Eigen::Vector3f axisLength(
+ proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
+ const core::Pose pose3d = conv::to3D(proxemicZone.pose);
+
+ layer.add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex))
+ .pose(pose3d)
+ .axisLengths(axisLength)
+ .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
+ i++;
}
}
+ }
- void
- visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
+ void
+ Visu::visualize(const human::HumanGroups& humanGroups, viz::Layer& layer)
+ {
+ for (const auto& humanGroup : humanGroups)
{
- for (const auto& humanGroup : humanGroups)
- {
- viz::Polygon polygon(std::to_string(layer.size()));
+ viz::Polygon polygon(std::to_string(layer.size()));
- const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
- std::vector<Eigen::Vector3f> verts3D;
- for (const auto& v2d : verts2D)
- {
- verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50));
- }
- polygon.points(verts3D);
- polygon.color(simox::Color::kit_yellow());
- polygon.lineWidth(10);
- layer.add(polygon);
+ const std::vector<Eigen::Vector2f> verts2D = humanGroup.shape.vertices;
+ std::vector<Eigen::Vector3f> verts3D;
+ for (const auto& v2d : verts2D)
+ {
+ verts3D.emplace_back(Eigen::Vector3f(v2d.x(), v2d.y(), 50));
}
+ polygon.points(verts3D);
+ polygon.color(simox::Color::kit_yellow());
+ polygon.lineWidth(10);
+ layer.add(polygon);
}
-
- } // namespace
+ }
void
Visu::drawCostmaps(std::vector<viz::Layer>& layers, bool enabled)
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 55b0e35f..5ed1ede9 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -31,6 +31,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/human/types.h"
namespace armarx::navigation::graph
@@ -69,6 +70,17 @@ namespace armarx::navigation::memory
const armem::server::wm::CoreSegment& humanGroupSegment;
std::unique_ptr<navigation::graph::GraphVisu> visu;
+
+ private:
+ const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+ simox::Color::blue()};
+ int visualizationIndex;
+ void visualize(const navigation::human::Humans& humans,
+ viz::Layer& layer,
+ const bool visuTransparent);
+ void visualize(const human::HumanGroups& humanGroups, viz::Layer& layer);
+ void
+ visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name);
};
--
GitLab
From 3cfff8b572779236c3902881a4598740a3e4fde5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 20 Dec 2022 18:39:29 +0100
Subject: [PATCH 60/62] Refactor test human creation and make 'em swing
---
.../dynamic_scene_provider/Component.cpp | 77 ++++++++++++-------
1 file changed, 50 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 126aad0a..ed8a9f43 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -41,7 +41,6 @@
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -191,6 +190,54 @@ namespace armarx::navigation::components::dynamic_scene_provider
return Component::defaultName;
}
+ using Human = armarx::navigation::human::Human;
+
+ void
+ armarx::navigation::components::dynamic_scene_provider::Component::addTestHumans(
+ std::vector<Human>& humans)
+ {
+ DateTime timestamp = Clock::Now();
+
+ ARMARX_INFO << "For testing purposes, we will add another 3 humans!";
+ core::Pose2D p1 = core::Pose2D::Identity();
+ p1.linear() = Eigen::Rotation2Df(1.25 * M_PI).toRotationMatrix();
+ p1.translation() = Eigen::Vector2f(1000, 500);
+ Human h1 = {
+ .pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = timestamp};
+ core::Pose2D p2 = core::Pose2D::Identity();
+ p2.linear() = Eigen::Rotation2Df(-0.25 * M_PI).toRotationMatrix();
+ p2.translation() = Eigen::Vector2f(1000, -500);
+ Human h2 = {
+ .pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = timestamp};
+
+
+ // Create third human that swings in a sine wave
+
+ double f_1_s = 0.25f;
+ double omega_1_s = 2 * M_PI * f_1_s;
+ double t_s = timestamp.toDurationSinceEpoch().toSecondsDouble();
+ double sinArg = std::fmod(t_s * omega_1_s, 2 * M_PI);
+ double amplitude_mm = 1000;
+
+ core::Pose2D p3 = core::Pose2D::Identity();
+ p3.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+ // in mm
+ p3.translation() = Eigen::Vector2f(2500 + amplitude_mm * sin(sinArg), 0);
+ Human h3 = {.pose = p3,
+ // linear velocity in mm/s
+ .linearVelocity = Eigen::Vector2f(amplitude_mm * omega_1_s * cos(sinArg), 0),
+ .detectionTime = timestamp};
+
+ humans.emplace_back(h1);
+ humans.emplace_back(h2);
+
+ // periodically toggle third human for showing group of 2 vs group of 3
+ // if ((timestamp.toSecondsSinceEpoch() / 5) % 2 == 0)
+ // {
+ humans.emplace_back(h3);
+ // }
+ }
+
void
Component::runPeriodically()
{
@@ -438,32 +485,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
using Human = armarx::navigation::human::Human;
std::vector<Human> humans = humanTracker.getTrackedHumans();
-
- ARMARX_INFO << "For testing purposes, we will add another 3 humans!";
- core::Pose2D p1 = core::Pose2D::Identity();
- p1.linear() = Eigen::Rotation2Df(1.25 * M_PI).toRotationMatrix();
- p1.translation() = Eigen::Vector2f(1000, 500);
- Human h1 = {
- .pose = p1, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
- core::Pose2D p2 = core::Pose2D::Identity();
- p2.linear() = Eigen::Rotation2Df(-0.25 * M_PI).toRotationMatrix();
- p2.translation() = Eigen::Vector2f(1000, -500);
- Human h2 = {
- .pose = p2, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
- core::Pose2D p3 = core::Pose2D::Identity();
- p3.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
- p3.translation() = Eigen::Vector2f(1500, 0);
- Human h3 = {
- .pose = p3, .linearVelocity = Eigen::Vector2f(0, 0), .detectionTime = DateTime::Now()};
-
-
- humans.emplace_back(h1);
- humans.emplace_back(h2);
- // periodically toggle third human for showing group of 2 vs group of 3
- if ((timestamp.toSecondsSinceEpoch() / 5) % 2 == 0)
- {
- humans.emplace_back(h3);
- }
+ // Add some testing humans
+ addTestHumans(humans);
humanWriterPlugin->get().store(humans, getName(), timestamp);
--
GitLab
From d9a083010879125d671f2e246658cb6dab4781f0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 20 Dec 2022 18:41:19 +0100
Subject: [PATCH 61/62] Add missing method to header
---
.../navigation/components/dynamic_scene_provider/Component.h | 2 ++
1 file changed, 2 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index c57b5e33..42a109c6 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -122,6 +122,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
VirtualRobot::RobotPtr robot = nullptr;
+ using Human = armarx::navigation::human::Human;
+ void addTestHumans(std::vector<Human>& humans);
private:
static const std::string defaultName;
--
GitLab
From 37b9fd9fd42346308072834a0cc464ecea9bc657 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Timo=20Weberru=C3=9F?= <timo.weberruss@student.kit.edu>
Date: Tue, 20 Dec 2022 19:06:03 +0100
Subject: [PATCH 62/62] Draw proxemic zones to own layer & clean up
---
.../components/navigation_memory/Visu.cpp | 35 +++++++++++--------
.../components/navigation_memory/Visu.h | 1 +
2 files changed, 21 insertions(+), 15 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 1418f8da..7a8390d1 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -187,16 +187,6 @@ namespace armarx::navigation::memory
ARMARX_INFO << "Visualizing " << humans.size() << " humans";
for (const auto& human : humans)
{
- // viz::Cylinder cylinder(std::to_string(layer.size()));
- // cylinder.fromTo(conv::to3D(human.pose.translation()),
- // conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
-
-
- // cylinder.color(simox::Color::orange());
- // cylinder.radius(300);
- // layer.add(cylinder);
-
-
viz::Robot mmm(std::to_string(layer.size()));
mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
mmm.pose(conv::to3D(human.pose) * human_T_mmm);
@@ -220,7 +210,14 @@ namespace armarx::navigation::memory
arrow.width(10);
layer.add(arrow);
}
+ }
+ }
+ void
+ Visu::visualizeProxemicZones(const human::Humans& humans, viz::Layer& layer)
+ {
+ for (const auto& human : humans)
+ {
auto proxemicZones = human.proxemicZones;
int i = 0;
@@ -230,10 +227,13 @@ namespace armarx::navigation::memory
proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
const core::Pose pose3d = conv::to3D(proxemicZone.pose);
- layer.add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex))
- .pose(pose3d)
- .axisLengths(axisLength)
- .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
+ viz::Ellipsoid zoneEllipse("proxemicZone_" + std::to_string(visualizationIndex));
+ zoneEllipse.pose(pose3d);
+ zoneEllipse.axisLengths(axisLength);
+ zoneEllipse.color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]);
+
+ layer.add(zoneEllipse);
+
i++;
}
}
@@ -325,11 +325,13 @@ namespace armarx::navigation::memory
[&namedProviderHumans](
const armarx::armem::wm::EntityInstance& instance)
{
+ ARMARX_TRACE;
const auto dto =
navigation::human::arondto::Human::FromAron(instance.data());
-
+ ARMARX_TRACE;
navigation::human::Human human;
fromAron(dto, human);
+ ARMARX_TRACE;
namedProviderHumans[instance.id().providerSegmentName].emplace_back(
std::move(human));
@@ -340,7 +342,10 @@ namespace armarx::navigation::memory
for (const auto& [providerName, humans] : namedProviderHumans)
{
viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ viz::Layer& proxZoneLayer =
+ layers.emplace_back(arviz.layer("proxemic_zones_" + providerName));
visualize(humans, layer, visuTransparent);
+ visualizeProxemicZones(humans, proxZoneLayer);
}
}
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 5ed1ede9..835823d9 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -78,6 +78,7 @@ namespace armarx::navigation::memory
void visualize(const navigation::human::Humans& humans,
viz::Layer& layer,
const bool visuTransparent);
+ void visualizeProxemicZones(const navigation::human::Humans& humans, viz::Layer& layer);
void visualize(const human::HumanGroups& humanGroups, viz::Layer& layer);
void
visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name);
--
GitLab