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 01/13] 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 02/13] 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 03/13] 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 04/13] 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 05/13] 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 06/13] 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 07/13] 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 08/13] 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 09/13] 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 10/13] 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 11/13] 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 12/13] 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 13/13] 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