From bf348ee62cc0648ab22a7577bde4843b5029399a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 6 Dec 2022 17:09:55 +0100
Subject: [PATCH 01/55] Add logging time to DebugObserver
---
.../dynamic_scene_provider/CMakeLists.txt | 2 +-
.../dynamic_scene_provider/Component.cpp | 118 ++++++++----------
.../dynamic_scene_provider/Component.h | 5 +-
.../local_planning/TimedElasticBands.cpp | 2 -
4 files changed, 54 insertions(+), 73 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
index 52e4efdc..7cc6fb58 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
+++ b/source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt
@@ -15,7 +15,7 @@ armarx_add_component(dynamic_scene_provider
DEPENDENCIES
# ArmarXCore
ArmarXCore
- ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9ddc696a..c8c999dc 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -28,6 +28,7 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
+#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/time.h>
@@ -107,7 +108,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
- // setDebugObserverBatchModeEnabled(true);
+ setDebugObserverBatchModeEnabled(true);
}
@@ -192,36 +193,36 @@ namespace armarx::navigation::components::dynamic_scene_provider
void
Component::runPeriodically()
{
- TIMING_START(FULL_DYNAMIC_SCENE_PROVIDER);
+ const auto logDuration = [this](const std::string& name, const Duration& duration)
+ { setDebugObserverDatafield("timing." + name + " [ms]", duration.toMilliSeconds()); };
- // obtain data from perception
+ const auto makeSWlog = [&](const std::string& name)
+ { return [=](const Duration& duration) { logDuration(name, duration); }; };
- const DateTime timestamp = Clock::Now();
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.full"));
+ // obtain data from perception
+ const DateTime timestamp = Clock::Now();
//
// Robot
//
-
- TIMING_START(READ_ROBOT_FROM_MEMORY);
+ armarx::core::time::StopWatch robotSW;
ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
const core::Pose global_T_robot(robot->getGlobalPose());
ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
- TIMING_END_COMMENT_STREAM(
- READ_ROBOT_FROM_MEMORY, "Timer: reading robot from memory", ARMARX_VERBOSE);
-
+ const Duration robotDuration = robotSW.stop();
+ logDuration("dynamic_scene.read_robot", robotDuration);
//
// Human
//
-
- TIMING_START(READ_HUMANS_FROM_MEMORY);
+ armarx::core::time::StopWatch humanSW;
ARMARX_VERBOSE << "Querying humans";
-
const armem::human::client::Reader::Query humanPoseQuery{
.providerName = properties.humanPoseProvider,
.timestamp = timestamp,
@@ -234,16 +235,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
- TIMING_END_COMMENT_STREAM(
- READ_HUMANS_FROM_MEMORY, "Timer: reading humans from memory", ARMARX_VERBOSE);
-
+ const Duration humanDuration = humanSW.stop();
+ logDuration("dynamic_scene.read_human", humanDuration);
//
// Laser scanner features
//
-
/*
- TIMING_START(READ_LASERSCANNER_FROM_MEMORY);
+ armarx::core::time::StopWatch laserscanSW;
ARMARX_INFO << "Querying laser scanner features";
const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
@@ -253,14 +252,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
-
ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success);
+ armem::vision::laser_scanner_features::client::Reader::Result::Success)
+ << laserFeaturesResult.errorMessage;
- ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+ ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
- TIMING_END_COMMENT_STREAM(
- READ_LASERSCANNER_FROM_MEMORY, "Timer: reading laserscanner from memory", ARMARX_VERBOSE);
+ const Duration laserscanDuration = humanSW.stop();
+ logDuration("dynamic_scene.read_laserscanner", laserscanDuration);
*/
@@ -370,48 +369,40 @@ namespace armarx::navigation::components::dynamic_scene_provider
// arviz.commit({layer});
}
+ */
// here ends: data fetching
- */
-
// process data from perception and write information back to memory
//
// Human tracking
//
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
- TIMING_START(RUNNING_HUMAN_TRACKER_WITH_CAMERA);
-
- ARMARX_VERBOSE << "Running human tracker with camera measurements";
-
- humanTracker.update(human::HumanTracker::CameraMeasurement{
- .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
-
- TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_CAMERA,
- "Timer: running human tracker with camera",
- ARMARX_VERBOSE);
-
+ ARMARX_VERBOSE << "Running human tracker with camera measurements";
+ humanTracker.update(human::HumanTracker::CameraMeasurement{
+ .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+ }
/*
- ARMARX_INFO << "Running human tracker with lasersensor measurements";
-
- TIMING_START(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER);
-
- //TODO why is result a vector of LSFs and not a vector of LSF?
- std::vector<armem::vision::LaserScannerFeature> flattened;
- for (auto const& fs : laserFeaturesResult.features)
{
- flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
- }
- std::vector<armem::vision::LaserScannerFeature> clusters =
- humanTracker.update(human::HumanTracker::LaserMeasurement{.detectionTime = timestamp,
- .clusters = flattened});
+ armarx::core::time::ScopedStopWatch sw(
+ makeSWlog("dynamic_scene.human_tracker.laserscanner"));
- TIMING_END_COMMENT_STREAM(RUNNING_HUMAN_TRACKER_WITH_LASERSCANNER,
- "Timer: running human tracker with laserscanner",
- ARMARX_VERBOSE);
+ ARMARX_INFO << "Running human tracker with lasersensor measurements";
+ //TODO why is result a vector of LSFs and not a vector of LSF?
+ std::vector<armem::vision::LaserScannerFeature> flattened;
+ for (auto const& fs : laserFeaturesResult.features)
+ {
+ flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
+ }
+ std::vector<armem::vision::LaserScannerFeature> clusters =
+ humanTracker.update(human::HumanTracker::LaserMeasurement{
+ .detectionTime = timestamp, .clusters = flattened});
+ }
*/
ARMARX_VERBOSE << "Human tracking done";
@@ -420,27 +411,20 @@ namespace armarx::navigation::components::dynamic_scene_provider
//
// Write dynamic scene back to memory
//
-
- TIMING_START(WRITE_BACK_HUMANS);
-
- //TODO use clusters for obstacle creation
-
- std::vector<human::Human> humans = humanTracker.getTrackedHumans();
-
-
- if(not humans.empty())
{
- ARMARX_INFO << "Detected " << humans.size() << " humans";
- humanWriterPlugin->get().store(humans, getName(), timestamp);
- }
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.write_back_human"));
+ //TODO use clusters for obstacle creation
+ std::vector<human::Human> humans = humanTracker.getTrackedHumans();
- TIMING_END_COMMENT_STREAM(
- WRITE_BACK_HUMANS, "Timer: write humans to memory", ARMARX_VERBOSE);
-
+ if (not humans.empty())
+ {
+ ARMARX_INFO << "Detected " << humans.size() << " humans";
+ humanWriterPlugin->get().store(humans, getName(), timestamp);
+ }
+ }
- TIMING_END_COMMENT_STREAM(
- FULL_DYNAMIC_SCENE_PROVIDER, "Timer: complete dynamic scene provider", ARMARX_VERBOSE);
+ sendDebugObserverBatch();
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index aec652dd..f89b96eb 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -30,6 +30,7 @@
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
@@ -38,8 +39,6 @@
#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include "RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h"
@@ -59,7 +58,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
class Component :
virtual public armarx::Component,
virtual public armarx::navigation::components::dynamic_scene_provider::ComponentInterface,
- // , virtual public armarx::DebugObserverComponentPluginUser
+ virtual public armarx::DebugObserverComponentPluginUser,
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
virtual public armarx::ArVizComponentPluginUser,
virtual public armarx::armem::client::plugins::PluginUser,
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index c603e85a..926a9473 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -87,7 +87,6 @@ namespace armarx::navigation::local_planning
std::optional<LocalPlannerResult>
TimedElasticBands::plan(const core::GlobalTrajectory& goal)
{
- TIMING_START(TEB_PLAN);
const core::Pose currentPose{scene.robot->getGlobalPose()};
// prune global trajectory
@@ -155,7 +154,6 @@ namespace armarx::navigation::local_planning
arviz.value().commit(layer);
}
- TIMING_END_COMMENT_STREAM(TEB_PLAN, "Timer: teb planning", ARMARX_VERBOSE);
return {{.trajectory = best}};
}
--
GitLab
From 6c1ccd41ef203ab460ebc36a0acc8d3ca5cbf36f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 9 Dec 2022 17:12:46 +0100
Subject: [PATCH 02/55] Improve logger
---
.../dynamic_scene_provider/Component.cpp | 87 +++++++++++--------
1 file changed, 50 insertions(+), 37 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index c8c999dc..a75b60d2 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -207,59 +207,69 @@ namespace armarx::navigation::components::dynamic_scene_provider
//
// Robot
//
- armarx::core::time::StopWatch robotSW;
-
- ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
- const core::Pose global_T_robot(robot->getGlobalPose());
+ ARMARX_TRACE;
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_robot"));
- ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
+ ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+ const core::Pose global_T_robot(robot->getGlobalPose());
- const Duration robotDuration = robotSW.stop();
- logDuration("dynamic_scene.read_robot", robotDuration);
+ ARMARX_VERBOSE << "Robot position: " << global_T_robot.translation().head<2>();
+ }
//
// Human
//
- armarx::core::time::StopWatch humanSW;
+ ARMARX_TRACE;
+ const armem::human::client::Reader::Result humanPoseResult = [&]
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_human"));
- ARMARX_VERBOSE << "Querying humans";
- const armem::human::client::Reader::Query humanPoseQuery{
- .providerName = properties.humanPoseProvider,
- .timestamp = timestamp,
- .maxAge = Duration::MilliSeconds(500)};
+ ARMARX_VERBOSE << "Querying humans";
+ const armem::human::client::Reader::Query humanPoseQuery{
+ .providerName = properties.humanPoseProvider,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
- const armem::human::client::Reader::Result humanPoseResult =
- humanPoseReaderPlugin->get().query(humanPoseQuery);
- ARMARX_CHECK_NOT_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Error)
- << humanPoseResult.errorMessage;
+ const armem::human::client::Reader::Result humanPoseResult =
+ humanPoseReaderPlugin->get().query(humanPoseQuery);
+ ARMARX_CHECK_NOT_EQUAL(humanPoseResult.status,
+ armem::human::client::Reader::Result::Error)
+ << humanPoseResult.errorMessage;
- ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
+ ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
- const Duration humanDuration = humanSW.stop();
- logDuration("dynamic_scene.read_human", humanDuration);
+ return humanPoseResult;
+ }();
//
// Laser scanner features
//
/*
- armarx::core::time::StopWatch laserscanSW;
-
- ARMARX_INFO << "Querying laser scanner features";
- const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
- .providerName = properties.laserScannerFeatures.providerName,
- .name = properties.laserScannerFeatures.name,
- .timestamp = timestamp};
-
+ ARMARX_TRACE;
const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
- laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
- ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success)
- << laserFeaturesResult.errorMessage;
-
- ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
-
- const Duration laserscanDuration = humanSW.stop();
- logDuration("dynamic_scene.read_laserscanner", laserscanDuration);
+ [&]
+ {
+ armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
+
+ ARMARX_INFO << "Querying laser scanner features";
+ const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
+ .providerName = properties.laserScannerFeatures.providerName,
+ .name = properties.laserScannerFeatures.name,
+ .timestamp = timestamp};
+
+ const armem::vision::laser_scanner_features::client::Reader::Result
+ laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ ARMARX_CHECK_EQUAL(
+ laserFeaturesResult.status,
+ armem::vision::laser_scanner_features::client::Reader::Result::Success)
+ << laserFeaturesResult.errorMessage;
+
+ ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
+
+ return laserFeaturesResult;
+ }();
*/
@@ -379,6 +389,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
//
// Human tracking
//
+ ARMARX_TRACE;
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
@@ -388,6 +399,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
/*
+ ARMARX_TRACE;
{
armarx::core::time::ScopedStopWatch sw(
makeSWlog("dynamic_scene.human_tracker.laserscanner"));
@@ -411,6 +423,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
//
// Write dynamic scene back to memory
//
+ ARMARX_TRACE;
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.write_back_human"));
--
GitLab
From 9b93e153b025ed4f3661e33b5adb6182e8dfe1d5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 9 Dec 2022 17:23:02 +0100
Subject: [PATCH 03/55] Fix visualization of humans when no humans in scene
---
.../components/navigation_memory/Visu.cpp | 22 +++++++++++++------
1 file changed, 15 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 882deb59..8ce7e02d 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -278,6 +278,8 @@ namespace armarx::navigation::memory
}
std::map<std::string, navigation::human::Humans> namedProviderHumans;
+ const DateTime timestamp = Clock::Now();
+ const Duration maxAge = Duration::MilliSeconds(500);
humanSegment.doLocked(
[&]()
@@ -289,17 +291,23 @@ namespace armarx::navigation::memory
{
namedProviderHumans[entity.id().providerSegmentName];
entity.getLatestSnapshot().forEachInstance(
- [&namedProviderHumans](
+ [&namedProviderHumans, ×tamp, &maxAge](
const armarx::armem::wm::EntityInstance& instance)
{
- const auto dto =
- navigation::human::arondto::Human::FromAron(instance.data());
+ const Duration dtToNow =
+ timestamp - instance.metadata().timeCreated;
- navigation::human::Human human;
- fromAron(dto, human);
+ if (dtToNow < maxAge and dtToNow.isPositive())
+ {
+ const auto dto = navigation::human::arondto::Human::FromAron(
+ instance.data());
- namedProviderHumans[instance.id().providerSegmentName].emplace_back(
- std::move(human));
+ navigation::human::Human human;
+ fromAron(dto, human);
+
+ namedProviderHumans[instance.id().providerSegmentName]
+ .emplace_back(std::move(human));
+ };
});
});
});
--
GitLab
From 8cf0361dfe5ba6d00b83b79f0a2ad379647839d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 15 Feb 2023 16:04:42 +0100
Subject: [PATCH 04/55] Uncomment laser scanner features in dynamic scene
provider
---
.../dynamic_scene_provider/Component.cpp | 28 ++++++++-----------
1 file changed, 12 insertions(+), 16 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index a75b60d2..9fffd44f 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -221,7 +221,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Human
//
ARMARX_TRACE;
- const armem::human::client::Reader::Result humanPoseResult = [&]
+ const std::vector<armem::human::HumanPose> humanPoses = [&]
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_human"));
@@ -239,16 +239,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE << humanPoseResult.humanPoses.size() << " humans in the scene.";
- return humanPoseResult;
+ return humanPoseResult.humanPoses;
}();
//
// Laser scanner features
//
- /*
ARMARX_TRACE;
- const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
- [&]
+ const std::vector<armem::vision::LaserScannerFeatures> laserFeatures = [&]
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
@@ -268,9 +266,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
- return laserFeaturesResult;
+ return laserFeaturesResult.features;
}();
- */
/* we don't need this at the moment
@@ -394,28 +391,27 @@ namespace armarx::navigation::components::dynamic_scene_provider
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
ARMARX_VERBOSE << "Running human tracker with camera measurements";
- humanTracker.update(human::HumanTracker::CameraMeasurement{
- .detectionTime = timestamp, .humanPoses = humanPoseResult.humanPoses});
+ humanTracker.update(human::HumanTracker::CameraMeasurement{.detectionTime = timestamp,
+ .humanPoses = humanPoses});
}
- /*
ARMARX_TRACE;
+ const std::vector<armem::vision::LaserScannerFeature> unusedFeatures = [&]
{
armarx::core::time::ScopedStopWatch sw(
makeSWlog("dynamic_scene.human_tracker.laserscanner"));
ARMARX_INFO << "Running human tracker with lasersensor measurements";
+
//TODO why is result a vector of LSFs and not a vector of LSF?
std::vector<armem::vision::LaserScannerFeature> flattened;
- for (auto const& fs : laserFeaturesResult.features)
+ for (auto const& fs : laserFeatures)
{
flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
}
- std::vector<armem::vision::LaserScannerFeature> clusters =
- humanTracker.update(human::HumanTracker::LaserMeasurement{
- .detectionTime = timestamp, .clusters = flattened});
- }
- */
+ return humanTracker.update(human::HumanTracker::LaserMeasurement{
+ .detectionTime = timestamp, .clusters = flattened});
+ }();
ARMARX_VERBOSE << "Human tracking done";
--
GitLab
From b17b0109185aee5fcace258e8fca3834b1611cd7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 21 Feb 2023 17:43:05 +0100
Subject: [PATCH 05/55] Disable broken test
This test doesn't compile, so comment it out.
---
source/armarx/navigation/server/CMakeLists.txt | 18 +++++++++---------
1 file changed, 9 insertions(+), 9 deletions(-)
diff --git a/source/armarx/navigation/server/CMakeLists.txt b/source/armarx/navigation/server/CMakeLists.txt
index a7dad944..460321db 100644
--- a/source/armarx/navigation/server/CMakeLists.txt
+++ b/source/armarx/navigation/server/CMakeLists.txt
@@ -62,12 +62,12 @@ armarx_add_library(server
range-v3::range-v3
)
-armarx_add_test(server_test
- TEST_FILES
- test/serverTest.cpp
- DEPENDENCIES
- ArmarXCore
- armarx_navigation::client
- armarx_navigation::factories
- armarx_navigation::server
-)
+#armarx_add_test(server_test
+# TEST_FILES
+# test/serverTest.cpp
+# DEPENDENCIES
+# ArmarXCore
+# armarx_navigation::client
+# armarx_navigation::factories
+# armarx_navigation::server
+#)
--
GitLab
From 0ee1a842e10cd1b1f83738dd778df2662d3c5365 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 21 Feb 2023 19:03:12 +0100
Subject: [PATCH 06/55] Make robot name a required property
---
.../navigation/components/dynamic_scene_provider/Component.cpp | 2 +-
.../navigation/components/dynamic_scene_provider/Component.h | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index c7505148..de561497 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -86,7 +86,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
"");
def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
- def->optional(properties.robot.name, "p.robot.name", "");
+ def->required(properties.robot.name, "p.robot.name", "");
def->optional(properties.occupancyGrid.providerName, "p.occupancyGrid.providerName", "");
def->optional(properties.occupancyGrid.name, "p.occupancyGrid.name", "");
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index e2046295..05e9214c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -140,7 +140,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct
{
- std::string name = "Armar6";
+ std::string name;
} robot;
struct
--
GitLab
From 85af63ab3b2730087fae561fc89417cbe17ed452 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Tue, 21 Feb 2023 19:03:34 +0100
Subject: [PATCH 07/55] Add property to disable LaserScannerFeatures for
dynamic scene provider
---
.../dynamic_scene_provider/Component.cpp | 44 ++++++++++++-------
.../dynamic_scene_provider/Component.h | 3 +-
2 files changed, 30 insertions(+), 17 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index de561497..ff304085 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -81,6 +81,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(
properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+ def->optional(properties.laserScannerFeatures.enabled,
+ "p.laserScannerFeatures.enabled",
+ "Whether laser scanner features are used.");
def->optional(properties.laserScannerFeatures.providerName,
"p.laserScannerFeatures.providerName",
"");
@@ -248,9 +251,14 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_TRACE;
const std::vector<armem::vision::LaserScannerFeatures> laserFeatures = [&]
{
+ if (!properties.laserScannerFeatures.enabled)
+ {
+ return std::vector<armem::vision::LaserScannerFeatures>{};
+ }
+
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
- ARMARX_INFO << "Querying laser scanner features";
+ ARMARX_VERBOSE << "Querying laser scanner features";
const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
@@ -395,23 +403,27 @@ namespace armarx::navigation::components::dynamic_scene_provider
.humanPoses = humanPoses});
}
+
ARMARX_TRACE;
- const std::vector<armem::vision::LaserScannerFeature> unusedFeatures = [&]
+ if (properties.laserScannerFeatures.enabled)
{
- armarx::core::time::ScopedStopWatch sw(
- makeSWlog("dynamic_scene.human_tracker.laserscanner"));
-
- ARMARX_INFO << "Running human tracker with lasersensor measurements";
-
- //TODO why is result a vector of LSFs and not a vector of LSF?
- std::vector<armem::vision::LaserScannerFeature> flattened;
- for (auto const& fs : laserFeatures)
+ const std::vector<armem::vision::LaserScannerFeature> unusedFeatures = [&]
{
- flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
- }
- return humanTracker.update(human::HumanTracker::LaserMeasurement{
- .detectionTime = timestamp, .clusters = flattened});
- }();
+ armarx::core::time::ScopedStopWatch sw(
+ makeSWlog("dynamic_scene.human_tracker.laserscanner"));
+
+ ARMARX_VERBOSE << "Running human tracker with lasersensor measurements";
+
+ //TODO why is result a vector of LSFs and not a vector of LSF?
+ std::vector<armem::vision::LaserScannerFeature> flattened;
+ for (auto const& fs : laserFeatures)
+ {
+ flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
+ }
+ return humanTracker.update(human::HumanTracker::LaserMeasurement{
+ .detectionTime = timestamp, .clusters = flattened});
+ }();
+ }
ARMARX_VERBOSE << "Human tracking done";
@@ -428,7 +440,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
if (not humans.empty())
{
- ARMARX_INFO << "Detected " << humans.size() << " humans";
+ ARMARX_VERBOSE << "Detected " << humans.size() << " humans";
humanWriterPlugin->get().store(humans, getName(), timestamp);
}
}
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 05e9214c..0ac046c2 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -46,10 +46,10 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
+#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
#include <armarx/navigation/human/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>
namespace armarx::navigation::components::dynamic_scene_provider
@@ -133,6 +133,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
struct
{
+ bool enabled = true;
std::string providerName = "LaserScannerFeatureExtraction";
std::string name = ""; // all
} laserScannerFeatures;
--
GitLab
From 254590502d48172d5204544a22c3bd2247321918 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 27 Feb 2023 20:29:21 +0100
Subject: [PATCH 08/55] Add polygon obstacle
---
.../local_planning/TebObstacleManager.cpp | 38 ++++++++++---------
.../local_planning/TebObstacleManager.h | 3 ++
2 files changed, 24 insertions(+), 17 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index e16fffd3..89a1b2e4 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -24,37 +24,41 @@ namespace armarx::navigation::local_planning
}
void
- TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
+ TebObstacleManager::addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer)
{
auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
- const Eigen::Vector2d min = conv::toRos(bbox.getMin());
- const Eigen::Vector2d max = conv::toRos(bbox.getMax());
-
- obst->pushBackVertex(min);
- obst->pushBackVertex(min.x(), max.y());
- obst->pushBackVertex(max);
- obst->pushBackVertex(max.x(), min.y());
-
+ for (const auto& vertex : polygon)
+ {
+ obst->pushBackVertex(conv::toRos2D(vertex));
+ }
obst->finalizePolygon();
- obst->setUseForOptimization(true);
container.push_back(obst);
// visualize bounding box if layer is available
if (visLayer != nullptr)
{
- const Eigen::Vector3f min3d = conv::fromRos(min);
- const Eigen::Vector3f max3d = conv::fromRos(max);
-
visLayer->add(viz::Polygon("polygon_" + std::to_string(visualizationIndex++))
- .addPoint(min3d)
- .addPoint(Eigen::Vector3f(min3d.x(), max3d.y(), 0))
- .addPoint(max3d)
- .addPoint(Eigen::Vector3f(max3d.x(), min3d.y(), 0))
+ .points(conv::to3D(polygon))
.color(simox::Color::gray()));
}
}
+ void
+ TebObstacleManager::addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer)
+ {
+ const Eigen::Vector2f min = conv::to2D(bbox.getMin());
+ const Eigen::Vector2f max = conv::to2D(bbox.getMax());
+
+ Polygon poly{};
+ poly.push_back(min);
+ poly.emplace_back(min.x(), max.y());
+ poly.push_back(max);
+ poly.emplace_back(max.x(), min.y());
+
+ addPolygonObstacle(poly, visLayer);
+ }
+
void
TebObstacleManager::addHumanObstacle(const human::Human& human, viz::Layer* visLayer)
{
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b8859c4b..df14dc4c 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -35,6 +35,8 @@ namespace armarx::navigation::local_planning
class TebObstacleManager
{
public:
+ using Polygon = std::vector<Eigen::Vector2f>;
+
TebObstacleManager(teb_local_planner::ObstContainer& container) : container(container)
{
}
@@ -43,6 +45,7 @@ namespace armarx::navigation::local_planning
size_t size();
+ void addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer = nullptr);
void addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer = nullptr);
void addHumanObstacle(const human::Human& human, viz::Layer* visLayer = nullptr);
--
GitLab
From f773b9f4c4cace792122d3299374dbf3d01155fc Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 1 Mar 2023 20:56:42 +0100
Subject: [PATCH 09/55] Improve logging and error handling of teb
---
.../navigation/local_planning/TimedElasticBands.cpp | 9 ++++++---
1 file changed, 6 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 82b3a3ab..8b3f9ed7 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -108,11 +108,14 @@ namespace armarx::navigation::local_planning
try
{
- hcp_->plan(start, end, &velocity_start, !planToDest);
+ if (!hcp_->plan(start, end, &velocity_start, !planToDest))
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
+ }
}
- catch (std::exception& e)
+ catch (...)
{
- ARMARX_ERROR << "Caugth exception while planning: " << e.what();
+ ARMARX_ERROR << "Caugth exception while planning: " << GetHandledExceptionString();
return std::nullopt;
}
--
GitLab
From 6e1ec4bd68528ccf10ec85a1021f721bfb5cfcd0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 6 Mar 2023 16:43:18 +0100
Subject: [PATCH 10/55] Setup new component for laser scanner feature
extraction
---
.../navigation/components/CMakeLists.txt | 2 +
.../CMakeLists.txt | 51 ++++
.../Component.cpp | 220 ++++++++++++++++++
.../Component.h | 137 +++++++++++
.../ComponentInterface.ice | 35 +++
5 files changed, 445 insertions(+)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index efa6ec80..651c7ef6 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -21,3 +21,5 @@ add_subdirectory(dynamic_scene_provider)
add_subdirectory(human_simulator)
add_subdirectory(navigation_skill_provider)
+
+add_subdirectory(laser_scanner_feature_extraction)
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
new file mode 100644
index 00000000..e2115cb0
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -0,0 +1,51 @@
+armarx_add_component(laser_scanner_feature_extraction
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ SOURCES
+ LaserScannerFeatureExtraction.cpp
+ FeatureExtractor.cpp
+ ArVizDrawer.cpp
+ ScanClustering.cpp
+ ChainApproximation.cpp
+ #conversions
+ conversions/eigen.cpp
+ conversions/pcl.cpp
+ EnclosingEllipsoid.cpp
+ Path.cpp
+ HEADERS
+ Component.h
+ LaserScannerFeatureExtraction.h
+ FeatureExtractor.h
+ ArVizDrawer.h
+ ScanClustering.h
+ ChainApproximation.h
+ EnclosingEllipsoid.h
+ Path.h
+ #conversions
+ conversions/eigen.h
+ conversions/pcl_eigen.h
+ conversions/opencv_eigen.h
+ conversions/opencv_pcl.h
+ conversions/pcl.h
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ ## RobotAPIComponentPlugins # For ArViz and other plugins.
+
+
+ # armarx_navigation
+ # armarx_navigation::my_library
+
+ # DEPENDENCIES_LEGACY
+ ## Add libraries that do not provide any targets but ${FOO_*} variables.
+ # FOO
+ # If you need a separate shared component library you can enable it with the following flag.
+ # SHARED_COMPONENT_LIBRARY
+)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
new file mode 100644
index 00000000..d94c3ade
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -0,0 +1,220 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::laser_scanner_feature_extraction
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#include "Component.h"
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ const std::string
+ Component::defaultName = "LaserScannerFeatureExtraction";
+
+
+ armarx::PropertyDefinitionsPtr
+ Component::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+
+ // Add a required property. (The component won't start without a value being set.)
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optional property.
+ // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+ return def;
+ }
+
+
+ void
+ Component::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requires the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
+
+ /* (Requires the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ // Draw boxes in ArViz.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ drawBoxes(properties, arviz);
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void
+ Component::onDisconnectComponent()
+ {
+ }
+
+
+ void
+ Component::onExitComponent()
+ {
+ }
+
+
+ std::string
+ Component::getDefaultName() const
+ {
+ return Component::defaultName;
+ }
+
+
+ std::string
+ Component::GetDefaultName()
+ {
+ return Component::defaultName;
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void
+ Component::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void
+ Component::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in separate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ void
+ Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ {
+ // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+ // See the ArVizExample in RobotAPI for more examples.
+
+ viz::Layer layer = arviz.layer(p.boxLayerName);
+ for (int i = 0; i < p.numBoxes; ++i)
+ {
+ layer.add(viz::Box("box_" + std::to_string(i))
+ .position(Eigen::Vector3f(i * 100, 0, 0))
+ .size(20).color(simox::Color::blue()));
+ }
+ arviz.commit(layer);
+ }
+ */
+
+
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
new file mode 100644
index 00000000..43520364
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -0,0 +1,137 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package navigation::ArmarXObjects::laser_scanner_feature_extraction
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+// #include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ class Component :
+ virtual public armarx::Component,
+ virtual public armarx::navigation::components::laser_scanner_feature_extraction::ComponentInterface
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ static const std::string defaultName;
+
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string boxLayerName = "boxes";
+ int numBoxes = 10;
+ };
+ Properties properties;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice b/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
new file mode 100644
index 00000000..2c380438
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
@@ -0,0 +1,35 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * package navigation::LaserScannerFeatureExtraction
+ * author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * date 2023
+ * copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armarx { module navigation { module components { module laser_scanner_feature_extraction
+{
+
+ interface ComponentInterface
+ {
+ // Define your interface here.
+ };
+
+};};};};
--
GitLab
From 388d49d03df0ae4032e15de114c9ad75bd6c95ac Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 6 Mar 2023 16:46:38 +0100
Subject: [PATCH 11/55] Copy files from old component
---
.../ArVizDrawer.cpp | 226 +++++++
.../ArVizDrawer.h | 109 ++++
.../CMakeLists.txt | 19 +-
.../ChainApproximation.cpp | 214 +++++++
.../ChainApproximation.h | 97 +++
.../Component.cpp | 566 ++++++++++++++----
.../Component.h | 171 ++++--
.../EnclosingEllipsoid.cpp | 124 ++++
.../EnclosingEllipsoid.h | 70 +++
.../FeatureExtractor.cpp | 239 ++++++++
.../FeatureExtractor.h | 91 +++
.../laser_scanner_feature_extraction/Path.cpp | 24 +
.../laser_scanner_feature_extraction/Path.h | 19 +
.../ScanClustering.cpp | 71 +++
.../ScanClustering.h | 71 +++
.../conversions/eigen.cpp | 23 +
.../conversions/eigen.h | 53 ++
.../conversions/opencv.h | 47 ++
.../conversions/opencv_eigen.h | 52 ++
.../conversions/opencv_pcl.h | 57 ++
.../conversions/pcl.cpp | 27 +
.../conversions/pcl.h | 39 ++
.../conversions/pcl_eigen.h | 92 +++
.../geometry.h | 57 ++
.../test/CMakeLists.txt | 5 +
.../LaserScannerFeatureExtractionTest.cpp | 65 ++
26 files changed, 2453 insertions(+), 175 deletions(-)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
new file mode 100644
index 00000000..fe927c0e
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -0,0 +1,226 @@
+#include "ArVizDrawer.h"
+
+#include <iterator>
+#include <string>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/impl/point_types.hpp>
+#include <pcl/point_cloud.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include "RobotAPI/components/ArViz/Client/Client.h"
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/Line.h"
+#include "RobotAPI/components/ArViz/Client/elements/Path.h"
+#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
+#include "RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h"
+
+#include "RobotComponents/libraries/cartographer/types.h"
+#include "RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h"
+
+#include "FeatureExtractor.h"
+#include "conversions/eigen.h"
+#include "conversions/pcl.h"
+#include "conversions/pcl_eigen.h"
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ void ArVizDrawer::draw(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ // drawCircles(features, frame, globalSensorPose);
+ drawConvexHulls(features, frame, globalSensorPose);
+ drawEllipsoids(features, frame, globalSensorPose);
+ drawChains(features, frame, globalSensorPose);
+ }
+
+ void ArVizDrawer::draw(const cartographer::LaserScannerMessage& msg,
+ const Eigen::Isometry3f& globalSensorPose, const simox::Color& color)
+ {
+ auto layer = arviz.layer("points_" + msg.frame);
+
+ const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.scan));
+
+ layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
+ .pointCloud(pointCloud, viz::Color(color))
+ .pointSizeInPixels(5)
+ .pose(globalSensorPose));
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::draw(const std::string& layerName,
+ const Circle& circle,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer(layerName);
+
+ drawCircle(layer, circle, robotGlobalPose, color);
+ arviz.commit(layer);
+ }
+
+ void
+ ArVizDrawer::drawCircle(viz::Layer& layer,
+ const Circle& circle,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+
+ const Eigen::Vector3f position =
+ globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), -1.F);
+
+ layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
+ .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
+ .position(position)
+ .color(simox::Color::red(200, 100)));
+ }
+
+ void
+ ArVizDrawer::drawCircles(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer("circles_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features& f)
+ {
+ if (not f.circle)
+ {
+ return;
+ }
+ drawCircle(layer, *f.circle, globalSensorPose, color);
+ });
+
+ arviz.commit(layer);
+ }
+
+ void ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("convex_hulls_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features & f)
+ {
+ if (not f.convexHull)
+ {
+ return;
+ }
+ drawConvexHull(layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
+ });
+
+ arviz.commit(layer);
+ }
+
+ void ArVizDrawer::draw(const std::string& layerName, const VirtualRobot::MathTools::ConvexHull2D& robotHull,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer(layerName);
+
+ drawConvexHull(layer, robotHull, robotGlobalPose, color);
+ arviz.commit(layer);
+ }
+
+ void ArVizDrawer::drawConvexHull(viz::Layer& layer,
+ const VirtualRobot::MathTools::ConvexHull2D& hull,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+ const auto points = conversions::to3D(hull.vertices);
+
+ layer.add(viz::Polygon("convex_hull_" + std::to_string(layer.size()))
+ .points(points)
+ .color(color)
+ .pose(globalSensorPose));
+ }
+
+ void ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("ellipsoids_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features & f)
+ {
+ if (not f.ellipsoid)
+ {
+ return;
+ }
+ drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
+ });
+
+ arviz.commit(layer);
+ }
+
+ void ArVizDrawer::drawEllipsoid(viz::Layer& layer,
+ const Ellipsoid& ellipsoid,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+
+ const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
+
+ layer.add(viz::Ellipsoid("ellipsoid_" + std::to_string(layer.size()))
+ .axisLengths(conversions::to3D(ellipsoid.radii))
+ .pose(pose)
+ .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
+ }
+
+ void ArVizDrawer::drawChains(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ auto layer = arviz.layer("chains_" + frame);
+
+ std::for_each(features.begin(),
+ features.end(),
+ [&](const Features & f)
+ {
+ if (not f.chain)
+ {
+ return;
+ }
+ drawChain(layer, *f.chain, globalSensorPose);
+
+ // const auto ellipsoids = f.linesAsEllipsoids(50);
+ // for (const auto& ellipsoid : ellipsoids)
+ // {
+ // drawEllipsoid(layer, ellipsoid, globalSensorPose);
+ // }
+ });
+
+ arviz.commit(layer);
+ }
+
+ void ArVizDrawer::drawChain(viz::Layer& layer,
+ const Points& chain,
+ const Eigen::Isometry3f& globalSensorPose)
+ {
+ if (chain.size() < 2)
+ {
+ return;
+ }
+
+ const auto cloud = conversions::to3D(chain);
+
+ layer.add(viz::Path("chain_" + std::to_string(layer.size()))
+ .points(cloud)
+ .width(7.F)
+ .color(viz::Color::blue())
+ .pose(globalSensorPose));
+ }
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
new file mode 100644
index 00000000..59045681
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -0,0 +1,109 @@
+/*
+ * 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 <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
+
+#include "RobotComponents/components/LaserScannerFeatureExtraction/FeatureExtractor.h"
+
+namespace armarx::cartographer
+{
+ struct LaserScannerMessage;
+}
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ struct LineSegment2Df;
+ // struct Circle;
+ // struct Ellipsoid;
+
+ class ArVizDrawer
+ {
+ public:
+ using Points = std::vector<Eigen::Vector2f>;
+
+ ArVizDrawer(armarx::viz::Client& arviz) : arviz(arviz)
+ {
+ }
+
+
+ void draw(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void draw(const cartographer::LaserScannerMessage& msg,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void draw(const std::string& layerName,
+ const Circle& circle,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color);
+
+ void draw(const std::string& layerName,
+ const VirtualRobot::MathTools::ConvexHull2D& robotHull,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color = simox::Color::red(100, 80));
+
+ private:
+ void drawCircles(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+ void drawCircle(viz::Layer& layer,
+ const Circle& circle,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void drawConvexHulls(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+ void drawConvexHull(viz::Layer& layer,
+ const VirtualRobot::MathTools::ConvexHull2D& hull,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color);
+
+ void drawEllipsoids(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void drawEllipsoid(viz::Layer& layer,
+ const Ellipsoid& ellipsoid,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ void drawChains(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose);
+ void drawChain(viz::Layer& layer,
+ const Points& chain,
+ const Eigen::Isometry3f& globalSensorPose);
+
+ armarx::viz::Client arviz;
+ };
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index e2115cb0..8e5de1c1 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -3,7 +3,7 @@ armarx_add_component(laser_scanner_feature_extraction
ArmarXCoreInterfaces
# RobotAPIInterfaces
SOURCES
- LaserScannerFeatureExtraction.cpp
+ Component.cpp
FeatureExtractor.cpp
ArVizDrawer.cpp
ScanClustering.cpp
@@ -15,7 +15,6 @@ armarx_add_component(laser_scanner_feature_extraction
Path.cpp
HEADERS
Component.h
- LaserScannerFeatureExtraction.h
FeatureExtractor.h
ArVizDrawer.h
ScanClustering.h
@@ -31,21 +30,13 @@ armarx_add_component(laser_scanner_feature_extraction
DEPENDENCIES
# ArmarXCore
ArmarXCore
- ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
## RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
-
-
- # armarx_navigation
- # armarx_navigation::my_library
-
- # DEPENDENCIES_LEGACY
- ## Add libraries that do not provide any targets but ${FOO_*} variables.
- # FOO
- # If you need a separate shared component library you can enable it with the following flag.
- # SHARED_COMPONENT_LIBRARY
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+ armem_vision
+ RobotComponents::Cartographer
)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp
new file mode 100644
index 00000000..905cf765
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.cpp
@@ -0,0 +1,214 @@
+#include "ChainApproximation.h"
+
+#include <iterator>
+#include <numeric>
+
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+namespace armarx
+{
+
+ ChainApproximation::ChainApproximation(const Points& points, const Params& params) :
+ points(points), params(params)
+ {
+ // fill indices
+ indices.resize(points.size());
+ std::iota(indices.begin(), indices.end(), 0);
+ }
+
+ ChainApproximation::ApproximationResult ChainApproximation::approximate()
+ {
+
+ int iterations = 0;
+
+ const auto maxIterConditionReached = [&]()
+ {
+ // inactive?
+ if (params.maxIterations <= 0)
+ {
+ return false;
+ }
+
+ return iterations >= params.maxIterations;
+ };
+
+ while (true)
+ {
+ if (maxIterConditionReached())
+ {
+ return ApproximationResult
+ {
+ .condition = ApproximationResult::TerminationCondition::IterationLimit,
+ .iterations = iterations};
+ }
+
+ if (not approximateStep())
+ {
+ return ApproximationResult
+ {
+ .condition = ApproximationResult::TerminationCondition::Converged,
+ .iterations = iterations,
+ .reductionFactor = 1.F - static_cast<float>(indices.size()) /
+ static_cast<float>(points.size())};
+ }
+
+ iterations++;
+ }
+ }
+
+ ChainApproximation::Triplets ChainApproximation::getTriplets() const
+ {
+ const int nIndices = static_cast<int>(indices.size());
+
+ if (nIndices < 3)
+ {
+ return {};
+ }
+
+ Triplets triplets;
+ triplets.reserve(indices.size());
+
+ // Here, we iterate over all elements under consideration.
+ // The aim is to create a view - a sliding window - over the
+ // indices. i will always point to the centered element.
+
+ // the first element
+ triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+ // intermediate elements
+ for (int i = 1; i < (nIndices - 1); i++)
+ {
+ triplets.emplace_back(indices.at(i - 1), indices.at(i), indices.at(i + 1));
+ }
+
+ // the last element
+ triplets.emplace_back(indices.back(), indices.front(), indices.at(1));
+
+ return triplets;
+ }
+
+ std::vector<float>
+ ChainApproximation::computeDistances(const ChainApproximation::Triplets& triplets)
+ {
+ std::vector<float> distances;
+ distances.reserve(triplets.size());
+
+ std::transform(triplets.begin(),
+ triplets.end(),
+ std::back_inserter(distances),
+ [&](const auto & triplet)
+ {
+ return computeDistance(triplet);
+ });
+
+ return distances;
+ }
+ float ChainApproximation::computeDistance(const ChainApproximation::Triplet& triplet) const
+ {
+ using Line = Eigen::ParametrizedLine<float, 2>;
+
+ const Eigen::Vector2f& ptBefore = points.at(triplet.a);
+ const Eigen::Vector2f& ptPivot = points.at(triplet.b);
+ const Eigen::Vector2f& ptAfter = points.at(triplet.c);
+
+ const auto line = Line::Through(ptBefore, ptAfter);
+ return line.distance(ptPivot);
+ }
+
+ bool ChainApproximation::approximateStep()
+ {
+ const size_t nIndices = indices.size();
+ if (nIndices <= 3)
+ {
+ return false;
+ }
+
+ const Triplets triplets = getTriplets();
+ const std::vector<float> distances = computeDistances(triplets);
+
+ ARMARX_CHECK_EQUAL(triplets.size(), distances.size());
+ const int n = static_cast<int>(triplets.size());
+
+ std::vector<int> indicesToBeRemoved;
+
+ // TODO(fabian.reister): consider boundaries
+ for (int i = 1; i < n - 1; i++)
+ {
+ const auto& distance = distances.at(i);
+
+ // check distance criterion (necessary conditio)
+ if (distance >= params.distanceTh)
+ {
+ continue;
+ }
+
+ // better remove this element than those left and right (sufficient condition)
+ if (distance < std::min(distances.at(i - 1), distances.at(i + 1)))
+ {
+ indicesToBeRemoved.emplace_back(triplets.at(i).b);
+ }
+ }
+
+ // termination condition
+ if (indicesToBeRemoved.empty())
+ {
+ return false;
+ }
+
+ const auto isMatch = [&](const int& idx) -> bool
+ {
+ return std::find(indicesToBeRemoved.begin(), indicesToBeRemoved.end(), idx) !=
+ indicesToBeRemoved.end();
+ };
+
+ indices.erase(std::remove_if(indices.begin(), indices.end(), isMatch), indices.end());
+
+ return true;
+ }
+ ChainApproximation::Points ChainApproximation::approximatedChain() const
+ {
+ Points extractedPoints;
+ extractedPoints.reserve(indices.size());
+
+ std::transform(indices.begin(),
+ indices.end(),
+ std::back_inserter(extractedPoints),
+ [&](const auto & idx)
+ {
+ return points.at(idx);
+ });
+
+ return extractedPoints;
+ }
+
+ std::ostream& detail::operator<<(std::ostream& str, const ApproximationResult& res)
+ {
+ using TerminationCondition = ApproximationResult::TerminationCondition;
+
+ const std::string condStr = [&res]() -> std::string
+ {
+ std::string repr;
+
+ switch (res.condition)
+ {
+ case TerminationCondition::Converged:
+ repr = "Converged";
+ break;
+ case TerminationCondition::IterationLimit:
+ repr = "IterationLimit";
+ break;
+ }
+ return repr;
+ }();
+
+ str << "ApproximationResult: ["
+ << "condition: " << condStr << " | "
+ << "iterations: " << res.iterations << " | "
+ << "reduction: " << res.reductionFactor * 100 << "%]";
+
+ return str;
+ }
+} // namespace armarx
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h
new file mode 100644
index 00000000..a7b5e6cd
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.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 <Eigen/Core>
+
+namespace armarx
+{
+
+ namespace detail
+ {
+ struct ChainApproximationParams
+ {
+ float distanceTh; // [mm]
+
+ int maxIterations{-1};
+ };
+
+
+ struct ApproximationResult
+ {
+ enum class TerminationCondition
+ {
+ Converged,
+ IterationLimit
+ };
+ TerminationCondition condition;
+
+ int iterations;
+
+ float reductionFactor{0.F};
+ };
+
+ std::ostream& operator<<(std::ostream& str, const ApproximationResult& res);
+ } // namespace detail
+
+ class ChainApproximation
+ {
+ public:
+ using Point = Eigen::Vector2f;
+ using Points = std::vector<Point>;
+
+ using Params = detail::ChainApproximationParams;
+ using ApproximationResult = detail::ApproximationResult;
+
+ ChainApproximation(const Points& points, const Params& params);
+
+ ApproximationResult approximate();
+
+ [[nodiscard]] Points approximatedChain() const;
+
+ private:
+ struct Triplet
+ {
+ Triplet(const int& a, const int& b, const int& c) : a(a), b(b), c(c) {}
+
+ int a;
+ int b;
+ int c;
+ };
+
+ using Triplets = std::vector<Triplet>;
+
+ Triplets getTriplets() const;
+
+ std::vector<float> computeDistances(const Triplets& triplets);
+ float computeDistance(const Triplet& triplet) const;
+
+ bool approximateStep();
+
+ Points points;
+
+ std::vector<int> indices;
+
+ const Params params;
+ };
+
+} // namespace armarx
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index d94c3ade..399b1012 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -1,4 +1,4 @@
-/**
+/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
@@ -13,208 +13,532 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package navigation::ArmarXObjects::laser_scanner_feature_extraction
- * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
- * @date 2023
+ * @package RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
*/
+#include "LaserScannerFeatureExtraction.h"
-#include "Component.h"
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <utility>
-// Include headers you only need in function definitions in the .cpp.
+#include <Eigen/Core>
+#include <Eigen/Geometry>
-// #include <Eigen/Core>
+#include <IceUtil/Time.h>
-// #include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/BoundingBox.h>
+#include <VirtualRobot/MathTools.h>
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
-namespace armarx::navigation::components::laser_scanner_feature_extraction
-{
+#include "RobotComponents/components/LaserScannerFeatureExtraction/geometry.h"
- const std::string
- Component::defaultName = "LaserScannerFeatureExtraction";
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+#include "conversions/eigen.h"
+#include "conversions/pcl_eigen.h"
+namespace armarx::laser_scanner_feature_extraction
+{
armarx::PropertyDefinitionsPtr
- Component::createPropertyDefinitions()
+ LaserScannerFeatureExtraction::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
// Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
+ def->topic(featuresTopic);
// Subscribe to a topic (passing the topic name).
- // def->topic<PlatformUnitListener>("MyTopic");
-
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- // def->component(myComponentProxy)
-
-
- // Add a required property. (The component won't start without a value being set.)
- // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
-
- // Add an optional property.
- // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+ def->topic<LaserScannerUnitListener>("LaserScans");
+
+ // // Add an optionalproperty.
+ def->optional(properties.queueSize,
+ "p.inputQueueSize",
+ "Size of the message queue (max). Should not be too large to avoid delays.");
+
+ def->optional(properties.robotHull.shape, "p.robotHull.shape", "Shape of the robot area.")
+ .map({std::make_pair("Rectangle", Properties::RobotHull::RECTANGLE),
+ std::make_pair("Circle", Properties::RobotHull::CIRCLE)});
+ def->optional(properties.robotHull.radius,
+ "p.robotHull.radius",
+ "The radius of the robot when using the circle shape.");
+ def->optional(
+ properties.robotHull.robotConvexHullMargin,
+ "p.robotHull.robotConvexHullMargin",
+ "Parameter to increase the robot's convex hull when using the rectangle shape.");
+
+ def->optional(properties.cableFix.enabled,
+ "p.cableFix.enabled",
+ "Try to supress clusters belonging to the power supply cable.");
+ def->optional(properties.cableFix.cableAreaWidth,
+ "p.cableFix.cableAreaWidth",
+ "Width of the area where to search for the cable.");
+
+ def->optional(
+ properties.cableFix.maxAreaTh,
+ "p.cableFix.maxAreaTh",
+ "The cable will only be removed if the cluster area is below this threshold.");
+
+ def->optional(
+ properties.chainApproximationParams.distanceTh, "p.chainApproximation.distanceTh", "");
+ def->optional(properties.chainApproximationParams.maxIterations,
+ "p.chainApproximation.maxIterations",
+ "");
+
+ def->optional(properties.scanClusteringParams.angleThreshold,
+ "p.scanClustering.angleThreshold",
+ "Angular distance between consecutive points in laser scan for clustering.");
+ def->optional(properties.scanClusteringParams.distanceThreshold,
+ "p.scanClustering.distanceThreshold",
+ "Radial distance between consecutive points in laser scan for clustering.");
+ def->optional(properties.scanClusteringParams.maxDistance,
+ "p.scanClustering.maxDistance",
+ "Maximum radius around sensors to detect clusters.");
+
+ def->optional(properties.arviz.drawRawPoints,
+ "p.arviz.drawRawPoints",
+ "If true, the laser scans will be drawn.");
+
+ laserScannerFeatureWriter.registerPropertyDefinitions(def);
return def;
}
+ LaserScannerFeatureExtraction::LaserScannerFeatureExtraction() :
+ laserScannerFeatureWriter(memoryNameSystem())
+ {
+ }
void
- Component::onInitComponent()
+ LaserScannerFeatureExtraction::onInitComponent()
{
// Topics and properties defined above are automagically registered.
// Keep debug observer data until calling `sendDebugObserverBatch()`.
- // (Requires the armarx::DebugObserverComponentPluginUser.)
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
}
-
void
- Component::onConnectComponent()
+ LaserScannerFeatureExtraction::onConnectComponent()
{
- // Do things after connecting to topics and components.
+ ARMARX_INFO << "Connecting";
+
+ frequencyReporterPublish = std::make_unique<FrequencyReporter>(
+ getDebugObserver(), "LaserScannerFeatureExtraction_publish");
+ frequencyReporterSubscribe = std::make_unique<FrequencyReporter>(
+ getDebugObserver(), "LaserScannerFeatureExtraction_subscribe");
+
+ arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient());
+
+ featureExtractor = std::make_unique<FeatureExtractor>(
+ properties.scanClusteringParams,
+ properties.chainApproximationParams,
+ [&](auto&&... args) { onFeatures(std::forward<decltype(args)>(args)...); });
- /* (Requires the armarx::DebugObserverComponentPluginUser.)
- // Use the debug observer to log data over time.
- // The data can be viewed in the ObserverView and the LivePlotter.
- // (Before starting any threads, we don't need to lock mutexes.)
+ laserMessageQueue.setQueueSize(properties.queueSize);
+ laserMessageQueue.connect([&](const auto& sharedArg)
+ { featureExtractor->onData(sharedArg); });
+
+ ARMARX_INFO << "Connected";
+
+ robot = RemoteRobot::createLocalClone(getRobotStateComponent());
+ RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ switch (properties.robotHull.shape)
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
+ case Properties::RobotHull::RECTANGLE:
+ {
+ // initialize robot platform convex hull
+ const Eigen::Vector2f ptFrontLeft = conversions::to2D(
+ robot->getRobotNode("PlatformCornerFrontLeft")->getPositionInRootFrame());
+ const Eigen::Vector2f ptFrontRight = conversions::to2D(
+ robot->getRobotNode("PlatformCornerFrontRight")->getPositionInRootFrame());
+ const Eigen::Vector2f ptBackRight = conversions::to2D(
+ robot->getRobotNode("PlatformCornerBackRight")->getPositionInRootFrame());
+ const Eigen::Vector2f ptBackLeft = conversions::to2D(
+ robot->getRobotNode("PlatformCornerBackLeft")->getPositionInRootFrame());
+
+ const auto addMargin = [&](const Eigen::Vector2f& pt) -> Eigen::Vector2f
+ {
+ Eigen::Vector2f ptWithMargin = pt;
+ ptWithMargin.x() +=
+ std::copysignf(properties.robotHull.robotConvexHullMargin, pt.x());
+ ptWithMargin.y() +=
+ std::copysignf(properties.robotHull.robotConvexHullMargin, pt.y());
+
+ return ptWithMargin;
+ };
+
+ {
+ std::vector<Eigen::Vector2f> hullPoints;
+ hullPoints.push_back(addMargin(ptFrontLeft));
+ hullPoints.push_back(addMargin(ptFrontRight));
+ hullPoints.push_back(addMargin(ptBackRight));
+ hullPoints.push_back(addMargin(ptBackLeft));
+
+ robotHull = VirtualRobot::MathTools::createConvexHull2D(hullPoints);
+ }
+
+ // cable fix is only applicable when the robot shape is a rectangle
+ if (properties.cableFix.enabled)
+ {
+ // the cable area and the robot hull overlap slightly. as all points must be within one region,
+ // the cable area should start at approx. the position of the wheels
+
+ std::vector<Eigen::Vector2f> cableAreaPoints;
+ cableAreaPoints.emplace_back(addMargin(ptBackRight) +
+ 100 * Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackLeft) +
+ 100 * Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackRight) -
+ properties.cableFix.cableAreaWidth *
+ Eigen::Vector2f::UnitY());
+ cableAreaPoints.emplace_back(addMargin(ptBackLeft) -
+ properties.cableFix.cableAreaWidth *
+ Eigen::Vector2f::UnitY());
+
+ cableArea = VirtualRobot::MathTools::createConvexHull2D(cableAreaPoints);
+ }
+
+ break;
+ }
+ case Properties::RobotHull::CIRCLE:
+ {
+ // cable fix is not supported for circular robots
+ if (properties.cableFix.enabled)
+ {
+ ARMARX_ERROR << "Cable fix is not supported for circular robots!";
+ }
+ const Eigen::Vector2f root =
+ conversions::to2D(robot->getRootNode()->getPositionInRootFrame());
+ robotHull = root;
+ break;
+ }
}
- */
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- // Setup the remote GUI.
+ laserScannerFeatureWriter.connect();
+ }
+
+ armem::vision::LaserScannerFeature
+ toArmemFeature(const Features& features)
+ {
+ armem::vision::LaserScannerFeature armemFeature;
+
+ if (features.chain)
{
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
+ armemFeature.chain = features.chain.value();
}
- */
- }
+ if (features.circle)
+ {
+ armemFeature.circle = features.circle.value();
+ }
- void
- Component::onDisconnectComponent()
- {
- }
+ if (features.convexHull)
+ {
+ armemFeature.convexHull = features.convexHull->vertices;
+ }
+ if (features.ellipsoid)
+ {
+ armemFeature.ellipsoid = features.ellipsoid.value();
+ }
- void
- Component::onExitComponent()
- {
- }
+ armemFeature.points = features.points;
+ return armemFeature;
+ }
- std::string
- Component::getDefaultName() const
+ armem::vision::LaserScannerFeatures
+ toArmemFeatures(const std::vector<Features>& features,
+ const Eigen::Isometry3f& global_T_sensor,
+ const std::string& sensorFrame)
{
- return Component::defaultName;
- }
+ armem::vision::LaserScannerFeatures armemFeatures;
+ armemFeatures.frame = sensorFrame;
+ armemFeatures.frameGlobalPose = global_T_sensor;
- std::string
- Component::GetDefaultName()
- {
- return Component::defaultName;
+ armemFeatures.features = simox::alg::apply(features, toArmemFeature);
+
+ return armemFeatures;
}
+ std::vector<Features>
+ removeInvalidFeatures(std::vector<Features> features)
+ {
+ const auto isInvalid = [](const Features& features) -> bool
+ {
+ if (features.points.size() < 2)
+ {
+ return true;
+ }
+
+ if (not features.convexHull.has_value())
+ {
+ return true;
+ }
+
+ // if(not features.circle.has_value())
+ // {
+ // return true;
+ // }
+
+ // if(not features.ellipsoid.has_value())
+ // {
+ // return true;
+ // }
+
+ return false;
+ };
+
+
+ features.erase(std::remove_if(features.begin(), features.end(), isInvalid), features.end());
+
+ return features;
+ }
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
- Component::createRemoteGuiTab()
+ LaserScannerFeatureExtraction::onFeatures(const cartographer::LaserScannerMessage& data,
+ const std::vector<Features>& featuresFromExtractor)
{
- using namespace armarx::RemoteGui::Client;
+ ARMARX_DEBUG << "Publishing data";
+
+ // obtain sensor pose
+ RemoteRobot::synchronizeLocalCloneToTimestamp(
+ robot, getRobotStateComponent(), data.timestamp);
+ const Eigen::Isometry3f global_T_sensor(robot->getRobotNode(data.frame)->getGlobalPose());
+ const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
+ const Eigen::Isometry3f robot_T_sensor(
+ robot->getRobotNode(data.frame)->getPoseInRootFrame());
+
+ //Eigen::AlignedBox2f box;
+ //box.extend(pt1).extend(pt2).extend(pt3).extend(pt4);
+
+ const auto transformPoints =
+ [](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& tf)
+ {
+ std::vector<Eigen::Vector2f> out;
+ out.reserve(points.size());
- // Setup the widgets.
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(out),
+ [&tf](const auto& pt)
+ { return conversions::to2D(tf * conversions::to3D(pt)); });
- tab.boxLayerName.setValue(properties.boxLayerName);
+ return out;
+ };
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
+ const auto isPointInsideRobot = [&](const Eigen::Vector2f& pt) -> bool
+ {
+ if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
+ {
+ return VirtualRobot::MathTools::isInside(
+ pt, std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull));
+ }
+ else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
+ {
+ return (std::get<Eigen::Vector2f>(robotHull) - pt).norm() <
+ properties.robotHull.radius;
+ }
+ return false;
+ };
+
+ const auto isPointInsideCableArea = [&](const Eigen::Vector2f& pt) -> bool
+ { return VirtualRobot::MathTools::isInside(pt, cableArea); };
+
+ const auto isClusterInvalid = [&](const Features& features) -> bool
+ {
+ // either use convex hull (compact representation) or raw points as fallbacck
+ const auto points = [&]()
+ {
+ if (features.convexHull)
+ {
+ return transformPoints(features.convexHull->vertices, robot_T_sensor);
+ }
+
+ return transformPoints(features.points, robot_T_sensor);
+ }();
+
+ const bool allPointsInsideRobot =
+ std::all_of(points.begin(), points.end(), isPointInsideRobot);
+ if (allPointsInsideRobot)
+ {
+ return true;
+ }
+
+ if (properties.cableFix.enabled)
+ {
- tab.drawBoxes.setLabel("Draw Boxes");
+ const bool allPointsInCableArea =
+ std::all_of(points.begin(), points.end(), isPointInsideCableArea);
+ if (allPointsInCableArea)
+ {
+ if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
+ {
+ return true;
+ }
+ }
+ }
- // Setup the layout.
+ return false;
+ };
- GridLayout grid;
- int row = 0;
+ const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
{
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
+ features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
+ features.end());
+ return features;
+ };
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
+ ARMARX_VERBOSE << featuresFromExtractor.size() << " features from extractor";
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
- }
+ const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
+ ARMARX_VERBOSE << features.size() << " features without cable region";
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
+ const auto validFeatures = removeInvalidFeatures(features);
+ ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
+ const auto armemFeatures = toArmemFeatures(validFeatures, global_T_sensor, data.frame);
- void
- Component::RemoteGui_update()
- {
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
+
+ // report the features
+ publishFeatures(armemFeatures,
+ armarx::core::time::DateTime(
+ armarx::core::time::Duration::MicroSeconds(data.timestamp)));
+
+ // check if arviz should be triggered
+ const auto getOrCreateThrottler = [&]() -> Throttler&
{
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
+ const auto it = throttlers.find(data.frame);
+ if (it == throttlers.end())
+ {
+ throttlers.emplace(data.frame, Throttler(10.F));
+ }
+
+ return throttlers.at(data.frame);
+ };
+
+ if (getOrCreateThrottler().check(data.timestamp))
+ {
+ arVizDrawer->draw(features, data.frame, global_T_sensor);
+ if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
+ VirtualRobot::MathTools::ConvexHull2DPtr& convexHullPtr =
+ std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull);
+ if (convexHullPtr != nullptr)
+ {
+ arVizDrawer->draw("robot_convex_hull",
+ *convexHullPtr,
+ global_T_robot,
+ simox::Color::azure(255, 80));
+ }
+ }
+ else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
+ {
+ Eigen::Vector2f& root = std::get<Eigen::Vector2f>(robotHull);
+ arVizDrawer->draw("robot_circle_hull",
+ Circle{root, properties.robotHull.radius},
+ global_T_robot,
+ simox::Color::azure(255, 80));
+ }
+
+ if (cableArea != nullptr)
+ {
+ arVizDrawer->draw(
+ "cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
+ }
+
+ if (properties.arviz.drawRawPoints)
+ {
+ arVizDrawer->draw(data, global_T_sensor, simox::Color::magenta(255, 100));
}
}
- if (tab.drawBoxes.wasClicked())
+
+ // some debugobserver reporting
+ frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
+ setDebugObserverDatafield("numClusters", features.size());
+ }
+
+ void
+ LaserScannerFeatureExtraction::publishFeatures(
+ const armem::vision::LaserScannerFeatures& features,
+ const armem::Time& timestamp)
+ {
+
+ // store in memory
+ laserScannerFeatureWriter.store(features, getName(), timestamp);
+
+ // legacy - topic
+ LineSegment2DChainSeq chains;
+ for (const auto& feature : features.features)
{
- // Lock shared variables in methods running in separate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex);
- drawBoxes(properties, arviz);
+ if (not feature.chain.empty())
+ {
+ LineSegment2DChain chain;
+ for (const auto& pt : feature.chain)
+ {
+ chain.push_back(
+ conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
+ }
+ chains.push_back(chain);
+ }
}
+
+ featuresTopic->reportExtractedLineSegments(chains);
}
- */
+ void
+ LaserScannerFeatureExtraction::onDisconnectComponent()
+ {
+ }
- /* (Requires the armarx::ArVizComponentPluginUser.)
void
- Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+ LaserScannerFeatureExtraction::onExitComponent()
{
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
+ }
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
- {
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
- }
- arviz.commit(layer);
+ std::string
+ LaserScannerFeatureExtraction::getDefaultName() const
+ {
+ return "LaserScannerFeatureExtraction";
}
- */
+ void
+ LaserScannerFeatureExtraction::reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& /*unused*/)
+ {
+ ARMARX_DEBUG << "Receiving data";
+
+ laserMessageQueue.push(cartographer::LaserScannerMessage{
+ .frame = name, .scan = scan, .timestamp = timestamp->timestamp});
- ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+ ARMARX_DEBUG << "Queue size" << laserMessageQueue.size();
+
+ frequencyReporterSubscribe->add(IceUtil::Time::now().toMicroSeconds());
+
+ setDebugObserverDatafield(name, scan.size());
+ }
-} // namespace armarx::navigation::components::laser_scanner_feature_extraction
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index 43520364..214ccf97 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -1,4 +1,4 @@
-/**
+/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
@@ -13,51 +13,75 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package navigation::ArmarXObjects::laser_scanner_feature_extraction
- * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
- * @date 2023
+ * @package RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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 <variant>
-// #include <mutex>
-
+#include "ArmarXCore/core/util/Throttler.h"
+#include "ArmarXCore/libraries/logging/FrequencyReporter.h"
#include <ArmarXCore/core/Component.h>
-
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-#include <armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.h>
-
-
-namespace armarx::navigation::components::laser_scanner_feature_extraction
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h"
+#include "RobotAPI/libraries/armem/client/forward_declarations.h"
+#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h>
+
+#include "RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h"
+#include "RobotComponents/components/LaserScannerFeatureExtraction/ScanClustering.h"
+#include "RobotComponents/components/LaserScannerFeatureExtraction/geometry.h"
+#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+#include <RobotComponents/libraries/cartographer/MessageQueue.h>
+#include <RobotComponents/libraries/cartographer/types.h>
+
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+
+namespace armarx::laser_scanner_feature_extraction
{
- class Component :
+ // class FeatureExtractor;
+ // class ArVizDrawer;
+
+ /**
+ * @defgroup Component-LaserScannerFeatureExtraction LaserScannerFeatureExtraction
+ * @ingroup RobotComponents-Components
+ * A description of the component LaserScannerFeatureExtraction.
+ *
+ * @class LaserScannerFeatureExtraction
+ * @ingroup Component-LaserScannerFeatureExtraction
+ * @brief Brief description of class LaserScannerFeatureExtraction.
+ *
+ * Detailed description of class LaserScannerFeatureExtraction.
+ */
+ class LaserScannerFeatureExtraction :
virtual public armarx::Component,
- virtual public armarx::navigation::components::laser_scanner_feature_extraction::ComponentInterface
- // , virtual public armarx::DebugObserverComponentPluginUser
+ virtual public armarx::DebugObserverComponentPluginUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ ,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public LaserScannerUnitListener,
+ virtual public RobotStateComponentPluginUser,
+ virtual public armem::ClientPluginUser
+
{
public:
+ LaserScannerFeatureExtraction();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
- /// Get the component's default name.
- static std::string GetDefaultName();
-
-
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -73,7 +97,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
/// @see armarx::ManagedIceObject::onExitComponent()
void onExitComponent() override;
-
/* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
/// This function should be called once in onConnect() or when you
/// need to re-create the Remote GUI tab.
@@ -85,8 +108,19 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
void RemoteGui_update() override;
*/
+ // LaserScannerUnitListener interface
+ void reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& /*unused*/) override;
private:
+ void onFeatures(const cartographer::LaserScannerMessage& data,
+ const std::vector<Features>& features);
+
+ void publishFeatures(const armem::vision::LaserScannerFeatures& features,
+ const armem::Time& timestamp);
// Private methods go here.
@@ -98,28 +132,62 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
void drawBoxes(const Properties& p, viz::Client& arviz);
*/
-
- private:
-
- static const std::string defaultName;
-
-
// Private member variables go here.
-
+ MessageQueue<cartographer::LaserScannerMessage> laserMessageQueue;
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string boxLayerName = "boxes";
- int numBoxes = 10;
+ // input message queue size
+ size_t queueSize{2};
+
+ //
+ struct RobotHull
+ {
+ enum Shape
+ {
+ RECTANGLE,
+ CIRCLE
+ };
+
+ Shape shape = RECTANGLE;
+ // for shape circle
+ float radius = 500.F; // [mm]
+ // for shape rectangle
+ float robotConvexHullMargin{50.F};
+ } robotHull;
+
+ //
+ struct CableFix
+ {
+ bool enabled = {true};
+ float cableAreaWidth{400};
+ float maxAreaTh{50 * 50};
+ } cableFix;
+
+ ScanClustering::Params scanClusteringParams{
+ .distanceThreshold = 30.F, // [mm]
+ // we know the scan resolution which produces a bit more than 1000 points
+ .angleThreshold = 2 * M_PIf32 / 1000.F * 5.F, // [rad]
+ .maxDistance = 3'000.F // [mm]
+ };
+
+ ChainApproximation::Params chainApproximationParams{
+ .distanceTh = 40.F // [mm]
+ };
+
+ struct ArViz
+ {
+ bool drawRawPoints = {true};
+ } arviz;
};
+
Properties properties;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
*/
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
/// Tab shown in the Remote GUI.
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
@@ -132,6 +200,29 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
RemoteGuiTab tab;
*/
- };
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
-} // namespace armarx::navigation::components::laser_scanner_feature_extraction
+ std::unique_ptr<FeatureExtractor> featureExtractor;
+ std::unique_ptr<ArVizDrawer> arVizDrawer;
+
+ std::unique_ptr<FrequencyReporter> frequencyReporterPublish;
+ std::unique_ptr<FrequencyReporter> frequencyReporterSubscribe;
+
+ std::unordered_map<std::string, Throttler> throttlers;
+
+ VirtualRobot::RobotPtr robot;
+
+ LaserScannerFeaturesListenerPrx featuresTopic;
+
+ std::variant<std::monostate, VirtualRobot::MathTools::ConvexHull2DPtr, Eigen::Vector2f>
+ robotHull;
+ VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
+
+
+ armem::vision::laser_scanner_features::client::Writer laserScannerFeatureWriter;
+ };
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
new file mode 100644
index 00000000..87bf8313
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
@@ -0,0 +1,124 @@
+#include "EnclosingEllipsoid.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+
+#include <SimoxUtility/math/periodic/periodic_clamp.h>
+#include <VirtualRobot/math/Helpers.h>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ // Eigen::Affine2f Ellipsoid::pose() const noexcept
+ // {
+ // Eigen::Affine2f pose;
+ // pose.translation() = center;
+ // pose.linear() = Eigen::Rotation2Df(angle).toRotationMatrix();
+
+ // return pose;
+ // }
+
+ // TODO(fabian.reister): use Eigen built-in stuff.
+ size_t
+ argmin(const Eigen::VectorXf& v)
+ {
+ const std::vector<float> vec(v.data(), v.data() + v.rows() * v.cols());
+ return std::distance(std::begin(vec), std::max_element(vec.begin(), vec.end()));
+ }
+
+ // https://github.com/minillinim/ellipsoid/blob/master/ellipsoid.py
+
+ EnclosingEllipsoid::EnclosingEllipsoid(const Points& points)
+ {
+ ARMARX_CHECK(compute(points));
+ }
+
+ bool
+ EnclosingEllipsoid::compute(const Points& points)
+ {
+ const float tolerance = 0.01;
+
+ const int N = static_cast<int>(points.size());
+ const int d = 2;
+
+ Eigen::MatrixXf P(N, d);
+
+ for (int i = 0; i < N; i++)
+ {
+ P.row(i) = points.at(i);
+ }
+
+ Eigen::MatrixXf Q(d + 1, N);
+ Q.topRows(d) = P.transpose();
+ Q.bottomRows(1).setOnes(); // last row
+
+ Eigen::MatrixXf Qt = Q.transpose();
+
+ float err = 1.F + tolerance;
+ Eigen::VectorXf u = (1.F / N) * Eigen::VectorXf::Ones(N);
+
+ // Khachiyan Algorithm
+ while (err > tolerance)
+ {
+ const Eigen::MatrixXf V = Q * (u.asDiagonal() * Qt);
+ const Eigen::VectorXf M = (Qt * (V.inverse() * Q)).diagonal();
+
+ const int j = static_cast<int>(argmin(M));
+ const float maximum = M(j);
+
+ const float stepSize = (maximum - d - 1.0) / ((d + 1.0) * (maximum - 1.0));
+
+ Eigen::VectorXf uNew = (1.0 - stepSize) * u;
+ uNew(j) += stepSize;
+
+ err = (uNew - u).norm();
+ u = uNew;
+ }
+
+ const Eigen::VectorXf center = P.transpose() * u;
+
+ Eigen::MatrixXf K(d, d);
+ for (int i = 0; i < d; i++)
+ {
+ for (int j = 0; j < d; j++)
+ {
+ K(i, j) = center(i) * center(j);
+ }
+ }
+
+ const Eigen::MatrixXf H = (P.transpose() * (u.asDiagonal() * P)) - K;
+ const Eigen::MatrixXf A = H.inverse() / d;
+
+ const Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
+
+ // angle
+ const Eigen::VectorXf v0 = svd.matrixV().col(0);
+ const float angle = math::Helpers::Angle(v0);
+
+ // radii
+ const Eigen::Vector2f radii = svd.singularValues().cwiseSqrt().cwiseInverse();
+
+ // As the singular values are sorted descending, the radii are sorted ascending.
+ // To fix this, the vector is reversed.
+ this->radii = radii.reverse();
+
+ // Thus, the angle does no longer match the radii. It has to be altered by 90°.
+ const float angle1 = simox::math::periodic_clamp(angle + M_PI_2f32, -M_PIf32, M_PIf32);
+
+ this->pose.setIdentity();
+ this->pose.translation().head<2>() = center;
+ this->pose.linear() =
+ Eigen::AngleAxisf(angle1, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ return true;
+ }
+
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
new file mode 100644
index 00000000..9628314b
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
@@ -0,0 +1,70 @@
+/*
+ * 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 <Eigen/Core>
+#include <Eigen/Geometry>
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ // struct Ellipsoid
+ // {
+ // Eigen::Vector2f center;
+ // float angle;
+
+ // Eigen::Vector2f radii;
+
+ // Eigen::Affine2f pose() const noexcept;
+ // };
+
+ using Ellipsoid = armem::vision::Ellipsoid;
+
+ /**
+ * @brief Minimum volume enclosing ellipsoid (MVEE) for a set of points.
+ *
+ * See https://de.mathworks.com/matlabcentral/fileexchange/9542-minimum-volume-enclosing-ellipsoid
+ *
+ */
+ class EnclosingEllipsoid : public Ellipsoid
+ {
+ public:
+ using Point = Eigen::Vector2f;
+ using Points = std::vector<Point>;
+
+ EnclosingEllipsoid(const Points& points);
+
+ private:
+ /**
+ * @brief Computes the enclosing ellipsoid for the given points by using Khachiyan's Algorithm.
+ *
+ * The implementation is based on
+ * https://github.com/minillinim/ellipsoid/blob/master/ellipsoid.py
+ *
+ * @return true
+ * @return false
+ */
+ bool compute(const Points& points);
+ };
+
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
new file mode 100644
index 00000000..90949d11
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -0,0 +1,239 @@
+#include "FeatureExtractor.h"
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <numeric>
+#include <optional>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Transform.h>
+
+#include <pcl/PointIndices.h>
+#include <pcl/point_cloud.h>
+
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include <VirtualRobot/MathTools.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h>
+#include <RobotComponents/components/LaserScannerFeatureExtraction/EnclosingEllipsoid.h>
+#include <RobotComponents/components/LaserScannerFeatureExtraction/ScanClustering.h>
+#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
+
+#include "Path.h"
+#include "conversions/eigen.h"
+#include "conversions/opencv.h"
+#include "conversions/opencv_eigen.h"
+#include "conversions/opencv_pcl.h"
+#include "conversions/pcl_eigen.h"
+
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ // Features
+
+ std::vector<Ellipsoid>
+ Features::linesAsEllipsoids(const float axisLength) const
+ {
+ if (not chain)
+ {
+ return {};
+ }
+
+ const auto asEllipsoid = [&](const Path::Segment& segment) -> Ellipsoid
+ {
+ const Eigen::Vector2f center = (segment.first + segment.second) / 2;
+ const Eigen::Vector2f v = segment.second - center;
+ const float angle = math::Helpers::Angle(v);
+ const float r = v.norm();
+
+ Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
+ globalPose.translation().head<2>() = center;
+ globalPose.linear() =
+ Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ return Ellipsoid{.pose = globalPose, .radii = Eigen::Vector2f{r, axisLength}};
+ };
+
+ const auto segments = Path{.points = *chain}.segments();
+
+ std::vector<Ellipsoid> ellipsoids;
+ std::transform(
+ segments.begin(), segments.end(), std::back_inserter(ellipsoids), asEllipsoid);
+
+ return ellipsoids;
+ }
+
+ // FeatureExtractor
+
+ FeatureExtractor::FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
+ const ChainApproximation::Params& chainApproximationParams,
+ const Callback& callback) :
+ scanClusteringParams(scanClusteringParams),
+ chainApproximationParams(chainApproximationParams),
+ callback(callback)
+ {
+ }
+
+ void
+ FeatureExtractor::onData(const cartographer::LaserScannerMessage& data)
+ {
+ ARMARX_DEBUG << "on data";
+ const auto clustersWithFeatures = features(data.scan);
+
+ ARMARX_DEBUG << "callback";
+ callback(data, clustersWithFeatures);
+ }
+
+ std::vector<Features>
+ FeatureExtractor::features(const LaserScan& scan) const
+ {
+ const auto clusters = detectClusters(scan);
+
+ std::vector<Features> fs;
+ std::transform(clusters.begin(),
+ clusters.end(),
+ std::back_inserter(fs),
+ [&](const LaserScan& cluster)
+ {
+ const auto points = toCartesian<Eigen::Vector2f>(cluster);
+ const auto hull = convexHull(points);
+
+ return Features{.convexHull = hull,
+ .circle = std::nullopt, //circle(points),
+ .ellipsoid = std::nullopt, //ellipsoid(hull),
+ .chain = chainApproximation(points, hull),
+ .points = points};
+ });
+
+ return fs;
+ }
+
+ std::vector<LaserScan>
+ FeatureExtractor::detectClusters(const LaserScan& scan) const
+ {
+ ScanClustering clustering(scanClusteringParams);
+ return clustering.detectClusters(scan);
+ }
+
+
+ std::optional<VirtualRobot::MathTools::ConvexHull2D>
+ FeatureExtractor::convexHull(const Points& points) const
+ {
+
+ if (points.size() < 3) // no meaningful area
+ {
+ return std::nullopt;
+ }
+
+ try
+ {
+ return *VirtualRobot::MathTools::createConvexHull2D(points);
+ }
+ catch (std::exception& e)
+ {
+ ARMARX_WARNING << "Couldn't create convex hull: " << e.what();
+ }
+ return std::nullopt;
+ }
+
+ // Legacy OpenCV implementation. Didn't work as expected.
+ // Also, tries to fit an ellipsoid which is NOT the enclosing ellipsoid!
+
+ // std::optional<Ellipsoid> FeatureExtractor::ellipsoid(const PointCloud &pointCloud) const
+ // {
+ // // OpenCV::fitEllipse requirement
+ // // "There should be at least 5 points to fit the ellipse"
+ // // => Too few points will cause algorithmic issues
+ // if (pointCloud.size() < 100)
+ // {
+ // return std::nullopt;
+ // }
+
+ // const auto points2f = conversions::pcl2cv(pointCloud);
+ // const auto points2i = conversions::cast<cv::Point2i>(points2f);
+
+ // cv::RotatedRect rect = cv::fitEllipse(points2i);
+
+ // Eigen::Affine2f pose;
+ // pose.translation() = conversions::cv2eigen(rect.center);
+ // pose.linear() =
+ // Eigen::Rotation2Df(VirtualRobot::MathTools::deg2rad(rect.angle)).toRotationMatrix();
+
+ // return Ellipsoid{.axisLengths = conversions::cv2eigen(rect.size), .pose = pose};
+ // }
+
+ std::optional<Ellipsoid>
+ FeatureExtractor::ellipsoid(
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull) const
+ {
+
+ if (not hull)
+ {
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): it might result in multiple ellipsoids if hull->segments is considered
+
+ // If there are not enough points, don't even try to fit an ellipse ...
+ if (hull->vertices.size() < 5)
+ {
+ return std::nullopt;
+ }
+
+ EnclosingEllipsoid mvee(hull->vertices);
+ return mvee;
+ }
+
+ std::optional<Circle>
+ FeatureExtractor::circle(const Points& points) const
+ {
+ // Too few points will cause algorithmic issues
+ if (points.size() < 5)
+ {
+ return std::nullopt;
+ }
+
+ const auto points2f = conversions::eigen2cv(points);
+ const auto points2i = conversions::cast<cv::Point2i>(points2f);
+
+ cv::Point2f center;
+ float radius = NAN;
+ cv::minEnclosingCircle(points2i, center, radius);
+
+ return Circle{.center = conversions::cv2eigen(center), .radius = radius};
+ }
+
+ std::optional<FeatureExtractor::Points>
+ FeatureExtractor::chainApproximation(
+ const Points& points,
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull) const
+ {
+ if (not convexHull)
+ {
+ return std::nullopt;
+ }
+
+ ChainApproximation chApprx(points, chainApproximationParams);
+ const auto result = chApprx.approximate();
+
+ if (result.condition !=
+ ChainApproximation::ApproximationResult::TerminationCondition::Converged)
+ {
+ return std::nullopt;
+ }
+
+ return chApprx.approximatedChain();
+ }
+
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
new file mode 100644
index 00000000..3dd5caac
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -0,0 +1,91 @@
+/*
+ * 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 <optional>
+
+#include <pcl/point_types.h>
+
+#include <VirtualRobot/MathTools.h>
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+#include "RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h"
+#include "RobotComponents/libraries/cartographer/types.h"
+#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
+
+#include "EnclosingEllipsoid.h"
+#include "ScanClustering.h"
+
+namespace armarx::laser_scanner_feature_extraction
+{
+ using Circle = armem::vision::Circle;
+
+
+ struct Features
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ std::optional<VirtualRobot::MathTools::ConvexHull2D> convexHull;
+
+ std::optional<Circle> circle;
+ std::optional<Ellipsoid> ellipsoid;
+
+ std::optional<Chain> chain;
+
+ Points points;
+ std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
+ class FeatureExtractor
+ {
+ public:
+ using Points = std::vector<Eigen::Vector2f>;
+ using Callback = std::function<void(const cartographer::LaserScannerMessage& data,
+ const std::vector<Features>& features)>;
+
+ FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
+ const ChainApproximation::Params& chainApproximationParams,
+ const Callback& callback);
+
+ void onData(const cartographer::LaserScannerMessage& data);
+
+ private:
+ std::vector<Features> features(const LaserScan& scan) const;
+
+ std::vector<LaserScan> detectClusters(const LaserScan& scan) const;
+
+ std::optional<VirtualRobot::MathTools::ConvexHull2D> convexHull(const Points& points) const;
+ std::optional<Ellipsoid>
+ ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull) const;
+ std::optional<Circle> circle(const Points& circle) const;
+
+ std::optional<Points> chainApproximation(
+ const Points& points,
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull) const;
+
+ const ScanClustering::Params scanClusteringParams;
+ const ChainApproximation::Params chainApproximationParams;
+
+ const Callback callback;
+ };
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
new file mode 100644
index 00000000..057ada4f
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
@@ -0,0 +1,24 @@
+#include "Path.h"
+
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ std::vector<Path::Segment> Path::segments() const noexcept
+ {
+ if (points.size() <= 1)
+ {
+ return {};
+ }
+
+ std::vector<Segment> segments;
+ segments.reserve(static_cast<int>(points.size()) - 1);
+
+ for (int i = 0; i < static_cast<int>(points.size()) - 1; i++)
+ {
+ segments.emplace_back(points.at(i), points.at(i + 1));
+ }
+
+ return segments;
+ }
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
new file mode 100644
index 00000000..8f63e260
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
@@ -0,0 +1,19 @@
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace armarx::laser_scanner_feature_extraction
+{
+
+ struct Path
+ {
+ std::vector<Eigen::Vector2f> points;
+
+ using Segment = std::pair<Eigen::Vector2f, Eigen::Vector2f>;
+
+ std::vector<Segment> segments() const noexcept;
+ };
+
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
new file mode 100644
index 00000000..e0376c54
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
@@ -0,0 +1,71 @@
+#include "ScanClustering.h"
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::laser_scanner_feature_extraction
+{
+ ScanClustering::ScanClustering(const Params& params) : params(params) {}
+
+ bool ScanClustering::add(const LaserScanStep& scanStep)
+ {
+ if (scan.empty())
+ {
+ scan.push_back(scanStep);
+ return true;
+ }
+
+ if (not supports(scanStep))
+ {
+ return false;
+ }
+
+ scan.push_back(scanStep);
+ return true;
+ }
+
+ std::vector<LaserScan> ScanClustering::detectClusters(const LaserScan& scan)
+ {
+ const auto isInvalid = [&](const LaserScanStep& step) -> bool
+ { return step.distance > params.maxDistance; };
+
+ std::vector<LaserScan> clusters;
+ for (const auto& laserScanStep : scan)
+ {
+ // cluster finished
+ if (isInvalid(laserScanStep) or (not add(laserScanStep)))
+ {
+ if (not cluster().empty())
+ {
+ clusters.push_back(cluster());
+ clear();
+
+ // work on a new cluster
+ add(laserScanStep);
+ }
+ }
+ }
+
+ return clusters;
+ }
+
+ const LaserScan& ScanClustering::cluster() const { return scan; }
+
+ void ScanClustering::clear() { scan.clear(); }
+
+ bool ScanClustering::supports(const LaserScanStep& scanStep)
+ {
+ // OK to create a new cluster if it's empty
+ if (scan.empty())
+ {
+ return true;
+ }
+
+ const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
+ const bool isWithinDistanceRange = distanceDiff < params.distanceThreshold;
+
+ const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
+ const bool isWithinAngleRange = angleDiff < params.angleThreshold;
+
+ return (isWithinDistanceRange and isWithinAngleRange);
+ }
+} // namespace armarx::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
new file mode 100644
index 00000000..6cee501d
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
@@ -0,0 +1,71 @@
+/*
+ * 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 <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::laser_scanner_feature_extraction
+{
+ namespace detail
+ {
+ struct ScanClusteringParams
+ {
+ // Clustering options to decide whether a point belongs to the cluster
+ float distanceThreshold; // [mm]
+ float angleThreshold; // [rad]
+
+ /// Range filter: only consider points that are closer than maxDistance
+ float maxDistance; // [mm]
+ };
+ } // namespace detail
+
+ class ScanClustering
+ {
+ public:
+ using Params = detail::ScanClusteringParams;
+ using Clusters = std::vector<LaserScan>;
+
+ ScanClustering(const Params& params);
+
+ /**
+ * @brief Performs cluster detection on a full laser scan.
+ *
+ * @param scan A full scan
+ * @return The input scan split into clusters.
+ */
+ Clusters detectClusters(const LaserScan& scan);
+
+ protected:
+ const LaserScan& cluster() const;
+
+ bool add(const LaserScanStep& scanStep);
+ bool supports(const LaserScanStep& scanStep);
+
+ void clear();
+
+ private:
+ LaserScan scan;
+
+ const Params params;
+ };
+
+} // namespace armarx::laser_scanner_feature_extraction
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp
new file mode 100644
index 00000000..41af90f4
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.cpp
@@ -0,0 +1,23 @@
+#include "eigen.h"
+
+#include <algorithm>
+
+namespace armarx::conversions
+{
+
+ std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v)
+
+ {
+ std::vector<Eigen::Vector3f> v3;
+ v3.reserve(v.size());
+
+ std::transform(
+ v.begin(),
+ v.end(),
+ std::back_inserter(v3),
+ static_cast<Eigen::Vector3f (*)(const Eigen::Vector2f&)>(&to3D));
+
+ return v3;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h
new file mode 100644
index 00000000..615d41c6
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h
@@ -0,0 +1,53 @@
+/*
+ * 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 <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace armarx::conversions
+{
+
+ inline Eigen::Vector2f to2D(const Eigen::Vector3f& v2)
+ {
+ return Eigen::Vector2f{v2.x(), v2.y()};
+ }
+
+ inline Eigen::Vector3f to3D(const Eigen::Vector2f& v2)
+ {
+ return Eigen::Vector3f{v2.x(), v2.y(), 0.F};
+ }
+
+ inline Eigen::Isometry3f to3D(const Eigen::Affine2f& p2)
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+ pose.linear().block<2, 2>(0, 0) = p2.linear();
+ pose.translation().head<2>() = p2.translation();
+
+ return pose;
+ }
+
+ std::vector<Eigen::Vector3f> to3D(const std::vector<Eigen::Vector2f>& v);
+
+} // namespace armarx::conversions
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h
new file mode 100644
index 00000000..ae12d627
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv.h
@@ -0,0 +1,47 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author 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 <vector>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+
+ // TODO(fabian.reister): this is a specialized method
+ template <typename CvT> CvT cast(const auto &pt) { return CvT(pt.x, pt.y); }
+
+ template <typename PointOutT, typename PointInT>
+ std::vector<PointOutT> cast(const std::vector<PointInT> &points)
+ {
+ std::vector<PointOutT> v;
+ v.reserve(points.size());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(v),
+ static_cast<PointOutT (*)(const PointInT &)>(cast));
+
+ return v;
+ }
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h
new file mode 100644
index 00000000..0e6b8182
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_eigen.h
@@ -0,0 +1,52 @@
+/*
+ * 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 <Eigen/Core>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+ inline cv::Point2f eigen2cv(const Eigen::Vector2f& pt)
+ {
+ return {pt.x(), pt.y()};
+ }
+
+ template <typename EigenT, typename CvT = decltype(eigen2cv(EigenT()))>
+ auto eigen2cv(const std::vector<EigenT>& points)
+ {
+ std::vector<CvT> v;
+ v.reserve(points.size());
+
+ std::transform(
+ points.begin(), points.end(), std::back_inserter(v), static_cast<CvT(*)(const EigenT&)>(eigen2cv));
+
+ return v;
+ }
+
+ inline Eigen::Vector2f cv2eigen(const cv::Point2f& pt)
+ {
+ return Eigen::Vector2f{pt.x, pt.y};
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h
new file mode 100644
index 00000000..375f166f
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/opencv_pcl.h
@@ -0,0 +1,57 @@
+/*
+ * 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 <Eigen/Core>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <opencv2/core/types.hpp>
+
+namespace armarx::conversions
+{
+
+ inline cv::Point2f pcl2cv(const pcl::PointXY& pt)
+ {
+ return cv::Point2f{pt.x, pt.y};
+ }
+ inline cv::Point3f pcl2cv(const pcl::PointXYZ& pt)
+ {
+ return cv::Point3f{pt.x, pt.y, pt.z};
+ }
+
+ template <typename PointT, typename CvT = decltype(pcl2cv(PointT()))>
+ auto pcl2cv(const pcl::PointCloud<PointT>& pointCloud)
+ {
+ std::vector<CvT> v;
+ v.reserve(pointCloud.points.size());
+
+ std::transform(pointCloud.points.begin(),
+ pointCloud.points.end(),
+ std::back_inserter(v),
+ static_cast<CvT (*)(const PointT&)>(pcl2cv));
+
+ return v;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp
new file mode 100644
index 00000000..c8ed47f2
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.cpp
@@ -0,0 +1,27 @@
+#include "pcl.h"
+
+#include <algorithm>
+
+namespace armarx::conversions
+{
+ pcl::PointCloud<pcl::PointXYZ> to3D(const pcl::PointCloud<pcl::PointXY>& v)
+ {
+ pcl::PointCloud<pcl::PointXYZ> pc;
+ pc.points.reserve(v.size());
+
+ std::transform(
+ v.begin(),
+ v.end(),
+ std::back_inserter(pc),
+ static_cast<pcl::PointXYZ (*)(const pcl::PointXY&)>(&to3D));
+
+ pc.width = v.height;
+ pc.height = v.width;
+ pc.is_dense = v.is_dense;
+
+ pc.header = v.header;
+
+ return pc;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h
new file mode 100644
index 00000000..2caf92b9
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl.h
@@ -0,0 +1,39 @@
+/*
+ * 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 <vector>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace armarx::conversions
+{
+
+ inline pcl::PointXYZ to3D(const pcl::PointXY& v2)
+ {
+ return pcl::PointXYZ{v2.x, v2.y, 0.F};
+ }
+
+ pcl::PointCloud<pcl::PointXYZ> to3D(const pcl::PointCloud<pcl::PointXY>& v);
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h
new file mode 100644
index 00000000..b72277e0
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/pcl_eigen.h
@@ -0,0 +1,92 @@
+/*
+ * 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 <algorithm>
+
+#include <Eigen/Core>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace armarx::conversions
+{
+ // pcl2eigen
+
+ inline Eigen::Vector2f pcl2eigen(const pcl::PointXY& pt)
+ {
+ return Eigen::Vector2f{pt.x, pt.y};
+ }
+ inline Eigen::Vector3f pcl2eigen(const pcl::PointXYZ& pt)
+ {
+ return Eigen::Vector3f{pt.x, pt.y, pt.z};
+ }
+
+ template <typename PointT,
+ typename EigenVector = decltype(pcl2eigen(PointT()))>
+ auto pcl2eigen(const pcl::PointCloud<PointT>& pointCloud)
+ -> std::vector<EigenVector>
+ {
+ std::vector<EigenVector> v;
+ v.reserve(pointCloud.points.size());
+
+ std::transform(pointCloud.points.begin(),
+ pointCloud.points.end(),
+ std::back_inserter(v),
+ static_cast<EigenVector(*)(const PointT&)>(pcl2eigen));
+
+ return v;
+ }
+
+ // eigen2pcl
+
+ inline pcl::PointXY eigen2pcl(const Eigen::Vector2f& pt)
+ {
+ return pcl::PointXY{pt.x(), pt.y()};
+ }
+
+ inline pcl::PointXYZ eigen2pcl(const Eigen::Vector3f& pt)
+ {
+ return pcl::PointXYZ{pt.x(), pt.y(), 0.F};
+ }
+
+ template <typename EigenVector,
+ typename PointT = decltype(eigen2pcl(EigenVector()))>
+ auto eigen2pcl(const std::vector<EigenVector>& points)
+ -> pcl::PointCloud<PointT>
+ {
+ pcl::PointCloud<PointT> pointCloud;
+ pointCloud.points.reserve(points.size());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(pointCloud.points),
+ static_cast<PointT(*)(const EigenVector&)>(eigen2pcl));
+
+ pointCloud.width = pointCloud.points.size();
+ pointCloud.height = 1;
+ pointCloud.is_dense = true;
+
+ return pointCloud;
+ }
+
+} // namespace armarx::conversions
\ No newline at end of file
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h
new file mode 100644
index 00000000..8b70a56d
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/geometry.h
@@ -0,0 +1,57 @@
+#pragma once
+
+// TODO To be moved to Simox after ROBDEKON demo
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/algorithms/assign.hpp>
+#include <boost/geometry/algorithms/detail/within/interface.hpp>
+#include <boost/geometry/algorithms/distance.hpp>
+#include <boost/geometry/algorithms/make.hpp>
+#include <boost/geometry/geometries/point_xy.hpp>
+#include <boost/geometry/geometries/polygon.hpp>
+#include <boost/geometry/geometries/register/point.hpp>
+#include <boost/geometry/strategies/strategies.hpp>
+
+#include <VirtualRobot/MathTools.h>
+
+BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2f, float, cs::cartesian, x(), y())
+
+namespace armarx::geometry
+{
+ namespace bg = boost::geometry;
+
+ class ConvexHull
+ {
+ public:
+ using Point = bg::model::d2::point_xy<float>;
+
+ ConvexHull(const VirtualRobot::MathTools::ConvexHull2D& hull) : ConvexHull(hull.vertices)
+ {
+ }
+
+ ConvexHull(const std::vector<Eigen::Vector2f>& hull)
+ {
+ boost::geometry::assign_points(polygon, hull);
+ }
+
+ bool contains(const Eigen::Vector2f& point) const
+ {
+ return bg::within(Point(point.x(), point.y()), polygon);
+ }
+
+ float area() const
+ {
+ return static_cast<float>(bg::area(polygon));
+ }
+
+ private:
+ bg::model::polygon<Point> polygon;
+ };
+
+ // inline float area()
+ // {
+
+ // return bg::area(polygon);
+ // }
+
+} // namespace armarx::geometry
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
new file mode 100644
index 00000000..3435a5f5
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore LaserScannerFeatureExtraction)
+
+armarx_add_test(LaserScannerFeatureExtractionTest LaserScannerFeatureExtractionTest.cpp "${LIBS}")
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
new file mode 100644
index 00000000..fdad60e2
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
@@ -0,0 +1,65 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
+ */
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotComponents/components/LaserScannerFeatureExtraction/FeatureExtractor.h"
+#include <boost/test/tools/old/interface.hpp>
+#define BOOST_TEST_MODULE RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotComponents/Test.h>
+
+#include <pcl/point_types.h>
+
+#include "../EnclosingEllipsoid.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ // Ground truth (GT) ellipsoid with params:
+ // center = (0,0)
+ // radii = (4,2)
+ // angle = 0
+ armarx::laser_scanner_feature_extraction::FeatureExtractor::Points points;
+ points.push_back(Eigen::Vector2f{-4.F, 0});
+ points.push_back(Eigen::Vector2f{0, 2.F});
+ points.push_back(Eigen::Vector2f{4.F, 0});
+ points.push_back(Eigen::Vector2f{0, -2.F});
+
+ const Eigen::Vector2f radii(4, 2);
+
+ armarx::laser_scanner_feature_extraction::EnclosingEllipsoid ee(points);
+
+ // The computed ellipsoid must contain the GT ellipsoid
+ BOOST_CHECK_GE(ee.radii.x(), 4.F);
+ BOOST_CHECK_GE(ee.radii.y(), 2.F);
+
+ // ... but should not be too large
+ BOOST_CHECK_LE(ee.radii.x(), 4.1F);
+ BOOST_CHECK_LE(ee.radii.y(), 2.1F);
+
+ // The computed ellipsoid must be roughly at the position of the GT ellipsoid
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().x()), 0.1);
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().y()), 0.1);
+}
--
GitLab
From 06df98298a8ed09678e11993eecd094677394d66 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 6 Mar 2023 18:13:19 +0100
Subject: [PATCH 12/55] Fix includes
---
.../laser_scanner_feature_extraction/ArVizDrawer.h | 2 +-
.../laser_scanner_feature_extraction/Component.cpp | 4 ++--
.../laser_scanner_feature_extraction/Component.h | 7 +++----
.../laser_scanner_feature_extraction/FeatureExtractor.cpp | 6 +++---
.../laser_scanner_feature_extraction/FeatureExtractor.h | 2 +-
.../test/LaserScannerFeatureExtractionTest.cpp | 2 +-
6 files changed, 11 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
index 59045681..06dc0f0d 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -29,7 +29,7 @@
#include <RobotAPI/components/ArViz/Client/ScopedClient.h>
-#include "RobotComponents/components/LaserScannerFeatureExtraction/FeatureExtractor.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
namespace armarx::cartographer
{
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 399b1012..e1b38a2d 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -20,7 +20,7 @@
* GNU General Public License
*/
-#include "LaserScannerFeatureExtraction.h"
+#include "Component.h"
#include <algorithm>
#include <cmath>
@@ -43,7 +43,7 @@
#include "RobotAPI/libraries/armem_vision/types.h"
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
-#include "RobotComponents/components/LaserScannerFeatureExtraction/geometry.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index 214ccf97..e02f8972 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -37,10 +37,9 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h>
-#include "RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h"
-#include "RobotComponents/components/LaserScannerFeatureExtraction/ScanClustering.h"
-#include "RobotComponents/components/LaserScannerFeatureExtraction/geometry.h"
-#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include <RobotComponents/libraries/cartographer/MessageQueue.h>
#include <RobotComponents/libraries/cartographer/types.h>
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index 90949d11..96cdfac5 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -24,9 +24,9 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h>
-#include <RobotComponents/components/LaserScannerFeatureExtraction/EnclosingEllipsoid.h>
-#include <RobotComponents/components/LaserScannerFeatureExtraction/ScanClustering.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
#include "Path.h"
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
index 3dd5caac..f581c8e3 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -28,7 +28,7 @@
#include <VirtualRobot/MathTools.h>
#include "RobotAPI/libraries/armem_vision/types.h"
-#include "RobotComponents/components/LaserScannerFeatureExtraction/ChainApproximation.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
#include "RobotComponents/libraries/cartographer/types.h"
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
index fdad60e2..db413af6 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
@@ -21,7 +21,7 @@
*/
#include "ArmarXCore/core/logging/Logging.h"
-#include "RobotComponents/components/LaserScannerFeatureExtraction/FeatureExtractor.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
#include <boost/test/tools/old/interface.hpp>
#define BOOST_TEST_MODULE RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
--
GitLab
From dd49354d45e43eaea06dcf4ee24edde3c199c9cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 6 Mar 2023 18:50:16 +0100
Subject: [PATCH 13/55] Copy laser scanner features memory classes
---
.../armarx/navigation/memory/CMakeLists.txt | 10 +
.../memory/aron/LaserScannerFeatures.xml | 67 ++++++
.../navigation/memory/aron_conversions.cpp | 120 ++++++++++
.../navigation/memory/aron_conversions.h | 86 +++++++
.../client/laser_scanner_features/Reader.cpp | 218 ++++++++++++++++++
.../client/laser_scanner_features/Reader.h | 113 +++++++++
.../client/laser_scanner_features/Writer.cpp | 111 +++++++++
.../client/laser_scanner_features/Writer.h | 85 +++++++
source/armarx/navigation/memory/constants.h | 1 +
source/armarx/navigation/memory/types.h | 91 ++++++++
10 files changed, 902 insertions(+)
create mode 100644 source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
create mode 100644 source/armarx/navigation/memory/aron_conversions.cpp
create mode 100644 source/armarx/navigation/memory/aron_conversions.h
create mode 100644 source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
create mode 100644 source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
create mode 100644 source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
create mode 100644 source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
create mode 100644 source/armarx/navigation/memory/types.h
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index c03f9a2b..71e7aee4 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -1,6 +1,12 @@
+armarx_add_aron_library(memory_aron
+ ARON_FILES
+ aron/LaserScannerFeatures.xml
+)
+
armarx_add_library(memory
SOURCES
#./memory.cpp
+ aron_conversions.cpp
client/stack_result/Writer.cpp
client/stack_result/Reader.cpp
client/parameterization/Reader.cpp
@@ -15,6 +21,7 @@ armarx_add_library(memory
# ./client/events/Reader.cpp
HEADERS
memory.h
+ aron_conversions.h
client/stack_result/Writer.h
client/stack_result/Reader.h
client/parameterization/Reader.h
@@ -25,8 +32,11 @@ armarx_add_library(memory
client/costmap/Reader.h
client/human/Reader.h
client/human/Writer.h
+ client/laser_scanner_features/Reader.h
+ client/laser_scanner_features/Writer.h
client/rooms/Reader.h
# ./client/events/Reader.h
+ types.h
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
diff --git a/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
new file mode 100644
index 00000000..32600d7d
--- /dev/null
+++ b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
@@ -0,0 +1,67 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::vision::arondto::Ellipsoid'>
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='radii'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::vision::arondto::Circle'>
+ <ObjectChild key='center'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
+ <ObjectChild key="convexHull">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ <ObjectChild key="circle">
+ <armarx::armem::vision::arondto::Circle/>
+ </ObjectChild>
+ <ObjectChild key="ellipsoid">
+ <armarx::armem::vision::arondto::Ellipsoid/>
+ </ObjectChild>
+ <!-- <ObjectChild key="chain">
+ <armarx::armem::vision::arondto::Chain/>
+ </ObjectChild> -->
+ <ObjectChild key="points">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
+ <ObjectChild key="frame">
+ <String/>
+ </ObjectChild>
+ <ObjectChild key="frameGlobalPose">
+ <Pose/>
+ </ObjectChild>
+ <ObjectChild key="features">
+ <List>
+ <armarx::armem::vision::arondto::LaserScannerFeature />
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/memory/aron_conversions.cpp b/source/armarx/navigation/memory/aron_conversions.cpp
new file mode 100644
index 00000000..814ec546
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.cpp
@@ -0,0 +1,120 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "types.h"
+
+
+namespace armarx::armem::vision
+{
+
+
+ /************ toAron ************/
+
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
+
+ int64_t
+ toAron(const armem::Time& timestamp)
+ {
+ return timestamp.toMicroSecondsSinceEpoch();
+ }
+
+
+ void
+ toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.pose, bo.pose);
+ aron::toAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+ void
+ fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.pose, bo.pose);
+ aron::fromAron(dto.resolution, bo.resolution);
+ // bo.grid is NdArray -> need special handling.
+ }
+
+ // LaserScannerFeatures
+
+
+ void
+ toAron(arondto::Circle& dto, const Circle& bo)
+ {
+ dto.center = bo.center;
+ dto.radius = bo.radius;
+ }
+
+ void
+ toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
+ {
+ dto.globalPose = bo.pose.matrix();
+ dto.radii = bo.radii;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
+ {
+ toAron(dto.circle, bo.circle);
+ dto.convexHull = bo.convexHull;
+ toAron(dto.ellipsoid, bo.ellipsoid);
+ dto.points = bo.points;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::toAron(dto.features, bo.features);
+ }
+
+ void
+ fromAron(const arondto::Circle& dto, Circle& bo)
+ {
+ bo.center = dto.center;
+ bo.radius = dto.radius;
+ }
+ void
+ fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
+ {
+ bo.pose = dto.globalPose;
+ bo.radii = dto.radii;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
+ {
+ bo.convexHull = dto.convexHull;
+ fromAron(dto.circle, bo.circle);
+ fromAron(dto.ellipsoid, bo.ellipsoid);
+
+ // bo.chain = dto.chain;
+ bo.points = dto.points;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::fromAron(dto.features, bo.features);
+ }
+
+} // namespace armarx::armem::vision
diff --git a/source/armarx/navigation/memory/aron_conversions.h b/source/armarx/navigation/memory/aron_conversions.h
new file mode 100644
index 00000000..bb1e5253
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.h
@@ -0,0 +1,86 @@
+/*
+ * 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 <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::armem::vision
+{
+
+ namespace arondto
+ {
+ struct LaserScanStamped;
+ } // namespace arondto
+
+ // struct LaserScan;
+ struct LaserScanStamped;
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName);
+
+ template <typename T>
+ auto
+ fromAron(const aron::data::NDArrayPtr& navigator)
+ {
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
+ }
+
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
+
+ void toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScan);
+
+ inline aron::data::NDArrayPtr
+ toAron(const LaserScan& laserScan)
+ {
+ using aron::converter::AronVectorConverter;
+ return AronVectorConverter::ConvertFromVector(laserScan);
+ }
+
+ // OccupancyGrid
+ void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+ void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+ inline aron::data::NDArrayPtr
+ toAron(const OccupancyGrid::Grid& grid)
+ {
+ return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+ }
+
+ // LaserScannerFeatures
+ void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
+ void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
+
+} // namespace armarx::armem::vision
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
new file mode 100644
index 00000000..216088b4
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -0,0 +1,218 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+// RobotAPI Armem
+#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/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Reader::~Reader() = default;
+
+
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+ // registerPropertyDefinitions(def);
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ try
+ {
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ qb.coreSegments()
+ .withName(properties.coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName)
+ .entities()
+ .all()
+ .snapshots()
+ .beforeOrAtTime(query.timestamp);
+
+ // auto entitySel = [&]()
+ // {
+ // if (query.name.empty())
+ // {
+ // ARMARX_INFO << "querying all entities";
+ // return sel.entities().all();
+ // }
+ // return sel.entities().withName(query.name);
+ // }();
+
+ // entitySel.snapshots().beforeOrAtTime(query.timestamp);
+
+ return qb;
+ }
+
+ std::vector<LaserScannerFeatures>
+ asFeaturesList(const wm::ProviderSegment& providerSegment)
+ {
+ if (providerSegment.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ return {};
+ }
+
+ // const auto convert = [](const auto& aronLaserScanStamped,
+ // const wm::EntityInstance& ei) -> LaserScanStamped
+ // {
+ // LaserScanStamped laserScanStamped;
+ // fromAron(aronLaserScanStamped, laserScanStamped);
+
+ // const auto ndArrayNavigator =
+ // aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
+
+ // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
+ // ARMARX_IMPORTANT << "4";
+
+ // return laserScanStamped;
+ // };
+
+ std::vector<LaserScannerFeatures> laserScannerFeatures;
+
+ // loop over all entities and their snapshots
+ providerSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << entity.id();
+ }
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
+ {
+ LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
+ fromAron(o.value(), f);
+ }
+ return true;
+ });
+ return true;
+ });
+
+ return laserScannerFeatures;
+ }
+
+ Reader::Result
+ Reader::queryData(const Query& query) const
+ {
+ const auto qb = buildQuery(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 {.features = {},
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const wm::ProviderSegment& providerSegment =
+ qResult.memory.getCoreSegment(properties.coreSegmentName)
+ .getProviderSegment(query.providerName);
+
+ const auto features = asFeaturesList(providerSegment);
+
+ // const auto laserScans = asLaserScans(providerSegment);
+ // std::vector<std::string> sensors;
+ // providerSegment.forEachEntity(
+ // [&sensors](const wm::Entity& entity)
+ // {
+ // sensors.push_back(entity.name());
+ // return true;
+ // });
+
+ return {.features = features,
+ // .sensors = sensors,
+ .status = Result::Status::Success,
+ .errorMessage = ""};
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
new file mode 100644
index 00000000..4752ca85
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -0,0 +1,113 @@
+/*
+ * 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 <string>
+#include <vector>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+
+ /**
+ * @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 Reader
+ {
+ public:
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
+
+ void connect();
+
+ struct Query
+ {
+ std::string providerName;
+
+ std::string name; // sensor name
+
+ armarx::core::time::DateTime timestamp;
+
+ // std::vector<std::string> sensorList;
+ };
+
+ struct Result
+ {
+ std::vector<LaserScannerFeatures> features;
+
+ // std::vector<std::string> sensors;
+
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+
+ std::string errorMessage;
+ };
+
+ Result queryData(const Query& query) const;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+
+ protected:
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Reader memoryReader;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
new file mode 100644
index 00000000..0b24561d
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -0,0 +1,111 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/constants.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ // def->optional(properties.coreSegmentName,
+ // prefix + "CoreSegment",
+ // "Name of the mapping memory core segment to use.");
+
+ // def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
+ << "' ...";
+ try
+ {
+ memoryWriter =
+ memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
+ }
+
+ bool
+ Writer::store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ // const auto result = memoryWriter.addSegment(constants::memoryName,
+ // constants::laserScannerFeaturesCoreSegment);
+
+ // if (not result.success)
+ // {
+ // ARMARX_ERROR << result.errorMessage;
+
+ // // TODO(fabian.reister): message
+ // return false;
+ // }
+
+ const auto entityID = armem::MemoryID()
+ .withMemoryName(constants::memoryName)
+ .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+ .withProviderSegmentName(providerName)
+ .withEntityName(features.frame)
+ .withTimestamp(timestamp);
+
+ ARMARX_VERBOSE << "Memory id is " << entityID.str();
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ ARMARX_TRACE;
+
+ arondto::LaserScannerFeatures dto;
+ toAron(dto, features);
+
+ ARMARX_TRACE;
+
+ update.instancesData = {dto.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+
+ ARMARX_TRACE;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
new file mode 100644
index 00000000..ca580057
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
@@ -0,0 +1,85 @@
+/*
+ * 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 <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ /**
+ * @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
+ {
+ public:
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
+
+ void connect();
+
+ // MappingDataWriterInterface
+ /// to be called in Component::onConnectComponent
+ // void connect() override;
+
+ /// to be called in Component::addPropertyDefinitions
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+
+ bool store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp);
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Writer memoryWriter;
+
+ // Properties
+ struct Properties
+ {
+ // std::string memoryName = "Vision";
+ // std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 177f9730..56538fb7 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,6 +33,7 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
+ inline const std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
diff --git a/source/armarx/navigation/memory/types.h b/source/armarx/navigation/memory/types.h
new file mode 100644
index 00000000..fe584e3d
--- /dev/null
+++ b/source/armarx/navigation/memory/types.h
@@ -0,0 +1,91 @@
+/*
+ * 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 <vector>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+
+namespace armarx::armem::vision
+{
+
+
+ // template<typename _ValueT = float>
+ struct OccupancyGrid
+ {
+ float resolution; // [mm]
+
+ std::string frame;
+ Eigen::Affine3f pose;
+
+ // using ValueType = _ValueT;
+ using CellType = float;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+
+ Grid grid;
+ };
+
+ struct Ellipsoid
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+
+ Eigen::Vector2f radii = Eigen::Vector2f::Zero();
+ };
+
+ struct Circle
+ {
+ Eigen::Vector2f center = Eigen::Vector2f::Zero();
+ float radius = 0.F;
+ };
+
+ struct LaserScannerFeature
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ Points convexHull;
+
+ Circle circle;
+ Ellipsoid ellipsoid;
+
+ Chain chain;
+
+ Points points;
+ };
+
+ struct LaserScannerFeatures
+ {
+ // TODO(fabian.reister): framed pose
+ std::string frame;
+ Eigen::Isometry3f frameGlobalPose;
+
+ std::vector<LaserScannerFeature> features;
+
+
+ // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
+
+} // namespace armarx::armem::vision
--
GitLab
From 14dcafba1e8e4eaeff8e0dc5298af4db6109f50c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 05:56:40 +0100
Subject: [PATCH 14/55] Adjust memory
Change to proper namespace and fix resulting conflicts.
---
.../armarx/navigation/memory/CMakeLists.txt | 3 ++
.../memory/aron/LaserScannerFeatures.xml | 16 +++---
.../navigation/memory/aron_conversions.cpp | 36 ++-----------
.../navigation/memory/aron_conversions.h | 53 ++-----------------
.../navigation/memory/client/human/Writer.h | 5 +-
.../client/laser_scanner_features/Reader.cpp | 39 +++++++-------
.../client/laser_scanner_features/Reader.h | 6 +--
.../client/laser_scanner_features/Writer.cpp | 40 +++++++-------
.../client/laser_scanner_features/Writer.h | 9 ++--
source/armarx/navigation/memory/types.h | 20 +------
10 files changed, 71 insertions(+), 156 deletions(-)
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 71e7aee4..c3910e06 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -17,6 +17,8 @@ armarx_add_library(memory
client/costmap/Reader.cpp
client/human/Reader.cpp
client/human/Writer.cpp
+ client/laser_scanner_features/Reader.cpp
+ client/laser_scanner_features/Writer.cpp
client/rooms/Reader.cpp
# ./client/events/Reader.cpp
HEADERS
@@ -41,6 +43,7 @@ armarx_add_library(memory
ArmarXCoreInterfaces
ArmarXCore
armem
+ armarx_navigation::memory_aron
armarx_navigation::core
armarx_navigation::algorithms
armarx_navigation::graph
diff --git a/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
index 32600d7d..f78dea48 100644
--- a/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
+++ b/source/armarx/navigation/memory/aron/LaserScannerFeatures.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::vision::arondto::Ellipsoid'>
+ <Object name='armarx::navigation::memory::arondto::Ellipsoid'>
<ObjectChild key='globalPose'>
<Pose />
</ObjectChild>
@@ -17,7 +17,7 @@
</ObjectChild>
</Object>
- <Object name='armarx::armem::vision::arondto::Circle'>
+ <Object name='armarx::navigation::memory::arondto::Circle'>
<ObjectChild key='center'>
<Matrix rows="2" cols="1" type="float32" />
</ObjectChild>
@@ -26,20 +26,20 @@
</ObjectChild>
</Object>
- <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeature">
<ObjectChild key="convexHull">
<List>
<Matrix rows="2" cols="1" type="float32" />
</List>
</ObjectChild>
<ObjectChild key="circle">
- <armarx::armem::vision::arondto::Circle/>
+ <armarx::navigation::memory::arondto::Circle/>
</ObjectChild>
<ObjectChild key="ellipsoid">
- <armarx::armem::vision::arondto::Ellipsoid/>
+ <armarx::navigation::memory::arondto::Ellipsoid/>
</ObjectChild>
<!-- <ObjectChild key="chain">
- <armarx::armem::vision::arondto::Chain/>
+ <armarx::navigation::memory::arondto::Chain/>
</ObjectChild> -->
<ObjectChild key="points">
<List>
@@ -48,7 +48,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeatures">
<ObjectChild key="frame">
<String/>
</ObjectChild>
@@ -57,7 +57,7 @@
</ObjectChild>
<ObjectChild key="features">
<List>
- <armarx::armem::vision::arondto::LaserScannerFeature />
+ <armarx::navigation::memory::arondto::LaserScannerFeature />
</List>
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/memory/aron_conversions.cpp b/source/armarx/navigation/memory/aron_conversions.cpp
index 814ec546..bfc55c27 100644
--- a/source/armarx/navigation/memory/aron_conversions.cpp
+++ b/source/armarx/navigation/memory/aron_conversions.cpp
@@ -4,18 +4,14 @@
#include <cstdint>
#include <iterator>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
#include "types.h"
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
-
-namespace armarx::armem::vision
+namespace armarx::navigation::memory
{
@@ -26,34 +22,9 @@ namespace armarx::armem::vision
// aronLaserScan.scan = toAron(laserScan);
// }
- int64_t
- toAron(const armem::Time& timestamp)
- {
- return timestamp.toMicroSecondsSinceEpoch();
- }
-
-
- void
- toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
- {
- aron::toAron(dto.frame, bo.frame);
- aron::toAron(dto.pose, bo.pose);
- aron::toAron(dto.resolution, bo.resolution);
- // bo.grid is NdArray -> need special handling.
- }
-
- void
- fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
- {
- aron::fromAron(dto.frame, bo.frame);
- aron::fromAron(dto.pose, bo.pose);
- aron::fromAron(dto.resolution, bo.resolution);
- // bo.grid is NdArray -> need special handling.
- }
// LaserScannerFeatures
-
void
toAron(arondto::Circle& dto, const Circle& bo)
{
@@ -91,6 +62,7 @@ namespace armarx::armem::vision
bo.center = dto.center;
bo.radius = dto.radius;
}
+
void
fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
{
@@ -117,4 +89,4 @@ namespace armarx::armem::vision
aron::fromAron(dto.features, bo.features);
}
-} // namespace armarx::armem::vision
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/memory/aron_conversions.h b/source/armarx/navigation/memory/aron_conversions.h
index bb1e5253..0ad52e46 100644
--- a/source/armarx/navigation/memory/aron_conversions.h
+++ b/source/armarx/navigation/memory/aron_conversions.h
@@ -21,66 +21,23 @@
#pragma once
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-namespace armarx::armem::vision
+namespace armarx::navigation::memory
{
- namespace arondto
- {
- struct LaserScanStamped;
- } // namespace arondto
-
- // struct LaserScan;
- struct LaserScanStamped;
-
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName);
-
- template <typename T>
- auto
- fromAron(const aron::data::NDArrayPtr& navigator)
- {
- return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
- }
+ struct LaserScannerFeatures;
- void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
- void toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScan);
-
- inline aron::data::NDArrayPtr
- toAron(const LaserScan& laserScan)
- {
- using aron::converter::AronVectorConverter;
- return AronVectorConverter::ConvertFromVector(laserScan);
- }
-
- // OccupancyGrid
- void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
- void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
-
- inline aron::data::NDArrayPtr
- toAron(const OccupancyGrid::Grid& grid)
+ namespace arondto
{
- return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+ struct LaserScannerFeatures;
}
// LaserScannerFeatures
void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
-} // namespace armarx::armem::vision
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/memory/client/human/Writer.h b/source/armarx/navigation/memory/client/human/Writer.h
index b224149c..3a55ffc4 100644
--- a/source/armarx/navigation/memory/client/human/Writer.h
+++ b/source/armarx/navigation/memory/client/human/Writer.h
@@ -25,7 +25,6 @@
#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>
@@ -51,12 +50,12 @@ namespace armarx::navigation::memory::client::human
~Writer() override;
bool store(const armarx::navigation::human::Humans& humans,
- // const std::string& name,
+ // const std::string& name,
const std::string& providerName,
const armem::Time& timestamp);
bool store(const armarx::navigation::human::HumanGroups& groups,
- // const std::string& name,
+ // const std::string& name,
const std::string& providerName,
const armem::Time& timestamp);
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index 216088b4..c7640bdc 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -28,10 +28,11 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
// RobotAPI Aron
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/core/Exception.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+
// RobotAPI Armem
#include <RobotAPI/libraries/armem/client/Query.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -41,19 +42,18 @@
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
+#include <armarx/navigation/memory/aron_conversions.h>
-namespace armarx::armem::vision::laser_scanner_features::client
+namespace armarx::navigation::memory::client::laser_scanner_features
{
Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
{
}
- Reader::~Reader() = default;
+ Reader::~Reader() = default;
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -79,7 +79,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
try
{
memoryReader =
- memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ memoryNameSystem.useReader(armem::MemoryID().withMemoryName(properties.memoryName));
ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
<< "'";
}
@@ -96,13 +96,13 @@ namespace armarx::armem::vision::laser_scanner_features::client
armarx::armem::client::query::Builder qb;
qb.coreSegments()
- .withName(properties.coreSegmentName)
- .providerSegments()
- .withName(query.providerName)
- .entities()
- .all()
- .snapshots()
- .beforeOrAtTime(query.timestamp);
+ .withName(properties.coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName)
+ .entities()
+ .all()
+ .snapshots()
+ .beforeOrAtTime(query.timestamp);
// auto entitySel = [&]()
// {
@@ -120,7 +120,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
}
std::vector<LaserScannerFeatures>
- asFeaturesList(const wm::ProviderSegment& providerSegment)
+ asFeaturesList(const armem::wm::ProviderSegment& providerSegment)
{
if (providerSegment.empty())
{
@@ -149,7 +149,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
// loop over all entities and their snapshots
providerSegment.forEachEntity(
- [&](const wm::Entity& entity)
+ [&](const armem::wm::Entity& entity)
{
// If we don't need this warning, we could directly iterate over the snapshots.
if (entity.empty())
@@ -159,9 +159,10 @@ namespace armarx::armem::vision::laser_scanner_features::client
ARMARX_DEBUG << "History size: " << entity.size();
entity.forEachInstance(
- [&](const wm::EntityInstance& entityInstance)
+ [&](const armem::wm::EntityInstance& entityInstance)
{
- if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
+ if (const auto o =
+ armem::tryCast<arondto::LaserScannerFeatures>(entityInstance))
{
LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
fromAron(o.value(), f);
@@ -194,7 +195,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
}
// now create result from memory
- const wm::ProviderSegment& providerSegment =
+ const armem::wm::ProviderSegment& providerSegment =
qResult.memory.getCoreSegment(properties.coreSegmentName)
.getProviderSegment(query.providerName);
@@ -215,4 +216,4 @@ namespace armarx::armem::vision::laser_scanner_features::client
.errorMessage = ""};
}
-} // namespace armarx::armem::vision::laser_scanner_features::client
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
index 4752ca85..14cca12a 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -31,15 +31,15 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
+#include <armarx/navigation/memory/types.h>
namespace armarx
{
class ManagedIceObject;
}
-namespace armarx::armem::vision::laser_scanner_features::client
+namespace armarx::navigation::memory::client::laser_scanner_features
{
@@ -110,4 +110,4 @@ namespace armarx::armem::vision::laser_scanner_features::client
const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
};
-} // namespace armarx::armem::vision::laser_scanner_features::client
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
index 0b24561d..5e453ea7 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -1,21 +1,20 @@
#include "Writer.h"
-#include "RobotAPI/libraries/armem_vision/constants.h"
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
-#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+#include <armarx/navigation/memory/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
-namespace armarx::armem::vision::laser_scanner_features::client
+namespace armarx::navigation::memory::client::laser_scanner_features
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
{
}
- Writer::~Writer() = default;
+ Writer::~Writer() = default;
void
Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
@@ -35,14 +34,14 @@ namespace armarx::armem::vision::laser_scanner_features::client
Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
- << "' ...";
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
+ << constants::NavigationMemoryName << "' ...";
try
{
- memoryWriter =
- memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
- << "'";
+ memoryWriter = memoryNameSystem.useWriter(
+ armem::MemoryID().withMemoryName(constants::NavigationMemoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '"
+ << constants::NavigationMemoryName << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -50,13 +49,14 @@ namespace armarx::armem::vision::laser_scanner_features::client
return;
}
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
+ << constants::NavigationMemoryName;
}
bool
Writer::store(const LaserScannerFeatures& features,
const std::string& providerName,
- const Time& timestamp)
+ const armem::Time& timestamp)
{
std::lock_guard g{memoryWriterMutex};
@@ -72,11 +72,11 @@ namespace armarx::armem::vision::laser_scanner_features::client
// }
const auto entityID = armem::MemoryID()
- .withMemoryName(constants::memoryName)
- .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
- .withProviderSegmentName(providerName)
- .withEntityName(features.frame)
- .withTimestamp(timestamp);
+ .withMemoryName(constants::NavigationMemoryName)
+ .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+ .withProviderSegmentName(providerName)
+ .withEntityName(features.frame)
+ .withTimestamp(timestamp);
ARMARX_VERBOSE << "Memory id is " << entityID.str();
@@ -108,4 +108,4 @@ namespace armarx::armem::vision::laser_scanner_features::client
return updateResult.success;
}
-} // namespace armarx::armem::vision::laser_scanner_features::client
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
index ca580057..30f42933 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
@@ -26,13 +26,12 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include "RobotAPI/libraries/armem_vision/types.h"
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <armarx/navigation/memory/types.h>
-namespace armarx::armem::vision::laser_scanner_features::client
+namespace armarx::navigation::memory::client::laser_scanner_features
{
/**
@@ -64,7 +63,7 @@ namespace armarx::armem::vision::laser_scanner_features::client
bool store(const LaserScannerFeatures& features,
const std::string& providerName,
- const Time& timestamp);
+ const armem::Time& timestamp);
private:
armem::client::MemoryNameSystem& memoryNameSystem;
@@ -82,4 +81,4 @@ namespace armarx::armem::vision::laser_scanner_features::client
const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
};
-} // namespace armarx::armem::vision::laser_scanner_features::client
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/types.h b/source/armarx/navigation/memory/types.h
index fe584e3d..73935f91 100644
--- a/source/armarx/navigation/memory/types.h
+++ b/source/armarx/navigation/memory/types.h
@@ -28,25 +28,9 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx::armem::vision
+namespace armarx::navigation::memory
{
-
- // template<typename _ValueT = float>
- struct OccupancyGrid
- {
- float resolution; // [mm]
-
- std::string frame;
- Eigen::Affine3f pose;
-
- // using ValueType = _ValueT;
- using CellType = float;
- using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
-
- Grid grid;
- };
-
struct Ellipsoid
{
Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
@@ -88,4 +72,4 @@ namespace armarx::armem::vision
};
-} // namespace armarx::armem::vision
+} // namespace armarx::navigation::memory
--
GitLab
From 1b3720e0e2e0515db3b02947e1d15c985881aa96 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 06:08:25 +0100
Subject: [PATCH 15/55] Add core segment to navigation memory
---
.../navigation/components/navigation_memory/Component.cpp | 6 ++++++
.../navigation/components/navigation_memory/Component.h | 2 ++
source/armarx/navigation/memory/constants.h | 2 +-
3 files changed, 9 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index 22f674ca..e7f220c1 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -55,6 +55,7 @@
#include <armarx/navigation/human/aron/Human.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/rooms/aron/Room.aron.generated.h>
@@ -153,6 +154,11 @@ namespace armarx::navigation::components::navigation_memory
workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
navigation::human::arondto::Human::ToAronType());
+ workingMemory()
+ .addCoreSegment(memory::constants::LaserScannerFeaturesCoreSegment,
+ navigation::memory::arondto::LaserScannerFeatures::ToAronType())
+ .setMaxHistorySize(properties.laserScannerFeaturesMaxHistorySize);
+
// workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
// navigation::human::arondto::Human::ToAronType());
workingMemory().addCoreSegment(navigation::rooms::coreSegmentID.coreSegmentName,
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index 77220afe..bc359652 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -99,6 +99,8 @@ namespace armarx::navigation::components::navigation_memory
{
std::string snapshotToLoad = "";
+ long laserScannerFeaturesMaxHistorySize = 20;
+
struct LocationGraph
{
bool visuLocations = true;
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 56538fb7..0df19cc8 100644
--- a/source/armarx/navigation/memory/constants.h
+++ b/source/armarx/navigation/memory/constants.h
@@ -33,7 +33,7 @@ namespace armarx::navigation::memory::constants
inline const std::string LocationCoreSegmentName = "Location";
inline const std::string CostmapCoreSegmentName = "Costmap";
inline const std::string HumanCoreSegmentName = "Human";
- inline const std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
+ inline const std::string LaserScannerFeaturesCoreSegment = "LaserScannerFeatures";
inline const std::string GlobalPlannerResultCoreSegment = "Results_GlobalPlanner";
inline const std::string LocalPlannerResultCoreSegment = "Results_LocalPlanner";
--
GitLab
From 45fcec89fbc13fd8f6ed2e8b9d8a9ef1fa51cbae Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 07:09:47 +0100
Subject: [PATCH 16/55] Fix HumanTracker
---
source/armarx/navigation/human/HumanTracker.h | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 1f27aed7..32c25cc4 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -24,7 +24,6 @@
#include <ArmarXCore/core/time.h>
-#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
#include <VisionX/libraries/armem_human/types.h>
@@ -33,11 +32,11 @@
#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::human
{
- using Cluster = armem::vision::LaserScannerFeature;
-
+ using Cluster = memory::LaserScannerFeature;
/**
* @brief The HumanTracker class can be used to track and filter multiple humans. It hides
@@ -115,6 +114,7 @@ namespace armarx::navigation::human
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
};
+
/**
* @brief HumanTracker::update Updates the tracked humans with the human measurements from a camera. When a
* measurement is close enough to an existing tracked human, they are associated, otherwise a
--
GitLab
From 80f124d1332164d37de9d5166515f942cb9b3a77 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 07:18:01 +0100
Subject: [PATCH 17/55] Fix includes of
dynamic_distance_to_obstacle_costmap_provider
---
.../Component.cpp | 19 +++----------------
.../Component.h | 15 +++++++--------
2 files changed, 10 insertions(+), 24 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
index ab0d5810..5bab4b0d 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp
@@ -35,13 +35,12 @@
#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
-#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
-#include <armarx/navigation/util/geometry.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
+#include <armarx/navigation/util/geometry.h>
// Include headers you only need in function definitions in the .cpp.
@@ -55,7 +54,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
const std::string Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
-
armarx::PropertyDefinitionsPtr
Component::createPropertyDefinitions()
{
@@ -97,7 +95,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
return def;
}
-
Component::Component() :
robotReader(memoryNameSystem()),
costmapReader(memoryNameSystem()),
@@ -116,7 +113,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// setDebugObserverBatchModeEnabled(true);
}
-
void
Component::onConnectComponent()
{
@@ -191,11 +187,8 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
// return false;
}
-
-
-
void
- fillCostmap(const std::vector<armem::vision::LaserScannerFeatures>& features,
+ fillCostmap(const std::vector<memory::LaserScannerFeatures>& features,
algorithms::Costmap& costmap)
{
@@ -257,7 +250,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
}
}
-
void
Component::drawCostmap(const armarx::navigation::algorithms::Costmap& costmap,
const std::string& name,
@@ -325,7 +317,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
const auto timestamp = armarx::core::time::Clock::Now();
- const armem::vision::laser_scanner_features::client::Reader::Query query{
+ const memory::client::laser_scanner_features::Reader::Query query{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
.timestamp = timestamp};
@@ -405,33 +397,28 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
return navigationPlanningCostmap;
}
-
void
Component::onDisconnectComponent()
{
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
return Component::defaultName;
}
-
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
diff --git a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
index 1837d8ac..92179ba8 100644
--- a/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.h
@@ -23,19 +23,18 @@
#pragma once
-#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
-#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
-#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
+#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/costmap/Writer.h>
-#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
-
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
{
@@ -114,7 +113,6 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
static const std::string defaultName;
-
// Private member variables go here.
@@ -141,6 +139,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
int updatePeriodMs = 100;
};
+
Properties properties;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
@@ -172,7 +171,7 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
memory::client::costmap::Reader costmapReader;
memory::client::costmap::Writer costmapWriter;
- armem::vision::laser_scanner_features::client::Reader laserScannerFeaturesReader;
+ memory::client::laser_scanner_features::Reader laserScannerFeaturesReader;
std::optional<algorithms::Costmap> staticCostmap;
--
GitLab
From 3d337cf7ca7ab6ff761e983a5ead6e87ff2153f2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 08:00:00 +0100
Subject: [PATCH 18/55] Adjust laser_scanner_feature_extraction for new layout
---
CMakeLists.txt | 4 +
.../navigation/components/CMakeLists.txt | 2 +-
.../dynamic_scene_provider/Component.h | 11 +-
.../ArVizDrawer.cpp | 162 ++++++++++--------
.../ArVizDrawer.h | 8 +-
.../CMakeLists.txt | 9 +-
.../Component.cpp | 58 ++-----
.../Component.h | 27 +--
.../EnclosingEllipsoid.cpp | 5 +-
.../EnclosingEllipsoid.h | 11 +-
.../FeatureExtractor.cpp | 12 +-
.../FeatureExtractor.h | 15 +-
.../laser_scanner_feature_extraction/Path.cpp | 8 +-
.../laser_scanner_feature_extraction/Path.h | 4 +-
.../ScanClustering.cpp | 33 ++--
.../ScanClustering.h | 14 +-
.../client/laser_scanner_features/Writer.cpp | 4 +-
17 files changed, 190 insertions(+), 197 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 494d9723..a79cf08d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -46,6 +46,10 @@ armarx_find_package(PUBLIC RVO QUIET)
armarx_find_package(PUBLIC teb_local_planner QUIET)
armarx_find_package(PUBLIC teb_extension QUIET)
+# laser scanner feature extraction
+armarx_find_package(PUBLIC RobotComponents QUIET)
+armarx_find_package(PUBLIC PCL QUIET COMPONENTS io common)
+
add_subdirectory(etc)
# FetchContent_Declare(
diff --git a/source/armarx/navigation/components/CMakeLists.txt b/source/armarx/navigation/components/CMakeLists.txt
index 651c7ef6..e65951f6 100644
--- a/source/armarx/navigation/components/CMakeLists.txt
+++ b/source/armarx/navigation/components/CMakeLists.txt
@@ -22,4 +22,4 @@ add_subdirectory(human_simulator)
add_subdirectory(navigation_skill_provider)
-add_subdirectory(laser_scanner_feature_extraction)
\ No newline at end of file
+add_subdirectory(laser_scanner_feature_extraction)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 11e0d654..c70b0f66 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -34,10 +34,11 @@
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
-#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
+
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -47,11 +48,10 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
+#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
#include <armarx/navigation/human/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>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -123,7 +123,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
private:
static const std::string defaultName;
-
// Private member variables go here.
@@ -138,7 +137,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::string name = ""; // all
} laserScannerFeatures;
-
struct
{
std::string name = "Armar6";
@@ -155,6 +153,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
std::string humanPoseProvider = "AzureKinectPointCloudProvider";
};
+
Properties properties;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
@@ -191,7 +190,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
ReaderWriterPlugin<armem::human::client::Reader>* humanPoseReaderPlugin = nullptr;
- ReaderWriterPlugin<armem::vision::laser_scanner_features::client::Reader>*
+ ReaderWriterPlugin<memory::client::laser_scanner_features::Reader>*
laserScannerFeaturesReaderPlugin = nullptr;
ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
index fe927c0e..3c30fd0f 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -27,12 +27,13 @@
#include "conversions/pcl.h"
#include "conversions/pcl_eigen.h"
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- void ArVizDrawer::draw(const std::vector<Features>& features,
- const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::draw(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
{
// drawCircles(features, frame, globalSensorPose);
drawConvexHulls(features, frame, globalSensorPose);
@@ -40,17 +41,19 @@ namespace armarx::laser_scanner_feature_extraction
drawChains(features, frame, globalSensorPose);
}
- void ArVizDrawer::draw(const cartographer::LaserScannerMessage& msg,
- const Eigen::Isometry3f& globalSensorPose, const simox::Color& color)
+ void
+ ArVizDrawer::draw(const cartographer::LaserScannerMessage& msg,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
{
auto layer = arviz.layer("points_" + msg.frame);
const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.scan));
layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
- .pointCloud(pointCloud, viz::Color(color))
- .pointSizeInPixels(5)
- .pose(globalSensorPose));
+ .pointCloud(pointCloud, viz::Color(color))
+ .pointSizeInPixels(5)
+ .pose(globalSensorPose));
arviz.commit(layer);
}
@@ -77,9 +80,9 @@ namespace armarx::laser_scanner_feature_extraction
globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), -1.F);
layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
- .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
- .position(position)
- .color(simox::Color::red(200, 100)));
+ .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
+ .position(position)
+ .color(simox::Color::red(200, 100)));
}
void
@@ -104,29 +107,33 @@ namespace armarx::laser_scanner_feature_extraction
arviz.commit(layer);
}
- void ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
- const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::drawConvexHulls(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
{
auto layer = arviz.layer("convex_hulls_" + frame);
std::for_each(features.begin(),
features.end(),
- [&](const Features & f)
- {
- if (not f.convexHull)
- {
- return;
- }
- drawConvexHull(layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
- });
+ [&](const Features& f)
+ {
+ if (not f.convexHull)
+ {
+ return;
+ }
+ drawConvexHull(
+ layer, *f.convexHull, globalSensorPose, simox::Color::red(100, 80));
+ });
arviz.commit(layer);
}
- void ArVizDrawer::draw(const std::string& layerName, const VirtualRobot::MathTools::ConvexHull2D& robotHull,
- const Eigen::Isometry3f& robotGlobalPose,
- const simox::Color& color)
+ void
+ ArVizDrawer::draw(const std::string& layerName,
+ const VirtualRobot::MathTools::ConvexHull2D& robotHull,
+ const Eigen::Isometry3f& robotGlobalPose,
+ const simox::Color& color)
{
auto layer = arviz.layer(layerName);
@@ -134,81 +141,86 @@ namespace armarx::laser_scanner_feature_extraction
arviz.commit(layer);
}
- void ArVizDrawer::drawConvexHull(viz::Layer& layer,
- const VirtualRobot::MathTools::ConvexHull2D& hull,
- const Eigen::Isometry3f& globalSensorPose,
- const simox::Color& color)
+ void
+ ArVizDrawer::drawConvexHull(viz::Layer& layer,
+ const VirtualRobot::MathTools::ConvexHull2D& hull,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
{
const auto points = conversions::to3D(hull.vertices);
layer.add(viz::Polygon("convex_hull_" + std::to_string(layer.size()))
- .points(points)
- .color(color)
- .pose(globalSensorPose));
+ .points(points)
+ .color(color)
+ .pose(globalSensorPose));
}
- void ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
- const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::drawEllipsoids(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
{
auto layer = arviz.layer("ellipsoids_" + frame);
std::for_each(features.begin(),
features.end(),
- [&](const Features & f)
- {
- if (not f.ellipsoid)
- {
- return;
- }
- drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
- });
+ [&](const Features& f)
+ {
+ if (not f.ellipsoid)
+ {
+ return;
+ }
+ drawEllipsoid(layer, *f.ellipsoid, globalSensorPose);
+ });
arviz.commit(layer);
}
- void ArVizDrawer::drawEllipsoid(viz::Layer& layer,
- const Ellipsoid& ellipsoid,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::drawEllipsoid(viz::Layer& layer,
+ const Ellipsoid& ellipsoid,
+ const Eigen::Isometry3f& globalSensorPose)
{
const Eigen::Isometry3f pose = globalSensorPose * ellipsoid.pose;
layer.add(viz::Ellipsoid("ellipsoid_" + std::to_string(layer.size()))
- .axisLengths(conversions::to3D(ellipsoid.radii))
- .pose(pose)
- .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
+ .axisLengths(conversions::to3D(ellipsoid.radii))
+ .pose(pose)
+ .color(simox::Color(255, 102, 0, 128))); // orange, but a bit more shiny
}
- void ArVizDrawer::drawChains(const std::vector<Features>& features,
- const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::drawChains(const std::vector<Features>& features,
+ const std::string& frame,
+ const Eigen::Isometry3f& globalSensorPose)
{
auto layer = arviz.layer("chains_" + frame);
std::for_each(features.begin(),
features.end(),
- [&](const Features & f)
- {
- if (not f.chain)
- {
- return;
- }
- drawChain(layer, *f.chain, globalSensorPose);
-
- // const auto ellipsoids = f.linesAsEllipsoids(50);
- // for (const auto& ellipsoid : ellipsoids)
- // {
- // drawEllipsoid(layer, ellipsoid, globalSensorPose);
- // }
- });
+ [&](const Features& f)
+ {
+ if (not f.chain)
+ {
+ return;
+ }
+ drawChain(layer, *f.chain, globalSensorPose);
+
+ // const auto ellipsoids = f.linesAsEllipsoids(50);
+ // for (const auto& ellipsoid : ellipsoids)
+ // {
+ // drawEllipsoid(layer, ellipsoid, globalSensorPose);
+ // }
+ });
arviz.commit(layer);
}
- void ArVizDrawer::drawChain(viz::Layer& layer,
- const Points& chain,
- const Eigen::Isometry3f& globalSensorPose)
+ void
+ ArVizDrawer::drawChain(viz::Layer& layer,
+ const Points& chain,
+ const Eigen::Isometry3f& globalSensorPose)
{
if (chain.size() < 2)
{
@@ -218,9 +230,9 @@ namespace armarx::laser_scanner_feature_extraction
const auto cloud = conversions::to3D(chain);
layer.add(viz::Path("chain_" + std::to_string(layer.size()))
- .points(cloud)
- .width(7.F)
- .color(viz::Color::blue())
- .pose(globalSensorPose));
+ .points(cloud)
+ .width(7.F)
+ .color(viz::Color::blue())
+ .pose(globalSensorPose));
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
index 06dc0f0d..ce0f1abb 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -36,14 +36,15 @@ namespace armarx::cartographer
struct LaserScannerMessage;
}
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
struct LineSegment2Df;
+
// struct Circle;
// struct Ellipsoid;
- class ArVizDrawer
+ class ArVizDrawer
{
public:
using Points = std::vector<Eigen::Vector2f>;
@@ -52,7 +53,6 @@ namespace armarx::laser_scanner_feature_extraction
{
}
-
void draw(const std::vector<Features>& features,
const std::string& frame,
const Eigen::Isometry3f& globalSensorPose);
@@ -106,4 +106,4 @@ namespace armarx::laser_scanner_feature_extraction
armarx::viz::Client arviz;
};
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index 8e5de1c1..aae16e62 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -31,12 +31,17 @@ armarx_add_component(laser_scanner_feature_extraction
# ArmarXCore
ArmarXCore
ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ ArmarXCoreInterfaces
+ ArmarXCoreLogging
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
## RobotAPICore
## RobotAPIInterfaces
RobotAPIComponentPlugins # For ArViz and other plugins.
- armem_vision
- RobotComponents::Cartographer
+ #armem_vision
+ #RobotComponents::Cartographer
+ armarx_navigation::memory
+ DEPENDENCIES_LEGACY
+ PCL
)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index e1b38a2d..5ac47033 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -40,17 +40,15 @@
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
-#include "RobotAPI/libraries/armem_vision/types.h"
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
-
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include "conversions/eigen.h"
#include "conversions/pcl_eigen.h"
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
armarx::PropertyDefinitionsPtr
@@ -59,12 +57,6 @@ namespace armarx::laser_scanner_feature_extraction
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
- // Publish to a topic (passing the TopicListenerPrx).
- def->topic(featuresTopic);
-
- // Subscribe to a topic (passing the topic name).
- def->topic<LaserScannerUnitListener>("LaserScans");
-
// // Add an optionalproperty.
def->optional(properties.queueSize,
"p.inputQueueSize",
@@ -234,10 +226,10 @@ namespace armarx::laser_scanner_feature_extraction
laserScannerFeatureWriter.connect();
}
- armem::vision::LaserScannerFeature
+ memory::LaserScannerFeature
toArmemFeature(const Features& features)
{
- armem::vision::LaserScannerFeature armemFeature;
+ memory::LaserScannerFeature armemFeature;
if (features.chain)
{
@@ -264,12 +256,12 @@ namespace armarx::laser_scanner_feature_extraction
return armemFeature;
}
- armem::vision::LaserScannerFeatures
+ memory::LaserScannerFeatures
toArmemFeatures(const std::vector<Features>& features,
const Eigen::Isometry3f& global_T_sensor,
const std::string& sensorFrame)
{
- armem::vision::LaserScannerFeatures armemFeatures;
+ memory::LaserScannerFeatures armemFeatures;
armemFeatures.frame = sensorFrame;
armemFeatures.frameGlobalPose = global_T_sensor;
@@ -479,31 +471,12 @@ namespace armarx::laser_scanner_feature_extraction
}
void
- LaserScannerFeatureExtraction::publishFeatures(
- const armem::vision::LaserScannerFeatures& features,
- const armem::Time& timestamp)
+ LaserScannerFeatureExtraction::publishFeatures(const memory::LaserScannerFeatures& features,
+ const armem::Time& timestamp)
{
// store in memory
laserScannerFeatureWriter.store(features, getName(), timestamp);
-
- // legacy - topic
- LineSegment2DChainSeq chains;
- for (const auto& feature : features.features)
- {
- if (not feature.chain.empty())
- {
- LineSegment2DChain chain;
- for (const auto& pt : feature.chain)
- {
- chain.push_back(
- conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
- }
- chains.push_back(chain);
- }
- }
-
- featuresTopic->reportExtractedLineSegments(chains);
}
void
@@ -523,14 +496,14 @@ namespace armarx::laser_scanner_feature_extraction
}
void
- LaserScannerFeatureExtraction::reportSensorValues(const std::string& device,
- const std::string& name,
- const LaserScan& scan,
- const TimestampBasePtr& timestamp,
- const Ice::Current& /*unused*/)
+ reportSensorValues(const std::string& device,
+ const std::string& name,
+ const LaserScan& scan,
+ const TimestampBasePtr& timestamp,
+ const Ice::Current& /*unused*/)
{
ARMARX_DEBUG << "Receiving data";
-
+ /*
laserMessageQueue.push(cartographer::LaserScannerMessage{
.frame = name, .scan = scan, .timestamp = timestamp->timestamp});
@@ -539,6 +512,7 @@ namespace armarx::laser_scanner_feature_extraction
frequencyReporterSubscribe->add(IceUtil::Time::now().toMicroSeconds());
setDebugObserverDatafield(name, scan.size());
+*/
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index e02f8972..664a8dbb 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -32,21 +32,20 @@
#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h"
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h>
-#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include <RobotComponents/libraries/cartographer/MessageQueue.h>
#include <RobotComponents/libraries/cartographer/types.h>
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
// class FeatureExtractor;
@@ -69,7 +68,6 @@ namespace armarx::laser_scanner_feature_extraction
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
,
virtual public armarx::ArVizComponentPluginUser,
- virtual public LaserScannerUnitListener,
virtual public RobotStateComponentPluginUser,
virtual public armem::ClientPluginUser
@@ -107,18 +105,11 @@ namespace armarx::laser_scanner_feature_extraction
void RemoteGui_update() override;
*/
- // LaserScannerUnitListener interface
- void reportSensorValues(const std::string& device,
- const std::string& name,
- const LaserScan& scan,
- const TimestampBasePtr& timestamp,
- const Ice::Current& /*unused*/) override;
-
private:
void onFeatures(const cartographer::LaserScannerMessage& data,
const std::vector<Features>& features);
- void publishFeatures(const armem::vision::LaserScannerFeatures& features,
+ void publishFeatures(const memory::LaserScannerFeatures& features,
const armem::Time& timestamp);
// Private methods go here.
@@ -215,13 +206,11 @@ namespace armarx::laser_scanner_feature_extraction
VirtualRobot::RobotPtr robot;
- LaserScannerFeaturesListenerPrx featuresTopic;
-
std::variant<std::monostate, VirtualRobot::MathTools::ConvexHull2DPtr, Eigen::Vector2f>
robotHull;
VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
- armem::vision::laser_scanner_features::client::Writer laserScannerFeatureWriter;
+ memory::client::laser_scanner_features::Writer laserScannerFeatureWriter;
};
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
index 87bf8313..eea10eeb 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
@@ -13,8 +13,7 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
-
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
// Eigen::Affine2f Ellipsoid::pose() const noexcept
@@ -121,4 +120,4 @@ namespace armarx::laser_scanner_feature_extraction
return true;
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
index 9628314b..277531e7 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h
@@ -23,9 +23,10 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include "RobotAPI/libraries/armem_vision/types.h"
-namespace armarx::laser_scanner_feature_extraction
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
// struct Ellipsoid
@@ -38,7 +39,7 @@ namespace armarx::laser_scanner_feature_extraction
// Eigen::Affine2f pose() const noexcept;
// };
- using Ellipsoid = armem::vision::Ellipsoid;
+ using Ellipsoid = memory::Ellipsoid;
/**
* @brief Minimum volume enclosing ellipsoid (MVEE) for a set of points.
@@ -49,7 +50,7 @@ namespace armarx::laser_scanner_feature_extraction
class EnclosingEllipsoid : public Ellipsoid
{
public:
- using Point = Eigen::Vector2f;
+ using Point = Eigen::Vector2f;
using Points = std::vector<Point>;
EnclosingEllipsoid(const Points& points);
@@ -67,4 +68,4 @@ namespace armarx::laser_scanner_feature_extraction
bool compute(const Points& points);
};
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index 96cdfac5..588cd302 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -24,9 +24,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
-#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
-#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
-#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
#include "Path.h"
@@ -35,9 +32,11 @@
#include "conversions/opencv_eigen.h"
#include "conversions/opencv_pcl.h"
#include "conversions/pcl_eigen.h"
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
-
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
// Features
@@ -126,7 +125,6 @@ namespace armarx::laser_scanner_feature_extraction
return clustering.detectClusters(scan);
}
-
std::optional<VirtualRobot::MathTools::ConvexHull2D>
FeatureExtractor::convexHull(const Points& points) const
{
@@ -236,4 +234,4 @@ namespace armarx::laser_scanner_feature_extraction
return chApprx.approximatedChain();
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
index f581c8e3..d2fa134f 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -26,23 +26,22 @@
#include <pcl/point_types.h>
#include <VirtualRobot/MathTools.h>
-#include "RobotAPI/libraries/armem_vision/types.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
#include "RobotComponents/libraries/cartographer/types.h"
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include "EnclosingEllipsoid.h"
#include "ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include <armarx/navigation/memory/types.h>
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- using Circle = armem::vision::Circle;
-
+ using Circle = memory::Circle;
struct Features
{
- using Points = std::vector<Eigen::Vector2f>;
+ using Points = std::vector<Eigen::Vector2f>;
using Chain = Points;
std::optional<VirtualRobot::MathTools::ConvexHull2D> convexHull;
@@ -59,7 +58,7 @@ namespace armarx::laser_scanner_feature_extraction
class FeatureExtractor
{
public:
- using Points = std::vector<Eigen::Vector2f>;
+ using Points = std::vector<Eigen::Vector2f>;
using Callback = std::function<void(const cartographer::LaserScannerMessage& data,
const std::vector<Features>& features)>;
@@ -88,4 +87,4 @@ namespace armarx::laser_scanner_feature_extraction
const Callback callback;
};
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
index 057ada4f..d57cae5c 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
@@ -1,10 +1,10 @@
#include "Path.h"
-
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- std::vector<Path::Segment> Path::segments() const noexcept
+ std::vector<Path::Segment>
+ Path::segments() const noexcept
{
if (points.size() <= 1)
{
@@ -21,4 +21,4 @@ namespace armarx::laser_scanner_feature_extraction
return segments;
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
index 8f63e260..2cf3a413 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.h
@@ -4,7 +4,7 @@
#include <Eigen/Core>
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
struct Path
@@ -16,4 +16,4 @@ namespace armarx::laser_scanner_feature_extraction
std::vector<Segment> segments() const noexcept;
};
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
index e0376c54..32503968 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
@@ -2,11 +2,14 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- ScanClustering::ScanClustering(const Params& params) : params(params) {}
+ ScanClustering::ScanClustering(const Params& params) : params(params)
+ {
+ }
- bool ScanClustering::add(const LaserScanStep& scanStep)
+ bool
+ ScanClustering::add(const LaserScanStep& scanStep)
{
if (scan.empty())
{
@@ -23,7 +26,8 @@ namespace armarx::laser_scanner_feature_extraction
return true;
}
- std::vector<LaserScan> ScanClustering::detectClusters(const LaserScan& scan)
+ std::vector<LaserScan>
+ ScanClustering::detectClusters(const LaserScan& scan)
{
const auto isInvalid = [&](const LaserScanStep& step) -> bool
{ return step.distance > params.maxDistance; };
@@ -48,11 +52,20 @@ namespace armarx::laser_scanner_feature_extraction
return clusters;
}
- const LaserScan& ScanClustering::cluster() const { return scan; }
+ const LaserScan&
+ ScanClustering::cluster() const
+ {
+ return scan;
+ }
- void ScanClustering::clear() { scan.clear(); }
+ void
+ ScanClustering::clear()
+ {
+ scan.clear();
+ }
- bool ScanClustering::supports(const LaserScanStep& scanStep)
+ bool
+ ScanClustering::supports(const LaserScanStep& scanStep)
{
// OK to create a new cluster if it's empty
if (scan.empty())
@@ -60,12 +73,12 @@ namespace armarx::laser_scanner_feature_extraction
return true;
}
- const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
+ const float distanceDiff = std::fabs(scanStep.distance - scan.back().distance);
const bool isWithinDistanceRange = distanceDiff < params.distanceThreshold;
- const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
+ const float angleDiff = std::fabs(scanStep.angle - scan.back().angle);
const bool isWithinAngleRange = angleDiff < params.angleThreshold;
return (isWithinDistanceRange and isWithinAngleRange);
}
-} // namespace armarx::laser_scanner_feature_extraction
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
index 6cee501d..1159c2c6 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h
@@ -23,7 +23,7 @@
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-namespace armarx::laser_scanner_feature_extraction
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
namespace detail
{
@@ -31,7 +31,7 @@ namespace armarx::laser_scanner_feature_extraction
{
// Clustering options to decide whether a point belongs to the cluster
float distanceThreshold; // [mm]
- float angleThreshold; // [rad]
+ float angleThreshold; // [rad]
/// Range filter: only consider points that are closer than maxDistance
float maxDistance; // [mm]
@@ -40,8 +40,8 @@ namespace armarx::laser_scanner_feature_extraction
class ScanClustering
{
- public:
- using Params = detail::ScanClusteringParams;
+ public:
+ using Params = detail::ScanClusteringParams;
using Clusters = std::vector<LaserScan>;
ScanClustering(const Params& params);
@@ -54,7 +54,7 @@ namespace armarx::laser_scanner_feature_extraction
*/
Clusters detectClusters(const LaserScan& scan);
- protected:
+ protected:
const LaserScan& cluster() const;
bool add(const LaserScanStep& scanStep);
@@ -62,10 +62,10 @@ namespace armarx::laser_scanner_feature_extraction
void clear();
- private:
+ private:
LaserScan scan;
const Params params;
};
-} // namespace armarx::laser_scanner_feature_extraction
\ No newline at end of file
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
index 5e453ea7..e2cb6739 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -61,7 +61,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
std::lock_guard g{memoryWriterMutex};
// const auto result = memoryWriter.addSegment(constants::memoryName,
- // constants::laserScannerFeaturesCoreSegment);
+ // constants::LaserScannerFeaturesCoreSegment);
// if (not result.success)
// {
@@ -73,7 +73,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
const auto entityID = armem::MemoryID()
.withMemoryName(constants::NavigationMemoryName)
- .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+ .withCoreSegmentName(constants::LaserScannerFeaturesCoreSegment)
.withProviderSegmentName(providerName)
.withEntityName(features.frame)
.withTimestamp(timestamp);
--
GitLab
From 6fb5d67b834ba373e6d42411712bc762c1a67cdf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 09:03:43 +0100
Subject: [PATCH 19/55] Rework feature extractor to work with memory instead of
topic
Read the laser scan data from the memory and run the extraction in a
periodic task instead of receiving from the legacy topic.
---
.../ArVizDrawer.cpp | 7 +-
.../ArVizDrawer.h | 12 +--
.../CMakeLists.txt | 4 +-
.../Component.cpp | 73 +++++++++++--------
.../Component.h | 20 +++--
.../FeatureExtractor.cpp | 4 +-
.../FeatureExtractor.h | 7 +-
7 files changed, 66 insertions(+), 61 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
index 3c30fd0f..ad8b55c8 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -19,7 +19,6 @@
#include "RobotAPI/components/ArViz/Client/elements/PointCloud.h"
#include "RobotAPI/gui-plugins/RobotUnitPlugin/QWidgets/StyleSheets.h"
-#include "RobotComponents/libraries/cartographer/types.h"
#include "RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h"
#include "FeatureExtractor.h"
@@ -42,13 +41,13 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
void
- ArVizDrawer::draw(const cartographer::LaserScannerMessage& msg,
+ ArVizDrawer::draw(const armem::laser_scans::LaserScanStamped& msg,
const Eigen::Isometry3f& globalSensorPose,
const simox::Color& color)
{
- auto layer = arviz.layer("points_" + msg.frame);
+ auto layer = arviz.layer("points_" + msg.header.frame);
- const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.scan));
+ const auto pointCloud = conversions::eigen2pcl(toCartesian<Eigen::Vector3f>(msg.data));
layer.add(viz::PointCloud("points_" + std::to_string(layer.size()))
.pointCloud(pointCloud, viz::Color(color))
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
index ce0f1abb..f1c51a4c 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -31,19 +31,9 @@
#include "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
-namespace armarx::cartographer
-{
- struct LaserScannerMessage;
-}
-
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- struct LineSegment2Df;
-
- // struct Circle;
- // struct Ellipsoid;
-
class ArVizDrawer
{
public:
@@ -57,7 +47,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const std::string& frame,
const Eigen::Isometry3f& globalSensorPose);
- void draw(const cartographer::LaserScannerMessage& msg,
+ void draw(const armem::laser_scans::LaserScanStamped& msg,
const Eigen::Isometry3f& globalSensorPose,
const simox::Color& color);
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index aae16e62..bc399a2c 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -21,6 +21,7 @@ armarx_add_component(laser_scanner_feature_extraction
ChainApproximation.h
EnclosingEllipsoid.h
Path.h
+ geometry.h
#conversions
conversions/eigen.h
conversions/pcl_eigen.h
@@ -39,9 +40,8 @@ armarx_add_component(laser_scanner_feature_extraction
## RobotAPICore
## RobotAPIInterfaces
RobotAPIComponentPlugins # For ArViz and other plugins.
- #armem_vision
- #RobotComponents::Cartographer
armarx_navigation::memory
+ armem_laser_scans
DEPENDENCIES_LEGACY
PCL
)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 5ac47033..d05e42d5 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -58,9 +58,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
new ComponentPropertyDefinitions(getConfigIdentifier());
// // Add an optionalproperty.
- def->optional(properties.queueSize,
- "p.inputQueueSize",
- "Size of the message queue (max). Should not be too large to avoid delays.");
+ def->optional(
+ properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
def->optional(properties.robotHull.shape, "p.robotHull.shape", "Shape of the robot area.")
.map({std::make_pair("Rectangle", Properties::RobotHull::RECTANGLE),
@@ -113,6 +112,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
LaserScannerFeatureExtraction::LaserScannerFeatureExtraction() :
laserScannerFeatureWriter(memoryNameSystem())
{
+ addPlugin(laserScannerReaderPlugin);
}
void
@@ -142,10 +142,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
properties.chainApproximationParams,
[&](auto&&... args) { onFeatures(std::forward<decltype(args)>(args)...); });
- laserMessageQueue.setQueueSize(properties.queueSize);
- laserMessageQueue.connect([&](const auto& sharedArg)
- { featureExtractor->onData(sharedArg); });
-
ARMARX_INFO << "Connected";
robot = RemoteRobot::createLocalClone(getRobotStateComponent());
@@ -224,6 +220,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
laserScannerFeatureWriter.connect();
+
+ task = new PeriodicTask<LaserScannerFeatureExtraction>(
+ this,
+ &LaserScannerFeatureExtraction::runPeriodically,
+ properties.taskPeriodMs,
+ false,
+ "runningTask");
+ task->start();
}
memory::LaserScannerFeature
@@ -306,18 +310,19 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
void
- LaserScannerFeatureExtraction::onFeatures(const cartographer::LaserScannerMessage& data,
+ LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
const std::vector<Features>& featuresFromExtractor)
{
ARMARX_DEBUG << "Publishing data";
// obtain sensor pose
RemoteRobot::synchronizeLocalCloneToTimestamp(
- robot, getRobotStateComponent(), data.timestamp);
- const Eigen::Isometry3f global_T_sensor(robot->getRobotNode(data.frame)->getGlobalPose());
+ robot, getRobotStateComponent(), data.header.timestamp.toMicroSecondsSinceEpoch());
+ const Eigen::Isometry3f global_T_sensor(
+ robot->getRobotNode(data.header.frame)->getGlobalPose());
const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
const Eigen::Isometry3f robot_T_sensor(
- robot->getRobotNode(data.frame)->getPoseInRootFrame());
+ robot->getRobotNode(data.header.frame)->getPoseInRootFrame());
//Eigen::AlignedBox2f box;
//box.extend(pt1).extend(pt2).extend(pt3).extend(pt4);
@@ -407,30 +412,29 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const auto validFeatures = removeInvalidFeatures(features);
ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
- const auto armemFeatures = toArmemFeatures(validFeatures, global_T_sensor, data.frame);
+ const auto armemFeatures =
+ toArmemFeatures(validFeatures, global_T_sensor, data.header.frame);
ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
// report the features
- publishFeatures(armemFeatures,
- armarx::core::time::DateTime(
- armarx::core::time::Duration::MicroSeconds(data.timestamp)));
+ publishFeatures(armemFeatures, data.header.timestamp);
// check if arviz should be triggered
const auto getOrCreateThrottler = [&]() -> Throttler&
{
- const auto it = throttlers.find(data.frame);
+ const auto it = throttlers.find(data.header.frame);
if (it == throttlers.end())
{
- throttlers.emplace(data.frame, Throttler(10.F));
+ throttlers.emplace(data.header.frame, Throttler(10.F));
}
- return throttlers.at(data.frame);
+ return throttlers.at(data.header.frame);
};
- if (getOrCreateThrottler().check(data.timestamp))
+ if (getOrCreateThrottler().check(data.header.timestamp))
{
- arVizDrawer->draw(features, data.frame, global_T_sensor);
+ arVizDrawer->draw(features, data.header.frame, global_T_sensor);
if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
{
@@ -482,6 +486,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
void
LaserScannerFeatureExtraction::onDisconnectComponent()
{
+ task->stop();
}
void
@@ -496,23 +501,27 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
void
- reportSensorValues(const std::string& device,
- const std::string& name,
- const LaserScan& scan,
- const TimestampBasePtr& timestamp,
- const Ice::Current& /*unused*/)
+ LaserScannerFeatureExtraction::runPeriodically()
{
- ARMARX_DEBUG << "Receiving data";
- /*
- laserMessageQueue.push(cartographer::LaserScannerMessage{
- .frame = name, .scan = scan, .timestamp = timestamp->timestamp});
+ const armem::laser_scans::client::Reader::Query laserScanQuery{
+ .agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
- ARMARX_DEBUG << "Queue size" << laserMessageQueue.size();
+ const armem::laser_scans::client::Reader::Result laserScanResult =
+ laserScannerReaderPlugin->get().queryData(laserScanQuery);
+ ARMARX_CHECK_EQUAL(laserScanResult.status,
+ armem::laser_scans::client::Reader::Result::Error)
+ << laserScanResult.errorMessage;
+
+ ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
+ << " sensors";
frequencyReporterSubscribe->add(IceUtil::Time::now().toMicroSeconds());
- setDebugObserverDatafield(name, scan.size());
-*/
+ for (const auto& scan : laserScanResult.laserScans)
+ {
+ setDebugObserverDatafield(getDefaultName() + scan.header.frame, scan.data.size());
+ featureExtractor->onData(scan);
+ }
}
} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index 664a8dbb..4b979483 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -32,11 +32,10 @@
#include "RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h"
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-
-#include <RobotComponents/libraries/cartographer/MessageQueue.h>
-#include <RobotComponents/libraries/cartographer/types.h>
+#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
@@ -106,9 +105,12 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
*/
private:
- void onFeatures(const cartographer::LaserScannerMessage& data,
+ void runPeriodically();
+
+ void onFeatures(const armem::laser_scans::LaserScanStamped& data,
const std::vector<Features>& features);
+
void publishFeatures(const memory::LaserScannerFeatures& features,
const armem::Time& timestamp);
@@ -123,13 +125,11 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
*/
// Private member variables go here.
- MessageQueue<cartographer::LaserScannerMessage> laserMessageQueue;
/// Properties shown in the Scenario GUI.
struct Properties
{
- // input message queue size
- size_t queueSize{2};
+ int taskPeriodMs = 100;
//
struct RobotHull
@@ -196,6 +196,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::mutex arvizMutex;
*/
+ PeriodicTask<LaserScannerFeatureExtraction>::pointer_type task;
+
std::unique_ptr<FeatureExtractor> featureExtractor;
std::unique_ptr<ArVizDrawer> arVizDrawer;
@@ -211,6 +213,10 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
+ armem::client::plugins::ReaderWriterPlugin<armem::laser_scans::client::Reader>*
+ laserScannerReaderPlugin = nullptr;
+
+
memory::client::laser_scanner_features::Writer laserScannerFeatureWriter;
};
} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index 588cd302..d5c875ec 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -85,10 +85,10 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
void
- FeatureExtractor::onData(const cartographer::LaserScannerMessage& data)
+ FeatureExtractor::onData(const armem::laser_scans::LaserScanStamped& data)
{
ARMARX_DEBUG << "on data";
- const auto clustersWithFeatures = features(data.scan);
+ const auto clustersWithFeatures = features(data.data);
ARMARX_DEBUG << "callback";
callback(data, clustersWithFeatures);
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
index d2fa134f..373fc748 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -27,7 +27,8 @@
#include <VirtualRobot/MathTools.h>
-#include "RobotComponents/libraries/cartographer/types.h"
+#include <RobotAPI/libraries/armem_laser_scans/types.h>
+
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include "EnclosingEllipsoid.h"
@@ -59,14 +60,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
public:
using Points = std::vector<Eigen::Vector2f>;
- using Callback = std::function<void(const cartographer::LaserScannerMessage& data,
+ using Callback = std::function<void(const armem::laser_scans::LaserScanStamped& data,
const std::vector<Features>& features)>;
FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
const ChainApproximation::Params& chainApproximationParams,
const Callback& callback);
- void onData(const cartographer::LaserScannerMessage& data);
+ void onData(const armem::laser_scans::LaserScanStamped& data);
private:
std::vector<Features> features(const LaserScan& scan) const;
--
GitLab
From 303084653eddb910747807f19a529998a7973ec1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Wed, 8 Mar 2023 19:50:15 +0100
Subject: [PATCH 20/55] Add back legacy topic
---
.../CMakeLists.txt | 2 +-
.../Component.cpp | 21 +++++++++++++++++++
.../Component.h | 9 ++++++++
3 files changed, 31 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index bc399a2c..ede04e8d 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -32,7 +32,6 @@ armarx_add_component(laser_scanner_feature_extraction
# ArmarXCore
ArmarXCore
ArmarXCoreComponentPlugins # For DebugObserver plugin.
- ArmarXCoreInterfaces
ArmarXCoreLogging
# ArmarXGui
## ArmarXGuiComponentPlugins # For RemoteGui plugin.
@@ -40,6 +39,7 @@ armarx_add_component(laser_scanner_feature_extraction
## RobotAPICore
## RobotAPIInterfaces
RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotComponentsInterfaces # For legacy LaserScannerFeatureExtraction topic
armarx_navigation::memory
armem_laser_scans
DEPENDENCIES_LEGACY
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index d05e42d5..6c2f235b 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -57,6 +57,9 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
armarx::PropertyDefinitionsPtr def =
new ComponentPropertyDefinitions(getConfigIdentifier());
+ // Publish to a topic (passing the TopicListenerPrx).
+ def->topic(featuresTopic);
+
// // Add an optionalproperty.
def->optional(
properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
@@ -481,6 +484,24 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// store in memory
laserScannerFeatureWriter.store(features, getName(), timestamp);
+
+ // legacy - topic
+ LineSegment2DChainSeq chains;
+ for (const auto& feature : features.features)
+ {
+ if (not feature.chain.empty())
+ {
+ LineSegment2DChain chain;
+ for (const auto& pt : feature.chain)
+ {
+ chain.push_back(
+ conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
+ }
+ chains.push_back(chain);
+ }
+ }
+
+ featuresTopic->reportExtractedLineSegments(chains);
}
void
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index 4b979483..fac5e425 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -37,6 +37,8 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
+#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
@@ -47,6 +49,11 @@
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
+ using LaserScannerFeaturesListenerPrx =
+ armarx::laser_scanner_feature_extraction::LaserScannerFeaturesListenerPrx;
+ using LineSegment2DChain = armarx::laser_scanner_feature_extraction::LineSegment2DChain;
+ using LineSegment2DChainSeq = armarx::laser_scanner_feature_extraction::LineSegment2DChainSeq;
+
// class FeatureExtractor;
// class ArVizDrawer;
@@ -208,6 +215,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
VirtualRobot::RobotPtr robot;
+ LaserScannerFeaturesListenerPrx featuresTopic;
+
std::variant<std::monostate, VirtualRobot::MathTools::ConvexHull2DPtr, Eigen::Vector2f>
robotHull;
VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
--
GitLab
From a801d28cf42e2b62116d74c1c131da4f9f37ecf0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 11:08:05 +0100
Subject: [PATCH 21/55] Fix laser scanner features reader/writer
Fix old memory name constants.
---
.../client/laser_scanner_features/Reader.h | 7 +++--
.../client/laser_scanner_features/Writer.cpp | 28 +++++++++----------
.../client/laser_scanner_features/Writer.h | 7 +++--
3 files changed, 21 insertions(+), 21 deletions(-)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
index 14cca12a..4e08755e 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -32,6 +32,7 @@
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/memory/types.h>
namespace armarx
@@ -103,11 +104,11 @@ namespace armarx::navigation::memory::client::laser_scanner_features
// Properties
struct Properties
{
- std::string memoryName = "Vision";
- std::string coreSegmentName = "LaserScannerFeatures";
+ std::string memoryName = memory::constants::NavigationMemoryName;
+ std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
} properties;
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ const std::string propertyPrefix = "mem.nav.laser_scanner_features.";
};
} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
index e2cb6739..425345e3 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -4,7 +4,6 @@
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/aron_conversions.h>
-#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::laser_scanner_features
{
@@ -23,25 +22,25 @@ namespace armarx::navigation::memory::client::laser_scanner_features
const std::string prefix = propertyPrefix;
- // def->optional(properties.coreSegmentName,
- // prefix + "CoreSegment",
- // "Name of the mapping memory core segment to use.");
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
- // def->optional(properties.memoryName, prefix + "MemoryName");
+ def->optional(properties.memoryName, prefix + "MemoryName");
}
void
Writer::connect()
{
// Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
- << constants::NavigationMemoryName << "' ...";
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << properties.memoryName
+ << "' ...";
try
{
- memoryWriter = memoryNameSystem.useWriter(
- armem::MemoryID().withMemoryName(constants::NavigationMemoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '"
- << constants::NavigationMemoryName << "'";
+ memoryWriter =
+ memoryNameSystem.useWriter(armem::MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName
+ << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
@@ -49,8 +48,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
return;
}
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
- << constants::NavigationMemoryName;
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << properties.memoryName;
}
bool
@@ -72,8 +70,8 @@ namespace armarx::navigation::memory::client::laser_scanner_features
// }
const auto entityID = armem::MemoryID()
- .withMemoryName(constants::NavigationMemoryName)
- .withCoreSegmentName(constants::LaserScannerFeaturesCoreSegment)
+ .withMemoryName(properties.memoryName)
+ .withCoreSegmentName(properties.coreSegmentName)
.withProviderSegmentName(providerName)
.withEntityName(features.frame)
.withTimestamp(timestamp);
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
index 30f42933..bd4cad88 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
@@ -29,6 +29,7 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::memory::client::laser_scanner_features
@@ -72,13 +73,13 @@ namespace armarx::navigation::memory::client::laser_scanner_features
// Properties
struct Properties
{
- // std::string memoryName = "Vision";
- // std::string coreSegmentName = "LaserScannerFeatures";
+ std::string memoryName = memory::constants::NavigationMemoryName;
+ std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
} properties;
std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ const std::string propertyPrefix = "mem.nav.laser_scanner_features.";
};
} // namespace armarx::navigation::memory::client::laser_scanner_features
--
GitLab
From 6e4b545bf3e4e2659fed17b9a106361f11ece870 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 11:09:22 +0100
Subject: [PATCH 22/55] Add features to dynamic scene
---
.../components/navigator/Component.cpp | 23 +++++++++++--------
.../components/navigator/Component.h | 17 +++++++-------
source/armarx/navigation/core/DynamicScene.h | 2 ++
.../server/scene_provider/SceneProvider.cpp | 20 +++++++++++-----
.../server/scene_provider/SceneProvider.h | 13 +++++++++--
5 files changed, 50 insertions(+), 25 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 3943986f..424e2851 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -168,6 +168,7 @@ namespace armarx::navigation::components::navigator
.costmapReader = &costmapReaderPlugin->get(),
.virtualRobotReader = &virtualRobotReaderPlugin->get(),
.humanReader = &humanReaderPlugin->get(),
+ .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
.objectPoseClient = ObjectPoseClientPluginUser::getClient()};
sceneProvider.emplace(srv, params.sceneCfg);
@@ -235,7 +236,6 @@ namespace armarx::navigation::components::navigator
return "navigator";
}
-
void
Component::createConfig(const aron::data::dto::DictPtr& stackConfig,
const std::string& callerId,
@@ -293,7 +293,8 @@ namespace armarx::navigation::components::navigator
{
ARMARX_TRACE;
- ARMARX_INFO << "moveTo() requested by caller '" << callerId << "' with navigation mode `" << navigationMode << "`.";
+ ARMARX_INFO << "moveTo() requested by caller '" << callerId << "' with navigation mode `"
+ << navigationMode << "`.";
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
@@ -345,7 +346,6 @@ namespace armarx::navigation::components::navigator
core::NavigationFrameNames.from_name(navigationMode));
}
-
void
Component::moveTo2(const client::detail::Waypoints& waypoints,
const std::string& navigationMode,
@@ -363,13 +363,15 @@ namespace armarx::navigation::components::navigator
navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
}
-
- void Component::moveToLocation(const std::string& location,
- const std::string& callerId,
- const Ice::Current& c)
+
+ void
+ Component::moveToLocation(const std::string& location,
+ const std::string& callerId,
+ const Ice::Current& c)
{
ARMARX_TRACE;
- ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId << "'";
+ ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId
+ << "'";
ARMARX_CHECK(navigators.count(callerId) > 0)
<< "Navigator config for caller `" << callerId << "` not registered!";
@@ -501,9 +503,12 @@ namespace armarx::navigation::components::navigator
"but won't execute it.");
def->required(params.sceneCfg.robotName, "p.scene.robotName");
- def->optional(params.sceneCfg.staticCostmapProviderName, "p.scene.staticCostmapProviderName");
+ def->optional(params.sceneCfg.staticCostmapProviderName,
+ "p.scene.staticCostmapProviderName");
def->optional(params.sceneCfg.staticCostmapName, "p.scene.staticCostmapName");
def->optional(params.sceneCfg.humanProviderName, "p.scene.humanProviderName");
+ def->optional(params.sceneCfg.laserScannerFeaturesProviderName,
+ "p.scene.laserScannerFeaturesProviderName");
return def;
}
diff --git a/source/armarx/navigation/components/navigator/Component.h b/source/armarx/navigation/components/navigator/Component.h
index aac593f1..b8526d92 100644
--- a/source/armarx/navigation/components/navigator/Component.h
+++ b/source/armarx/navigation/components/navigator/Component.h
@@ -51,11 +51,11 @@
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <armarx/control/client/ComponentPlugin.h>
-
-#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
+#include <armarx/navigation/memory/client/human/Reader.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/memory/client/parameterization/Reader.h>
#include <armarx/navigation/memory/client/stack_result/Writer.h>
#include <armarx/navigation/server/Navigator.h>
@@ -66,7 +66,6 @@
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
-
namespace armarx::navigation::components::navigator
{
@@ -115,13 +114,13 @@ namespace armarx::navigation::components::navigator
const Ice::Current& c = Ice::emptyCurrent) override;
void moveToLocation(const std::string& location,
- const std::string& callerId,
- const Ice::Current& c = Ice::emptyCurrent) override;
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current& c = Ice::emptyCurrent) override;
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current& c = Ice::emptyCurrent) override;
void moveTowards(const Eigen::Vector3f& direction,
const std::string& navigationMode,
@@ -208,6 +207,8 @@ namespace armarx::navigation::components::navigator
costmapReaderPlugin = nullptr;
armem::client::plugins::ReaderWriterPlugin<memory::client::human::Reader>*
humanReaderPlugin = nullptr;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Reader>*
+ laserScannerFeaturesReaderPlugin = nullptr;
// armem::vision::occupancy_grid::client::Reader occupancyGridReader;
diff --git a/source/armarx/navigation/core/DynamicScene.h b/source/armarx/navigation/core/DynamicScene.h
index 172957d0..66f31707 100644
--- a/source/armarx/navigation/core/DynamicScene.h
+++ b/source/armarx/navigation/core/DynamicScene.h
@@ -24,6 +24,7 @@
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::core
{
@@ -31,6 +32,7 @@ namespace armarx::navigation::core
struct DynamicScene
{
human::Humans humans;
+ std::vector<memory::LaserScannerFeatures> laserScannerFeatures;
};
} // namespace armarx::navigation::core
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 85e0d623..3fa2eb01 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
@@ -14,9 +14,9 @@
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/graph/Reader.h>
#include <armarx/navigation/memory/client/human/Reader.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Reader.h>
#include <armarx/navigation/util/util.h>
-
namespace armarx::navigation::server::scene_provider
{
@@ -124,11 +124,19 @@ namespace armarx::navigation::server::scene_provider
core::DynamicScene
SceneProvider::getDynamicScene(const DateTime& timestamp) const
{
- const memory::client::human::Reader::Query query{.providerName = config.humanProviderName,
- .timestamp = timestamp,
- .maxAge = Duration::MilliSeconds(500)};
-
- return {.humans = srv.humanReader->queryHumans(query).humans};
+ const memory::client::human::Reader::Query queryHumans{
+ .providerName = config.humanProviderName,
+ .timestamp = timestamp,
+ .maxAge = Duration::MilliSeconds(500)};
+
+ const memory::client::laser_scanner_features::Reader::Query queryFeatures{
+ .providerName = config.laserScannerFeaturesProviderName,
+ .name = {},
+ .timestamp = timestamp};
+
+ return {.humans = srv.humanReader->queryHumans(queryHumans).humans,
+ .laserScannerFeatures =
+ srv.laserScannerFeaturesReader->queryData(queryFeatures).features};
}
core::SceneGraph
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
index 1f21e85e..df8a9e77 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.h
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -35,23 +35,29 @@
#include <armarx/navigation/memory/client/human/Reader.h>
#include <armarx/navigation/server/scene_provider/SceneProviderInterface.h>
-
namespace armarx::navigation::memory::client
{
namespace graph
{
class Reader;
}
+
namespace costmap
{
class Reader;
}
+
namespace human
{
class Reader;
}
-} // namespace armarx::navigation::memory::client
+ namespace laser_scanner_features
+ {
+ class Reader;
+ }
+
+} // namespace armarx::navigation::memory::client
namespace armarx::navigation::server::scene_provider
{
@@ -72,6 +78,8 @@ namespace armarx::navigation::server::scene_provider
memory::client::human::Reader* humanReader;
+ memory::client::laser_scanner_features::Reader* laserScannerFeaturesReader;
+
objpose::ObjectPoseClient objectPoseClient;
};
@@ -83,6 +91,7 @@ namespace armarx::navigation::server::scene_provider
std::string staticCostmapName = "distance_to_obstacles";
std::string humanProviderName = "dynamic_scene_provider";
+ std::string laserScannerFeaturesProviderName = "LaserScannerFeatureExtraction";
};
SceneProvider(const InjectedServices& srv, const Config& config);
--
GitLab
From b7cde6187e89eea1c1f792c2d013b93a26b6446a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 11:31:32 +0100
Subject: [PATCH 23/55] Fix laser scanner features memory reader/writer, store
features
Fix usage of laser scanner features reader/writer to the new ones moved
into the navigation memory.
Store unused features in another provider segment in the memory.
---
.../dynamic_scene_provider/Component.cpp | 39 +++++++++----------
.../dynamic_scene_provider/Component.h | 3 ++
2 files changed, 22 insertions(+), 20 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index c7505148..6e44b178 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -34,20 +34,18 @@
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
-
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
+#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
namespace armarx::navigation::components::dynamic_scene_provider
{
const std::string Component::defaultName = "dynamic_scene_provider";
-
Component::Component()
{
addPlugin(humanPoseReaderPlugin);
@@ -56,6 +54,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
addPlugin(costmapReaderPlugin);
addPlugin(occupancyGridReaderPlugin);
addPlugin(humanWriterPlugin);
+ addPlugin(laserScannerFeaturesWriterPlugin);
}
armarx::PropertyDefinitionsPtr
@@ -100,7 +99,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
return def;
}
-
void
Component::onInitComponent()
{
@@ -111,7 +109,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
setDebugObserverBatchModeEnabled(true);
}
-
void
Component::onConnectComponent()
{
@@ -163,27 +160,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
task->start();
}
-
void
Component::onDisconnectComponent()
{
task->stop();
}
-
void
Component::onExitComponent()
{
}
-
std::string
Component::getDefaultName() const
{
return Component::defaultName;
}
-
std::string
Component::GetDefaultName()
{
@@ -246,22 +239,20 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
ARMARX_TRACE;
- const std::vector<armem::vision::LaserScannerFeatures> laserFeatures = [&]
+ const std::vector<memory::LaserScannerFeatures> laserFeatures = [&]
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
ARMARX_INFO << "Querying laser scanner features";
- const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
+ const memory::client::laser_scanner_features::Reader::Query laserFeaturesQuery{
.providerName = properties.laserScannerFeatures.providerName,
.name = properties.laserScannerFeatures.name,
.timestamp = timestamp};
- const armem::vision::laser_scanner_features::client::Reader::Result
- laserFeaturesResult =
- laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
- ARMARX_CHECK_EQUAL(
- laserFeaturesResult.status,
- armem::vision::laser_scanner_features::client::Reader::Result::Success)
+ const memory::client::laser_scanner_features::Reader::Result laserFeaturesResult =
+ laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+ memory::client::laser_scanner_features::Reader::Result::Success)
<< laserFeaturesResult.errorMessage;
ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
@@ -396,7 +387,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
}
ARMARX_TRACE;
- const std::vector<armem::vision::LaserScannerFeature> unusedFeatures = [&]
+ const std::vector<memory::LaserScannerFeature> unusedFeatures = [&]
{
armarx::core::time::ScopedStopWatch sw(
makeSWlog("dynamic_scene.human_tracker.laserscanner"));
@@ -404,7 +395,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Running human tracker with lasersensor measurements";
//TODO why is result a vector of LSFs and not a vector of LSF?
- std::vector<armem::vision::LaserScannerFeature> flattened;
+ std::vector<memory::LaserScannerFeature> flattened;
for (auto const& fs : laserFeatures)
{
flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
@@ -431,12 +422,20 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_INFO << "Detected " << humans.size() << " humans";
humanWriterPlugin->get().store(humans, getName(), timestamp);
}
+
+ if (not unusedFeatures.empty())
+ {
+ ARMARX_INFO << "Detected " << unusedFeatures.size()
+ << " laser scanner features not associated with humans";
+ //TODO(groeger): check frame, do we need it?
+ laserScannerFeaturesWriterPlugin->get().store(
+ memory::LaserScannerFeatures{.features = unusedFeatures}, getName(), timestamp);
+ }
}
sendDebugObserverBatch();
}
-
/* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void
Component::createRemoteGuiTab()
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index d696a6a2..d95adb47 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -51,6 +51,7 @@
#include <armarx/navigation/human/HumanTracker.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/human/Writer.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -201,6 +202,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
occupancyGridReaderPlugin = nullptr;
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+ ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
+ laserScannerFeaturesWriterPlugin = nullptr;
human::HumanTracker humanTracker;
--
GitLab
From 73e95fb3d15a962908c1e5a744a0ade4b9f93f05 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 12:10:28 +0100
Subject: [PATCH 24/55] Create option to disable laser scanner features and
humans in dynamic scene provider
---
.../components/dynamic_scene_provider/Component.cpp | 12 ++++++++++--
.../components/dynamic_scene_provider/Component.h | 6 +++++-
2 files changed, 15 insertions(+), 3 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index 9435165e..f09bb41c 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -97,7 +97,9 @@ namespace armarx::navigation::components::dynamic_scene_provider
def->optional(
properties.occupancyGrid.occupiedThreshold, "p.occupancyGrid.occupiedThreshold", "");
- def->optional(properties.humanPoseProvider, "p.humanPoseProvider", "");
+ def->optional(
+ properties.humanPoses.enabled, "p.humanPoses.enabled", "Whether human poses are used.");
+ def->optional(properties.humanPoses.providerName, "p.humanPoses.providerName", "");
return def;
}
@@ -219,11 +221,16 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_TRACE;
const std::vector<armem::human::HumanPose> humanPoses = [&]
{
+ if (!properties.humanPoses.enabled)
+ {
+ return std::vector<armem::human::HumanPose>{};
+ }
+
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_human"));
ARMARX_VERBOSE << "Querying humans";
const armem::human::client::Reader::Query humanPoseQuery{
- .providerName = properties.humanPoseProvider,
+ .providerName = properties.humanPoses.providerName,
.timestamp = timestamp,
.maxAge = Duration::MilliSeconds(500)};
@@ -386,6 +393,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Human tracking
//
ARMARX_TRACE;
+ if (properties.humanPoses.enabled)
{
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.human_tracker.camera"));
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 6e24908b..1ab600a3 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -152,7 +152,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
float occupiedThreshold = 0.55;
} occupancyGrid;
- std::string humanPoseProvider = "AzureKinectPointCloudProvider";
+ struct
+ {
+ bool enabled = true;
+ std::string providerName = "AzureKinectPointCloudProvider";
+ } humanPoses;
};
Properties properties;
--
GitLab
From 6cb5c5b29073fba5f4e04c8e7e14f0260113c5e4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 12:11:02 +0100
Subject: [PATCH 25/55] Fix navigator crashing
Laser scanner features reader plugin was not initialized.
---
source/armarx/navigation/components/navigator/Component.cpp | 1 +
1 file changed, 1 insertion(+)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 424e2851..0adbd4a8 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -126,6 +126,7 @@ namespace armarx::navigation::components::navigator
addPlugin(graphReaderPlugin);
addPlugin(costmapReaderPlugin);
addPlugin(humanReaderPlugin);
+ addPlugin(laserScannerFeaturesReaderPlugin);
addPlugin(virtualRobotReaderPlugin);
--
GitLab
From 5cef91ca53e0809b88c7de974d54322e19689b9b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 12:11:28 +0100
Subject: [PATCH 26/55] Do not crash when provider segment doesnt exist
Return error result when provider segment doesn't exist when reading
laser scanner features.
---
.../memory/client/laser_scanner_features/Reader.cpp | 10 ++++++++++
1 file changed, 10 insertions(+)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index c7640bdc..149904ef 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -194,6 +194,16 @@ namespace armarx::navigation::memory::client::laser_scanner_features
.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 {.features = {},
+ .status = Result::Status::Error,
+ .errorMessage = "Provider segment " + query.providerName + " does not exist"};
+ }
+
// now create result from memory
const armem::wm::ProviderSegment& providerSegment =
qResult.memory.getCoreSegment(properties.coreSegmentName)
--
GitLab
From d25f99be954ab1659510f2d134d868b67df913c7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 15:03:11 +0100
Subject: [PATCH 27/55] Fix component
Fix registering of component and success check when reading laser
scanner data.
---
.../Component.cpp | 16 ++++++++++++++--
.../laser_scanner_feature_extraction/Component.h | 4 ++++
2 files changed, 18 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 6c2f235b..69be108b 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -39,6 +39,7 @@
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
@@ -51,6 +52,8 @@
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
+ const std::string LaserScannerFeatureExtraction::defaultName = "LaserScannerFeatureExtraction";
+
armarx::PropertyDefinitionsPtr
LaserScannerFeatureExtraction::createPropertyDefinitions()
{
@@ -518,7 +521,13 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::string
LaserScannerFeatureExtraction::getDefaultName() const
{
- return "LaserScannerFeatureExtraction";
+ return defaultName;
+ }
+
+ std::string
+ LaserScannerFeatureExtraction::GetDefaultName()
+ {
+ return defaultName;
}
void
@@ -530,7 +539,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const armem::laser_scans::client::Reader::Result laserScanResult =
laserScannerReaderPlugin->get().queryData(laserScanQuery);
ARMARX_CHECK_EQUAL(laserScanResult.status,
- armem::laser_scans::client::Reader::Result::Error)
+ armem::laser_scans::client::Reader::Result::Success)
<< laserScanResult.errorMessage;
ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
@@ -545,4 +554,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
}
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
+ LaserScannerFeatureExtraction::GetDefaultName());
+
} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index fac5e425..970041d9 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -84,6 +84,9 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ /// Get the component's default name.
+ static std::string GetDefaultName();
+
protected:
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -132,6 +135,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
*/
// Private member variables go here.
+ static const std::string defaultName;
/// Properties shown in the Scenario GUI.
struct Properties
--
GitLab
From feb511b258f9bff78407c56057c189a980f5641d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 19:23:32 +0100
Subject: [PATCH 28/55] Fix laser scanner features reader
Honor entity name in query.
---
.../client/laser_scanner_features/Reader.cpp | 34 ++++++++-----------
1 file changed, 15 insertions(+), 19 deletions(-)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index 149904ef..dde8e569 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -95,26 +95,22 @@ namespace armarx::navigation::memory::client::laser_scanner_features
{
armarx::armem::client::query::Builder qb;
- qb.coreSegments()
- .withName(properties.coreSegmentName)
- .providerSegments()
- .withName(query.providerName)
- .entities()
- .all()
- .snapshots()
- .beforeOrAtTime(query.timestamp);
-
- // auto entitySel = [&]()
- // {
- // if (query.name.empty())
- // {
- // ARMARX_INFO << "querying all entities";
- // return sel.entities().all();
- // }
- // return sel.entities().withName(query.name);
- // }();
+ auto& sel = qb.coreSegments()
+ .withName(properties.coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName);
+
+ auto& entitySel = [&]() -> armem::client::query::EntitySelector&
+ {
+ if (query.name.empty())
+ {
+ ARMARX_INFO << "querying all entities";
+ return sel.entities().all();
+ }
+ return sel.entities().withName(query.name);
+ }();
- // entitySel.snapshots().beforeOrAtTime(query.timestamp);
+ entitySel.snapshots().beforeOrAtTime(query.timestamp);
return qb;
}
--
GitLab
From 5926bf366c0493a307f1b9e1499ea412d4b26882 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 20 Mar 2023 19:26:45 +0100
Subject: [PATCH 29/55] Fix laser scanner features merging and coordinate
transformation
Merge all laser scanner features into one entity and transform them into
the global frame.
Use the features properly.
---
.../dynamic_scene_provider/Component.cpp | 31 +++---
.../dynamic_scene_provider/Component.h | 2 +-
.../Component.cpp | 103 ++++++++++++++++--
.../Component.h | 12 +-
.../FeatureExtractor.cpp | 10 +-
.../FeatureExtractor.h | 9 +-
.../local_planning/TebObstacleManager.cpp | 22 +++-
.../local_planning/TebObstacleManager.h | 3 +
.../local_planning/TimedElasticBands.cpp | 5 +
.../server/scene_provider/SceneProvider.h | 2 +-
10 files changed, 158 insertions(+), 41 deletions(-)
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
index f09bb41c..a5302a50 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.cpp
@@ -34,13 +34,13 @@
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
+
#include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/util.h>
-#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
-
namespace armarx::navigation::components::dynamic_scene_provider
{
@@ -249,11 +249,11 @@ namespace armarx::navigation::components::dynamic_scene_provider
// Laser scanner features
//
ARMARX_TRACE;
- const std::vector<memory::LaserScannerFeatures> laserFeatures = [&]
+ const memory::LaserScannerFeatures laserFeatures = [&]
{
if (!properties.laserScannerFeatures.enabled)
{
- return std::vector<memory::LaserScannerFeatures>{};
+ return memory::LaserScannerFeatures{};
}
armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
@@ -272,7 +272,10 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
- return laserFeaturesResult.features;
+ ARMARX_CHECK_EQUAL(laserFeaturesResult.features.size(), 1)
+ << "We request the merged instance with only one result";
+
+ return laserFeaturesResult.features.front();
}();
@@ -416,14 +419,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
ARMARX_VERBOSE << "Running human tracker with lasersensor measurements";
- //TODO why is result a vector of LSFs and not a vector of LSF?
- std::vector<memory::LaserScannerFeature> flattened;
- for (auto const& fs : laserFeatures)
- {
- flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
- }
return humanTracker.update(human::HumanTracker::LaserMeasurement{
- .detectionTime = timestamp, .clusters = flattened});
+ .detectionTime = timestamp, .clusters = laserFeatures.features});
}();
@@ -448,11 +445,15 @@ namespace armarx::navigation::components::dynamic_scene_provider
if (not unusedFeatures.empty())
{
- ARMARX_INFO << "Detected " << unusedFeatures.size()
- << " laser scanner features not associated with humans";
+ ARMARX_VERBOSE << "Detected " << unusedFeatures.size()
+ << " laser scanner features not associated with humans";
//TODO(groeger): check frame, do we need it?
laserScannerFeaturesWriterPlugin->get().store(
- memory::LaserScannerFeatures{.features = unusedFeatures}, getName(), timestamp);
+ memory::LaserScannerFeatures{.frame = "global",
+ .frameGlobalPose = Eigen::Isometry3f::Identity(),
+ .features = unusedFeatures},
+ 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 1ab600a3..5c36522d 100644
--- a/source/armarx/navigation/components/dynamic_scene_provider/Component.h
+++ b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
@@ -135,7 +135,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
{
bool enabled = true;
std::string providerName = "LaserScannerFeatureExtraction";
- std::string name = ""; // all
+ std::string name = "global"; // the merged entity of all sensors
} laserScannerFeatures;
struct
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 69be108b..37cffb26 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -46,6 +46,7 @@
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include "armarx/navigation/conversions/eigen.h"
#include "conversions/eigen.h"
#include "conversions/pcl_eigen.h"
@@ -143,10 +144,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient());
- featureExtractor = std::make_unique<FeatureExtractor>(
- properties.scanClusteringParams,
- properties.chainApproximationParams,
- [&](auto&&... args) { onFeatures(std::forward<decltype(args)>(args)...); });
+ featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
+ properties.chainApproximationParams);
ARMARX_INFO << "Connected";
@@ -315,7 +314,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
return features;
}
- void
+ std::vector<Features>
LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
const std::vector<Features>& featuresFromExtractor)
{
@@ -362,10 +361,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
return false;
};
-
const auto isPointInsideCableArea = [&](const Eigen::Vector2f& pt) -> bool
{ return VirtualRobot::MathTools::isInside(pt, cableArea); };
-
const auto isClusterInvalid = [&](const Features& features) -> bool
{
// either use convex hull (compact representation) or raw points as fallbacck
@@ -478,6 +475,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// some debugobserver reporting
frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
setDebugObserverDatafield("numClusters", features.size());
+
+ return validFeatures;
}
void
@@ -547,13 +546,101 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
frequencyReporterSubscribe->add(IceUtil::Time::now().toMicroSeconds());
+ std::vector<FramedFeatures> allFeatures;
+ allFeatures.reserve(laserScanResult.laserScans.size());
+ armem::Time mostRecentTimestamp{0};
+
for (const auto& scan : laserScanResult.laserScans)
{
+ mostRecentTimestamp = std::max(mostRecentTimestamp, scan.header.timestamp);
+
+ // run clustering for every sensor
setDebugObserverDatafield(getDefaultName() + scan.header.frame, scan.data.size());
- featureExtractor->onData(scan);
+ const auto features = featureExtractor->onData(scan);
+
+ auto& framedFeatures = allFeatures.emplace_back();
+
+ // onFeatures filters out some clusters (cable area, inside robot hull etc.), we only want to keep the filtered points
+ framedFeatures.features = onFeatures(scan, features);
+ framedFeatures.frame = scan.header.frame;
+ }
+
+ if (!allFeatures.empty())
+ {
+ mergeFeatures(allFeatures, mostRecentTimestamp);
+ }
+ }
+
+ void
+ LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
+ const armem::Time& timestamp)
+ {
+ const auto transformPoint =
+ [](const Eigen::Vector2f& point, const Eigen::Isometry3f& global_T_frame)
+ { return conv::to2D(global_T_frame * conv::to3D(point)); };
+
+ const auto transformPoints =
+ [&](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& global_T_frame)
+ {
+ return simox::alg::apply(points,
+ [&](const Eigen::Vector2f& point)
+ { return transformPoint(point, global_T_frame); });
+ };
+
+ const auto transformFeature = [&](const Features& feature,
+ const Eigen::Isometry3f& global_T_frame) -> Features
+ {
+ Features transformed;
+
+ if (feature.convexHull)
+ {
+ transformed.convexHull = {
+ .vertices = transformPoints(feature.convexHull->vertices, global_T_frame),
+ .segments = feature.convexHull->segments};
+ }
+ if (feature.circle)
+ {
+ transformed.circle = {.center =
+ transformPoint(feature.circle->center, global_T_frame),
+ .radius = feature.circle->radius};
+ }
+ if (feature.ellipsoid)
+ {
+ transformed.ellipsoid = {.pose = global_T_frame * feature.ellipsoid->pose,
+ .radii = feature.ellipsoid->radii};
+ }
+ if (feature.chain)
+ {
+ transformed.chain = transformPoints(feature.chain.value(), global_T_frame);
+ }
+ transformed.points = transformPoints(feature.points, global_T_frame);
+
+ return transformed;
+ };
+
+ // TODO(groeger, navarro): implement actual merging
+
+ std::vector<Features> merged;
+ for (const auto& features : framedFeatures)
+ {
+ merged.reserve(merged.size() + features.features.size());
+ Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
+
+ for (const auto& singleFeature : features.features)
+ {
+ merged.push_back(transformFeature(singleFeature, global_T_frame));
+ }
}
+
+ std::string frame = "global";
+ const Eigen::Isometry3f global_T_global = Eigen::Isometry3f::Identity();
+
+ const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
+
+ laserScannerFeatureWriter.store(armemFeatures, getName(), timestamp);
}
+
ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
LaserScannerFeatureExtraction::GetDefaultName());
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index 970041d9..1a968d16 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -115,11 +115,19 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
*/
private:
+ struct FramedFeatures
+ {
+ std::vector<Features> features;
+ std::string frame;
+ };
+
void runPeriodically();
- void onFeatures(const armem::laser_scans::LaserScanStamped& data,
- const std::vector<Features>& features);
+ std::vector<Features> onFeatures(const armem::laser_scans::LaserScanStamped& data,
+ const std::vector<Features>& features);
+ void mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
+ const armem::Time& timestamp);
void publishFeatures(const memory::LaserScannerFeatures& features,
const armem::Time& timestamp);
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index d5c875ec..64e190d8 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -76,22 +76,20 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// FeatureExtractor
FeatureExtractor::FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
- const ChainApproximation::Params& chainApproximationParams,
- const Callback& callback) :
+ const ChainApproximation::Params& chainApproximationParams) :
scanClusteringParams(scanClusteringParams),
- chainApproximationParams(chainApproximationParams),
- callback(callback)
+ chainApproximationParams(chainApproximationParams)
{
}
- void
+ std::vector<Features>
FeatureExtractor::onData(const armem::laser_scans::LaserScanStamped& data)
{
ARMARX_DEBUG << "on data";
const auto clustersWithFeatures = features(data.data);
ARMARX_DEBUG << "callback";
- callback(data, clustersWithFeatures);
+ return clustersWithFeatures;
}
std::vector<Features>
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
index 373fc748..11389bdd 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -60,14 +60,11 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
public:
using Points = std::vector<Eigen::Vector2f>;
- using Callback = std::function<void(const armem::laser_scans::LaserScanStamped& data,
- const std::vector<Features>& features)>;
FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
- const ChainApproximation::Params& chainApproximationParams,
- const Callback& callback);
+ const ChainApproximation::Params& chainApproximationParams);
- void onData(const armem::laser_scans::LaserScanStamped& data);
+ std::vector<Features> onData(const armem::laser_scans::LaserScanStamped& data);
private:
std::vector<Features> features(const LaserScan& scan) const;
@@ -85,7 +82,5 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const ScanClustering::Params scanClusteringParams;
const ChainApproximation::Params chainApproximationParams;
-
- const Callback callback;
};
} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 89a1b2e4..f12d5a08 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -1,6 +1,6 @@
#include "TebObstacleManager.h"
-#include <ArmarXCore/core/logging/Logging.h>
+#include <SimoxUtility/algorithm/apply.hpp>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/local_planning/ros_conversions.h>
@@ -22,6 +22,26 @@ namespace armarx::navigation::local_planning
{
return container.size();
}
+ void
+ TebObstacleManager::addLaserScannerFeaturesObstacle(
+ const memory::LaserScannerFeatures& features,
+ viz::Layer* visLayer)
+ {
+
+ Eigen::Isometry3f global_T_frame = features.frameGlobalPose;
+ for (const auto& feature : features.features)
+ {
+ std::vector<Eigen::Vector2f> convexHull =
+ simox::alg::apply(feature.convexHull,
+ [&](const Eigen::Vector2f& point)
+ {
+ Eigen::Vector3f point3d = conv::to3D(point);
+ return conv::to2D(global_T_frame * point3d);
+ });
+
+ addPolygonObstacle(convexHull, visLayer);
+ }
+ }
void
TebObstacleManager::addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer)
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index df14dc4c..d61906a2 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -26,6 +26,7 @@
#include <armarx/navigation/human/ProxemicZoneCreator.h>
#include <armarx/navigation/human/types.h>
+#include <armarx/navigation/memory/types.h>
#include <teb_local_planner/obstacles.h>
namespace armarx::navigation::local_planning
@@ -45,6 +46,8 @@ namespace armarx::navigation::local_planning
size_t size();
+ void addLaserScannerFeaturesObstacle(const memory::LaserScannerFeatures& features,
+ viz::Layer* visLayer = nullptr);
void addPolygonObstacle(const Polygon& polygon, viz::Layer* visLayer = nullptr);
void addBoxObstacle(const VirtualRobot::BoundingBox& bbox, viz::Layer* visLayer = nullptr);
void addHumanObstacle(const human::Human& human, viz::Layer* visLayer = nullptr);
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 8b3f9ed7..a7ab7d2b 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -189,6 +189,11 @@ namespace armarx::navigation::local_planning
{
obstManager.addHumanObstacle(obst, visPtr);
}
+
+ for (const auto& obst : scene.dynamicScene->laserScannerFeatures)
+ {
+ obstManager.addLaserScannerFeaturesObstacle(obst, visPtr);
+ }
}
if (arviz)
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.h b/source/armarx/navigation/server/scene_provider/SceneProvider.h
index df8a9e77..04780c67 100644
--- a/source/armarx/navigation/server/scene_provider/SceneProvider.h
+++ b/source/armarx/navigation/server/scene_provider/SceneProvider.h
@@ -91,7 +91,7 @@ namespace armarx::navigation::server::scene_provider
std::string staticCostmapName = "distance_to_obstacles";
std::string humanProviderName = "dynamic_scene_provider";
- std::string laserScannerFeaturesProviderName = "LaserScannerFeatureExtraction";
+ std::string laserScannerFeaturesProviderName = "dynamic_scene_provider";
};
SceneProvider(const InjectedServices& srv, const Config& config);
--
GitLab
From da40ea6ddb3431de1e0d8e7d35c94cdc764e47d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 09:38:55 +0200
Subject: [PATCH 30/55] Fix armem renamed time in human visualization
---
source/armarx/navigation/components/navigation_memory/Visu.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 8283b059..506b8b9e 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -310,7 +310,7 @@ namespace armarx::navigation::memory
const armarx::armem::wm::EntityInstance& instance)
{
const Duration dtToNow =
- timestamp - instance.metadata().timeCreated;
+ timestamp - instance.metadata().referencedTime;
if (dtToNow < maxAge and dtToNow.isPositive())
{
--
GitLab
From 9ecc395a1bedb38d2996cb7d902fe9fc8c4195df Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 09:39:25 +0200
Subject: [PATCH 31/55] Update LaserScannerFeaturesReader to new
SimpleReaderBase implementation
---
.../client/laser_scanner_features/Reader.cpp | 69 ++++---------------
.../client/laser_scanner_features/Reader.h | 35 ++--------
2 files changed, 18 insertions(+), 86 deletions(-)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index 149904ef..65548c80 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -10,32 +10,13 @@
#include <utility>
#include <vector>
-// ICE
-#include <IceUtil/Handle.h>
-#include <IceUtil/Time.h>
-
-// Simox
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
// ArmarXCore
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
-// RobotAPI Interfaces
-#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
-#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-
-// RobotAPI Aron
-#include <RobotAPI/libraries/aron/core/Exception.h>
-#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
// RobotAPI Armem
#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/error.h>
@@ -44,50 +25,24 @@
#include <RobotAPI/libraries/armem_laser_scans/aron/LaserScan.aron.generated.h>
#include <armarx/navigation/memory/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::laser_scanner_features
{
- Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
Reader::~Reader() = default;
- void
- Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ std::string
+ Reader::propertyPrefix() const
{
- // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
- // registerPropertyDefinitions(def);
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
+ return "mem.nav.laser_scanner_features.";
}
- void
- Reader::connect()
+ armarx::armem::client::util::SimpleReaderBase::Properties
+ Reader::defaultProperties() const
{
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
- << "' ...";
- try
- {
- memoryReader =
- memoryNameSystem.useReader(armem::MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
+ return {.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment};
}
armarx::armem::client::query::Builder
@@ -96,7 +51,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
armarx::armem::client::query::Builder qb;
qb.coreSegments()
- .withName(properties.coreSegmentName)
+ .withName(properties().coreSegmentName)
.providerSegments()
.withName(query.providerName)
.entities()
@@ -182,7 +137,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
ARMARX_DEBUG << "[MappingDataReader] query ... ";
- const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+ const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
@@ -194,7 +149,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
.errorMessage = qResult.errorMessage};
}
- const auto coreSegment = qResult.memory.getCoreSegment(properties.coreSegmentName);
+ const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
if (not coreSegment.hasProviderSegment(query.providerName))
{
ARMARX_VERBOSE << "Provider segment `" << query.providerName
@@ -206,7 +161,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
// now create result from memory
const armem::wm::ProviderSegment& providerSegment =
- qResult.memory.getCoreSegment(properties.coreSegmentName)
+ qResult.memory.getCoreSegment(properties().coreSegmentName)
.getProviderSegment(query.providerName);
const auto features = asFeaturesList(providerSegment);
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
index 4e08755e..6823d34f 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -24,22 +24,13 @@
#include <string>
#include <vector>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/time/DateTime.h>
-#include <RobotAPI/libraries/armem/client.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleReaderBase.h>
-#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/memory/types.h>
-namespace armarx
-{
- class ManagedIceObject;
-}
-
namespace armarx::navigation::memory::client::laser_scanner_features
{
@@ -55,13 +46,12 @@ namespace armarx::navigation::memory::client::laser_scanner_features
*
* Detailed description of class ExampleClient.
*/
- class Reader
+ class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
{
public:
- Reader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Reader();
+ using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
- void connect();
+ virtual ~Reader();
struct Query
{
@@ -91,24 +81,11 @@ namespace armarx::navigation::memory::client::laser_scanner_features
Result queryData(const Query& query) const;
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
-
-
protected:
armarx::armem::client::query::Builder buildQuery(const Query& query) const;
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Reader memoryReader;
-
- // Properties
- struct Properties
- {
- std::string memoryName = memory::constants::NavigationMemoryName;
- std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
- } properties;
-
- const std::string propertyPrefix = "mem.nav.laser_scanner_features.";
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
};
} // namespace armarx::navigation::memory::client::laser_scanner_features
--
GitLab
From 02c3d912bf3d529b8bda07c89ab05a1e98451c5c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 09:39:56 +0200
Subject: [PATCH 32/55] Update LaserScannerFeaturesWriter to new
SimpleWriterBase implementation
---
.../client/laser_scanner_features/Writer.cpp | 54 +++++--------------
.../client/laser_scanner_features/Writer.h | 40 +++-----------
2 files changed, 21 insertions(+), 73 deletions(-)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
index 425345e3..9a60a4fb 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -4,51 +4,25 @@
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
#include <armarx/navigation/memory/aron_conversions.h>
+#include <armarx/navigation/memory/constants.h>
namespace armarx::navigation::memory::client::laser_scanner_features
{
- Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
- memoryNameSystem(memoryNameSystem)
- {
- }
-
Writer::~Writer() = default;
- void
- Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ std::string
+ Writer::propertyPrefix() const
{
- ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
-
- const std::string prefix = propertyPrefix;
-
- def->optional(properties.coreSegmentName,
- prefix + "CoreSegment",
- "Name of the mapping memory core segment to use.");
-
- def->optional(properties.memoryName, prefix + "MemoryName");
+ return "mem.nav.laser_scanner_features.";
}
- void
- Writer::connect()
+ armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+ Writer::defaultProperties() const
{
- // Wait for the memory to become available and add it as dependency.
- ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << properties.memoryName
- << "' ...";
- try
- {
- memoryWriter =
- memoryNameSystem.useWriter(armem::MemoryID().withMemoryName(properties.memoryName));
- ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName
- << "'";
- }
- catch (const armem::error::CouldNotResolveMemoryServer& e)
- {
- ARMARX_ERROR << e.what();
- return;
- }
-
- ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << properties.memoryName;
+ return SimpleWriterBase::Properties{.memoryName = memory::constants::NavigationMemoryName,
+ .coreSegmentName =
+ memory::constants::LaserScannerFeaturesCoreSegment};
}
bool
@@ -56,7 +30,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
const std::string& providerName,
const armem::Time& timestamp)
{
- std::lock_guard g{memoryWriterMutex};
+ std::lock_guard g{memoryWriterMutex()};
// const auto result = memoryWriter.addSegment(constants::memoryName,
// constants::LaserScannerFeaturesCoreSegment);
@@ -70,8 +44,8 @@ namespace armarx::navigation::memory::client::laser_scanner_features
// }
const auto entityID = armem::MemoryID()
- .withMemoryName(properties.memoryName)
- .withCoreSegmentName(properties.coreSegmentName)
+ .withMemoryName(properties().memoryName)
+ .withCoreSegmentName(properties().coreSegmentName)
.withProviderSegmentName(providerName)
.withEntityName(features.frame)
.withTimestamp(timestamp);
@@ -89,12 +63,12 @@ namespace armarx::navigation::memory::client::laser_scanner_features
ARMARX_TRACE;
update.instancesData = {dto.toAron()};
- update.timeCreated = timestamp;
+ update.referencedTime = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
ARMARX_TRACE;
- armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+ armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
ARMARX_DEBUG << updateResult;
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
index bd4cad88..93bc1d0b 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.h
@@ -22,14 +22,8 @@
#pragma once
-#include <mutex>
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
-#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-
-#include <armarx/navigation/memory/constants.h>
#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::memory::client::laser_scanner_features
@@ -46,40 +40,20 @@ namespace armarx::navigation::memory::client::laser_scanner_features
*
* Detailed description of class ExampleClient.
*/
- class Writer
+ class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
{
public:
- Writer(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~Writer();
-
-
- void connect();
+ using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
- // MappingDataWriterInterface
- /// to be called in Component::onConnectComponent
- // void connect() override;
-
- /// to be called in Component::addPropertyDefinitions
- void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+ virtual ~Writer();
bool store(const LaserScannerFeatures& features,
const std::string& providerName,
const armem::Time& timestamp);
- private:
- armem::client::MemoryNameSystem& memoryNameSystem;
- armem::client::Writer memoryWriter;
-
- // Properties
- struct Properties
- {
- std::string memoryName = memory::constants::NavigationMemoryName;
- std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
- } properties;
-
- std::mutex memoryWriterMutex;
-
- const std::string propertyPrefix = "mem.nav.laser_scanner_features.";
+ protected:
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
};
} // namespace armarx::navigation::memory::client::laser_scanner_features
--
GitLab
From b85a33d8912874d7fd4c7d409640f63a26291cbf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 09:41:36 +0200
Subject: [PATCH 33/55] Use Reader/Writer as plugin in
LaserScannerFeatureExtraction component
---
.../laser_scanner_feature_extraction/Component.cpp | 13 +++++--------
.../laser_scanner_feature_extraction/Component.h | 12 +++++++-----
2 files changed, 12 insertions(+), 13 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 6c2f235b..e301f9ce 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -42,9 +42,10 @@
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include "conversions/eigen.h"
#include "conversions/pcl_eigen.h"
@@ -107,15 +108,13 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
"p.arviz.drawRawPoints",
"If true, the laser scans will be drawn.");
- laserScannerFeatureWriter.registerPropertyDefinitions(def);
-
return def;
}
- LaserScannerFeatureExtraction::LaserScannerFeatureExtraction() :
- laserScannerFeatureWriter(memoryNameSystem())
+ LaserScannerFeatureExtraction::LaserScannerFeatureExtraction()
{
addPlugin(laserScannerReaderPlugin);
+ addPlugin(laserScannerFeatureWriterPlugin);
}
void
@@ -222,8 +221,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
- laserScannerFeatureWriter.connect();
-
task = new PeriodicTask<LaserScannerFeatureExtraction>(
this,
&LaserScannerFeatureExtraction::runPeriodically,
@@ -483,7 +480,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
// store in memory
- laserScannerFeatureWriter.store(features, getName(), timestamp);
+ laserScannerFeatureWriterPlugin->get().store(features, getName(), timestamp);
// legacy - topic
LineSegment2DChainSeq chains;
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index fac5e425..7ebceb54 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -37,15 +37,16 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
-#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
-
-#include "ArVizDrawer.h"
-#include "FeatureExtractor.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
+#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
@@ -226,6 +227,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
laserScannerReaderPlugin = nullptr;
- memory::client::laser_scanner_features::Writer laserScannerFeatureWriter;
+ armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
+ laserScannerFeatureWriterPlugin = nullptr;
};
} // namespace armarx::navigation::components::laser_scanner_feature_extraction
--
GitLab
From 31f9ba6d5e179e086ddd4b739724a703465bb2ef Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 15:45:14 +0200
Subject: [PATCH 34/55] Add simple union find implementation
---
.../UnionFind.cpp | 70 +++++++++++++++++++
.../UnionFind.h | 65 +++++++++++++++++
2 files changed, 135 insertions(+)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp
new file mode 100644
index 00000000..7e994dae
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.cpp
@@ -0,0 +1,70 @@
+#include "UnionFind.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ UnionFind::UnionFind(std::size_t n)
+ {
+ rank.reserve(n);
+ parent.reserve(n);
+ for (std::size_t i = 0; i < n; i++)
+ {
+ rank.push_back(0);
+ parent.push_back(i);
+ }
+ }
+
+ std::size_t
+ UnionFind::find(std::size_t x)
+ {
+ // Finds the representative of the set
+ // that x is an element of
+ if (parent[x] != x)
+ {
+
+ // if x is not the parent of itself
+ // Then x is not the representative of
+ // his set,
+ parent[x] = find(parent[x]);
+
+ // so we recursively call Find on its parent
+ // and move i's node directly under the
+ // representative of this set
+ }
+
+ return parent[x];
+ }
+
+ void
+ UnionFind::unite(std::size_t a, std::size_t b)
+ {
+ // Find current sets of x and y
+ std::size_t xset = find(a);
+ std::size_t yset = find(b);
+
+ // If they are already in same set
+ if (xset == yset)
+ return;
+
+ // Put smaller ranked item under
+ // bigger ranked item if ranks are
+ // different
+ if (rank[xset] < rank[yset])
+ {
+ parent[xset] = yset;
+ }
+ else if (rank[xset] > rank[yset])
+ {
+ parent[yset] = xset;
+ }
+
+ // If ranks are same, then increment
+ // rank.
+ else
+ {
+ parent[yset] = xset;
+ rank[xset] = rank[xset] + 1;
+ }
+ }
+
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h
new file mode 100644
index 00000000..b7d6ab85
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h
@@ -0,0 +1,65 @@
+/*
+ * 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 )
+ * @author Corvin Navarro ( corvin dot ecker at student dot kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <vector>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // The implementation of this class is taken (and modified) from
+ // https://www.geeksforgeeks.org/disjoint-set-data-structures/
+
+ /*! \brief A simple union find data structure
+ *
+ * The data structure supports a fixed size of n elements which must be the
+ * integers from [0, n).
+ */
+ class UnionFind
+ {
+ public:
+ /*! \brief Construct a new union find data structure with n elements
+ * \param n the number of elements
+ *
+ * After construction each element will be in its own individual set.
+ */
+ UnionFind(std::size_t n);
+
+ /*! \brief find the representative of x
+ * \return the representative of x
+ */
+ std::size_t find(std::size_t x);
+
+ /*! \brief unite the sets of the elements a and b
+ * \param a
+ * \param b
+ */
+ void unite(std::size_t a, std::size_t b);
+
+ private:
+ std::vector<std::size_t> rank;
+ std::vector<std::size_t> parent;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
--
GitLab
From bed3349007ea910a28dac4c895da3bdc3335abfa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 15:46:51 +0200
Subject: [PATCH 35/55] Make feature computation inside FeatureExtractor static
Allow the FeatureMerger to use the functions to compute convex hull,
enclosing ellipsoid, chain etc. of FeatureExtractor.
---
.../FeatureExtractor.cpp | 30 ++++++++++---------
.../FeatureExtractor.h | 26 +++++++++-------
2 files changed, 31 insertions(+), 25 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index d5c875ec..faefb5c6 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -24,6 +24,10 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
+
#include <RobotComponents/libraries/cartographer/util/laser_scanner_conversion.h>
#include "Path.h"
@@ -32,9 +36,6 @@
#include "conversions/opencv_eigen.h"
#include "conversions/opencv_pcl.h"
#include "conversions/pcl_eigen.h"
-#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
-#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
-#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
@@ -108,11 +109,12 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const auto points = toCartesian<Eigen::Vector2f>(cluster);
const auto hull = convexHull(points);
- return Features{.convexHull = hull,
- .circle = std::nullopt, //circle(points),
- .ellipsoid = std::nullopt, //ellipsoid(hull),
- .chain = chainApproximation(points, hull),
- .points = points};
+ return Features{
+ .convexHull = hull,
+ .circle = std::nullopt, //circle(points),
+ .ellipsoid = std::nullopt, //ellipsoid(hull),
+ .chain = chainApproximation(points, hull, chainApproximationParams),
+ .points = points};
});
return fs;
@@ -126,7 +128,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
std::optional<VirtualRobot::MathTools::ConvexHull2D>
- FeatureExtractor::convexHull(const Points& points) const
+ FeatureExtractor::convexHull(const Points& points)
{
if (points.size() < 3) // no meaningful area
@@ -172,8 +174,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// }
std::optional<Ellipsoid>
- FeatureExtractor::ellipsoid(
- const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull) const
+ FeatureExtractor::ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull)
{
if (not hull)
@@ -194,7 +195,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
std::optional<Circle>
- FeatureExtractor::circle(const Points& points) const
+ FeatureExtractor::circle(const Points& points)
{
// Too few points will cause algorithmic issues
if (points.size() < 5)
@@ -215,14 +216,15 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::optional<FeatureExtractor::Points>
FeatureExtractor::chainApproximation(
const Points& points,
- const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull) const
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull,
+ const ChainApproximation::Params& params)
{
if (not convexHull)
{
return std::nullopt;
}
- ChainApproximation chApprx(points, chainApproximationParams);
+ ChainApproximation chApprx(points, params);
const auto result = chApprx.approximate();
if (result.condition !=
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
index 373fc748..f049237b 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h
@@ -29,12 +29,13 @@
#include <RobotAPI/libraries/armem_laser_scans/types.h>
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include <armarx/navigation/memory/types.h>
+
#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
#include "EnclosingEllipsoid.h"
#include "ScanClustering.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
-#include <armarx/navigation/memory/types.h>
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
@@ -69,20 +70,23 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
void onData(const armem::laser_scans::LaserScanStamped& data);
+ public:
+ static std::optional<VirtualRobot::MathTools::ConvexHull2D>
+ convexHull(const Points& points);
+ static std::optional<Ellipsoid>
+ ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull);
+ static std::optional<Circle> circle(const Points& circle);
+ static std::optional<Points>
+ chainApproximation(const Points& points,
+ const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull,
+ const ChainApproximation::Params& params);
+
+
private:
std::vector<Features> features(const LaserScan& scan) const;
std::vector<LaserScan> detectClusters(const LaserScan& scan) const;
- std::optional<VirtualRobot::MathTools::ConvexHull2D> convexHull(const Points& points) const;
- std::optional<Ellipsoid>
- ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull) const;
- std::optional<Circle> circle(const Points& circle) const;
-
- std::optional<Points> chainApproximation(
- const Points& points,
- const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull) const;
-
const ScanClustering::Params scanClusteringParams;
const ChainApproximation::Params chainApproximationParams;
--
GitLab
From 817817025a1b847ddfae676c9dfc623b3f57b7e6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 15:48:00 +0200
Subject: [PATCH 36/55] Implement feature merger
---
.../CMakeLists.txt | 6 +
.../FeatureMerger.cpp | 160 ++++++++++++++++++
.../FeatureMerger.h | 53 ++++++
3 files changed, 219 insertions(+)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index ede04e8d..9b910c8e 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -5,6 +5,7 @@ armarx_add_component(laser_scanner_feature_extraction
SOURCES
Component.cpp
FeatureExtractor.cpp
+ FeatureMerger.cpp
ArVizDrawer.cpp
ScanClustering.cpp
ChainApproximation.cpp
@@ -13,9 +14,11 @@ armarx_add_component(laser_scanner_feature_extraction
conversions/pcl.cpp
EnclosingEllipsoid.cpp
Path.cpp
+ UnionFind.cpp
HEADERS
Component.h
FeatureExtractor.h
+ FeatureMerger.h
ArVizDrawer.h
ScanClustering.h
ChainApproximation.h
@@ -28,6 +31,7 @@ armarx_add_component(laser_scanner_feature_extraction
conversions/opencv_eigen.h
conversions/opencv_pcl.h
conversions/pcl.h
+ UnionFind.h
DEPENDENCIES
# ArmarXCore
ArmarXCore
@@ -42,6 +46,8 @@ armarx_add_component(laser_scanner_feature_extraction
RobotComponentsInterfaces # For legacy LaserScannerFeatureExtraction topic
armarx_navigation::memory
armem_laser_scans
+ DEPENDENCIES_PRIVATE
+ range-v3::range-v3
DEPENDENCIES_LEGACY
PCL
)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
new file mode 100644
index 00000000..ccc12ed3
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
@@ -0,0 +1,160 @@
+#include "FeatureMerger.h"
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h>
+
+#include <range/v3/view/enumerate.hpp>
+#include <range/v3/view/sliding.hpp>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+
+ FeatureMerger::FeatureMerger(std::vector<Features>&& features,
+ const ChainApproximation::Params& chainApproximationParams) :
+ inputFeatures(std::move(features)), chainApproximationParams(chainApproximationParams)
+ {
+ }
+
+ FeatureMerger::FeatureMerger(const std::vector<Features>& features,
+ const ChainApproximation::Params& chainApproximationParams) :
+ inputFeatures(features), chainApproximationParams(chainApproximationParams)
+ {
+ }
+
+ std::vector<Features>
+ FeatureMerger::merge()
+ {
+ UnionFind uf{inputFeatures.size()};
+
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ std::optional<std::size_t> lastFeature = std::nullopt;
+ for (const auto& p : f.points)
+ {
+ if (auto res = checkPoint(p, lastFeature); res)
+ {
+ uf.unite(i, res.value());
+ lastFeature = res.value();
+ }
+ }
+ }
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ std::size_t representative = uf.find(i);
+ if (i != representative)
+ {
+ addToFeature(inputFeatures[representative], f);
+ }
+ }
+
+ std::vector<Features> merged;
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ if (i == uf.find(i))
+ {
+ merged.push_back(f);
+ }
+ }
+
+ return merged;
+ }
+
+ bool
+ FeatureMerger::insideConvexPoly(const Eigen::Vector2f& p,
+ const std::vector<Eigen::Vector2f>& vertices)
+ {
+ // algorithm to check whether a point is inside a convex polygon
+ // see https://stackoverflow.com/questions/1119627/how-to-test-if-a-point-is-inside-of-a-convex-polygon-in-2d-integer-coordinates
+
+ const auto signum = [](int x)
+ {
+ if (x == 0)
+ return 0;
+ return x > 0 ? 1 : -1;
+ };
+
+ const auto checkSide = [&](const Eigen::Vector2f& a, const Eigen::Vector2f& b)
+ { return signum((p.x() - a.x()) * (b.y() - a.y()) - (p.y() - a.y()) * (b.x() - a.x())); };
+
+ int previousSide = 0;
+ for (const auto edge : vertices | ranges::views::sliding(2))
+ {
+ int d = checkSide(edge[0], edge[1]);
+ if (d == 0)
+ return true;
+
+ if (previousSide != 0 && d != previousSide)
+ {
+ return false;
+ }
+ previousSide = d;
+ }
+ int d = checkSide(vertices.back(), vertices.front());
+ return d == previousSide;
+ }
+
+ std::optional<std::size_t>
+ FeatureMerger::checkPoint(const Eigen::Vector2f p, const std::optional<std::size_t> checkFirst)
+ {
+ // when try first is given check this feature before all others
+ if (checkFirst)
+ {
+ if (insideConvexPoly(p, inputFeatures[checkFirst.value()].convexHull->vertices))
+ {
+ return checkFirst.value();
+ }
+ }
+
+ for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
+ {
+ if (insideConvexPoly(p, f.convexHull->vertices))
+ {
+ return i;
+ }
+ }
+ return std::nullopt;
+ }
+
+ void
+ FeatureMerger::addToFeature(Features& f, const Features& toAdd)
+ {
+ f.points.insert(f.points.end(), toAdd.points.begin(), toAdd.points.end());
+
+ f.convexHull = std::nullopt;
+ f.circle = std::nullopt;
+ f.ellipsoid = std::nullopt;
+ f.chain = std::nullopt;
+ }
+
+ void
+ FeatureMerger::recalculateFeatures(std::vector<Features> features)
+ {
+ for (auto& f : features)
+ {
+ if (!f.convexHull)
+ {
+ f.convexHull = FeatureExtractor::convexHull(f.points);
+ }
+
+ // if (!f.circle)
+ // {
+ // f.circle = FeatureExtractor::circle(f.points);
+ // }
+
+ // if (!f.ellipsoid)
+ // {
+ // f.ellipsoid = FeatureExtractor::ellipsoid(f.convexHull);
+ // }
+
+ if (!f.chain)
+ {
+ f.chain = FeatureExtractor::chainApproximation(
+ f.points, f.convexHull, chainApproximationParams);
+ }
+ }
+ }
+
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
new file mode 100644
index 00000000..a61e2637
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
@@ -0,0 +1,53 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ class FeatureMerger
+ {
+ public:
+ FeatureMerger(std::vector<Features>&& features,
+ const ChainApproximation::Params& chainApproximationParams);
+ FeatureMerger(const std::vector<Features>& features,
+ const ChainApproximation::Params& chainApproximationParams);
+
+ std::vector<Features> merge();
+
+ private:
+ bool insideConvexPoly(const Eigen::Vector2f& p,
+ const std::vector<Eigen::Vector2f>& vertices);
+ std::optional<std::size_t>
+ checkPoint(const Eigen::Vector2f p,
+ const std::optional<std::size_t> checkFirst = std::nullopt);
+ void addToFeature(Features& f, const Features& toAdd);
+ void recalculateFeatures(std::vector<Features> features);
+
+ private:
+ std::vector<Features> inputFeatures;
+ const ChainApproximation::Params chainApproximationParams;
+ };
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
--
GitLab
From 3daa2a1105c94d0d95e65a7a6bb56cc6e76a6999 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 15:50:35 +0200
Subject: [PATCH 37/55] Fix merge conflict from Armar7
---
.../Component.cpp | 7 +-----
.../client/laser_scanner_features/Reader.cpp | 23 +------------------
2 files changed, 2 insertions(+), 28 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 7a97843a..dd6582c9 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -44,14 +44,10 @@
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include "armarx/navigation/conversions/eigen.h"
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
-<<<<<<< HEAD
-#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
-#include "armarx/navigation/conversions/eigen.h"
-=======
->>>>>>> b85a33d8912874d7fd4c7d409640f63a26291cbf
#include "conversions/eigen.h"
#include "conversions/pcl_eigen.h"
@@ -641,7 +637,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
laserScannerFeatureWriter.store(armemFeatures, getName(), timestamp);
}
-
ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
LaserScannerFeatureExtraction::GetDefaultName());
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index fb15fef7..cf013d07 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -50,9 +50,8 @@ namespace armarx::navigation::memory::client::laser_scanner_features
{
armarx::armem::client::query::Builder qb;
-<<<<<<< HEAD
auto& sel = qb.coreSegments()
- .withName(properties.coreSegmentName)
+ .withName(properties().coreSegmentName)
.providerSegments()
.withName(query.providerName);
@@ -65,26 +64,6 @@ namespace armarx::navigation::memory::client::laser_scanner_features
}
return sel.entities().withName(query.name);
}();
-=======
- qb.coreSegments()
- .withName(properties().coreSegmentName)
- .providerSegments()
- .withName(query.providerName)
- .entities()
- .all()
- .snapshots()
- .beforeOrAtTime(query.timestamp);
-
- // auto entitySel = [&]()
- // {
- // if (query.name.empty())
- // {
- // ARMARX_INFO << "querying all entities";
- // return sel.entities().all();
- // }
- // return sel.entities().withName(query.name);
- // }();
->>>>>>> b85a33d8912874d7fd4c7d409640f63a26291cbf
entitySel.snapshots().beforeOrAtTime(query.timestamp);
--
GitLab
From b53c7436e6432d93b97df389a19f8a2b14c6f1bd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 16:08:11 +0200
Subject: [PATCH 38/55] Fix new writer
---
.../components/laser_scanner_feature_extraction/Component.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index dd6582c9..3d648c33 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -634,7 +634,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
- laserScannerFeatureWriter.store(armemFeatures, getName(), timestamp);
+ laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
}
ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
--
GitLab
From 43bae0aa7b7291c58ee87a3efb477b171816004f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 16:08:28 +0200
Subject: [PATCH 39/55] Move feature transformation to own file
---
.../CMakeLists.txt | 2 +
.../Component.cpp | 48 +----------------
.../conversions/features.cpp | 53 +++++++++++++++++++
.../conversions/features.h | 33 ++++++++++++
4 files changed, 90 insertions(+), 46 deletions(-)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index 9b910c8e..f57118af 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -12,6 +12,7 @@ armarx_add_component(laser_scanner_feature_extraction
#conversions
conversions/eigen.cpp
conversions/pcl.cpp
+ conversions/features.cpp
EnclosingEllipsoid.cpp
Path.cpp
UnionFind.cpp
@@ -31,6 +32,7 @@ armarx_add_component(laser_scanner_feature_extraction
conversions/opencv_eigen.h
conversions/opencv_pcl.h
conversions/pcl.h
+ conversions/features.h
UnionFind.h
DEPENDENCIES
# ArmarXCore
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 3d648c33..3d136d11 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -32,7 +32,6 @@
#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/MathTools.h>
@@ -44,7 +43,7 @@
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
-#include "armarx/navigation/conversions/eigen.h"
+#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h>
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
@@ -572,49 +571,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
const armem::Time& timestamp)
{
- const auto transformPoint =
- [](const Eigen::Vector2f& point, const Eigen::Isometry3f& global_T_frame)
- { return conv::to2D(global_T_frame * conv::to3D(point)); };
-
- const auto transformPoints =
- [&](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& global_T_frame)
- {
- return simox::alg::apply(points,
- [&](const Eigen::Vector2f& point)
- { return transformPoint(point, global_T_frame); });
- };
-
- const auto transformFeature = [&](const Features& feature,
- const Eigen::Isometry3f& global_T_frame) -> Features
- {
- Features transformed;
-
- if (feature.convexHull)
- {
- transformed.convexHull = {
- .vertices = transformPoints(feature.convexHull->vertices, global_T_frame),
- .segments = feature.convexHull->segments};
- }
- if (feature.circle)
- {
- transformed.circle = {.center =
- transformPoint(feature.circle->center, global_T_frame),
- .radius = feature.circle->radius};
- }
- if (feature.ellipsoid)
- {
- transformed.ellipsoid = {.pose = global_T_frame * feature.ellipsoid->pose,
- .radii = feature.ellipsoid->radii};
- }
- if (feature.chain)
- {
- transformed.chain = transformPoints(feature.chain.value(), global_T_frame);
- }
- transformed.points = transformPoints(feature.points, global_T_frame);
-
- return transformed;
- };
-
// TODO(groeger, navarro): implement actual merging
std::vector<Features> merged;
@@ -625,7 +581,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
for (const auto& singleFeature : features.features)
{
- merged.push_back(transformFeature(singleFeature, global_T_frame));
+ merged.push_back(conversions::transformFeature(singleFeature, global_T_frame));
}
}
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp
new file mode 100644
index 00000000..f4121bb6
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.cpp
@@ -0,0 +1,53 @@
+#include "features.h"
+
+#include <SimoxUtility/algorithm/apply.hpp>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h>
+
+namespace armarx::conversions
+{
+ using Features = armarx::navigation::components::laser_scanner_feature_extraction::Features;
+
+ Features
+ transformFeature(const Features& feature, const Eigen::Isometry3f& transformation)
+ {
+ const auto transformPoint =
+ [](const Eigen::Vector2f& point, const Eigen::Isometry3f& global_T_frame)
+ { return to2D(global_T_frame * to3D(point)); };
+
+ const auto transformPoints =
+ [&](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& global_T_frame)
+ {
+ return simox::alg::apply(points,
+ [&](const Eigen::Vector2f& point)
+ { return transformPoint(point, global_T_frame); });
+ };
+
+ Features transformed;
+
+ if (feature.convexHull)
+ {
+ transformed.convexHull = {
+ .vertices = transformPoints(feature.convexHull->vertices, transformation),
+ .segments = feature.convexHull->segments};
+ }
+ if (feature.circle)
+ {
+ transformed.circle = {.center = transformPoint(feature.circle->center, transformation),
+ .radius = feature.circle->radius};
+ }
+ if (feature.ellipsoid)
+ {
+ transformed.ellipsoid = {.pose = transformation * feature.ellipsoid->pose,
+ .radii = feature.ellipsoid->radii};
+ }
+ if (feature.chain)
+ {
+ transformed.chain = transformPoints(feature.chain.value(), transformation);
+ }
+ transformed.points = transformPoints(feature.points, transformation);
+
+ return transformed;
+ }
+
+} // namespace armarx::conversions
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h
new file mode 100644
index 00000000..b77546be
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h
@@ -0,0 +1,33 @@
+/*
+ * 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 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
+
+namespace armarx::conversions
+{
+
+ navigation::components::laser_scanner_feature_extraction::Features transformFeature(
+ const navigation::components::laser_scanner_feature_extraction::Features& feature,
+ const Eigen::Isometry3f& transformation);
+
+} // namespace armarx::conversions
--
GitLab
From 052d9c2dfe8c593cf6ceffb4ed82a8a49d7a7794 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 18:04:41 +0200
Subject: [PATCH 40/55] Use feature merger update visualization
There two new properties for LaserScannerFeatureExtraction to
enable/disable visualization of features from individual sensors and
visualization of the collected merged features.
---
.../Component.cpp | 103 +++++++++++++-----
.../Component.h | 12 +-
2 files changed, 86 insertions(+), 29 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index 3d136d11..fc391cc8 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -43,6 +43,7 @@
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h>
#include "ArVizDrawer.h"
@@ -111,6 +112,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
"p.arviz.drawRawPoints",
"If true, the laser scans will be drawn.");
+ def->optional(properties.arviz.visualizeSeparateFeatures,
+ "p.arviz.visualizeSeparateFeatures",
+ "If true, the features from each sensor will be drawn.");
+
+ def->optional(properties.arviz.visualizeMergedFeatures,
+ "p.arviz.visualizeMergedFeatures",
+ "If true, the merged features from all sensors will be drawn.");
+
return def;
}
@@ -137,8 +146,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
frequencyReporterPublish = std::make_unique<FrequencyReporter>(
getDebugObserver(), "LaserScannerFeatureExtraction_publish");
- frequencyReporterSubscribe = std::make_unique<FrequencyReporter>(
- getDebugObserver(), "LaserScannerFeatureExtraction_subscribe");
+ frequencyReporterRun = std::make_unique<FrequencyReporter>(
+ getDebugObserver(), "LaserScannerFeatureExtraction_run");
arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient());
@@ -321,7 +330,6 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
robot, getRobotStateComponent(), data.header.timestamp.toMicroSecondsSinceEpoch());
const Eigen::Isometry3f global_T_sensor(
robot->getRobotNode(data.header.frame)->getGlobalPose());
- const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
const Eigen::Isometry3f robot_T_sensor(
robot->getRobotNode(data.header.frame)->getPoseInRootFrame());
@@ -419,21 +427,34 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// report the features
publishFeatures(armemFeatures, data.header.timestamp);
- // check if arviz should be triggered
- const auto getOrCreateThrottler = [&]() -> Throttler&
+ if (properties.arviz.visualizeSeparateFeatures)
{
- const auto it = throttlers.find(data.header.frame);
- if (it == throttlers.end())
- {
- throttlers.emplace(data.header.frame, Throttler(10.F));
- }
+ drawFeatures(
+ features, &data, data.header.frame, data.header.timestamp, global_T_sensor);
+ }
- return throttlers.at(data.header.frame);
- };
+ return validFeatures;
+ }
+
+ Throttler&
+ LaserScannerFeatureExtraction::getOrCreateThrottler(std::string frame)
+ {
+ const auto it = throttlers.find(frame);
+ if (it == throttlers.end())
+ {
+ throttlers.emplace(frame, Throttler(10.F));
+ }
+
+ return throttlers.at(frame);
+ }
- if (getOrCreateThrottler().check(data.header.timestamp))
+ void
+ LaserScannerFeatureExtraction::drawRobot(const armem::Time& timestamp)
+ {
+ // check if arviz should be triggered
+ if (getOrCreateThrottler(robot->getName()).check(timestamp))
{
- arVizDrawer->draw(features, data.header.frame, global_T_sensor);
+ const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
{
@@ -461,18 +482,27 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
arVizDrawer->draw(
"cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
}
+ }
+ }
- if (properties.arviz.drawRawPoints)
+ void
+ LaserScannerFeatureExtraction::drawFeatures(
+ const std::vector<Features>& features,
+ const armem::laser_scans::LaserScanStamped* laserPoints,
+ const std::string frame,
+ const armem::Time& timestamp,
+ const Eigen::Isometry3f& global_T_sensor)
+ {
+ // check if arviz should be triggered
+ if (getOrCreateThrottler(frame).check(timestamp))
+ {
+ arVizDrawer->draw(features, frame, global_T_sensor);
+
+ if (properties.arviz.drawRawPoints && laserPoints != nullptr)
{
- arVizDrawer->draw(data, global_T_sensor, simox::Color::magenta(255, 100));
+ arVizDrawer->draw(*laserPoints, global_T_sensor, simox::Color::magenta(255, 100));
}
}
-
- // some debugobserver reporting
- frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
- setDebugObserverDatafield("numClusters", features.size());
-
- return validFeatures;
}
void
@@ -540,7 +570,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
<< " sensors";
- frequencyReporterSubscribe->add(IceUtil::Time::now().toMicroSeconds());
+ frequencyReporterRun->add(IceUtil::Time::now().toMicroSeconds());
std::vector<FramedFeatures> allFeatures;
allFeatures.reserve(laserScanResult.laserScans.size());
@@ -565,32 +595,49 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
mergeFeatures(allFeatures, mostRecentTimestamp);
}
+
+ // visualize robot with bounding box/cable area etc.
+ drawRobot(mostRecentTimestamp);
+
+ // some debugobserver reporting
+ frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
}
void
LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
const armem::Time& timestamp)
{
- // TODO(groeger, navarro): implement actual merging
-
- std::vector<Features> merged;
+ // convert features to global coordinate system
+ std::vector<Features> converted;
for (const auto& features : framedFeatures)
{
- merged.reserve(merged.size() + features.features.size());
+ converted.reserve(converted.size() + features.features.size());
Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
for (const auto& singleFeature : features.features)
{
- merged.push_back(conversions::transformFeature(singleFeature, global_T_frame));
+ converted.push_back(conversions::transformFeature(singleFeature, global_T_frame));
}
}
+ // merge overlapping features
+ FeatureMerger merger{std::move(converted), properties.chainApproximationParams};
+ std::vector<Features> merged = merger.merge();
+
+ // save merged features in another entity with name 'global'
std::string frame = "global";
const Eigen::Isometry3f global_T_global = Eigen::Isometry3f::Identity();
const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
+
+ setDebugObserverDatafield("numClusters", merged.size());
+
+ if (properties.arviz.visualizeMergedFeatures)
+ {
+ drawFeatures(merged, nullptr, frame, timestamp, global_T_global);
+ }
}
ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
index ee32d9ca..e634dd27 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -127,6 +127,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::vector<Features> onFeatures(const armem::laser_scans::LaserScanStamped& data,
const std::vector<Features>& features);
+ Throttler& getOrCreateThrottler(std::string frame);
+ void drawRobot(const armem::Time& timestamp);
+ void drawFeatures(const std::vector<Features>& features,
+ const armem::laser_scans::LaserScanStamped* laserPoints,
+ const std::string frame,
+ const armem::Time& timestamp,
+ const Eigen::Isometry3f& global_T_sensor);
+
void mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
const armem::Time& timestamp);
@@ -189,6 +197,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
struct ArViz
{
bool drawRawPoints = {true};
+ bool visualizeSeparateFeatures = {false};
+ bool visualizeMergedFeatures = {true};
} arviz;
};
@@ -222,7 +232,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::unique_ptr<ArVizDrawer> arVizDrawer;
std::unique_ptr<FrequencyReporter> frequencyReporterPublish;
- std::unique_ptr<FrequencyReporter> frequencyReporterSubscribe;
+ std::unique_ptr<FrequencyReporter> frequencyReporterRun;
std::unordered_map<std::string, Throttler> throttlers;
--
GitLab
From a89f196d4742fd07594c93ffad5c87324dfe9214 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 19:21:44 +0200
Subject: [PATCH 41/55] Remove invalid DEPENDENCIES_PRIVATE
---
.../components/laser_scanner_feature_extraction/CMakeLists.txt | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
index f57118af..7148135e 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -48,8 +48,9 @@ armarx_add_component(laser_scanner_feature_extraction
RobotComponentsInterfaces # For legacy LaserScannerFeatureExtraction topic
armarx_navigation::memory
armem_laser_scans
- DEPENDENCIES_PRIVATE
range-v3::range-v3
DEPENDENCIES_LEGACY
PCL
)
+
+add_subdirectory(test)
--
GitLab
From 5a9f213e6f66159caa037427e1c6677f1918da1a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 19:44:46 +0200
Subject: [PATCH 42/55] Add simple test for feature merger
---
.../test/CMakeLists.txt | 64 ++++++++++++++-
.../LaserScannerFeatureExtractionTest.cpp | 78 ++++++++++---------
.../test/MergerTest.cpp | 66 ++++++++++++++++
3 files changed, 168 insertions(+), 40 deletions(-)
create mode 100644 source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
index 3435a5f5..dee3f438 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/CMakeLists.txt
@@ -1,5 +1,61 @@
+# we can't let tests depend on the component so we create a dummy library
+# with the same code (except the component itself)
+armarx_add_library(laser_scanner_feature_extraction_test_library
+ SOURCES
+ #../Component.cpp
+ ../FeatureExtractor.cpp
+ ../FeatureMerger.cpp
+ ../ArVizDrawer.cpp
+ ../ScanClustering.cpp
+ ../ChainApproximation.cpp
+ ../conversions/eigen.cpp
+ ../conversions/pcl.cpp
+ ../conversions/features.cpp
+ ../EnclosingEllipsoid.cpp
+ ../Path.cpp
+ ../UnionFind.cpp
+ HEADERS
+ #../Component.h
+ ../FeatureExtractor.h
+ ../FeatureMerger.h
+ ../ArVizDrawer.h
+ ../ScanClustering.h
+ ../ChainApproximation.h
+ ../EnclosingEllipsoid.h
+ ../Path.h
+ ../geometry.h
+ ../conversions/eigen.h
+ ../conversions/pcl_eigen.h
+ ../conversions/opencv_eigen.h
+ ../conversions/opencv_pcl.h
+ ../conversions/pcl.h
+ ../conversions/features.h
+ ../UnionFind.h
+ DEPENDENCIES
+ ArmarXCore
+ ArmarXCoreComponentPlugins
+ ArmarXCoreLogging
+ RobotAPIComponentPlugins
+ RobotComponentsInterfaces
+ armarx_navigation::memory
+ armem_laser_scans
+ range-v3::range-v3
+ DEPENDENCIES_LEGACY
+ PCL
+)
-# Libs required for the tests
-SET(LIBS ${LIBS} ArmarXCore LaserScannerFeatureExtraction)
-
-armarx_add_test(LaserScannerFeatureExtractionTest LaserScannerFeatureExtractionTest.cpp "${LIBS}")
+armarx_add_test(laser_scanner_feature_extraction_test
+ TEST_FILES
+ LaserScannerFeatureExtractionTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::laser_scanner_feature_extraction_test_library
+)
+
+armarx_add_test(laser_scanner_feature_extraction_merger_test
+ TEST_FILES
+ MergerTest.cpp
+ DEPENDENCIES
+ ArmarXCore
+ armarx_navigation::laser_scanner_feature_extraction_test_library
+)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
index db413af6..a6efd4ed 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/LaserScannerFeatureExtractionTest.cpp
@@ -20,46 +20,52 @@
* GNU General Public License
*/
-#include "ArmarXCore/core/logging/Logging.h"
-#include "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
-#include <boost/test/tools/old/interface.hpp>
-#define BOOST_TEST_MODULE RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
-
-#define ARMARX_BOOST_TEST
+#include <iostream>
-#include <RobotComponents/Test.h>
+#include <ArmarXCore/core/logging/Logging.h>
-#include <pcl/point_types.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h>
#include "../EnclosingEllipsoid.h"
-#include <iostream>
-BOOST_AUTO_TEST_CASE(testExample)
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXObjects::LaserScannerFeatureExtraction
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
{
- // Ground truth (GT) ellipsoid with params:
- // center = (0,0)
- // radii = (4,2)
- // angle = 0
- armarx::laser_scanner_feature_extraction::FeatureExtractor::Points points;
- points.push_back(Eigen::Vector2f{-4.F, 0});
- points.push_back(Eigen::Vector2f{0, 2.F});
- points.push_back(Eigen::Vector2f{4.F, 0});
- points.push_back(Eigen::Vector2f{0, -2.F});
-
- const Eigen::Vector2f radii(4, 2);
-
- armarx::laser_scanner_feature_extraction::EnclosingEllipsoid ee(points);
-
- // The computed ellipsoid must contain the GT ellipsoid
- BOOST_CHECK_GE(ee.radii.x(), 4.F);
- BOOST_CHECK_GE(ee.radii.y(), 2.F);
-
- // ... but should not be too large
- BOOST_CHECK_LE(ee.radii.x(), 4.1F);
- BOOST_CHECK_LE(ee.radii.y(), 2.1F);
-
- // The computed ellipsoid must be roughly at the position of the GT ellipsoid
- BOOST_CHECK_LT(std::fabs(ee.pose.translation().x()), 0.1);
- BOOST_CHECK_LT(std::fabs(ee.pose.translation().y()), 0.1);
-}
+
+ BOOST_AUTO_TEST_CASE(testExample)
+ {
+ // Ground truth (GT) ellipsoid with params:
+ // center = (0,0)
+ // radii = (4,2)
+ // angle = 0
+
+ FeatureExtractor::Points points;
+ points.push_back(Eigen::Vector2f{-4.F, 0});
+ points.push_back(Eigen::Vector2f{0, 2.F});
+ points.push_back(Eigen::Vector2f{4.F, 0});
+ points.push_back(Eigen::Vector2f{0, -2.F});
+
+ const Eigen::Vector2f radii(4, 2);
+
+ EnclosingEllipsoid ee(points);
+
+ // The computed ellipsoid must contain the GT ellipsoid
+ BOOST_CHECK_GE(ee.radii.x(), 4.F);
+ BOOST_CHECK_GE(ee.radii.y(), 2.F);
+
+ // ... but should not be too large
+ BOOST_CHECK_LE(ee.radii.x(), 4.1F);
+ BOOST_CHECK_LE(ee.radii.y(), 2.1F);
+
+ // The computed ellipsoid must be roughly at the position of the GT ellipsoid
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().x()), 0.1);
+ BOOST_CHECK_LT(std::fabs(ee.pose.translation().y()), 0.1);
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp
new file mode 100644
index 00000000..399a5d15
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/test/MergerTest.cpp
@@ -0,0 +1,66 @@
+/*
+ * 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 RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+ * @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
+ */
+
+#include <iostream>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
+
+#include "../EnclosingEllipsoid.h"
+
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXObjects::LaserScannerFeatureExtraction
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ BOOST_AUTO_TEST_CASE(insideConvexPolyTest)
+ {
+ std::vector<Eigen::Vector2f> polygon;
+ polygon.push_back({0, 0});
+ polygon.push_back({1, 1});
+ polygon.push_back({0, 2});
+ polygon.push_back({-1, 1});
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0, 1}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0.5, 1}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 1}, polygon));
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({0, 2}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({1, 1}, polygon));
+
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 0.5}, polygon));
+ BOOST_CHECK(FeatureMerger::insideConvexPoly({-0.5, 0.51}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({-0.5, 0.49}, polygon));
+
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({1.1, 1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({0, 2.1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({0, -0.1}, polygon));
+ BOOST_CHECK(!FeatureMerger::insideConvexPoly({-1, 2}, polygon));
+ }
+
+} // namespace armarx::navigation::components::laser_scanner_feature_extraction
--
GitLab
From 1a3346f61852097bb27bfe12321b56a01ac36001 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sat, 1 Apr 2023 20:59:49 +0200
Subject: [PATCH 43/55] Some fixes and clean up
---
.../Component.cpp | 7 +++--
.../FeatureMerger.cpp | 26 ++++++++++++++-----
.../FeatureMerger.h | 9 ++++---
3 files changed, 31 insertions(+), 11 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
index fc391cc8..aa5ef331 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -632,11 +632,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
- setDebugObserverDatafield("numClusters", merged.size());
+ setDebugObserverDatafield("numMergedClusters", merged.size());
if (properties.arviz.visualizeMergedFeatures)
{
- drawFeatures(merged, nullptr, frame, timestamp, global_T_global);
+ // visualize the features above the floor
+ auto global_T_vis = global_T_global;
+ global_T_vis.translation() = Eigen::Vector3f{0, 0, 20};
+ drawFeatures(merged, nullptr, frame, timestamp, global_T_vis);
}
}
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
index ccc12ed3..b545f5d0 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
@@ -1,5 +1,7 @@
#include "FeatureMerger.h"
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <armarx/navigation/components/laser_scanner_feature_extraction/UnionFind.h>
#include <range/v3/view/enumerate.hpp>
@@ -24,15 +26,15 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::vector<Features>
FeatureMerger::merge()
{
+ ARMARX_DEBUG << "Starting merge of " << inputFeatures.size() << " features";
UnionFind uf{inputFeatures.size()};
-
for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
{
std::optional<std::size_t> lastFeature = std::nullopt;
for (const auto& p : f.points)
{
- if (auto res = checkPoint(p, lastFeature); res)
+ if (auto res = checkPoint(p, i, lastFeature); res)
{
uf.unite(i, res.value());
lastFeature = res.value();
@@ -45,6 +47,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::size_t representative = uf.find(i);
if (i != representative)
{
+ ARMARX_DEBUG << "add feature " << i << " to feature " << representative;
addToFeature(inputFeatures[representative], f);
}
}
@@ -54,10 +57,14 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
if (i == uf.find(i))
{
+ ARMARX_DEBUG << "feature " << i << " is representative";
merged.push_back(f);
}
}
+ // recalculate convex hull etc. for all merged features
+ recalculateFeatures(merged);
+ ARMARX_DEBUG << "Merged " << inputFeatures.size() << " features into " << merged.size();
return merged;
}
@@ -68,7 +75,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
// algorithm to check whether a point is inside a convex polygon
// see https://stackoverflow.com/questions/1119627/how-to-test-if-a-point-is-inside-of-a-convex-polygon-in-2d-integer-coordinates
- const auto signum = [](int x)
+ const auto signum = [](float x)
{
if (x == 0)
return 0;
@@ -78,7 +85,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const auto checkSide = [&](const Eigen::Vector2f& a, const Eigen::Vector2f& b)
{ return signum((p.x() - a.x()) * (b.y() - a.y()) - (p.y() - a.y()) * (b.x() - a.x())); };
- int previousSide = 0;
+ float previousSide = 0;
for (const auto edge : vertices | ranges::views::sliding(2))
{
int d = checkSide(edge[0], edge[1]);
@@ -92,11 +99,15 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
previousSide = d;
}
int d = checkSide(vertices.back(), vertices.front());
+ if (d == 0)
+ return true;
return d == previousSide;
}
std::optional<std::size_t>
- FeatureMerger::checkPoint(const Eigen::Vector2f p, const std::optional<std::size_t> checkFirst)
+ FeatureMerger::checkPoint(const Eigen::Vector2f p,
+ std::size_t ignore,
+ const std::optional<std::size_t> checkFirst)
{
// when try first is given check this feature before all others
if (checkFirst)
@@ -109,6 +120,9 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
for (const auto [i, f] : ranges::views::enumerate(inputFeatures))
{
+ if (i == ignore || (checkFirst && i == checkFirst.value()))
+ continue;
+
if (insideConvexPoly(p, f.convexHull->vertices))
{
return i;
@@ -129,7 +143,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
}
void
- FeatureMerger::recalculateFeatures(std::vector<Features> features)
+ FeatureMerger::recalculateFeatures(std::vector<Features>& features)
{
for (auto& f : features)
{
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
index a61e2637..9a71c66d 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h
@@ -36,14 +36,17 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
std::vector<Features> merge();
+ public:
+ static bool insideConvexPoly(const Eigen::Vector2f& p,
+ const std::vector<Eigen::Vector2f>& vertices);
+
private:
- bool insideConvexPoly(const Eigen::Vector2f& p,
- const std::vector<Eigen::Vector2f>& vertices);
std::optional<std::size_t>
checkPoint(const Eigen::Vector2f p,
+ std::size_t ignore,
const std::optional<std::size_t> checkFirst = std::nullopt);
void addToFeature(Features& f, const Features& toAdd);
- void recalculateFeatures(std::vector<Features> features);
+ void recalculateFeatures(std::vector<Features>& features);
private:
std::vector<Features> inputFeatures;
--
GitLab
From c253e1b50b7d97e2aad74ffefa57cb69306ecede Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 21 Apr 2023 02:56:37 +0200
Subject: [PATCH 44/55] Make teb more robust
---
.../local_planning/TimedElasticBands.cpp | 23 +++++++++++++++----
1 file changed, 19 insertions(+), 4 deletions(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index a7ab7d2b..d1c4faa9 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -23,6 +23,7 @@
#include <armarx/navigation/local_planning/ros_conversions.h>
// #include <teb_local_planner/extension/obstacles/EllipseObstacle.h>
#include <armarx/navigation/local_planning/aron_conversions_teb.h>
+
#include <teb_local_planner/homotopy_class_planner.h>
namespace armarx::navigation::local_planning
@@ -106,21 +107,35 @@ namespace armarx::navigation::local_planning
fillObstacles();
+ bool success;
try
{
- if (!hcp_->plan(start, end, &velocity_start, !planToDest))
- {
- ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
- }
+ success = hcp_->plan(start, end, &velocity_start, !planToDest);
}
catch (...)
{
+ hcp_->clearPlanner();
ARMARX_ERROR << "Caugth exception while planning: " << GetHandledExceptionString();
return std::nullopt;
}
+ if (!success)
+ {
+ hcp_->clearPlanner();
+ ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
+ return std::nullopt;
+ }
+
+ if (hcp_->hasDiverged())
+ {
+ hcp_->clearPlanner();
+ ARMARX_WARNING << deactivateSpam(5) << "Planner has diverged!";
+ return std::nullopt;
+ }
+
if (hcp_->getTrajectoryContainer().empty())
{
+ hcp_->clearPlanner();
ARMARX_INFO << deactivateSpam(5) << "Did not find any trajectory!";
return std::nullopt;
}
--
GitLab
From d06d5d8d70c0f0146aa1d3a0b2bba5d5b90e84b4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 21 Apr 2023 02:59:22 +0200
Subject: [PATCH 45/55] Remove visualization of local plan when none was found
---
source/armarx/navigation/server/Navigator.cpp | 7 +-----
.../introspection/ArvizIntrospector.cpp | 25 ++++++++++++++++---
.../server/introspection/ArvizIntrospector.h | 7 +++---
.../introspection/IntrospectorInterface.h | 3 ++-
.../introspection/MemoryIntrospector.cpp | 4 +--
.../server/introspection/MemoryIntrospector.h | 3 ++-
6 files changed, 33 insertions(+), 16 deletions(-)
diff --git a/source/armarx/navigation/server/Navigator.cpp b/source/armarx/navigation/server/Navigator.cpp
index 17eb7d47..ad478485 100644
--- a/source/armarx/navigation/server/Navigator.cpp
+++ b/source/armarx/navigation/server/Navigator.cpp
@@ -1064,12 +1064,7 @@ namespace armarx::navigation::server
{
ARMARX_CHECK_NOT_NULL(srv.introspector);
- if (not localPlan.has_value())
- {
- return;
- }
-
- srv.introspector->onLocalPlannerResult(localPlan.value());
+ srv.introspector->onLocalPlannerResult(localPlan);
}
void
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index 89a0a92d..ebf4d66e 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -53,9 +53,14 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
+ ArvizIntrospector::onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result)
{
- drawLocalTrajectory(result.trajectory);
+ if (result)
+ drawLocalTrajectory(result.value().trajectory);
+ else
+ drawLocalTrajectory(std::nullopt);
+
arviz.commit(simox::alg::get_values(layers));
}
@@ -175,8 +180,22 @@ namespace armarx::navigation::server
}
void
- ArvizIntrospector::drawLocalTrajectory(const core::LocalTrajectory& trajectory)
+ ArvizIntrospector::drawLocalTrajectory(const std::optional<core::LocalTrajectory>& input)
{
+ if (!input)
+ {
+ ARMARX_IMPORTANT << "reset trajectory visualization!";
+ // no local trajectory found, remove old one
+ auto layer = arviz.layer("local_planner");
+ auto velLayer = arviz.layer("local_planner_velocity");
+
+ layers[layer.data_.name] = std::move(layer);
+ layers[velLayer.data_.name] = std::move(velLayer);
+ return;
+ }
+
+ const auto trajectory = input.value();
+
auto layer = arviz.layer("local_planner");
const std::vector<Eigen::Vector3f> points =
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.h b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
index 3b286515..b97da8bc 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.h
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.h
@@ -54,10 +54,11 @@ namespace armarx::navigation::server
~ArvizIntrospector() override = default;
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result) override;
void onRobotPose(const core::Pose& pose) override;
-
+
void onGoal(const core::Pose& goal) override;
void success() override;
@@ -76,7 +77,7 @@ namespace armarx::navigation::server
private:
void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
- void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
+ void drawLocalTrajectory(const std::optional<core::LocalTrajectory>& trajectory);
void drawRawVelocity(const core::Twist& twist);
void drawSafeVelocity(const core::Twist& twist);
diff --git a/source/armarx/navigation/server/introspection/IntrospectorInterface.h b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
index 1b403203..54980724 100644
--- a/source/armarx/navigation/server/introspection/IntrospectorInterface.h
+++ b/source/armarx/navigation/server/introspection/IntrospectorInterface.h
@@ -43,7 +43,8 @@ namespace armarx::navigation::server
virtual void failure() = 0;
virtual void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) = 0;
- virtual void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) = 0;
+ virtual void
+ onLocalPlannerResult(const std::optional<local_planning::LocalPlannerResult>& result) = 0;
virtual void onRobotPose(const core::Pose& pose) = 0;
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
index c0b97ecf..b9afc87d 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.cpp
@@ -17,12 +17,12 @@ namespace armarx::navigation::server
}
void
- MemoryIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
+ MemoryIntrospector::onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result)
{
// TODO(fabian.reister): implement
}
-
void
MemoryIntrospector::onGoal(const core::Pose& goal)
{
diff --git a/source/armarx/navigation/server/introspection/MemoryIntrospector.h b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
index 55b5c4f9..ede5f470 100644
--- a/source/armarx/navigation/server/introspection/MemoryIntrospector.h
+++ b/source/armarx/navigation/server/introspection/MemoryIntrospector.h
@@ -39,7 +39,8 @@ namespace armarx::navigation::server
void onGlobalPlannerResult(const global_planning::GlobalPlannerResult& result) override;
- void onLocalPlannerResult(const local_planning::LocalPlannerResult& result) override;
+ void onLocalPlannerResult(
+ const std::optional<local_planning::LocalPlannerResult>& result) override;
void onRobotPose(const core::Pose& pose) override {}
--
GitLab
From e06af39dbf8a1bc8c52429a4b978f73f6952792c Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Fri, 21 Apr 2023 11:29:14 +0200
Subject: [PATCH 46/55] Set different log level
---
source/armarx/navigation/local_planning/TimedElasticBands.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index a7ab7d2b..d90708db 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -52,7 +52,7 @@ namespace armarx::navigation::local_planning
setTebCostmap();
if (teb_costmap)
{
- ARMARX_VERBOSE << "Costmap available.";
+ ARMARX_INFO << "Costmap available.";
// TODO: where to put all the parameters
const human::ExponentialPenaltyModel penalty{
.minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
--
GitLab
From f31e27c032f8da3f4d7d313943597d48f4dd5f0b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Fri, 21 Apr 2023 11:33:15 +0200
Subject: [PATCH 47/55] Add circle calculation to features
---
.../laser_scanner_feature_extraction/ArVizDrawer.cpp | 12 ++++++------
.../laser_scanner_feature_extraction/ArVizDrawer.h | 3 +--
.../FeatureExtractor.cpp | 2 +-
.../FeatureMerger.cpp | 8 ++++----
4 files changed, 12 insertions(+), 13 deletions(-)
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
index ad8b55c8..ef07b9ca 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -34,7 +34,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
const std::string& frame,
const Eigen::Isometry3f& globalSensorPose)
{
- // drawCircles(features, frame, globalSensorPose);
+ drawCircles(features, frame, globalSensorPose);
drawConvexHulls(features, frame, globalSensorPose);
drawEllipsoids(features, frame, globalSensorPose);
drawChains(features, frame, globalSensorPose);
@@ -76,19 +76,18 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
const Eigen::Vector3f position =
- globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), -1.F);
+ globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), 10.F);
layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
.axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
.position(position)
- .color(simox::Color::red(200, 100)));
+ .color(color));
}
void
ArVizDrawer::drawCircles(const std::vector<Features>& features,
const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose,
- const simox::Color& color)
+ const Eigen::Isometry3f& globalSensorPose)
{
auto layer = arviz.layer("circles_" + frame);
@@ -100,7 +99,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
{
return;
}
- drawCircle(layer, *f.circle, globalSensorPose, color);
+ drawCircle(
+ layer, *f.circle, globalSensorPose, simox::Color::red(200, 100));
});
arviz.commit(layer);
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
index f1c51a4c..c5668778 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -64,8 +64,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
private:
void drawCircles(const std::vector<Features>& features,
const std::string& frame,
- const Eigen::Isometry3f& globalSensorPose,
- const simox::Color& color);
+ const Eigen::Isometry3f& globalSensorPose);
void drawCircle(viz::Layer& layer,
const Circle& circle,
const Eigen::Isometry3f& globalSensorPose,
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
index ce41f5fe..6a7ab54b 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -109,7 +109,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
return Features{
.convexHull = hull,
- .circle = std::nullopt, //circle(points),
+ .circle = circle(points),
.ellipsoid = std::nullopt, //ellipsoid(hull),
.chain = chainApproximation(points, hull, chainApproximationParams),
.points = points};
diff --git a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
index b545f5d0..173e3aef 100644
--- a/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.cpp
@@ -152,10 +152,10 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
f.convexHull = FeatureExtractor::convexHull(f.points);
}
- // if (!f.circle)
- // {
- // f.circle = FeatureExtractor::circle(f.points);
- // }
+ if (!f.circle)
+ {
+ f.circle = FeatureExtractor::circle(f.points);
+ }
// if (!f.ellipsoid)
// {
--
GitLab
From 2b9568b5831fb5cf55cae59784fa59b4fbcc39cf Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Mon, 24 Apr 2023 18:43:22 +0200
Subject: [PATCH 48/55] Improve human visualization
---
.../navigation_memory/Component.cpp | 6 ++++-
.../components/navigation_memory/Component.h | 1 +
.../components/navigation_memory/Visu.cpp | 22 ++++++++++---------
.../components/navigation_memory/Visu.h | 2 +-
4 files changed, 19 insertions(+), 12 deletions(-)
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index 95ee3744..b6bf6427 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Component.cpp
@@ -106,6 +106,10 @@ namespace armarx::navigation::components::navigation_memory
"p.visuTransparent",
"Enable visualization of humans a bit transparent.");
+ def->optional(properties.visuHumanMaxAgeMs,
+ "p.visuHumanMaxAgeMs",
+ "The maximum age of humans to be drawn in ms.");
+
def->optional(properties.visuRooms,
"p.visuRooms",
"Enable visualization of rooms.");
@@ -562,7 +566,7 @@ namespace armarx::navigation::components::navigation_memory
visu.drawCostmaps(layers, p.visuCostmaps);
// Humans
- visu.drawHumans(layers, p.visuHumans, p.visuTransparent);
+ visu.drawHumans(layers, p.visuHumans, p.visuTransparent, Duration::MilliSeconds(p.visuHumanMaxAgeMs));
// Rooms
visu.drawRooms(layers, p.visuRooms);
diff --git a/source/armarx/navigation/components/navigation_memory/Component.h b/source/armarx/navigation/components/navigation_memory/Component.h
index bc359652..32ca3ad9 100644
--- a/source/armarx/navigation/components/navigation_memory/Component.h
+++ b/source/armarx/navigation/components/navigation_memory/Component.h
@@ -114,6 +114,7 @@ namespace armarx::navigation::components::navigation_memory
bool visuHumans = true;
bool visuTransparent = false;
+ int visuHumanMaxAgeMs = 1000;
bool visuRooms = true;
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.cpp b/source/armarx/navigation/components/navigation_memory/Visu.cpp
index 506b8b9e..06c56ea0 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.cpp
+++ b/source/armarx/navigation/components/navigation_memory/Visu.cpp
@@ -156,22 +156,24 @@ namespace armarx::navigation::memory
// cylinder.radius(300);
// layer.add(cylinder);
+ core::Pose human3d = conv::to3D(human.pose) * human_T_mmm;
viz::Robot mmm(std::to_string(layer.size()));
mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
- mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+ mmm.pose(human3d);
mmm.scale(1.7); // 1.7m
mmm.overrideColor(viz::Color::orange(255, visuTransparent ? 100 : 255));
layer.add(mmm);
- core::Pose pose3d = conv::to3D(human.pose);
- pose3d.translation() += Eigen::Vector3f{0, 0, 1000};
- auto arrow = viz::Arrow(std::to_string(layer.size()))
- .pose(pose3d)
- .length(200)
- .color(simox::Color::red());
- layer.add(arrow);
+ if(human.linearVelocity != Eigen::Vector2f::Zero()) {
+ core::Pose vel3d = human3d;
+ vel3d.translation().head<2>() += human.linearVelocity * 2;
+ auto arrow = viz::Arrow(std::to_string(layer.size()))
+ .fromTo(human3d.translation(), vel3d.translation())
+ .color(simox::Color::red());
+ layer.add(arrow);
+ }
}
}
@@ -285,7 +287,8 @@ namespace armarx::navigation::memory
void
Visu::drawHumans(std::vector<viz::Layer>& layers,
const bool enabled,
- const bool visuTransparent)
+ const bool visuTransparent,
+ const Duration maxAge)
{
if (not enabled)
{
@@ -294,7 +297,6 @@ namespace armarx::navigation::memory
std::map<std::string, navigation::human::Humans> namedProviderHumans;
const DateTime timestamp = Clock::Now();
- const Duration maxAge = Duration::MilliSeconds(500);
humanSegment.doLocked(
[&]()
diff --git a/source/armarx/navigation/components/navigation_memory/Visu.h b/source/armarx/navigation/components/navigation_memory/Visu.h
index 86515aa8..9ee945cd 100644
--- a/source/armarx/navigation/components/navigation_memory/Visu.h
+++ b/source/armarx/navigation/components/navigation_memory/Visu.h
@@ -57,7 +57,7 @@ namespace armarx::navigation::memory
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, bool visuTransparent);
+ void drawHumans(std::vector<viz::Layer>& layers, bool enabled, bool visuTransparent, Duration maxAge);
void drawRooms(std::vector<viz::Layer>& layers, bool enabled);
--
GitLab
From b620bfe79152fd81efd017e9c048a520dddd524f Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Mon, 24 Apr 2023 18:43:50 +0200
Subject: [PATCH 49/55] Make use of kalman filter optional in HumanTracker
---
.../armarx/navigation/human/HumanFilter.cpp | 87 +++++++++++--------
source/armarx/navigation/human/HumanFilter.h | 3 +-
.../armarx/navigation/human/HumanTracker.cpp | 2 +-
source/armarx/navigation/human/HumanTracker.h | 3 +
4 files changed, 57 insertions(+), 38 deletions(-)
diff --git a/source/armarx/navigation/human/HumanFilter.cpp b/source/armarx/navigation/human/HumanFilter.cpp
index baf14d07..cb4172d8 100644
--- a/source/armarx/navigation/human/HumanFilter.cpp
+++ b/source/armarx/navigation/human/HumanFilter.cpp
@@ -3,7 +3,7 @@
namespace armarx::navigation::human
{
- HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime)
+ HumanFilter::HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter) : useKalmanFilter(useKalmanFilter)
{
//initialize new human for detected human
human.pose = pose0;
@@ -11,29 +11,31 @@ namespace armarx::navigation::human
human.detectionTime = detectionTime;
lastDetectedHuman = human;
- //initialize new kalman filter for new detected human
- UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
- UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
- Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
- Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
+ if (useKalmanFilter) {
+ //initialize new kalman filter for new detected human
+ UnscentedKalmanFilter<SystemModelT>::PropCovT Q =
+ UnscentedKalmanFilter<SystemModelT>::PropCovT::Identity();
+ Q.block<2, 2>(0, 0) *= params.control_pos_cov * params.control_pos_cov;
+ Q.block<1, 1>(2, 2) *= params.control_rot_cov * params.control_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
- UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
- R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
- R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT R =
+ UnscentedKalmanFilter<SystemModelT>::ObsCovT::Identity();
+ R.block<2, 2>(0, 0) *= params.obs_pos_cov * params.obs_pos_cov;
+ R.block<1, 1>(2, 2) *= params.obs_rot_cov * params.obs_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
- UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
- P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
- P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
+ UnscentedKalmanFilter<SystemModelT>::StateCovT P0 =
+ UnscentedKalmanFilter<SystemModelT>::StateCovT::Identity();
+ P0.block<2, 2>(0, 0) *= params.initial_state_pos_cov * params.initial_state_pos_cov;
+ P0.block<1, 1>(2, 2) *= params.initial_state_rot_cov * params.initial_state_rot_cov;
- UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
- UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
+ UnscentedKalmanFilter<SystemModelT>::AlphaT alpha =
+ UnscentedKalmanFilter<SystemModelT>::AlphaT::Ones() * params.alpha;
- //initial position and orientation according to detected human
- SystemModelT::StateT state0 = toUkfState(pose0);
+ //initial position and orientation according to detected human
+ SystemModelT::StateT state0 = toUkfState(pose0);
- ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ ukf = std::make_unique<UnscentedKalmanFilter<SystemModelT>>(Q, R, alpha, state0, P0);
+ }
}
void
@@ -41,32 +43,45 @@ namespace armarx::navigation::human
{
double dt = (detectionTime - human.detectionTime).toSecondsDouble();
- SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
- ukf->propagation(control, dt);
+ if (useKalmanFilter) {
+ SystemModelT::ControlT control = toUkfControl(human.linearVelocity);
+ ukf->propagation(control, dt);
- human.pose = fromUkfState(ukf->getCurrentState());
- human.detectionTime = detectionTime;
+ human.pose = fromUkfState(ukf->getCurrentState());
+ human.detectionTime = detectionTime;
+ } else {
+ human.pose = human.estimateAt(detectionTime);
+ human.detectionTime = detectionTime;
+ }
}
void
HumanFilter::update(const core::Pose2D& pose, const DateTime& detectionTime)
{
double dt = (detectionTime - lastDetectedHuman.detectionTime).toSecondsDouble();
-
- //update ukf with new observation
- SystemModelT::ObsT observation = toUkfObs(pose);
- ukf->update(observation);
-
- core::Pose2D newPose = fromUkfState(ukf->getCurrentState());
- newPose.linear() = pose.linear();
+ core::Pose2D newPose;
+
+ if (useKalmanFilter) {
+ //update ukf with new observation
+ SystemModelT::ObsT observation = toUkfObs(pose);
+ ukf->update(observation);
+
+ newPose = fromUkfState(ukf->getCurrentState());
+ newPose.linear() = pose.linear();
+ } else
+ {
+ newPose = pose;
+ }
// calculate velocity
- Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
- Eigen::Vector2f linVelocity = ds / dt;
- // apply exponential smoothing to velocity
- //TODO try other approaches?
- Eigen::Vector2f newVelocity =
- params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+ Eigen::Vector2f newVelocity = human.linearVelocity;
+ if (dt > 0) {
+ Eigen::Vector2f ds = newPose.translation() - lastDetectedHuman.pose.translation();
+ Eigen::Vector2f linVelocity = ds / dt;
+ // apply exponential smoothing to velocity
+ //TODO try other approaches?
+ newVelocity = params.velocityAlpha * linVelocity + (1 - params.velocityAlpha) * human.linearVelocity;
+ }
//update stored information about the human
human.detectionTime = detectionTime;
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index aaed6eec..ad0725da 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -59,7 +59,7 @@ namespace armarx::navigation::human
* @param pose0 the first known pose of the human
* @param detectionTime the point of detection
*/
- HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime);
+ HumanFilter(const core::Pose2D& pose0, const DateTime& detectionTime, bool useKalmanFilter);
/**
* @brief HumanFilter::propagation propagate the system model to the given point in time. This
@@ -105,6 +105,7 @@ namespace armarx::navigation::human
*/
Human human{};
std::unique_ptr<UnscentedKalmanFilter<SystemModelT>> ukf = nullptr;
+ bool useKalmanFilter;
};
} // namespace armarx::navigation::human
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 7273cdd7..4c54b2a3 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -39,7 +39,7 @@ namespace armarx::navigation::human
{
//add new tracked human to list of tracked humans
trackedHumans.push_back(TrackedHuman{
- .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
+ .humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime, parameters.useKalmanFilter},
.trackingId = detectedHuman.trackingId,
.associatedCluster = nullptr,
.associated = true});
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 32c25cc4..de04a014 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -113,6 +113,9 @@ namespace armarx::navigation::human
// alpha value from interval [0,1] to determine how much the current (and respectively
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
+
+ // whether to use the kalman filter inside the HumanFilter
+ bool useKalmanFilter = false;
};
/**
--
GitLab
From bcc191018eb27bff2aaaf651f51912aa43ab3409 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Mon, 24 Apr 2023 18:44:53 +0200
Subject: [PATCH 50/55] Remove some unnecessary debug messages
---
.../navigation/memory/client/laser_scanner_features/Reader.cpp | 2 +-
.../navigation/server/introspection/ArvizIntrospector.cpp | 1 -
2 files changed, 1 insertion(+), 2 deletions(-)
diff --git a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
index cf013d07..bfc47031 100644
--- a/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -59,7 +59,7 @@ namespace armarx::navigation::memory::client::laser_scanner_features
{
if (query.name.empty())
{
- ARMARX_INFO << "querying all entities";
+ ARMARX_DEBUG << "querying all entities";
return sel.entities().all();
}
return sel.entities().withName(query.name);
diff --git a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
index ebf4d66e..eaafd1b9 100644
--- a/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
+++ b/source/armarx/navigation/server/introspection/ArvizIntrospector.cpp
@@ -184,7 +184,6 @@ namespace armarx::navigation::server
{
if (!input)
{
- ARMARX_IMPORTANT << "reset trajectory visualization!";
// no local trajectory found, remove old one
auto layer = arviz.layer("local_planner");
auto velLayer = arviz.layer("local_planner_velocity");
--
GitLab
From 182ddc74a686a9bdf372ca625a3bf84111feadcb Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 24 Apr 2023 18:45:44 +0200
Subject: [PATCH 51/55] Add confidence to tracker
---
.../armarx/navigation/human/HumanTracker.cpp | 18 ++++++++++++------
source/armarx/navigation/human/HumanTracker.h | 8 ++++++--
2 files changed, 18 insertions(+), 8 deletions(-)
diff --git a/source/armarx/navigation/human/HumanTracker.cpp b/source/armarx/navigation/human/HumanTracker.cpp
index 7273cdd7..9f585d64 100644
--- a/source/armarx/navigation/human/HumanTracker.cpp
+++ b/source/armarx/navigation/human/HumanTracker.cpp
@@ -6,12 +6,12 @@
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include <armarx/navigation/human/types.h>
#include <armarx/navigation/conversions/eigen.h>
+#include <armarx/navigation/human/types.h>
+
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
-
namespace armarx::navigation::human
{
@@ -42,7 +42,8 @@ namespace armarx::navigation::human
.humanFilter = HumanFilter{detectedHuman.pose, detectedHuman.detectionTime},
.trackingId = detectedHuman.trackingId,
.associatedCluster = nullptr,
- .associated = true});
+ .associated = true,
+ .confidence = parameters.confidenceCamera});
}
}
}
@@ -108,6 +109,7 @@ namespace armarx::navigation::human
posDistance.oldHuman->associatedCluster = nullptr;
posDistance.oldHuman->associated = true;
posDistance.newCluster->associated = true;
+ posDistance.oldHuman->confidence += parameters.confidenceLaser;
}
}
@@ -131,6 +133,7 @@ namespace armarx::navigation::human
human.associatedCluster = nullptr;
human.associated = true;
+ human.confidence += parameters.confidenceLaser;
}
}
@@ -160,7 +163,6 @@ namespace armarx::navigation::human
trackedHumans.clear();
}
-
HumanTracker::DetectedHuman
HumanTracker::convertHumanPoseToDetectedHuman(const DateTime& time,
const armem::human::HumanPose& humanPose)
@@ -326,6 +328,8 @@ namespace armarx::navigation::human
trackedHuman->humanFilter.update(detectedHuman->pose, detectedHuman->detectionTime);
trackedHuman->trackingId = detectedHuman->trackingId;
+
+ trackedHuman->confidence = parameters.confidenceCamera;
}
float
@@ -365,8 +369,10 @@ namespace armarx::navigation::human
for (auto it = trackedHumans.begin(); it != trackedHumans.end();)
{
auto& human = *it;
- // when the tracked human recieved no new measurement for too long, remove it
- if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge)
+ // when the tracked human recieved no new measurement for too long or
+ // wasn't detected by the camera for too long, remove it
+ if (human.humanFilter.detectionAge(time) >= parameters.maxTrackingAge ||
+ human.confidence <= 0)
{
it = trackedHumans.erase(it);
}
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 32c25cc4..3e43f9a4 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -26,14 +26,14 @@
#include <RobotAPI/libraries/ukfm/UnscentedKalmanFilter.h>
-#include <VisionX/libraries/armem_human/types.h>
-
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/human/HumanFilter.h>
#include <armarx/navigation/human/HumanSystemModel.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/memory/types.h>
+#include <VisionX/libraries/armem_human/types.h>
+
namespace armarx::navigation::human
{
using Cluster = memory::LaserScannerFeature;
@@ -82,6 +82,7 @@ namespace armarx::navigation::human
std::optional<std::string> trackingId = std::nullopt;
AdvancedCluster* associatedCluster;
bool associated;
+ int confidence;
};
struct PosHumanDistance
@@ -113,6 +114,9 @@ namespace armarx::navigation::human
// alpha value from interval [0,1] to determine how much the current (and respectively
// the old) velocity should be weighted when calculating the new velocity
float velocityAlpha = 0.7;
+
+ int confidenceCamera = 100;
+ int confidenceLaser = -2;
};
/**
--
GitLab
From 57726e8d9efecfffe3d3a0f930173776ffb974a5 Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Mon, 24 Apr 2023 20:32:20 +0200
Subject: [PATCH 52/55] Adjust some parameters
---
source/armarx/navigation/human/HumanFilter.h | 2 +-
source/armarx/navigation/human/ProxemicZoneCreator.h | 2 +-
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/human/HumanFilter.h b/source/armarx/navigation/human/HumanFilter.h
index ad0725da..c252166c 100644
--- a/source/armarx/navigation/human/HumanFilter.h
+++ b/source/armarx/navigation/human/HumanFilter.h
@@ -50,7 +50,7 @@ namespace armarx::navigation::human
double initial_state_rot_cov = 0.01;
double alpha = 0.5;
- float velocityAlpha = 0.7;
+ float velocityAlpha = 0.5;
};
/**
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
index fed4c18c..084a9215 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.h
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -37,7 +37,7 @@ namespace armarx::navigation::human
float intimateRadius = 400;
float personalRadius = 1000;
- float movementInfluence = 1;
+ float movementInfluence = 0.8;
// an offset applied to the personal zone in the coordinate system of the human
// a positive x-value means an offset to the right
// a positive y-value means an offset to the front
--
GitLab
From 48e0a17f9956343fb4a5da6864ea09506659645e Mon Sep 17 00:00:00 2001
From: Your Name <you@example.com>
Date: Fri, 5 May 2023 14:42:39 +0200
Subject: [PATCH 53/55] FR: local changes on ARMAR-DE
---
.../TimedElasticBands/default.json | 26 +-
.../PlatformNavigation/PlatformNavigation.scx | 3 +
.../config/HumanMemoryApp.cfg | 368 +++++++++++++++
.../config/VisionMemory.cfg | 64 +++
.../distance_to_obstacle_costmap_provider.cfg | 2 +-
..._distance_to_obstacle_costmap_provider.cfg | 10 +-
.../config/dynamic_scene_provider.cfg | 439 ++++++++++++++++++
.../laser_scanner_feature_extraction.cfg | 395 ++++++++++++++++
.../config/navigation_memory.cfg | 60 ++-
.../PlatformNavigation/config/navigator.cfg | 34 +-
source/armarx/navigation/human/HumanTracker.h | 2 +-
11 files changed, 1370 insertions(+), 33 deletions(-)
create mode 100644 scenarios/PlatformNavigation/config/HumanMemoryApp.cfg
create mode 100644 scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg
create mode 100644 scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg
diff --git a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
index 67fe56d7..6ff029f5 100644
--- a/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
+++ b/data/armarx_navigation/local_planner_config/TimedElasticBands/default.json
@@ -1,12 +1,12 @@
{
"robot_footprint_radius": 0.5,
- "planning_distance": 3,
+ "planning_distance": 5,
"teb_config": {
"pse": {
"pse_costum_obstacle_penalties": true,
"pse_costum_obstacle_penalties_dynamic": true,
- "weight_costmap": 1,
+ "weight_costmap": 0,
"weight_global_path_position": 0.3,
"weight_global_path_orientation": 0.3,
"lrk_use_alternative_approach": false,
@@ -26,20 +26,20 @@
"exact_arc_length": false,
"force_reinit_new_goal_dist": 1,
"force_reinit_new_goal_angular": 1.5707963267948966,
- "feasibility_check_no_poses": 5,
+ "feasibility_check_no_poses": -1,
"feasibility_check_lookahead_distance": -1,
"min_resolution_collision_check_angular": 3.141592653589793
},
"robot": {
- "max_vel_x": 0.5,
- "max_vel_x_backwards": 0.5,
- "max_vel_y": 0.5,
- "max_vel_trans": 0.5,
- "max_vel_theta": 0.5,
- "acc_lim_x": 0.5,
- "acc_lim_y": 0.5,
- "acc_lim_theta": 0.5,
+ "max_vel_x": 0.3,
+ "max_vel_x_backwards": 0.3,
+ "max_vel_y": 0.3,
+ "max_vel_trans": 0.3,
+ "max_vel_theta": 0.3,
+ "acc_lim_x": 0.3,
+ "acc_lim_y": 0.3,
+ "acc_lim_theta": 0.3,
"min_turning_radius": 0
},
@@ -74,8 +74,8 @@
"weight_kinematics_nh": 1,
"weight_kinematics_forward_drive": 1,
"weight_kinematics_turning_radius": 1,
- "weight_optimaltime": 1,
- "weight_shortest_path": 1,
+ "weight_optimaltime": 0.5,
+ "weight_shortest_path": 0.5,
"weight_obstacle": 50,
"weight_inflation": 0.1,
"weight_dynamic_obstacle": 50,
diff --git a/scenarios/PlatformNavigation/PlatformNavigation.scx b/scenarios/PlatformNavigation/PlatformNavigation.scx
index 5bce3045..15bced3c 100644
--- a/scenarios/PlatformNavigation/PlatformNavigation.scx
+++ b/scenarios/PlatformNavigation/PlatformNavigation.scx
@@ -13,5 +13,8 @@
<application name="dynamic_distance_to_obstacle_costmap_provider" instance="" package="armarx_navigation" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="navigation_skill_provider" instance="" package="armarx_navigation" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="SkillsMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="laser_scanner_feature_extraction" 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"/>
</scenario>
diff --git a/scenarios/PlatformNavigation/config/HumanMemoryApp.cfg b/scenarios/PlatformNavigation/config/HumanMemoryApp.cfg
new file mode 100644
index 00000000..28099ebc
--- /dev/null
+++ b/scenarios/PlatformNavigation/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/PlatformNavigation/config/VisionMemory.cfg b/scenarios/PlatformNavigation/config/VisionMemory.cfg
index 1b95447f..d93d1e81 100644
--- a/scenarios/PlatformNavigation/config/VisionMemory.cfg
+++ b/scenarios/PlatformNavigation/config/VisionMemory.cfg
@@ -253,6 +253,70 @@
# ArmarX.VisionMemory.occupancyGridMaxHistorySize = 20
+# ArmarX.VisionMemory.pcxyz.seg.CoreMaxHistorySize: Maximal size of the PointCloudXYZ entity histories (-1 for infinite).
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyz.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyz.seg.CoreSegmentName: Name of the PointCloudXYZ core segment.
+# Attributes:
+# - Default: PointCloudXYZ
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyz.seg.CoreSegmentName = PointCloudXYZ
+
+
+# ArmarX.VisionMemory.pcxyzl.seg.CoreMaxHistorySize: Maximal size of the PointCloudXYZL entity histories (-1 for infinite).
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzl.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzl.seg.CoreSegmentName: Name of the PointCloudXYZL core segment.
+# Attributes:
+# - Default: PointCloudXYZL
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzl.seg.CoreSegmentName = PointCloudXYZL
+
+
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreMaxHistorySize: Maximal size of the PointCloudXYZRGBA entity histories (-1 for infinite).
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreSegmentName: Name of the PointCloudXYZRGBA core segment.
+# Attributes:
+# - Default: PointCloudXYZRGBA
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzrgba.seg.CoreSegmentName = PointCloudXYZRGBA
+
+
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreMaxHistorySize: Maximal size of the PointCloudXYZRGBL entity histories (-1 for infinite).
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreMaxHistorySize = 20
+
+
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreSegmentName: Name of the PointCloudXYZRGBL core segment.
+# Attributes:
+# - Default: PointCloudXYZRGBL
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.VisionMemory.pcxyzrgbl.seg.CoreSegmentName = PointCloudXYZRGBL
+
+
# ArmarX.VisionMemory.pointCloudMaxHistorySize:
# Attributes:
# - Default: 20
diff --git a/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg b/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg
index 6059c7a8..08bb95d4 100644
--- a/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/PlatformNavigation/config/distance_to_obstacle_costmap_provider.cfg
@@ -257,6 +257,6 @@
# - Default: Armar6
# - Case sensitivity: yes
# - Required: no
-# ArmarX.distance_to_obstacle_costmap_provider.p.robotName = Armar6
+ArmarX.distance_to_obstacle_costmap_provider.p.robotName = ArmarDE
diff --git a/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
index 81d7cebf..48635a5e 100644
--- a/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
+++ b/scenarios/PlatformNavigation/config/dynamic_distance_to_obstacle_costmap_provider.cfg
@@ -226,20 +226,20 @@
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.costmap.Provider = ""
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment: Name of the mapping memory core segment to use.
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.CoreSegment:
# Attributes:
# - Default: LaserScannerFeatures
# - Case sensitivity: yes
# - Required: no
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.CoreSegment = LaserScannerFeatures
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName:
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.Memory:
# Attributes:
-# - Default: Vision
+# - Default: Navigation
# - Case sensitivity: yes
# - Required: no
-# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.vision.laser_scanner_features.MemoryName = Vision
+# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mem.nav.laser_scanner_features.Memory = Navigation
# ArmarX.dynamic_distance_to_obstacle_costmap_provider.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg b/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg
new file mode 100644
index 00000000..8b2592fd
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/dynamic_scene_provider.cfg
@@ -0,0 +1,439 @@
+# ==================================================================
+# dynamic_scene_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.dynamic_scene_provider.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.dynamic_scene_provider.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.dynamic_scene_provider.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.EnableProfiling = false
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.dynamic_scene_provider.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectMemoryName = ObjectMemory
+
+
+# ArmarX.dynamic_scene_provider.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.ObjectName = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment:
+# Attributes:
+# - Default: Pose
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.CoreSegment = Pose
+
+
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory:
+# Attributes:
+# - Default: Human
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.human.pose.Memory = Human
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# 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.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Memory = Navigation
+
+
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.nav.laser_scanner_features.Provider = ""
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory:
+# Attributes:
+# - Default: RobotState
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.Memory = RobotState
+
+
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment: Name of the localization memory core segment to use.
+# Attributes:
+# - Default: Localization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.robot_state.localizationSegment = Localization
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment:
+# Attributes:
+# - Default: OccupancyGrid
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.CoreSegment = OccupancyGrid
+
+
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory:
+# Attributes:
+# - Default: Vision
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mem.vision.occupancy_grid.Memory = Vision
+
+
+# ArmarX.dynamic_scene_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.dynamic_scene_provider.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.dynamic_scene_provider.p.humanPoses.enabled: Whether human poses are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+ArmarX.dynamic_scene_provider.p.humanPoses.enabled = true
+
+
+# ArmarX.dynamic_scene_provider.p.humanPoses.providerName:
+# Attributes:
+# - Default: AzureKinectPointCloudProvider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.humanPoses.providerName = AzureKinectPointCloudProvider
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.enabled: Whether laser scanner features are used.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.enabled = true
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name:
+# Attributes:
+# - Default: global
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.name = global
+
+
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName:
+# Attributes:
+# - Default: LaserScannerFeatureExtraction
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.laserScannerFeatures.providerName = LaserScannerFeatureExtraction
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold:
+# Attributes:
+# - Default: 0.449999988
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.freespaceThreshold = 0.449999988
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.name = ""
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold:
+# Attributes:
+# - Default: 0.550000012
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.occupiedThreshold = 0.550000012
+
+
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName:
+# Attributes:
+# - Default: CartographerMappingAndLocalization
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.occupancyGrid.providerName = CartographerMappingAndLocalization
+
+
+# ArmarX.dynamic_scene_provider.p.robot.name:
+# Attributes:
+# - Case sensitivity: yes
+# - Required: yes
+ArmarX.dynamic_scene_provider.p.robot.name = ArmarDE
+
+
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs: Update rate of the running task.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.dynamic_scene_provider.p.taskPeriodMs = 100
+
+
diff --git a/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg b/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg
new file mode 100644
index 00000000..9c77a56d
--- /dev/null
+++ b/scenarios/PlatformNavigation/config/laser_scanner_feature_extraction.cfg
@@ -0,0 +1,395 @@
+# ==================================================================
+# laser_scanner_feature_extraction 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.LaserScannerFeatureExtraction.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.LaserScannerFeatureExtraction.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ArVizTopicName = ArVizTopic
+
+
+# ArmarX.LaserScannerFeatureExtraction.DebugObserverTopicName: Name of the topic the DebugObserver listens on
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.DebugObserverTopicName = DebugObserver
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.EnableProfiling = false
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.LaserScannerFeatureExtraction.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.ObjectName = ""
+
+
+# ArmarX.LaserScannerFeatureExtraction.RemoteStateComponentName: Name of the robot state component
+# Attributes:
+# - Default: RobotStateComponent
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.RemoteStateComponentName = RobotStateComponent
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Memory = Navigation
+
+
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Provider: Name of this provider
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mem.nav.laser_scanner_features.Provider = ""
+
+
+# ArmarX.LaserScannerFeatureExtraction.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.LaserScannerFeatureExtraction.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.drawRawPoints: If true, the laser scans will be drawn.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.drawRawPoints = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeMergedFeatures: If true, the merged features from all sensors will be drawn.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeMergedFeatures = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeSeparateFeatures: If true, the features from each sensor will be drawn.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.arviz.visualizeSeparateFeatures = false
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.cableAreaWidth: Width of the area where to search for the cable.
+# Attributes:
+# - Default: 400
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.cableAreaWidth = 400
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.enabled: Try to supress clusters belonging to the power supply cable.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.enabled = true
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.maxAreaTh: The cable will only be removed if the cluster area is below this threshold.
+# Attributes:
+# - Default: 2500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.cableFix.maxAreaTh = 2500
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.distanceTh:
+# Attributes:
+# - Default: 40
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.distanceTh = 40
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.maxIterations:
+# Attributes:
+# - Default: -1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.chainApproximation.maxIterations = -1
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.radius: The radius of the robot when using the circle shape.
+# Attributes:
+# - Default: 500
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.radius = 500
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.robotConvexHullMargin: Parameter to increase the robot's convex hull when using the rectangle shape.
+# Attributes:
+# - Default: 50
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.LaserScannerFeatureExtraction.p.robotHull.robotConvexHullMargin = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.shape: Shape of the robot area.
+# Attributes:
+# - Default: Rectangle
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {Circle, Rectangle}
+# ArmarX.LaserScannerFeatureExtraction.p.robotHull.shape = Rectangle
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.angleThreshold: Angular distance between consecutive points in laser scan for clustering.
+# Attributes:
+# - Default: 0.0314159282
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.angleThreshold = 0.0314159282
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.distanceThreshold: Radial distance between consecutive points in laser scan for clustering.
+# Attributes:
+# - Default: 30
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.LaserScannerFeatureExtraction.p.scanClustering.distanceThreshold = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.maxDistance: Maximum radius around sensors to detect clusters.
+# Attributes:
+# - Default: 3000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.scanClustering.maxDistance = 3000
+
+
+# ArmarX.LaserScannerFeatureExtraction.p.taskPeriodMs: Update rate of the running task.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.p.taskPeriodMs = 100
+
+
+# ArmarX.LaserScannerFeatureExtraction.tpc.pub.LaserScannerFeatures: Name of the `LaserScannerFeatures` topic to publish data to.
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.LaserScannerFeatureExtraction.tpc.pub.LaserScannerFeatures = LaserScannerFeatures
+
+
+# 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/PlatformNavigation/config/navigation_memory.cfg b/scenarios/PlatformNavigation/config/navigation_memory.cfg
index 37ca7194..29d4ee55 100644
--- a/scenarios/PlatformNavigation/config/navigation_memory.cfg
+++ b/scenarios/PlatformNavigation/config/navigation_memory.cfg
@@ -261,14 +261,6 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
# ArmarX.navigation_memory.mns.MemoryNameSystemName = MemoryNameSystem
-# ArmarX.navigation_memory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
-# Attributes:
-# - Default: 2
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 2
-
-
# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
# Attributes:
# - Default: true
@@ -296,3 +288,55 @@ ArmarX.NavigationMemory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graph
ArmarX.navigation_memory.p.snapshotToLoad = ./PriorKnowledgeData/navigation-graphs/R003
+# ArmarX.navigation_memory.p.visuCostmaps: Enable visualization of costmaps.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuCostmaps = true
+
+
+# ArmarX.navigation_memory.p.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
+# Attributes:
+# - Default: 2
+# - Case sensitivity: yes
+# - Required: no
+ArmarX.navigation_memory.p.visuFrequency = 5
+
+
+# ArmarX.navigation_memory.p.visuHumanMaxAgeMs: The maximum age of humans to be drawn in ms.
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigation_memory.p.visuHumanMaxAgeMs = 1000
+
+
+# ArmarX.navigation_memory.p.visuHumans: Enable visualization of humans.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuHumans = true
+
+
+# ArmarX.navigation_memory.p.visuRooms: Enable visualization of rooms.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuRooms = true
+
+
+# ArmarX.navigation_memory.p.visuTransparent: Enable visualization of humans a bit transparent.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.navigation_memory.p.visuTransparent = false
+
+
diff --git a/scenarios/PlatformNavigation/config/navigator.cfg b/scenarios/PlatformNavigation/config/navigator.cfg
index 51a93adb..0496b5dc 100644
--- a/scenarios/PlatformNavigation/config/navigator.cfg
+++ b/scenarios/PlatformNavigation/config/navigator.cfg
@@ -102,10 +102,10 @@ ArmarX.Navigator.ObjectName = navigator
# ArmarX.Navigator.RobotUnitName: No Description
# Attributes:
-# - Default: Armar6Unit
+# - Default: ArmarDEUnit
# - Case sensitivity: no
# - Required: no
-ArmarX.Navigator.RobotUnitName = Armar6Unit
+ArmarX.Navigator.RobotUnitName = ArmarDEUnit
# ArmarX.Navigator.p.occupancy_grid.occopied_threshold: No Description
@@ -181,7 +181,7 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
# ArmarX.navigator.ArVizStorageName: Name of the ArViz storage
@@ -246,7 +246,7 @@ ArmarX.Navigator.p.occupancy_grid.occopied_threshold = 0.8
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-ArmarX.navigator.RobotUnitName = Armar6Unit
+ArmarX.navigator.RobotUnitName = ArmarDEUnit
# ArmarX.navigator.cmp.RemoteGui: Ice object name of the `RemoteGui` component.
@@ -329,6 +329,22 @@ ArmarX.navigator.RobotUnitName = Armar6Unit
# ArmarX.navigator.mem.nav.human.Memory = Navigation
+# ArmarX.navigator.mem.nav.laser_scanner_features.CoreSegment:
+# Attributes:
+# - Default: LaserScannerFeatures
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.laser_scanner_features.CoreSegment = LaserScannerFeatures
+
+
+# ArmarX.navigator.mem.nav.laser_scanner_features.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.mem.nav.laser_scanner_features.Memory = Navigation
+
+
# ArmarX.navigator.mem.nav.param.CoreSegment:
# Attributes:
# - Default: Parameterization
@@ -436,11 +452,19 @@ ArmarX.navigator.RobotUnitName = Armar6Unit
# ArmarX.navigator.p.scene.humanProviderName = dynamic_scene_provider
+# ArmarX.navigator.p.scene.laserScannerFeaturesProviderName:
+# Attributes:
+# - Default: dynamic_scene_provider
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.navigator.p.scene.laserScannerFeaturesProviderName = dynamic_scene_provider
+
+
# ArmarX.navigator.p.scene.robotName:
# Attributes:
# - Case sensitivity: yes
# - Required: yes
-ArmarX.navigator.p.scene.robotName = Armar6
+ArmarX.navigator.p.scene.robotName = ArmarDE
# ArmarX.navigator.p.scene.staticCostmapName:
diff --git a/source/armarx/navigation/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index ca6bedf8..23583351 100644
--- a/source/armarx/navigation/human/HumanTracker.h
+++ b/source/armarx/navigation/human/HumanTracker.h
@@ -119,7 +119,7 @@ namespace armarx::navigation::human
int confidenceLaser = -2;
// whether to use the kalman filter inside the HumanFilter
- bool useKalmanFilter = false;
+ bool useKalmanFilter = true;
};
/**
--
GitLab
From 94a86e7a994da88f182621f53a25c2f04a893cde Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 8 May 2023 16:29:14 +0200
Subject: [PATCH 54/55] Fix aron_conversions
---
source/armarx/navigation/memory/aron_conversions.cpp | 4 ++--
source/armarx/navigation/memory/aron_conversions.h | 5 -----
2 files changed, 2 insertions(+), 7 deletions(-)
diff --git a/source/armarx/navigation/memory/aron_conversions.cpp b/source/armarx/navigation/memory/aron_conversions.cpp
index bfc55c27..b3b5510c 100644
--- a/source/armarx/navigation/memory/aron_conversions.cpp
+++ b/source/armarx/navigation/memory/aron_conversions.cpp
@@ -5,12 +5,12 @@
#include <iterator>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-#include "types.h"
#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+#include "types.h"
+
namespace armarx::navigation::memory
{
diff --git a/source/armarx/navigation/memory/aron_conversions.h b/source/armarx/navigation/memory/aron_conversions.h
index 0ad52e46..2d01ac55 100644
--- a/source/armarx/navigation/memory/aron_conversions.h
+++ b/source/armarx/navigation/memory/aron_conversions.h
@@ -21,11 +21,6 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
-#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
-#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-
namespace armarx::navigation::memory
{
--
GitLab
From 69572faba6e1dc8046450ad9ac1bb56abd522806 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 8 May 2023 18:26:59 +0200
Subject: [PATCH 55/55] Fix angle in remote gui and reload navigator on every
move to
---
.../navigation/components/navigator/Component.cpp | 2 ++
.../navigation/components/navigator/RemoteGui.cpp | 14 +++++++++-----
2 files changed, 11 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 0adbd4a8..dadc0c3b 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -272,6 +272,8 @@ namespace armarx::navigation::components::navigator
executorPtr = &executor.value();
}
+ // if we emplace an existing key it is not replaced, so remove it first.
+ navigators.erase(callerId);
navigators.emplace(
std::piecewise_construct,
std::forward_as_tuple(callerId),
diff --git a/source/armarx/navigation/components/navigator/RemoteGui.cpp b/source/armarx/navigation/components/navigator/RemoteGui.cpp
index 42d9ad21..d8012278 100644
--- a/source/armarx/navigation/components/navigator/RemoteGui.cpp
+++ b/source/armarx/navigation/components/navigator/RemoteGui.cpp
@@ -15,18 +15,19 @@
#include <ArmarXGui/interface/RemoteGuiInterface.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetProxy.h>
-#include "Component.h"
-#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/global_planning/SPFA.h>
+#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
#include <armarx/navigation/util/util.h>
+#include "Component.h"
+
namespace armarx::navigation::components::navigator
{
namespace gui = RemoteGui;
@@ -267,9 +268,12 @@ namespace armarx::navigation::components::navigator
tab.targetPoseGroup.targetPoseY.getValue(),
tab.targetPoseGroup.targetPoseZ.getValue()};
- const Eigen::Vector3f targetOri{tab.targetPoseGroup.targetPoseRoll.getValue(),
- tab.targetPoseGroup.targetPosePitch.getValue(),
- tab.targetPoseGroup.targetPoseYaw.getValue()};
+ const auto degToRad = [](float deg) { return deg * M_PI / 180.; };
+
+ const Eigen::Vector3f targetOri{
+ degToRad(tab.targetPoseGroup.targetPoseRoll.getValue()),
+ degToRad(tab.targetPoseGroup.targetPosePitch.getValue()),
+ degToRad(tab.targetPoseGroup.targetPoseYaw.getValue())};
const core::Pose targetPose = core::Pose(Eigen::Translation3f(targetPos)) *
core::Pose(simox::math::rpy_to_mat3f(targetOri));
--
GitLab