From 5a5cf7af9691ffee16d214603c5788ffe02909c7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 3 Aug 2022 15:14:26 +0200
Subject: [PATCH 01/34] human tracker skeleton
---
.../dynamic_scene_provider/CMakeLists.txt | 2 +
.../dynamic_scene_provider/Component.cpp | 11 ++++++
.../dynamic_scene_provider/Component.h | 4 ++
.../dynamic_scene_provider/HumanTracker.cpp | 6 +++
.../dynamic_scene_provider/HumanTracker.h | 37 +++++++++++++++++++
5 files changed, 60 insertions(+)
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
create mode 100644 source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 146e5b7b..42507ca0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -9,9 +9,11 @@ armarx_add_component(dynamic_scene_provider
SOURCES
Component.cpp
ArVizDrawer.cpp
+ HumanTracker.cpp
HEADERS
Component.h
ArVizDrawer.h
+ HumanTracker.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9f8cd9f7..a4d10991 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -137,6 +137,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
ARMARX_CHECK_NOT_NULL(robot);
+ humanTracker.reset();
+
task = new PeriodicTask<Component>(
this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
}
@@ -314,6 +316,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
// arviz.commit({layer});
}
+
+ // here ends: data fetching
+
+ humanTracker.update(HumanTracker::Measurements{
+ .humanPoses = humanPoseResult.humanPoses
+ });
+
+
+
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 1d7d92fc..171b8638 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -47,6 +47,7 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
+#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
@@ -197,6 +198,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
occupancyGridReaderPlugin = nullptr;
+
+
+ HumanTracker humanTracker;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
new file mode 100644
index 00000000..866fa4aa
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -0,0 +1,6 @@
+#include "HumanTracker.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
new file mode 100644
index 00000000..770901f1
--- /dev/null
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -0,0 +1,37 @@
+#pragma once
+
+
+#include "VisionX/libraries/armem_human/types.h"
+#include "armarx/navigation/core/basic_types.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+ class HumanTracker
+ {
+ public:
+ HumanTracker() = default;
+
+ struct Measurements
+ {
+ std::vector<armem::human::HumanPose> humanPoses;
+ };
+
+ void update(const Measurements& measurements);
+
+ struct TrackedHuman
+ {
+ // TODO ...
+ core::Pose2D global_T_human;
+
+ Eigen::Vector2f linearVelocity;
+ };
+
+ std::vector<TrackedHuman> getTrackedHumans() const;
+
+ void reset();
+
+ private:
+ std::vector<TrackedHuman> trackedHumans;
+ };
+} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From 807b0e196049a0f9169b65b04a875d4966cdc8dc Mon Sep 17 00:00:00 2001
From: Marius Baden <marius.baden@student.kit.edu>
Date: Wed, 3 Aug 2022 17:40:28 +0200
Subject: [PATCH 02/34] Add skeleton for HumanTracker.cpp
---
.../dynamic_scene_provider/HumanTracker.cpp | 27 +++++++++++++++++++
1 file changed, 27 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 866fa4aa..699a0954 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -2,5 +2,32 @@
namespace armarx::navigation::components::dynamic_scene_provider
{
+
+HumanTracker::HumanTracker()
+{
+
+}
+
+
+void HumanTracker::update(const Measurements &measurements)
+{
+ this->trackedHumans = HumanTracker::TrackedHuman{
+ .global_T_human = ;// pose here;
+ .linearVelocity = 0;
+ }
+}
+
+
+std::vector<HumanTracker::TrackedHuman> HumanTracker::getTrackedHumans() const
+{
+
+}
+
+
+void HumanTracker::reset()
+{
+
+}
+
}
--
GitLab
From 890b1d9621569d6710b3193e527ca07fecc66ac1 Mon Sep 17 00:00:00 2001
From: Marius Baden <marius.baden@student.kit.edu>
Date: Wed, 3 Aug 2022 17:47:52 +0200
Subject: [PATCH 03/34] Create tracked human
---
.../components/dynamic_scene_provider/HumanTracker.cpp | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 699a0954..94ee8e37 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -12,7 +12,10 @@ HumanTracker::HumanTracker()
void HumanTracker::update(const Measurements &measurements)
{
this->trackedHumans = HumanTracker::TrackedHuman{
- .global_T_human = ;// pose here;
+ .global_T_human = core::Pose2D{
+ .x = 0;
+ .y = 0;
+ };
.linearVelocity = 0;
}
}
--
GitLab
From 7db0cd2896eb4521cfc4a4e053c2a3601866ed1b Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Wed, 3 Aug 2022 18:07:16 +0200
Subject: [PATCH 04/34] Fix broken test
---
.../navigation/algorithms/test/algorithms_spfa_test.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
index e2a24383..98a6934a 100644
--- a/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
+++ b/source/armarx/navigation/algorithms/test/algorithms_spfa_test.cpp
@@ -212,8 +212,8 @@ BOOST_AUTO_TEST_CASE(testSPFAPlanWObstacleDistance)
const float cellSize = j.at("cell_size"); // [mm]
BOOST_REQUIRE_GT(cellSize, 0);
- std::vector<float> sceneBoundsMinV(j.at("scene_bounds"]["min"));
- std::vector<float> sceneBoundsMaxV(j.at("scene_bounds"]["max"));
+ std::vector<float> sceneBoundsMinV(j.at("scene_bounds").at("min"));
+ std::vector<float> sceneBoundsMaxV(j.at("scene_bounds").at("max"));
BOOST_REQUIRE_EQUAL(sceneBoundsMinV.size(), 2);
BOOST_REQUIRE_EQUAL(sceneBoundsMaxV.size(), 2);
--
GitLab
From c9bb2065192b0609ae2a28ca8df5d772443fafac Mon Sep 17 00:00:00 2001
From: Marius Baden <marius.baden@student.kit.edu>
Date: Wed, 3 Aug 2022 18:32:26 +0200
Subject: [PATCH 05/34] Create basic tracked humans from OpenPose
---
.../dynamic_scene_provider/HumanTracker.cpp | 25 +++++++++++++++++++
1 file changed, 25 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 94ee8e37..41649ff4 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -11,6 +11,31 @@ HumanTracker::HumanTracker()
void HumanTracker::update(const Measurements &measurements)
{
+ trackedHumans.clear();
+
+ foreach (humanPose, measurements.humanPoses) {
+
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+ const Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+ for (const auto& [_, v] : keypoints)
+ {
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
+
+ const HumanTracker::TrackedHuman newHuman = HumanTracker::TrackedHuman{
+ .global_T_human = core::Pose2D{
+ .x = centerPos.x;
+ .y = centerPos.y;
+ };;
+ .linearVelocity = 0; //TODO more sophisticated guess
+ }
+ trackedHumans.push_back(newHuman);
+ }
+
+
this->trackedHumans = HumanTracker::TrackedHuman{
.global_T_human = core::Pose2D{
.x = 0;
--
GitLab
From fc47ac5cfba574a8662823bcd7fe1d296396ff92 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 17:22:33 +0200
Subject: [PATCH 06/34] Implement basic version of human tracker.
Uses average position of KeypointMap.
Missing angle (currently set to 0).
Missing velocity (currently set to 0).
---
.../dynamic_scene_provider/HumanTracker.cpp | 77 +++++++++----------
.../dynamic_scene_provider/HumanTracker.h | 3 +-
2 files changed, 37 insertions(+), 43 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 41649ff4..1cd4c42f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -1,61 +1,54 @@
+
#include "HumanTracker.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
namespace armarx::navigation::components::dynamic_scene_provider
{
-HumanTracker::HumanTracker()
-{
+ void
+ HumanTracker::update(const Measurements& measurements)
+ {
+ trackedHumans.clear();
-}
+ for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+ {
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
-void HumanTracker::update(const Measurements &measurements)
-{
- trackedHumans.clear();
+ Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
+ for (const auto& [_, v] : keypoints)
+ {
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
- foreach (humanPose, measurements.humanPoses) {
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
+ pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
+ const HumanTracker::TrackedHuman newHuman = {
+ .global_T_human = pose,
+ .linearVelocity = Eigen::Vector2f::Zero() //TODO more sophisticated guess
+ };
- const Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
- for (const auto& [_, v] : keypoints)
- {
- centerPos += v.positionGlobal;
- }
- centerPos /= static_cast<float>(keypoints.size());
-
- const HumanTracker::TrackedHuman newHuman = HumanTracker::TrackedHuman{
- .global_T_human = core::Pose2D{
- .x = centerPos.x;
- .y = centerPos.y;
- };;
- .linearVelocity = 0; //TODO more sophisticated guess
+ trackedHumans.push_back(newHuman);
}
- trackedHumans.push_back(newHuman);
}
-
- this->trackedHumans = HumanTracker::TrackedHuman{
- .global_T_human = core::Pose2D{
- .x = 0;
- .y = 0;
- };
- .linearVelocity = 0;
+ const std::vector<HumanTracker::TrackedHuman>&
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans;
}
-}
-
-
-std::vector<HumanTracker::TrackedHuman> HumanTracker::getTrackedHumans() const
-{
-
-}
-void HumanTracker::reset()
-{
+ void
+ HumanTracker::reset()
+ {
+ trackedHumans.clear();
+ }
-}
-
-}
+} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 770901f1..556d516b 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -2,6 +2,7 @@
#include "VisionX/libraries/armem_human/types.h"
+
#include "armarx/navigation/core/basic_types.h"
namespace armarx::navigation::components::dynamic_scene_provider
@@ -27,7 +28,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
Eigen::Vector2f linearVelocity;
};
- std::vector<TrackedHuman> getTrackedHumans() const;
+ const std::vector<TrackedHuman>& getTrackedHumans() const;
void reset();
--
GitLab
From a818cb35e89b380fa44f0ede6db4c88a01175c48 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 18:04:16 +0200
Subject: [PATCH 07/34] Setup of new library human
---
source/armarx/navigation/CMakeLists.txt | 1 +
source/armarx/navigation/human/CMakeLists.txt | 13 ++++++
source/armarx/navigation/human/types.h | 42 +++++++++++++++++++
3 files changed, 56 insertions(+)
create mode 100644 source/armarx/navigation/human/CMakeLists.txt
create mode 100644 source/armarx/navigation/human/types.h
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index dfd3bf35..ca819773 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -15,6 +15,7 @@ add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
add_subdirectory(platform_controller)
+add_subdirectory(human)
# Components.
add_subdirectory(components)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
new file mode 100644
index 00000000..d8258b10
--- /dev/null
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -0,0 +1,13 @@
+armarx_add_aron_library(human_aron
+ ARON_FILES
+)
+
+armarx_add_library(human
+ DEPENDENCIES_PUBLIC
+ armarx_navigation::core
+ DEPENDENCIES_PRIVATE
+ SOURCES
+ #types.cpp
+ HEADERS
+ types.h
+)
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
new file mode 100644
index 00000000..8acd8c84
--- /dev/null
+++ b/source/armarx/navigation/human/types.h
@@ -0,0 +1,42 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human
+{
+ struct Human
+ {
+ core::Pose2D global_T_human;
+ Eigen::Vector2f linearVelocity;
+ };
+
+ struct HumanGroup
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ std::vector<Human> humans;
+ };
+
+} // namespace armarx::navigation::human
--
GitLab
From 6d1259a9e9a4808d49a5036a8a1f54577103a10e Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 18:45:48 +0200
Subject: [PATCH 08/34] Change usage of TrackedHuman to Human
---
.../dynamic_scene_provider/CMakeLists.txt | 1 +
.../dynamic_scene_provider/HumanTracker.cpp | 4 ++--
.../dynamic_scene_provider/HumanTracker.h | 13 +++----------
3 files changed, 6 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 42507ca0..c53238db 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -31,6 +31,7 @@ armarx_add_component(dynamic_scene_provider
armarx_navigation::util
armarx_navigation::memory
armarx_navigation::dynamic_scene
+ armarx_navigation::human
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 1cd4c42f..030c8b63 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -28,7 +28,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
- const HumanTracker::TrackedHuman newHuman = {
+ const human::Human newHuman = {
.global_T_human = pose,
.linearVelocity = Eigen::Vector2f::Zero() //TODO more sophisticated guess
};
@@ -37,7 +37,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
}
- const std::vector<HumanTracker::TrackedHuman>&
+ const std::vector<human::Human>&
HumanTracker::getTrackedHumans() const
{
return trackedHumans;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 556d516b..da448f63 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -4,6 +4,7 @@
#include "VisionX/libraries/armem_human/types.h"
#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -20,19 +21,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
void update(const Measurements& measurements);
- struct TrackedHuman
- {
- // TODO ...
- core::Pose2D global_T_human;
-
- Eigen::Vector2f linearVelocity;
- };
-
- const std::vector<TrackedHuman>& getTrackedHumans() const;
+ const std::vector<human::Human>& getTrackedHumans() const;
void reset();
private:
- std::vector<TrackedHuman> trackedHumans;
+ std::vector<human::Human> trackedHumans;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From 2801d2d74daf89f9b251153f91a1b4e322a4dd37 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 18:53:32 +0200
Subject: [PATCH 09/34] Add detection time to Human/HumanGroup
---
source/armarx/navigation/human/CMakeLists.txt | 3 ++-
source/armarx/navigation/human/types.h | 4 +++-
2 files changed, 5 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index d8258b10..c173b591 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -4,7 +4,8 @@ armarx_add_aron_library(human_aron
armarx_add_library(human
DEPENDENCIES_PUBLIC
- armarx_navigation::core
+ ArmarXCore
+ armarx_navigation::core
DEPENDENCIES_PRIVATE
SOURCES
#types.cpp
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 8acd8c84..3dc3f663 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -21,7 +21,7 @@
#pragma once
-#include <SimoxUtility/shapes.h>
+#include <ArmarXCore/core/time.h>
#include <armarx/navigation/core/basic_types.h>
@@ -31,12 +31,14 @@ namespace armarx::navigation::human
{
core::Pose2D global_T_human;
Eigen::Vector2f linearVelocity;
+ DateTime detectionTime;
};
struct HumanGroup
{
std::vector<Eigen::Vector2f> vertices;
std::vector<Human> humans;
+ DateTime detectionTime;
};
} // namespace armarx::navigation::human
--
GitLab
From f5badc149a3e53f15a366160ccfb66dcfd078213 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 18:54:25 +0200
Subject: [PATCH 10/34] Add detection time to Measurement
---
.../components/dynamic_scene_provider/HumanTracker.h | 3 +++
1 file changed, 3 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index da448f63..612db064 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -1,6 +1,8 @@
#pragma once
+#include <ArmarXCore/core/time.h>
+
#include "VisionX/libraries/armem_human/types.h"
#include "armarx/navigation/core/basic_types.h"
@@ -16,6 +18,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct Measurements
{
+ DateTime detectionTime;
std::vector<armem::human::HumanPose> humanPoses;
};
--
GitLab
From 4f4bc5da793efdb980c0ac68a163b3f23fa9cb11 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 19:21:50 +0200
Subject: [PATCH 11/34] Add proxemic zone struct, rename parameter
---
source/armarx/navigation/human/types.h | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 3dc3f663..ae490622 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -29,7 +29,7 @@ namespace armarx::navigation::human
{
struct Human
{
- core::Pose2D global_T_human;
+ core::Pose2D pose;
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
};
@@ -41,4 +41,9 @@ namespace armarx::navigation::human
DateTime detectionTime;
};
+ struct ProxemicZone
+ {
+ core::Pose2D pose;
+ };
+
} // namespace armarx::navigation::human
--
GitLab
From 5713e4727272100328fdc5f0ff200c481adbbb94 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 19:26:18 +0200
Subject: [PATCH 12/34] Define HumanTracker::getAssociatedHumans()
---
.../dynamic_scene_provider/HumanTracker.cpp | 11 ++++++++---
.../components/dynamic_scene_provider/HumanTracker.h | 3 +++
2 files changed, 11 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 030c8b63..ea338135 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -30,8 +30,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
const human::Human newHuman = {
.global_T_human = pose,
- .linearVelocity = Eigen::Vector2f::Zero() //TODO more sophisticated guess
- };
+ .linearVelocity = Eigen::Vector2f::Zero(), //TODO more sophisticated guess
+ .detectionTime = measurements.detectionTime};
trackedHumans.push_back(newHuman);
}
@@ -44,11 +44,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
+ std::map<armem::human::HumanPose&, human::Human&>
+ HumanTracker::getAssociatedHumans(const Measurements& measurements) const
+ {
+ }
+
+
void
HumanTracker::reset()
{
trackedHumans.clear();
}
-
} // namespace armarx::navigation::components::dynamic_scene_provider
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 612db064..209fd345 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -30,5 +30,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<human::Human> trackedHumans;
+
+ std::map<armem::human::HumanPose&, human::Human&>
+ getAssociatedHumans(const Measurements& measurements) const;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From 38637b2df2dc2d5ec4bf8481860bd2f0b8090954 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 19:30:00 +0200
Subject: [PATCH 13/34] Rename global_T_human to pose
---
.../components/dynamic_scene_provider/HumanTracker.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index ea338135..fd15dc3b 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -29,7 +29,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
const human::Human newHuman = {
- .global_T_human = pose,
+ .pose = pose,
.linearVelocity = Eigen::Vector2f::Zero(), //TODO more sophisticated guess
.detectionTime = measurements.detectionTime};
--
GitLab
From 14fe938221a9a23b28d8c065984dbff4a1ee4f19 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 19:51:42 +0200
Subject: [PATCH 14/34] Implement HumanTracker::mostRecentTrackedHumans()
---
.../dynamic_scene_provider/HumanTracker.cpp | 26 +++++++++++++++++++
1 file changed, 26 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index fd15dc3b..dcd76e7d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -49,6 +49,32 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
}
+ std::vector<human::Human>
+ HumanTracker::mostRecentTrackedHumans() const
+ {
+ if (trackedHumans.empty())
+ {
+ return {};
+ }
+
+ std::sort(trackedHumans.begin(),
+ trackedHumans.end(),
+ [](human::Human a, human::Human b) -> bool
+ { return a.detectionTime.operator<=(b.detectionTime); });
+ DateTime mostRecentTime = trackedHumans.back().detectionTime;
+
+ std::vector<human::Human> mostRecentHumans;
+
+ for (human::Human trackedHuman : trackedHumans)
+ {
+ if (trackedHuman.detectionTime == mostRecentTime)
+ {
+ mostRecentHumans.push_back(trackedHuman);
+ }
+ }
+
+ return mostRecentHumans;
+ }
void
HumanTracker::reset()
--
GitLab
From da681818071ad9467bb3055c44f059e6e7e92806 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 20:13:35 +0200
Subject: [PATCH 15/34] Implement HumanTracker::getMeasurementPositions()
---
.../dynamic_scene_provider/HumanTracker.cpp | 40 ++++++++++++++-----
.../dynamic_scene_provider/HumanTracker.h | 5 +++
2 files changed, 34 insertions(+), 11 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index dcd76e7d..6bae4e5d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -11,18 +11,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
trackedHumans.clear();
+ auto positions = getMeasurementPositions(measurements);
+
for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
{
-
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
-
- Eigen::Vector3f centerPos = Eigen::Vector3f::Zero();
- for (const auto& [_, v] : keypoints)
- {
- centerPos += v.positionGlobal;
- }
- centerPos /= static_cast<float>(keypoints.size());
+ auto centerPos = positions.find(humanPose);
core::Pose2D pose = core::Pose2D::Identity();
pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
@@ -44,13 +37,38 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
+ std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
+ HumanTracker::getMeasurementPositions(const Measurements& measurements) const
+ {
+ std::map<armem::human::HumanPose&, armarx::navigation::core::Position> map;
+
+ for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+ {
+
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
+
+ auto centerPos = armarx::navigation::core::Position::Zero();
+ for (const auto& [_, v] : keypoints)
+ {
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
+
+ map.insert(
+ std::pair<const armem::human::HumanPose&, armarx::navigation::core::Position>(
+ humanPose, centerPos));
+ }
+ }
+
+
std::map<armem::human::HumanPose&, human::Human&>
HumanTracker::getAssociatedHumans(const Measurements& measurements) const
{
}
std::vector<human::Human>
- HumanTracker::mostRecentTrackedHumans() const
+ HumanTracker::getMostRecentTrackedHumans() const
{
if (trackedHumans.empty())
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 209fd345..76c6ce50 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -31,7 +31,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<human::Human> trackedHumans;
+ std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
+ getMeasurementPositions(const Measurements& measurements) const;
+
std::map<armem::human::HumanPose&, human::Human&>
getAssociatedHumans(const Measurements& measurements) const;
+
+ std::vector<human::Human> getMostRecentTrackedHumans() const;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From 65284dbe483f3d49765b38f89938606d59906ba0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 20:31:55 +0200
Subject: [PATCH 16/34] Add estimateAt to Human
---
source/armarx/navigation/human/types.cpp | 14 ++++++++++++++
source/armarx/navigation/human/types.h | 2 ++
2 files changed, 16 insertions(+)
create mode 100644 source/armarx/navigation/human/types.cpp
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
new file mode 100644
index 00000000..5b6d3d88
--- /dev/null
+++ b/source/armarx/navigation/human/types.cpp
@@ -0,0 +1,14 @@
+#include "types.h"
+
+namespace armarx::navigation::human
+{
+ core::Pose2D
+ Human::estimateAt(DateTime& time) const
+ {
+ double dt = (time - detectionTime).toSecondsDouble();
+ core::Pose2D estimation{pose};
+ estimation.translation() += linearVelocity * dt;
+ return estimation;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index ae490622..867849a1 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -32,6 +32,8 @@ namespace armarx::navigation::human
core::Pose2D pose;
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
+
+ core::Pose2D estimateAt(DateTime& time) const;
};
struct HumanGroup
--
GitLab
From 1dbc8494f02c1ccf30e75d25e5a780e96211cbe0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 20:36:21 +0200
Subject: [PATCH 17/34] Add aron conversion
---
source/armarx/navigation/human/CMakeLists.txt | 8 ++-
source/armarx/navigation/human/aron/Human.xml | 34 +++++++++
.../navigation/human/aron_conversions.cpp | 69 +++++++++++++++++++
.../navigation/human/aron_conversions.h | 46 +++++++++++++
source/armarx/navigation/human/shapes.h | 48 +++++++++++++
source/armarx/navigation/human/types.h | 4 +-
6 files changed, 207 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/human/aron/Human.xml
create mode 100644 source/armarx/navigation/human/aron_conversions.cpp
create mode 100644 source/armarx/navigation/human/aron_conversions.h
create mode 100644 source/armarx/navigation/human/shapes.h
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index c173b591..670c1869 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -1,14 +1,20 @@
armarx_add_aron_library(human_aron
ARON_FILES
+ aron/Human.xml
)
armarx_add_library(human
DEPENDENCIES_PUBLIC
ArmarXCore
armarx_navigation::core
+ armarx_navigation::conversions
DEPENDENCIES_PRIVATE
+ range-v3::range-v3
SOURCES
- #types.cpp
+ types.cpp
+ aron_conversions.cpp
HEADERS
types.h
+ aron_conversions.h
+ shapes.h
)
diff --git a/source/armarx/navigation/human/aron/Human.xml b/source/armarx/navigation/human/aron/Human.xml
new file mode 100644
index 00000000..57d104d8
--- /dev/null
+++ b/source/armarx/navigation/human/aron/Human.xml
@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <Object name='armarx::navigation::human::arondto::Human'>
+ <ObjectChild key='pose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='linearVelocity'>
+ <Position />
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::human::arondto::HumanGroup'>
+ <ObjectChild key='shape'>
+ <List>
+ <Position />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='humans'>
+ <List>
+ <armarx::navigation::human::arondto::Human />
+ </List>
+ </ObjectChild>
+ <ObjectChild key='detectionTime'>
+ <Time />
+ </ObjectChild>
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/human/aron_conversions.cpp b/source/armarx/navigation/human/aron_conversions.cpp
new file mode 100644
index 00000000..e09a3ca3
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.cpp
@@ -0,0 +1,69 @@
+#include "aron_conversions.h"
+
+#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
+#include <armarx/navigation/human/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
+namespace armarx::navigation::human
+{
+ void
+ toAron(arondto::Human& dto, const Human& bo)
+ {
+ dto.pose = conv::to3D(bo.pose).matrix();
+ dto.linearVelocity = conv::to3D(bo.linearVelocity);
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::Human& dto, Human& bo)
+ {
+ bo.pose = conv::to2D(core::Pose(dto.pose));
+ bo.linearVelocity = conv::to2D(dto.linearVelocity);
+ bo.detectionTime = dto.detectionTime;
+ }
+
+
+ void
+ toAron(arondto::HumanGroup& dto, const HumanGroup& bo)
+ {
+ dto.shape = bo.shape.vertices |
+ ranges::views::transform([](const Eigen::Vector2f& boVer) -> Eigen::Vector3f
+ { return conv::to3D(boVer); }) |
+ ranges::to_vector;
+
+ dto.humans = bo.humans |
+ ranges::views::transform(
+ [](const Human& boHuman) -> arondto::Human
+ {
+ arondto::Human dtoHuman;
+ toAron(dtoHuman, boHuman);
+ return dtoHuman;
+ }) |
+ ranges::to_vector;
+ dto.detectionTime = bo.detectionTime;
+ }
+
+ void
+ fromAron(const arondto::HumanGroup& dto, HumanGroup& bo)
+ {
+ bo.shape.vertices =
+ dto.shape |
+ ranges::views::transform([](const Eigen::Vector3f& dtoVer) -> Eigen::Vector2f
+ { return conv::to2D(dtoVer); }) |
+ ranges::to_vector;
+
+ bo.humans = dto.humans |
+ ranges::views::transform(
+ [](const arondto::Human& dtoHuman) -> Human
+ {
+ Human boHuman;
+ fromAron(dtoHuman, boHuman);
+ return boHuman;
+ }) |
+ ranges::to_vector;
+ bo.detectionTime = dto.detectionTime;
+ }
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/aron_conversions.h b/source/armarx/navigation/human/aron_conversions.h
new file mode 100644
index 00000000..71159748
--- /dev/null
+++ b/source/armarx/navigation/human/aron_conversions.h
@@ -0,0 +1,46 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+namespace armarx::navigation::human
+{
+ struct Human;
+ struct HumanGroup;
+
+ namespace arondto
+ {
+ struct Human;
+ struct HumanGroup;
+
+ } // namespace arondto
+
+} // namespace armarx::navigation::human
+
+namespace armarx::navigation::human
+{
+ void toAron(arondto::Human& dto, const Human& bo);
+ void fromAron(const arondto::Human& dto, Human& bo);
+
+ void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
+ void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
+
+} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/shapes.h b/source/armarx/navigation/human/shapes.h
new file mode 100644
index 00000000..f6fd25a5
--- /dev/null
+++ b/source/armarx/navigation/human/shapes.h
@@ -0,0 +1,48 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/shapes.h>
+
+#include <armarx/navigation/core/basic_types.h>
+
+namespace armarx::navigation::human::shapes
+{
+
+ /**
+ * @brief An axis oriented ellipse with half-axes a and b along the x- and y-axis respectively.
+ */
+ struct Ellipse
+ {
+ float a;
+ float b;
+ };
+
+ /**
+ * @brief A polygon with arbitrarily many vertices. The polygon will always be closed automatically.
+ */
+ struct Polygon
+ {
+ std::vector<Eigen::Vector2f> vertices;
+ };
+
+} // namespace armarx::navigation::human::shapes
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 867849a1..6b66e7c6 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/time.h>
#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/shapes.h>
namespace armarx::navigation::human
{
@@ -38,7 +39,7 @@ namespace armarx::navigation::human
struct HumanGroup
{
- std::vector<Eigen::Vector2f> vertices;
+ shapes::Polygon shape;
std::vector<Human> humans;
DateTime detectionTime;
};
@@ -46,6 +47,7 @@ namespace armarx::navigation::human
struct ProxemicZone
{
core::Pose2D pose;
+ shapes::Ellipse shape;
};
} // namespace armarx::navigation::human
--
GitLab
From 00272e81f240f1a4c58da85ce223e0c4e7043121 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 15 Aug 2022 22:52:19 +0200
Subject: [PATCH 18/34] Fix const
---
source/armarx/navigation/human/types.cpp | 2 +-
source/armarx/navigation/human/types.h | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/types.cpp b/source/armarx/navigation/human/types.cpp
index 5b6d3d88..6296ba6e 100644
--- a/source/armarx/navigation/human/types.cpp
+++ b/source/armarx/navigation/human/types.cpp
@@ -3,7 +3,7 @@
namespace armarx::navigation::human
{
core::Pose2D
- Human::estimateAt(DateTime& time) const
+ Human::estimateAt(const DateTime& time) const
{
double dt = (time - detectionTime).toSecondsDouble();
core::Pose2D estimation{pose};
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 6b66e7c6..625fa764 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -34,7 +34,7 @@ namespace armarx::navigation::human
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
- core::Pose2D estimateAt(DateTime& time) const;
+ core::Pose2D estimateAt(const DateTime& time) const;
};
struct HumanGroup
--
GitLab
From 87e3ad4e552b6aeb6bb6b7d432be2ff2b83c3a7c Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Mon, 15 Aug 2022 23:57:10 +0200
Subject: [PATCH 19/34] Implement human tracking.
TODO: Integrate new definitions of keypoints
---
.../dynamic_scene_provider/CMakeLists.txt | 2 +
.../dynamic_scene_provider/HumanTracker.cpp | 185 ++++++++++++------
.../dynamic_scene_provider/HumanTracker.h | 29 ++-
3 files changed, 152 insertions(+), 64 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index c53238db..d9dce5d0 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -35,6 +35,8 @@ armarx_add_component(dynamic_scene_provider
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
# FOO
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 6bae4e5d..279d579d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -3,97 +3,172 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
+
namespace armarx::navigation::components::dynamic_scene_provider
{
- void
- HumanTracker::update(const Measurements& measurements)
- {
- trackedHumans.clear();
- auto positions = getMeasurementPositions(measurements);
+ HumanTracker::DetectedHuman
+ convertHumanPoseToPosition(const armem::human::HumanPose& humanPose)
+ {
+ const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ ARMARX_CHECK_NOT_EMPTY(keypoints);
- for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
+ auto centerPos = core::Position::Zero();
+ for (const auto& [_, v] : keypoints)
{
- auto centerPos = positions.find(humanPose);
-
- core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = Eigen::Vector2f{centerPos.x(), centerPos.y()};
- pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
+ centerPos += v.positionGlobal;
+ }
+ centerPos /= static_cast<float>(keypoints.size());
- const human::Human newHuman = {
- .pose = pose,
- .linearVelocity = Eigen::Vector2f::Zero(), //TODO more sophisticated guess
- .detectionTime = measurements.detectionTime};
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation() = centerPos;
+ pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
- trackedHumans.push_back(newHuman);
- }
+ return pose;
}
- const std::vector<human::Human>&
- HumanTracker::getTrackedHumans() const
+ void
+ HumanTracker::update(const Measurements& measurements)
{
- return trackedHumans;
- }
+ //TODO: proper time to live
+ Duration maxAge = Duration::MilliSeconds(500);
+ for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
+ {
+ auto& human = *it;
+ if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+ {
+ it = trackedHumans.erase(it);
+ }
+ else
+ {
+ human.associated = false;
+ human.human.pose = human.human.estimateAt(measurements.detectionTime);
+ it++;
+ }
+ }
- std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
- HumanTracker::getMeasurementPositions(const Measurements& measurements) const
- {
- std::map<armem::human::HumanPose&, armarx::navigation::core::Position> map;
+ std::vector<DetectedHuman> newPoses = measurements.humanPoses |
+ ranges::views::transform(convertHumanPoseToPosition) |
+ ranges::to_vector;
- for (const armem::human::HumanPose& humanPose : measurements.humanPoses)
- {
+ associateHumans(newPoses);
+ }
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
- ARMARX_CHECK_NOT_EMPTY(keypoints);
+ struct PosDistance
+ {
+ HumanTracker::TrackedHuman& oldHuman;
+ HumanTracker::DetectedHuman& newHuman;
+ const float distance;
+ };
+
+ std::vector<PosDistance>
+ getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ const std::vector<HumanTracker::DetectedHuman>& newHumans)
+ {
+ std::vector<PosDistance> posDistances;
- auto centerPos = armarx::navigation::core::Position::Zero();
- for (const auto& [_, v] : keypoints)
+ for (const auto& oldHuman : oldHumans)
+ {
+ if (oldHuman.associated)
{
- centerPos += v.positionGlobal;
+ continue;
+ }
+ for (const auto& newHuman : newHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ posDistances.emplace_back(
+ oldHuman,
+ newHuman,
+ (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
}
- centerPos /= static_cast<float>(keypoints.size());
-
- map.insert(
- std::pair<const armem::human::HumanPose&, armarx::navigation::core::Position>(
- humanPose, centerPos));
}
- }
+ std::sort(posDistances.begin(),
+ posDistances.end(),
+ [](const PosDistance& a, const PosDistance& b) -> bool
+ { return a.distance < b.distance; });
- std::map<armem::human::HumanPose&, human::Human&>
- HumanTracker::getAssociatedHumans(const Measurements& measurements) const
- {
+ return posDistances;
}
- std::vector<human::Human>
- HumanTracker::getMostRecentTrackedHumans() const
+ void
+ HumanTracker::associateHumans(std::vector<DetectedHuman>& detectedHumans)
{
- if (trackedHumans.empty())
+ // associate humans by their tracking id
+ for (auto& oldHuman : trackedHumans)
{
- return {};
+ if (oldHuman.associated || !oldHuman.trackingId)
+ {
+ continue;
+ }
+ for (auto& newHuman : detectedHumans)
+ {
+ if (newHuman.associated)
+ {
+ continue;
+ }
+ if (oldHuman.trackingId.value() == newHuman.trackingId)
+ {
+ associate(oldHuman, newHuman);
+ }
+ }
}
- std::sort(trackedHumans.begin(),
- trackedHumans.end(),
- [](human::Human a, human::Human b) -> bool
- { return a.detectionTime.operator<=(b.detectionTime); });
- DateTime mostRecentTime = trackedHumans.back().detectionTime;
- std::vector<human::Human> mostRecentHumans;
+ // associate leftover humans by their distances
+ const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
- for (human::Human trackedHuman : trackedHumans)
+ for (auto& posDistance : sortedDistances)
{
- if (trackedHuman.detectionTime == mostRecentTime)
+ if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated)
{
- mostRecentHumans.push_back(trackedHuman);
+ continue;
}
+ associate(posDistance.oldHuman, posDistance.newHuman);
}
+ }
- return mostRecentHumans;
+ void
+ HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+ {
+ ARMARX_CHECK(!trackedHuman.associated);
+ ARMARX_CHECK(!detectedHuman.associated);
+
+ trackedHuman.associated = true;
+ detectedHuman.associated = true;
+
+ // TODO alpha parameter
+ float a = 0.7;
+
+ float dt =
+ (detectedHuman.detectionTime - trackedHuman.human.detectionTime).toSecondsDouble();
+ Eigen::Vector2f ds =
+ (detectedHuman.pose.translation() - trackedHuman.human.pose.translation());
+ Eigen::Vector2f linVelocity = ds / dt;
+
+ Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman.human.linearVelocity;
+
+ trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
+ trackedHuman.trackingId = detectedHuman.trackingId;
}
+ std::vector<human::Human>
+ HumanTracker::getTrackedHumans() const
+ {
+ return trackedHumans |
+ ranges::views::transform([](const TrackedHuman& h) -> human::Human
+ { return h.human; }) |
+ ranges::to_vector;
+ }
+
+
void
HumanTracker::reset()
{
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 76c6ce50..132464db 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -22,21 +22,32 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::vector<armem::human::HumanPose> humanPoses;
};
+ struct DetectedHuman
+ {
+ core::Pose2D pose;
+ std::string trackingId;
+ DateTime detectionTime;
+ bool associated;
+ };
+
+ struct TrackedHuman
+ {
+ human::Human human;
+ std::optional<std::string> trackingId = std::nullopt;
+ bool associated;
+ };
+
void update(const Measurements& measurements);
- const std::vector<human::Human>& getTrackedHumans() const;
+ std::vector<human::Human> getTrackedHumans() const;
void reset();
private:
- std::vector<human::Human> trackedHumans;
-
- std::map<const armem::human::HumanPose&, armarx::navigation::core::Position>
- getMeasurementPositions(const Measurements& measurements) const;
+ void associateHumans(std::vector<DetectedHuman>& detectedHumans);
+ void associate(TrackedHuman& tracked, DetectedHuman& detected);
- std::map<armem::human::HumanPose&, human::Human&>
- getAssociatedHumans(const Measurements& measurements) const;
-
- std::vector<human::Human> getMostRecentTrackedHumans() const;
+ private:
+ std::vector<TrackedHuman> trackedHumans;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From b43ce686014462297db376d6bac4e846b54acde9 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 00:22:49 +0200
Subject: [PATCH 20/34] Use new HumanPose definition with TrackingID
---
.../dynamic_scene_provider/HumanTracker.cpp | 40 +++++++++++++------
.../dynamic_scene_provider/HumanTracker.h | 6 +--
2 files changed, 31 insertions(+), 15 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 279d579d..61eaaa69 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -11,23 +11,29 @@ namespace armarx::navigation::components::dynamic_scene_provider
HumanTracker::DetectedHuman
- convertHumanPoseToPosition(const armem::human::HumanPose& humanPose)
+ convertHumanPoseToPosition(const DateTime& time, const armem::human::HumanPose& humanPose)
{
- const armem::human::Keypoint3DIdMap& keypoints = humanPose.keypoint3dMap;
+ const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints);
auto centerPos = core::Position::Zero();
+ int size = 0;
for (const auto& [_, v] : keypoints)
{
- centerPos += v.positionGlobal;
+ if (v.positionGlobal.has_value())
+ {
+ centerPos += v.positionGlobal.value().toEigen();
+ size++;
+ }
}
- centerPos /= static_cast<float>(keypoints.size());
+ centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity();
pose.translation() = centerPos;
- pose.linear() = Eigen::Rotation2Df(0 /*angle*/).toRotationMatrix();
+ //TODO: angle
+ pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
- return pose;
+ return {pose, humanPose.humanTrackingId, time, false};
}
void
@@ -51,9 +57,12 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
}
- std::vector<DetectedHuman> newPoses = measurements.humanPoses |
- ranges::views::transform(convertHumanPoseToPosition) |
- ranges::to_vector;
+ std::vector<DetectedHuman> newPoses =
+ measurements.humanPoses |
+ ranges::views::transform(
+ [measurements](const armem::human::HumanPose& humanPose) -> DetectedHuman
+ { return convertHumanPoseToPosition(measurements.detectionTime, humanPose); }) |
+ ranges::to_vector;
associateHumans(newPoses);
}
@@ -110,11 +119,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
for (auto& newHuman : detectedHumans)
{
- if (newHuman.associated)
+ if (newHuman.associated || !newHuman.trackingId)
{
continue;
}
- if (oldHuman.trackingId.value() == newHuman.trackingId)
+ if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{
associate(oldHuman, newHuman);
}
@@ -125,9 +134,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
// associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
+ //TODO max distance parameter
+ float maxDistance = 600;
+
for (auto& posDistance : sortedDistances)
{
- if (!posDistance.oldHuman.associated || !posDistance.newHuman.associated)
+ if (posDistance.distance > maxDistance)
+ {
+ break;
+ }
+ if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
{
continue;
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index 132464db..f13a6653 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct DetectedHuman
{
- core::Pose2D pose;
- std::string trackingId;
- DateTime detectionTime;
+ const core::Pose2D pose;
+ const std::optional<std::string> trackingId;
+ const DateTime detectionTime;
bool associated;
};
--
GitLab
From 7c38b6f4ebdbf6fca408ca47c53e5fbf397d884b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 16 Aug 2022 00:38:07 +0200
Subject: [PATCH 21/34] Change human library CMake target due to naming
conflict
---
source/armarx/navigation/human/CMakeLists.txt | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 670c1869..fd5fb36c 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -1,9 +1,9 @@
-armarx_add_aron_library(human_aron
+armarx_add_aron_library(teb_human_aron
ARON_FILES
aron/Human.xml
)
-armarx_add_library(human
+armarx_add_library(teb_human
DEPENDENCIES_PUBLIC
ArmarXCore
armarx_navigation::core
--
GitLab
From e27109b4d39e1a63b943d242e1c33fdf0fa88280 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 00:42:21 +0200
Subject: [PATCH 22/34] New library name
---
.../navigation/components/dynamic_scene_provider/CMakeLists.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index d9dce5d0..39d89c1a 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -31,7 +31,7 @@ armarx_add_component(dynamic_scene_provider
armarx_navigation::util
armarx_navigation::memory
armarx_navigation::dynamic_scene
- armarx_navigation::human
+ armarx_navigation::teb_human
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
--
GitLab
From b7dcb8b4c0ea349a5aca664152535ff5bbe9e0ae Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 01:24:25 +0200
Subject: [PATCH 23/34] Fix eigen errors
---
.../dynamic_scene_provider/Component.cpp | 8 +--
.../dynamic_scene_provider/HumanTracker.cpp | 50 +++++++++----------
.../dynamic_scene_provider/HumanTracker.h | 8 +--
3 files changed, 31 insertions(+), 35 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index a4d10991..e8abd88f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -318,13 +318,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
// here ends: data fetching
-
- humanTracker.update(HumanTracker::Measurements{
- .humanPoses = humanPoseResult.humanPoses
- });
-
-
+ humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
+ .humanPoses = humanPoseResult.humanPoses});
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 61eaaa69..a63f8a30 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -16,7 +16,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
const std::map<std::string, armem::human::PoseKeypoint>& keypoints = humanPose.keypoints;
ARMARX_CHECK_NOT_EMPTY(keypoints);
- auto centerPos = core::Position::Zero();
+ Eigen::Vector3f centerPos;
int size = 0;
for (const auto& [_, v] : keypoints)
{
@@ -29,7 +29,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = centerPos;
+ pose.translation() = centerPos.head(2);
//TODO: angle
pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
@@ -69,33 +69,33 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct PosDistance
{
- HumanTracker::TrackedHuman& oldHuman;
- HumanTracker::DetectedHuman& newHuman;
- const float distance;
+ HumanTracker::TrackedHuman* oldHuman;
+ HumanTracker::DetectedHuman* newHuman;
+ float distance;
};
std::vector<PosDistance>
- getSortedDistances(const std::vector<HumanTracker::TrackedHuman>& oldHumans,
- const std::vector<HumanTracker::DetectedHuman>& newHumans)
+ getSortedDistances(std::vector<HumanTracker::TrackedHuman>& oldHumans,
+ std::vector<HumanTracker::DetectedHuman>& newHumans)
{
std::vector<PosDistance> posDistances;
- for (const auto& oldHuman : oldHumans)
+ for (auto& oldHuman : oldHumans)
{
if (oldHuman.associated)
{
continue;
}
- for (const auto& newHuman : newHumans)
+ for (auto& newHuman : newHumans)
{
if (newHuman.associated)
{
continue;
}
- posDistances.emplace_back(
- oldHuman,
- newHuman,
- (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm());
+ posDistances.push_back(
+ {&oldHuman,
+ &newHuman,
+ (newHuman.pose.translation() - oldHuman.human.pose.translation()).norm()});
}
}
@@ -125,7 +125,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
if (oldHuman.trackingId.value() == newHuman.trackingId.value())
{
- associate(oldHuman, newHuman);
+ associate(&oldHuman, &newHuman);
}
}
}
@@ -143,7 +143,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
break;
}
- if (posDistance.oldHuman.associated || posDistance.newHuman.associated)
+ if (posDistance.oldHuman->associated || posDistance.newHuman->associated)
{
continue;
}
@@ -152,27 +152,27 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
void
- HumanTracker::associate(TrackedHuman& trackedHuman, DetectedHuman& detectedHuman)
+ HumanTracker::associate(TrackedHuman* trackedHuman, DetectedHuman* detectedHuman)
{
- ARMARX_CHECK(!trackedHuman.associated);
- ARMARX_CHECK(!detectedHuman.associated);
+ ARMARX_CHECK(!trackedHuman->associated);
+ ARMARX_CHECK(!detectedHuman->associated);
- trackedHuman.associated = true;
- detectedHuman.associated = true;
+ trackedHuman->associated = true;
+ detectedHuman->associated = true;
// TODO alpha parameter
float a = 0.7;
float dt =
- (detectedHuman.detectionTime - trackedHuman.human.detectionTime).toSecondsDouble();
+ (detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
Eigen::Vector2f ds =
- (detectedHuman.pose.translation() - trackedHuman.human.pose.translation());
+ (detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
Eigen::Vector2f linVelocity = ds / dt;
- Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman.human.linearVelocity;
+ Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman->human.linearVelocity;
- trackedHuman.human = {detectedHuman.pose, velocity, detectedHuman.detectionTime};
- trackedHuman.trackingId = detectedHuman.trackingId;
+ trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
+ trackedHuman->trackingId = detectedHuman->trackingId;
}
std::vector<human::Human>
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index f13a6653..e5197d82 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -24,9 +24,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct DetectedHuman
{
- const core::Pose2D pose;
- const std::optional<std::string> trackingId;
- const DateTime detectionTime;
+ core::Pose2D pose;
+ std::optional<std::string> trackingId;
+ DateTime detectionTime;
bool associated;
};
@@ -45,7 +45,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
void associateHumans(std::vector<DetectedHuman>& detectedHumans);
- void associate(TrackedHuman& tracked, DetectedHuman& detected);
+ void associate(TrackedHuman* tracked, DetectedHuman* detected);
private:
std::vector<TrackedHuman> trackedHumans;
--
GitLab
From 8c3e1e57edd7c1c5a3fff6b8b9835a46c74d5bc1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 16 Aug 2022 03:07:21 +0200
Subject: [PATCH 24/34] Use eigen conversions
---
.../components/dynamic_scene_provider/HumanTracker.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index a63f8a30..4adc1106 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -3,6 +3,7 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <armarx/navigation/conversions/eigen.h>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
@@ -29,7 +30,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
centerPos /= size;
core::Pose2D pose = core::Pose2D::Identity();
- pose.translation() = centerPos.head(2);
+ pose.translation() = conv::to2D(centerPos);
//TODO: angle
pose.linear() = Eigen::Rotation2Df(0).toRotationMatrix();
--
GitLab
From f73113a36c1d33eb98f8b4a31bc1f2dca29f2c62 Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Tue, 16 Aug 2022 23:43:35 +0000
Subject: [PATCH 25/34] Create parameter struct in header file for human
tracking parameters
Not quite sure if it builds though as I edited this in the online editor, should be checked at some time
---
.../dynamic_scene_provider/HumanTracker.cpp | 15 +++------------
.../dynamic_scene_provider/HumanTracker.h | 11 +++++++++++
2 files changed, 14 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 4adc1106..a86c684c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -40,13 +40,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
void
HumanTracker::update(const Measurements& measurements)
{
- //TODO: proper time to live
- Duration maxAge = Duration::MilliSeconds(500);
-
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+ if ((measurements.detectionTime - human.human.detectionTime) >= parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
@@ -135,12 +132,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
- //TODO max distance parameter
- float maxDistance = 600;
-
for (auto& posDistance : sortedDistances)
{
- if (posDistance.distance > maxDistance)
+ if (posDistance.distance > parameters.maxAssociationDistance)
{
break;
}
@@ -161,16 +155,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
trackedHuman->associated = true;
detectedHuman->associated = true;
- // TODO alpha parameter
- float a = 0.7;
-
float dt =
(detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
Eigen::Vector2f ds =
(detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
Eigen::Vector2f linVelocity = ds / dt;
- Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman->human.linearVelocity;
+ Eigen::Vector2f velocity = parameters.velocityAlpha * linVelocity + (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
trackedHuman->trackingId = detectedHuman->trackingId;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index e5197d82..b29011b4 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -37,6 +37,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
bool associated;
};
+ struct Parameters
+ {
+ // the duration after which tracked humans will be erased if no new measurement for this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(500);
+ // the maximum distance in millimeters of two human measurements to be associated with each other
+ float maxAssociationDistance = 600;
+ // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
+ float velocityAlpha = 0.7;
+ };
+
void update(const Measurements& measurements);
std::vector<human::Human> getTrackedHumans() const;
@@ -49,5 +59,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<TrackedHuman> trackedHumans;
+ Parameter parameters;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From c668ada987838a18990ccfebd33df4c15a539e15 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 17 Aug 2022 02:15:32 +0200
Subject: [PATCH 26/34] Fix spelling mistake, apply formatting
---
.../components/dynamic_scene_provider/HumanTracker.cpp | 7 +++++--
.../components/dynamic_scene_provider/HumanTracker.h | 2 +-
2 files changed, 6 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index a86c684c..59158f28 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -43,7 +43,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >= parameters.maxTrackingAge)
+ if ((measurements.detectionTime - human.human.detectionTime) >=
+ parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
@@ -161,7 +162,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
(detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
Eigen::Vector2f linVelocity = ds / dt;
- Eigen::Vector2f velocity = parameters.velocityAlpha * linVelocity + (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
+ Eigen::Vector2f velocity =
+ parameters.velocityAlpha * linVelocity +
+ (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
trackedHuman->trackingId = detectedHuman->trackingId;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index b29011b4..9146a798 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -59,6 +59,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<TrackedHuman> trackedHumans;
- Parameter parameters;
+ Parameters parameters;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab
From 80d2baa4b8c98015e348e91b3743ffe20f28e1b7 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:24:05 +0200
Subject: [PATCH 27/34] scenario HumanAwareNavigation update
---
.../HumanAwareNavigation.scx | 2 +
.../config/HumanMemoryApp.cfg | 368 ++++++++++++++++++
.../config/control_memory.cfg | 26 +-
.../distance_to_obstacle_costmap_provider.cfg | 262 +++++++++++++
..._distance_to_obstacle_costmap_provider.cfg | 39 ++
.../config/dynamic_scene_provider.cfg | 34 +-
.../config/navigation_memory.cfg | 20 +-
.../HumanAwareNavigation/config/navigator.cfg | 8 -
8 files changed, 711 insertions(+), 48 deletions(-)
create mode 100644 scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
create mode 100644 scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
diff --git a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
index 3c4d6c5a..570cffa4 100644
--- a/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
+++ b/scenarios/HumanAwareNavigation/HumanAwareNavigation.scx
@@ -11,5 +11,7 @@
<application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="dynamic_scene_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="HumanMemoryApp" instance="" package="VisionX" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg b/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
new file mode 100644
index 00000000..28099ebc
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/HumanMemoryApp.cfg
@@ -0,0 +1,368 @@
+# ==================================================================
+# HumanMemoryApp 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.
+# 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.HumanMemory.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.HumanMemory.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.HumanMemory.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.HumanMemory.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.HumanMemory.EnableProfiling = false
+
+
+# ArmarX.HumanMemory.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.HumanMemory.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.HumanMemory.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ObjectName = ""
+
+
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize: Maximal size of the FaceRecognition entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.face.seg.CoreSegmentName: Name of the FaceRecognition core segment.
+# Attributes:
+# - Default: FaceRecognition
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.face.seg.CoreSegmentName = FaceRecognition
+
+
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize: Maximal size of the Identification entity histories (-1 for infinite).
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreMaxHistorySize = -1
+
+
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName: Name of the Identification core segment.
+# Attributes:
+# - Default: Identification
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.ident.seg.CoreSegmentName = Identification
+
+
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize: Maximal size of the PersonInstance entity histories (-1 for infinite).
+# Attributes:
+# - Default: 32
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreMaxHistorySize = 32
+
+
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName: Name of the PersonInstance core segment.
+# Attributes:
+# - Default: PersonInstance
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.instanceseg.CoreSegmentName = PersonInstance
+
+
+# ArmarX.HumanMemory.mem.MemoryName: Name of this memory server.
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.MemoryName = Human
+
+
+# ArmarX.HumanMemory.mem.ltm.configuration:
+# Attributes:
+# - Default: {}
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.ltm.configuration = {}
+
+
+# ArmarX.HumanMemory.mem.ltm.enabled:
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.mem.ltm.enabled = false
+
+
+# ArmarX.HumanMemory.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.HumanMemory.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.HumanMemory.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.HumanMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize: Maximal size of the Pose entity histories (-1 for infinite).
+# Attributes:
+# - Default: 256
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreMaxHistorySize = 256
+
+
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName: Name of the Pose core segment.
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.pose.seg.CoreSegmentName = Pose
+
+
+# ArmarX.HumanMemory.profile.pk.load: Load profiles from prior knowledge on startup.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.HumanMemory.profile.pk.load = true
+
+
+# ArmarX.HumanMemory.profile.pk.packageName: ArmarX package to load human profiles from.
+# Attributes:
+# - Default: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.pk.packageName = PriorKnowledgeData
+
+
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize: Maximal size of the Profile entity histories (-1 for infinite).
+# Attributes:
+# - Default: 64
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreMaxHistorySize = 64
+
+
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName: Name of the Profile core segment.
+# Attributes:
+# - Default: Profile
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.HumanMemory.profile.seg.CoreSegmentName = Profile
+
+
+# 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
+
+
diff --git a/scenarios/HumanAwareNavigation/config/control_memory.cfg b/scenarios/HumanAwareNavigation/config/control_memory.cfg
index 4aad1c46..53efd6fe 100644
--- a/scenarios/HumanAwareNavigation/config/control_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/control_memory.cfg
@@ -92,37 +92,21 @@
# ArmarX.ControlMemory.mem.MemoryName = Control
-# ArmarX.ControlMemory.mem.ltm..configuration:
+# ArmarX.ControlMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ControlMemory.mem.ltm..configuration = ""
+# ArmarX.ControlMemory.mem.ltm.configuration = {}
-# ArmarX.ControlMemory.mem.ltm..enabled:
+# ArmarX.ControlMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ControlMemory.mem.ltm..enabled = false
-
-
-# ArmarX.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ControlMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.ControlMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ControlMemory.mem.ltm.storagepath = Default value not mapped.
+# ArmarX.ControlMemory.mem.ltm.enabled = false
# ArmarX.ControlMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
new file mode 100644
index 00000000..6059c7a8
--- /dev/null
+++ b/scenarios/HumanAwareNavigation/config/distance_to_obstacle_costmap_provider.cfg
@@ -0,0 +1,262 @@
+# ==================================================================
+# distance_to_obstacle_costmap_provider 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.
+# 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.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.EnableProfiling = false
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.ObjectName = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Memory = Navigation
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.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.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName: Robot name.
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+
+
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
index 27479d20..565bb501 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -260,6 +260,45 @@
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.name = ""
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name:
+# Attributes:
+# - Default: Armar6
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.robot.name = Armar6
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name:
+# Attributes:
+# - Default: distance_to_obstacles
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.name = distance_to_obstacles
+
+
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.staticCostmap.providerName = ::_NOT_SET_::
+
+
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.p.updatePeriodMs:
# Attributes:
# - Default: 100
diff --git a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
index 98cd68b7..c632df10 100644
--- a/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
+++ b/scenarios/HumanAwareNavigation/config/dynamic_scene_provider.cfg
@@ -191,7 +191,7 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.dynamic_scene_provider.MinimumLoggingLevel = Undefined
+ArmarX.dynamic_scene_provider.MinimumLoggingLevel = Verbose
# ArmarX.dynamic_scene_provider.ObjectMemoryName: Name of the object memory.
@@ -242,6 +242,30 @@
# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory = Navigation
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.CoreSegment = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.human.Provider = ""
+
+
# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
@@ -308,6 +332,14 @@
# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider:
+# Attributes:
+# - Default: OpenNIPointCloudProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.humanPoseProvider = OpenNIPointCloudProvider
+
+
# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name:
# Attributes:
# - Default: ""
diff --git a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
index c024b256..38cf4a09 100644
--- a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
@@ -152,10 +152,10 @@ ArmarX.NavigationMemory.MinimumLoggingLevel = Verbose
# ArmarX.NavigationMemory.mem.ltm.configuration:
# Attributes:
-# - Default: ""
+# - Default: {}
# - Case sensitivity: yes
# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.configuration = ""
+# ArmarX.NavigationMemory.mem.ltm.configuration = {}
# ArmarX.NavigationMemory.mem.ltm.enabled:
@@ -167,22 +167,6 @@ ArmarX.NavigationMemory.MinimumLoggingLevel = Verbose
# ArmarX.NavigationMemory.mem.ltm.enabled = false
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes: The size in MB to compress away the current export. Exports are numbered (lower number means newer).
-# Attributes:
-# - Default: 1024
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.sizeToCompressDataInMegaBytes = 1024
-
-
-# ArmarX.NavigationMemory.mem.ltm.storagepath: The path to the memory storage (the memory will be stored in a seperate subfolder).
-# Attributes:
-# - Default: Default value not mapped.
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.NavigationMemory.mem.ltm.storagepath = Default value not mapped.
-
-
# ArmarX.NavigationMemory.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:
diff --git a/scenarios/HumanAwareNavigation/config/navigator.cfg b/scenarios/HumanAwareNavigation/config/navigator.cfg
index 0484c3f0..5d038290 100644
--- a/scenarios/HumanAwareNavigation/config/navigator.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigator.cfg
@@ -181,14 +181,6 @@ ArmarX.Navigator.cmp.PlatformUnit = Armar6PlatformUnit
# ArmarX.Navigator.mem.nav.costmap.Memory = Navigation
-# ArmarX.Navigator.mem.nav.costmap.Provider: Name of this provider
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.Navigator.mem.nav.costmap.Provider = ""
-
-
# ArmarX.Navigator.mem.nav.events.CoreSegment:
# Attributes:
# - Default: Events
--
GitLab
From 2be9abaf22bce090e709ecef8bc166b1b76e7e24 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:25:15 +0200
Subject: [PATCH 28/34] memory: humans and groups client reader and writer
---
source/armarx/navigation/human/types.h | 8 +-
.../armarx/navigation/memory/CMakeLists.txt | 5 +
.../navigation/memory/client/human/Reader.cpp | 236 ++++++++++++++++++
.../navigation/memory/client/human/Reader.h | 97 +++++++
.../navigation/memory/client/human/Writer.cpp | 133 ++++++++++
.../navigation/memory/client/human/Writer.h | 69 +++++
source/armarx/navigation/memory/constants.h | 1 +
7 files changed, 548 insertions(+), 1 deletion(-)
create mode 100644 source/armarx/navigation/memory/client/human/Reader.cpp
create mode 100644 source/armarx/navigation/memory/client/human/Reader.h
create mode 100644 source/armarx/navigation/memory/client/human/Writer.cpp
create mode 100644 source/armarx/navigation/memory/client/human/Writer.h
diff --git a/source/armarx/navigation/human/types.h b/source/armarx/navigation/human/types.h
index 625fa764..b447df92 100644
--- a/source/armarx/navigation/human/types.h
+++ b/source/armarx/navigation/human/types.h
@@ -37,13 +37,19 @@ namespace armarx::navigation::human
core::Pose2D estimateAt(const DateTime& time) const;
};
+ using Humans = std::vector<Human>;
+
+
struct HumanGroup
{
shapes::Polygon shape;
- std::vector<Human> humans;
+ Humans humans;
DateTime detectionTime;
};
+ using HumanGroups = std::vector<HumanGroup>;
+
+
struct ProxemicZone
{
core::Pose2D pose;
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 8f4483b1..eba21c61 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -8,6 +8,8 @@ armarx_add_library(memory
client/events/Writer.cpp
client/costmap/Writer.cpp
client/costmap/Reader.cpp
+ client/human/Reader.cpp
+ client/human/Writer.cpp
# ./client/events/Reader.cpp
HEADERS
memory.h
@@ -18,6 +20,8 @@ armarx_add_library(memory
client/events/Writer.h
client/costmap/Writer.h
client/costmap/Reader.h
+ client/human/Reader.h
+ client/human/Writer.h
# ./client/events/Reader.h
DEPENDENCIES
ArmarXCoreInterfaces
@@ -27,6 +31,7 @@ armarx_add_library(memory
armarx_navigation::algorithms
armarx_navigation::graph
armarx_navigation::location
+ armarx_navigation::teb_human
)
armarx_add_test(memory_test
diff --git a/source/armarx/navigation/memory/client/human/Reader.cpp b/source/armarx/navigation/memory/client/human/Reader.cpp
new file mode 100644
index 00000000..e3bee9f2
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.cpp
@@ -0,0 +1,236 @@
+#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
+{
+ Reader::~Reader() = default;
+
+ armarx::armem::client::query::Builder
+ Reader::buildHumansQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties().coreSegmentName)
+ .providerSegments().withName(query.providerName)
+ .entities().withName("humans")
+ .snapshots().beforeOrAtTime(query.timestamp);
+ // clang-format on
+
+ 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
+ {
+ return "mem.nav.human.";
+ }
+
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
+ {
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::HumanCoreSegmentName};
+ }
+
+ navigation::human::Humans
+ asHumans(const armem::wm::ProviderSegment& providerSegment)
+ {
+ navigation::human::Humans humans;
+
+ ARMARX_CHECK(not providerSegment.empty()) << "No entities";
+ ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
+
+ providerSegment.forEachEntity(
+ [&humans](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&humans](const armem::wm::EntityInstance& entityInstance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(entityInstance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+ humans.push_back(human);
+ });
+ });
+
+ return humans;
+ }
+
+ navigation::human::HumanGroups
+ asGroups(const armem::wm::ProviderSegment& providerSegment)
+ {
+ 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](const armem::wm::Entity& entity)
+ {
+ const auto& entitySnapshot = entity.getLatestSnapshot();
+ ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+ entitySnapshot.forEachInstance(
+ [&humans](const armem::wm::EntityInstance& entityInstance)
+ {
+ 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_VERBOSE << "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_VERBOSE << "No entities.";
+ return {.groups = {},
+ .status = HumanGroupResult::Status::NoData,
+ .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanGroupResult{.groups = asGroups(providerSegment),
+ .status = HumanGroupResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanGroupResult{.groups = {},
+ .status = HumanGroupResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+
+ Reader::HumanResult
+ Reader::queryHumans(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 {.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
+
+ if (not coreSegment.hasProviderSegment(query.providerName))
+ {
+ ARMARX_VERBOSE << "Provider segment `" << query.providerName
+ << "` does not exist (yet).";
+ return {.humans = {}, .status = HumanResult::Status::NoData};
+ }
+
+ const armem::wm::ProviderSegment& providerSegment =
+ coreSegment.getProviderSegment(query.providerName);
+
+ if (providerSegment.empty())
+ {
+ ARMARX_VERBOSE << "No entities.";
+ return {
+ .humans = {}, .status = HumanResult::Status::NoData, .errorMessage = "No entities"};
+ }
+
+ try
+ {
+ return HumanResult{.humans = asHumans(providerSegment),
+ .status = HumanResult::Status::Success};
+ }
+ catch (...)
+ {
+ return HumanResult{.humans = {},
+ .status = HumanResult::Status::Error,
+ .errorMessage = GetHandledExceptionString()};
+ }
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Reader.h b/source/armarx/navigation/memory/client/human/Reader.h
new file mode 100644
index 00000000..2e693237
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Reader.h
@@ -0,0 +1,97 @@
+/*
+ * 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
+{
+
+ 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;
+ };
+
+ struct HumanResult
+ {
+ armarx::navigation::human::Humans humans;
+
+ enum Status
+ {
+ Success,
+ NoData,
+ Error
+ } status;
+
+ std::string errorMessage = "";
+
+ operator bool() const noexcept
+ {
+ return status == Status::Success;
+ }
+ };
+
+ 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;
+ };
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.cpp b/source/armarx/navigation/memory/client/human/Writer.cpp
new file mode 100644
index 00000000..c38c2ac7
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.cpp
@@ -0,0 +1,133 @@
+#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/memory/constants.h>
+#include <armarx/navigation/human/aron_conversions.h>
+
+
+namespace armarx::navigation::memory::client::human
+{
+ Writer::~Writer() = default;
+
+ bool
+ Writer::store(const armarx::navigation::human::Humans& humans,
+ // 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("humans").withTimestamp(timestamp);
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ 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);
+
+ 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.";
+ }
+
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
+ {
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::HumanCoreSegmentName};
+ }
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
new file mode 100644
index 00000000..b224149c
--- /dev/null
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -0,0 +1,69 @@
+/*
+ * 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
+{
+
+ /**
+ * @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::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& providerName,
+ const armem::Time& timestamp);
+
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+ };
+
+
+} // namespace armarx::navigation::memory::client::human
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index fe57b76c..2b3364a8 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -32,5 +32,6 @@ namespace armarx::navigation::memory::constants
inline const std::string GraphCoreSegmentName = "Graph";
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
+ inline const std::string HumanCoreSegmentName = "Human";
} // namespace armarx::navigation::memory::constants
--
GitLab
From 3742c7965a3419125c03cc0f9ad5fc5530e583df Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:26:23 +0200
Subject: [PATCH 29/34] cmake: adding comment to rename teb_human once the
CMake migration is complete
---
source/armarx/navigation/human/CMakeLists.txt | 3 +++
1 file changed, 3 insertions(+)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index fd5fb36c..76a44730 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -1,3 +1,6 @@
+# TODO remove teb_ once the ArmarX CMake migration is complete.
+# Note: "human" is a library defined in VisionX
+
armarx_add_aron_library(teb_human_aron
ARON_FILES
aron/Human.xml
--
GitLab
From e9de6c98d04b2d090b0127f8923fc7723989d69e Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:26:57 +0200
Subject: [PATCH 30/34] dummy implementation of HumanTracker
---
.../dynamic_scene_provider/HumanTracker.cpp | 18 ++++++++++++++++++
1 file changed, 18 insertions(+)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 4adc1106..7dd61dc6 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -3,6 +3,7 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "armarx/navigation/human/types.h"
#include <armarx/navigation/conversions/eigen.h>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
@@ -40,6 +41,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
void
HumanTracker::update(const Measurements& measurements)
{
+
+ trackedHumans.clear();
+
+ for (const armem::human::HumanPose& measurement : measurements.humanPoses)
+ {
+ human::Human human{
+ .pose = conv::to2D(
+ core::Pose(Eigen::Translation3f(measurement.keypoints.begin()->second.positionGlobal->toEigen()))),
+ .linearVelocity = Eigen::Vector2f::Zero(),
+ .detectionTime = 0};
+
+ trackedHumans.push_back(TrackedHuman{
+ .human = human, .trackingId = std::to_string(trackedHumans.size()), .associated = true});
+ }
+
+ return; // FIXME remove section above
+
//TODO: proper time to live
Duration maxAge = Duration::MilliSeconds(500);
--
GitLab
From 872c4c50c2ef3100cd85f61f1b7a076c2f40f163 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:27:21 +0200
Subject: [PATCH 31/34] navigation memory: new core segments + visu
---
.../NavigationMemory/CMakeLists.txt | 1 +
.../NavigationMemory/NavigationMemory.cpp | 12 +++-
.../NavigationMemory/NavigationMemory.h | 1 +
.../components/NavigationMemory/Visu.cpp | 68 ++++++++++++++++++-
.../components/NavigationMemory/Visu.h | 6 +-
5 files changed, 85 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 3f8da95e..5f88e9cf 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -18,6 +18,7 @@ armarx_add_component(navigation_memory
armarx_navigation::graph
armarx_navigation::location
armarx_navigation::algorithms
+ armarx_navigation::teb_human
SOURCES
NavigationMemory.cpp
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index a18764b0..97d8fa36 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -48,6 +48,7 @@
#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/location/constants.h>
@@ -127,6 +128,11 @@ namespace armarx::navigation
workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
navigation::core::arondto::Graph::ToAronType());
+ workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
+ navigation::human::arondto::Human::ToAronType());
+
+ // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+ // navigation::human::arondto::Human::ToAronType());
if (not properties.snapshotToLoad.empty())
{
@@ -432,7 +438,8 @@ namespace armarx::navigation
memory::Visu visu{arviz,
workingMemory().getCoreSegment(navigation::location::coreSegmentID),
workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
- workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName)};
+ workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+ workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
Properties::LocationGraph p;
@@ -462,6 +469,9 @@ namespace armarx::navigation
// Costmaps
visu.drawCostmaps(layers, p.visuCostmaps);
+ // Humans
+ visu.drawHumans(layers, p.visuHumans);
+
arviz.commit(layers);
metronome.waitForNextTick();
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index c67da7fc..e34aa21e 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -97,6 +97,7 @@ namespace armarx::navigation
bool visuLocations = true;
bool visuGraphEdges = true;
bool visuCostmaps = true;
+ bool visuHumans = true;
float visuFrequency = 2;
};
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 73df72de..f3ae9cbc 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -22,18 +22,24 @@
#include "Visu.h"
+#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
+#include "RobotAPI/components/ArViz/Client/Elements.h"
#include "RobotAPI/components/ArViz/Client/Layer.h"
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/human/aron/Human.aron.generated.h"
+#include "armarx/navigation/human/aron_conversions.h"
+#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/core/Graph.h>
#include <armarx/navigation/core/aron/Graph.aron.generated.h>
#include <armarx/navigation/core/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
namespace armarx::navigation::memory
@@ -42,11 +48,13 @@ namespace armarx::navigation::memory
Visu::Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment) :
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment) :
arviz(arviz),
locSegment(locSegment),
graphSegment(graphSegment),
costmapSegment(costmapSegment),
+ humanSegment(humanSegment),
visu(std::make_unique<graph::GraphVisu>())
{
}
@@ -165,6 +173,24 @@ namespace armarx::navigation::memory
layer.add(mesh);
}
+ void
+ visualize(const human::Humans& humans, viz::Layer& layer)
+ {
+ 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);
+ }
+ }
+
} // namespace
void
@@ -207,5 +233,45 @@ namespace armarx::navigation::memory
}
}
+ void
+ Visu::drawHumans(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ if (not enabled)
+ {
+ return;
+ }
+
+ std::map<std::string, navigation::human::Humans> namedProviderHumans;
+
+ humanSegment.doLocked(
+ [&]()
+ {
+ using namespace armem::server;
+
+ humanSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ entity.getLatestSnapshot().forEachInstance(
+ [&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
+ {
+ const auto dto =
+ navigation::human::arondto::Human::FromAron(instance.data());
+
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName]
+ .emplace_back(std::move(human));
+ });
+ });
+ });
+
+ for (const auto& [providerName, humans] : namedProviderHumans)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+ visualize(humans, layer);
+ }
+ }
+
} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
index cfd732ad..123a865d 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -28,6 +28,7 @@
#include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
#include "armarx/navigation/algorithms/Costmap.h"
@@ -45,13 +46,15 @@ namespace armarx::navigation::memory
Visu(viz::Client arviz,
const armem::server::wm::CoreSegment& locSegment,
const armem::server::wm::CoreSegment& graphSegment,
- const armem::server::wm::CoreSegment& costmapSegment);
+ const armem::server::wm::CoreSegment& costmapSegment,
+ const armem::server::wm::CoreSegment& humanSegment);
~Visu();
void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
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);
public:
@@ -60,6 +63,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;
std::unique_ptr<navigation::graph::GraphVisu> visu;
};
--
GitLab
From f3159e8c60629ed451c89fdde36ddde019393870 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:28:07 +0200
Subject: [PATCH 32/34] dynamic_scene_provider component: tested on ARMAR-6;
minor fixes
---
.../dynamic_scene_provider/CMakeLists.txt | 2 +-
.../dynamic_scene_provider/Component.cpp | 24 ++++++++++++++++---
.../dynamic_scene_provider/Component.h | 5 ++++
3 files changed, 27 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 39d89c1a..e061ea23 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -35,7 +35,7 @@ armarx_add_component(dynamic_scene_provider
## RobotAPICore
## RobotAPIInterfaces
## RobotAPIComponentPlugins # For ArViz and other plugins.
- DEPENDENCIES_PRIVATE
+ # DEPENDENCIES_PRIVATE
range-v3::range-v3
# DEPENDENCIES_LEGACY
## Add libraries that do not provide any targets but ${FOO_*} variables.
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index e8abd88f..4a1cd5e8 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -53,6 +53,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(virtualRobotReaderPlugin);
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
+ addPlugin(humanWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -86,6 +87,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
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.humanPoseProvider, "p.humanPoseProvider", "");
return def;
}
@@ -141,12 +144,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
task = new PeriodicTask<Component>(
this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+ task->start();
}
void
Component::onDisconnectComponent()
{
+ task->stop();
}
@@ -189,12 +194,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Human
//
- const armem::human::client::Reader::Query humanPoseQuery{.providerName = "", // all
+ ARMARX_INFO << "Querying humans";
+
+ const armem::human::client::Reader::Query humanPoseQuery{.providerName = properties.humanPoseProvider,
.timestamp = timestamp};
const armem::human::client::Reader::Result humanPoseResult =
humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success);
+ ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success) << humanPoseResult.errorMessage;
ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
@@ -202,6 +209,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
+ ARMARX_INFO << "Querying laser scanner features";
+
const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
@@ -219,6 +228,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Objects in the scene (both static and dynamic)
//
+ ARMARX_INFO << "Querying object poses";
+
const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
// remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
@@ -243,8 +254,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Costmaps
//
+ ARMARX_INFO << "Querying costmap";
+
const memory::client::costmap::Reader::Query costmapQuery{.providerName =
- "navigator", // TODO check
+ "distance_to_obstacle_costmap_provider", // TODO check
.name = "distance_to_obstacles",
.timestamp = timestamp};
@@ -319,8 +332,13 @@ namespace armarx::navigation::components::dynamic_scene_provider
// here ends: data fetching
+ ARMARX_INFO << "Running human tracker";
+
humanTracker.update(HumanTracker::Measurements{.detectionTime = timestamp,
.humanPoses = humanPoseResult.humanPoses});
+
+
+ humanWriterPlugin->get().store(humanTracker.getTrackedHumans(),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 171b8638..bcc3ad7d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -49,6 +49,7 @@
#include "armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h"
#include "armarx/navigation/components/dynamic_scene_provider/HumanTracker.h"
#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>
@@ -151,6 +152,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
float freespaceThreshold = 0.45F;
float occupiedThreshold = 0.55;
} occupancyGrid;
+
+ std::string humanPoseProvider = "OpenNIPointCloudProvider";
};
Properties properties;
/* Use a mutex if you access variables from different threads
@@ -199,6 +202,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::vision::occupancy_grid::client::Reader>*
occupancyGridReaderPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
HumanTracker humanTracker;
};
--
GitLab
From f854d1f4e51904d160eb677e3cc12af8465847fe Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:28:43 +0200
Subject: [PATCH 33/34] cmake: moved human lib up as other libs depend on it
---
source/armarx/navigation/CMakeLists.txt | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index ca819773..60936986 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -3,6 +3,7 @@ add_subdirectory(core)
add_subdirectory(util)
add_subdirectory(conversions)
add_subdirectory(algorithms)
+add_subdirectory(human)
add_subdirectory(dynamic_scene)
add_subdirectory(global_planning)
add_subdirectory(local_planning)
@@ -15,7 +16,6 @@ add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
add_subdirectory(platform_controller)
-add_subdirectory(human)
# Components.
add_subdirectory(components)
--
GitLab
From 16ccec9a3bb7ecd47294ac7b77c88c0c7bd594b8 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 17 Aug 2022 09:32:42 +0200
Subject: [PATCH 34/34] merge with origin/feature/human-tracker
---
.../dynamic_scene_provider/HumanTracker.cpp | 15 ++++++---------
.../dynamic_scene_provider/HumanTracker.h | 11 +++++++++++
2 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
index 7dd61dc6..e7771cb3 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.cpp
@@ -64,7 +64,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- if ((measurements.detectionTime - human.human.detectionTime) >= maxAge)
+ if ((measurements.detectionTime - human.human.detectionTime) >=
+ parameters.maxTrackingAge)
{
it = trackedHumans.erase(it);
}
@@ -153,12 +154,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
// associate leftover humans by their distances
const auto sortedDistances = getSortedDistances(trackedHumans, detectedHumans);
- //TODO max distance parameter
- float maxDistance = 600;
-
for (auto& posDistance : sortedDistances)
{
- if (posDistance.distance > maxDistance)
+ if (posDistance.distance > parameters.maxAssociationDistance)
{
break;
}
@@ -179,16 +177,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
trackedHuman->associated = true;
detectedHuman->associated = true;
- // TODO alpha parameter
- float a = 0.7;
-
float dt =
(detectedHuman->detectionTime - trackedHuman->human.detectionTime).toSecondsDouble();
Eigen::Vector2f ds =
(detectedHuman->pose.translation() - trackedHuman->human.pose.translation());
Eigen::Vector2f linVelocity = ds / dt;
- Eigen::Vector2f velocity = a * linVelocity + (1 - a) * trackedHuman->human.linearVelocity;
+ Eigen::Vector2f velocity =
+ parameters.velocityAlpha * linVelocity +
+ (1 - parameters.velocityAlpha) * trackedHuman->human.linearVelocity;
trackedHuman->human = {detectedHuman->pose, velocity, detectedHuman->detectionTime};
trackedHuman->trackingId = detectedHuman->trackingId;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
index e5197d82..9146a798 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/HumanTracker.h
@@ -37,6 +37,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
bool associated;
};
+ struct Parameters
+ {
+ // the duration after which tracked humans will be erased if no new measurement for this human is found
+ Duration maxTrackingAge = Duration::MilliSeconds(500);
+ // the maximum distance in millimeters of two human measurements to be associated with each other
+ float maxAssociationDistance = 600;
+ // alpha value from interval [0,1] to determine how much the new (and respectively the old) velocity should be weighted
+ float velocityAlpha = 0.7;
+ };
+
void update(const Measurements& measurements);
std::vector<human::Human> getTrackedHumans() const;
@@ -49,5 +59,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
std::vector<TrackedHuman> trackedHumans;
+ Parameters parameters;
};
} // namespace armarx::navigation::components::dynamic_scene_provider
--
GitLab