diff --git a/CMakeLists.txt b/CMakeLists.txt
index 494d9723528425a5413069441c067276bcd6efa0..a79cf08d167397002ade8cbd28a2b5440ccbcb68 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 efa6ec80e5a10ff6f1a4667c6c4792b29ef71f7f..e65951f6cdada3248f25aea846787eed6840de31 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)
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 ab0d58102f6d3514940cabac0de4e8bea7d22f4f..5bab4b0deb3d568fde424ff1c32b6dc0bc47fdc2 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 1837d8ac9cf76718fef5978213a5786235a50a08..92179ba82dc43eb2f989f8ea6cc92bae11307b4c 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;
diff --git a/source/armarx/navigation/components/dynamic_scene_provider/Component.h b/source/armarx/navigation/components/dynamic_scene_provider/Component.h
index 11e0d65404a2c24d6179fe6327ee3e7b32fcc5c6..c70b0f6678d7822438197cf88e312bfe990758a5 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
new file mode 100644
index 0000000000000000000000000000000000000000..ad8b55c8d351fd1cb01b7d27a45b37466c347b96
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp
@@ -0,0 +1,237 @@
+#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/util/laser_scanner_conversion.h"
+
+#include "FeatureExtractor.h"
+#include "conversions/eigen.h"
+#include "conversions/pcl.h"
+#include "conversions/pcl_eigen.h"
+
+namespace armarx::navigation::components::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 armem::laser_scans::LaserScanStamped& msg,
+ const Eigen::Isometry3f& globalSensorPose,
+ const simox::Color& color)
+ {
+ auto layer = arviz.layer("points_" + msg.header.frame);
+
+ 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))
+ .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::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
new file mode 100644
index 0000000000000000000000000000000000000000..f1c51a4c3b865a52be36d29a755afd935619f504
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.h
@@ -0,0 +1,99 @@
+/*
+ * 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 "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ 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 armem::laser_scans::LaserScanStamped& 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::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
new file mode 100644
index 0000000000000000000000000000000000000000..ede04e8d40a18b71e5e2009583de4d6d534dd887
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/CMakeLists.txt
@@ -0,0 +1,47 @@
+armarx_add_component(laser_scanner_feature_extraction
+ ICE_DEPENDENCIES
+ ArmarXCoreInterfaces
+ # RobotAPIInterfaces
+ SOURCES
+ Component.cpp
+ FeatureExtractor.cpp
+ ArVizDrawer.cpp
+ ScanClustering.cpp
+ ChainApproximation.cpp
+ #conversions
+ conversions/eigen.cpp
+ conversions/pcl.cpp
+ EnclosingEllipsoid.cpp
+ Path.cpp
+ HEADERS
+ Component.h
+ FeatureExtractor.h
+ ArVizDrawer.h
+ ScanClustering.h
+ ChainApproximation.h
+ EnclosingEllipsoid.h
+ Path.h
+ geometry.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.
+ ArmarXCoreLogging
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotComponentsInterfaces # For legacy LaserScannerFeatureExtraction topic
+ armarx_navigation::memory
+ armem_laser_scans
+ DEPENDENCIES_LEGACY
+ PCL
+)
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 0000000000000000000000000000000000000000..905cf765c2c9f9c7c207f270c02b3d27b511992b
--- /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 0000000000000000000000000000000000000000..a7b5e6cdaeb46a8909f4f572863e08c92751a649
--- /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
new file mode 100644
index 0000000000000000000000000000000000000000..6c2f235b9f61f4482d5bc90fdf554e0985748fb0
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.cpp
@@ -0,0 +1,548 @@
+/*
+ * 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 "Component.h"
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <utility>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/BoundingBox.h>
+#include <VirtualRobot/MathTools.h>
+
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+
+#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.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::navigation::components::laser_scanner_feature_extraction
+{
+
+ armarx::PropertyDefinitionsPtr
+ LaserScannerFeatureExtraction::createPropertyDefinitions()
+ {
+ 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.");
+
+ 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())
+ {
+ addPlugin(laserScannerReaderPlugin);
+ }
+
+ void
+ LaserScannerFeatureExtraction::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+ void
+ LaserScannerFeatureExtraction::onConnectComponent()
+ {
+ 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)...); });
+
+ ARMARX_INFO << "Connected";
+
+ robot = RemoteRobot::createLocalClone(getRobotStateComponent());
+ RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ switch (properties.robotHull.shape)
+ {
+ 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;
+ }
+ }
+
+
+ laserScannerFeatureWriter.connect();
+
+ task = new PeriodicTask<LaserScannerFeatureExtraction>(
+ this,
+ &LaserScannerFeatureExtraction::runPeriodically,
+ properties.taskPeriodMs,
+ false,
+ "runningTask");
+ task->start();
+ }
+
+ memory::LaserScannerFeature
+ toArmemFeature(const Features& features)
+ {
+ memory::LaserScannerFeature armemFeature;
+
+ if (features.chain)
+ {
+ armemFeature.chain = features.chain.value();
+ }
+
+ if (features.circle)
+ {
+ armemFeature.circle = features.circle.value();
+ }
+
+ if (features.convexHull)
+ {
+ armemFeature.convexHull = features.convexHull->vertices;
+ }
+
+ if (features.ellipsoid)
+ {
+ armemFeature.ellipsoid = features.ellipsoid.value();
+ }
+
+ armemFeature.points = features.points;
+
+ return armemFeature;
+ }
+
+ memory::LaserScannerFeatures
+ toArmemFeatures(const std::vector<Features>& features,
+ const Eigen::Isometry3f& global_T_sensor,
+ const std::string& sensorFrame)
+ {
+ memory::LaserScannerFeatures armemFeatures;
+ armemFeatures.frame = sensorFrame;
+ armemFeatures.frameGlobalPose = global_T_sensor;
+
+
+ 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;
+ }
+
+ void
+ 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.header.timestamp.toMicroSecondsSinceEpoch());
+ const Eigen::Isometry3f global_T_sensor(
+ robot->getRobotNode(data.header.frame)->getGlobalPose());
+ const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
+ const Eigen::Isometry3f robot_T_sensor(
+ robot->getRobotNode(data.header.frame)->getPoseInRootFrame());
+
+ //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());
+
+ std::transform(points.begin(),
+ points.end(),
+ std::back_inserter(out),
+ [&tf](const auto& pt)
+ { return conversions::to2D(tf * conversions::to3D(pt)); });
+
+ return out;
+ };
+
+ 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)
+ {
+
+ const bool allPointsInCableArea =
+ std::all_of(points.begin(), points.end(), isPointInsideCableArea);
+ if (allPointsInCableArea)
+ {
+ if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
+ {
+ return true;
+ }
+ }
+ }
+
+ return false;
+ };
+
+ const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
+ {
+ features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
+ features.end());
+ return features;
+ };
+
+ ARMARX_VERBOSE << featuresFromExtractor.size() << " features from extractor";
+
+ const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
+ ARMARX_VERBOSE << features.size() << " features without cable region";
+
+ const auto validFeatures = removeInvalidFeatures(features);
+ ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
+
+ const auto armemFeatures =
+ toArmemFeatures(validFeatures, global_T_sensor, data.header.frame);
+
+ ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
+
+ // report the features
+ publishFeatures(armemFeatures, data.header.timestamp);
+
+ // check if arviz should be triggered
+ const auto getOrCreateThrottler = [&]() -> Throttler&
+ {
+ const auto it = throttlers.find(data.header.frame);
+ if (it == throttlers.end())
+ {
+ throttlers.emplace(data.header.frame, Throttler(10.F));
+ }
+
+ return throttlers.at(data.header.frame);
+ };
+
+ if (getOrCreateThrottler().check(data.header.timestamp))
+ {
+ arVizDrawer->draw(features, data.header.frame, global_T_sensor);
+
+ if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
+ {
+ 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));
+ }
+ }
+
+ // some debugobserver reporting
+ frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
+ setDebugObserverDatafield("numClusters", features.size());
+ }
+
+ void
+ 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
+ LaserScannerFeatureExtraction::onDisconnectComponent()
+ {
+ task->stop();
+ }
+
+ void
+ LaserScannerFeatureExtraction::onExitComponent()
+ {
+ }
+
+ std::string
+ LaserScannerFeatureExtraction::getDefaultName() const
+ {
+ return "LaserScannerFeatureExtraction";
+ }
+
+ void
+ LaserScannerFeatureExtraction::runPeriodically()
+ {
+ const armem::laser_scans::client::Reader::Query laserScanQuery{
+ .agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
+
+ 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());
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..fac5e42539378b10d5ea6c8fefa4751969100560
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Component.h
@@ -0,0 +1,231 @@
+/*
+ * 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
+ */
+
+#pragma once
+
+#include <variant>
+
+#include "ArmarXCore/core/util/Throttler.h"
+#include "ArmarXCore/libraries/logging/FrequencyReporter.h"
+#include <ArmarXCore/core/Component.h>
+#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/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
+
+#include <RobotComponents/interface/components/LaserScannerFeatureExtraction/LaserScannerFeatureExtraction.h>
+
+#include "ArVizDrawer.h"
+#include "FeatureExtractor.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/geometry.h"
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
+
+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;
+
+ /**
+ * @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::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ ,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public RobotStateComponentPluginUser,
+ virtual public armem::ClientPluginUser
+
+ {
+ public:
+ LaserScannerFeatureExtraction();
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+ 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:
+ 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);
+
+ // 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 member variables go here.
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ int taskPeriodMs = 100;
+
+ //
+ 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
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ 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;
+ */
+
+ PeriodicTask<LaserScannerFeatureExtraction>::pointer_type task;
+
+ 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::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/ComponentInterface.ice b/source/armarx/navigation/components/laser_scanner_feature_extraction/ComponentInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..2c38043810363d0b8273a1e50e6234a2de2d9cae
--- /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.
+ };
+
+};};};};
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 0000000000000000000000000000000000000000..eea10eeb503434a73080dc9b1092b42a767293ba
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.cpp
@@ -0,0 +1,123 @@
+#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::navigation::components::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..277531e7ab181ad3e5a21572ce16e13eaae967de
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.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 <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // struct Ellipsoid
+ // {
+ // Eigen::Vector2f center;
+ // float angle;
+
+ // Eigen::Vector2f radii;
+
+ // Eigen::Affine2f pose() const noexcept;
+ // };
+
+ using Ellipsoid = memory::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..d5c875ec258cc6bb58c8368287ce8cd206e65b07
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.cpp
@@ -0,0 +1,237 @@
+#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/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"
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
+#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+
+ // 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 armem::laser_scans::LaserScanStamped& data)
+ {
+ ARMARX_DEBUG << "on data";
+ const auto clustersWithFeatures = features(data.data);
+
+ 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::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
new file mode 100644
index 0000000000000000000000000000000000000000..373fc7487b00f90bc6a3f9e40762886d3b57a95d
--- /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_laser_scans/types.h>
+
+#include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
+
+#include "EnclosingEllipsoid.h"
+#include "ScanClustering.h"
+#include "armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h"
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx::navigation::components::laser_scanner_feature_extraction
+{
+ using Circle = memory::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 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 armem::laser_scans::LaserScanStamped& 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::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
new file mode 100644
index 0000000000000000000000000000000000000000..d57cae5caa509a13b7271d43b9c1adb7e8ea377c
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/Path.cpp
@@ -0,0 +1,24 @@
+#include "Path.h"
+
+namespace armarx::navigation::components::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..2cf3a4134e580ee2e7558e42e5a36dff9fa8aae5
--- /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::navigation::components::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..32503968e0ba7e865cea541f25cc03c9a4bbde59
--- /dev/null
+++ b/source/armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.cpp
@@ -0,0 +1,84 @@
+#include "ScanClustering.h"
+
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+namespace armarx::navigation::components::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..1159c2c648057ae5275a5989d90bf150f09a8893
--- /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::navigation::components::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::navigation::components::laser_scanner_feature_extraction
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 0000000000000000000000000000000000000000..41af90f4e15b87cecec5d046697ba5897e9e8c5d
--- /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 0000000000000000000000000000000000000000..615d41c6d8e80a0f5ff2390079f3627d5281206a
--- /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 0000000000000000000000000000000000000000..ae12d627f1438237d4cc05069ba544ae3648073b
--- /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 0000000000000000000000000000000000000000..0e6b818219a8f143fc88f523800a850f275eba53
--- /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 0000000000000000000000000000000000000000..375f166f08b391a9462b7bb8fc2ebf7d8b7ff530
--- /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 0000000000000000000000000000000000000000..c8ed47f21519ffcf26d40a55bd8a288118a0a25f
--- /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 0000000000000000000000000000000000000000..2caf92b95541f850b0db004c78fa8751e8faf751
--- /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 0000000000000000000000000000000000000000..b72277e033c4a51e374d0e9ba87cc12aad48b6b7
--- /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 0000000000000000000000000000000000000000..8b70a56da0dcd6ba78b76b3c3ec187872d66f0c1
--- /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 0000000000000000000000000000000000000000..3435a5f51ea072c80f48513c034d54f7c3d800fb
--- /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 0000000000000000000000000000000000000000..db413af6aa13db704fc06decbeadba0f59151d19
--- /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 "armarx/navigation/components/laser_scanner_feature_extraction/FeatureExtractor.h"
+#include <boost/test/tools/old/interface.hpp>
+#define BOOST_TEST_MODULE RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
+
+#define ARMARX_BOOST_TEST
+
+#include <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);
+}
diff --git a/source/armarx/navigation/components/navigation_memory/Component.cpp b/source/armarx/navigation/components/navigation_memory/Component.cpp
index 22f674ca75fbe8e24770ad6cfb47d12efa814cee..e7f220c11398fcea3a8f6704e907f2552691ad3c 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 77220afebd7eafae930ab19c7a436f35ee641d08..bc359652c277c5e1e5e6d9fe6ee2fbd041158ddb 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/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 3943986f1cccd1496240f844b0d3f08a8f714478..424e28511cb9974c05cb428e41884c857911de23 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 aac593f1e2ef8bb2bf405bfa2dff65cffeff3e58..b8526d92dc78d976ee3e1a45686685fcc522d90c 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 172957d00e1adb5b42a1bb47ca9fea7f80083b17..66f3170748191d475ff0bd2088f04c78a56fea15 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/human/HumanTracker.h b/source/armarx/navigation/human/HumanTracker.h
index 1f27aed75375018ce613579ee07f4577564b4249..32c25cc4f3c050b54490f6f5e72e48a6d4ffa7a9 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
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index c03f9a2b8fb622325659fd65c4ac9016fd22bfaf..c3910e067f645d1a066c478c81f702735c7afff8 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
@@ -11,10 +17,13 @@ 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
memory.h
+ aron_conversions.h
client/stack_result/Writer.h
client/stack_result/Reader.h
client/parameterization/Reader.h
@@ -25,12 +34,16 @@ 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
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
new file mode 100644
index 0000000000000000000000000000000000000000..f78dea48892fab0e2bb8a477eaedc2425a484fc8
--- /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::navigation::memory::arondto::Ellipsoid'>
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='radii'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::navigation::memory::arondto::Circle'>
+ <ObjectChild key='center'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeature">
+ <ObjectChild key="convexHull">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ <ObjectChild key="circle">
+ <armarx::navigation::memory::arondto::Circle/>
+ </ObjectChild>
+ <ObjectChild key="ellipsoid">
+ <armarx::navigation::memory::arondto::Ellipsoid/>
+ </ObjectChild>
+ <!-- <ObjectChild key="chain">
+ <armarx::navigation::memory::arondto::Chain/>
+ </ObjectChild> -->
+ <ObjectChild key="points">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::navigation::memory::arondto::LaserScannerFeatures">
+ <ObjectChild key="frame">
+ <String/>
+ </ObjectChild>
+ <ObjectChild key="frameGlobalPose">
+ <Pose/>
+ </ObjectChild>
+ <ObjectChild key="features">
+ <List>
+ <armarx::navigation::memory::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 0000000000000000000000000000000000000000..bfc55c27da1c04e4ae9f3e35c8ac8c4559216d04
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.cpp
@@ -0,0 +1,92 @@
+#include "aron_conversions.h"
+
+#include <algorithm>
+#include <cstdint>
+#include <iterator>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/aron/converter/common/Converter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include "types.h"
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+
+namespace armarx::navigation::memory
+{
+
+
+ /************ toAron ************/
+
+ // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
+ // {
+ // aronLaserScan.scan = toAron(laserScan);
+ // }
+
+
+ // 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::navigation::memory
diff --git a/source/armarx/navigation/memory/aron_conversions.h b/source/armarx/navigation/memory/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ad52e46393096a7b03c8e7a0898839a7d2eaf14
--- /dev/null
+++ b/source/armarx/navigation/memory/aron_conversions.h
@@ -0,0 +1,43 @@
+/*
+ * 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/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+namespace armarx::navigation::memory
+{
+
+ struct LaserScannerFeatures;
+
+ namespace arondto
+ {
+ struct LaserScannerFeatures;
+ }
+
+ // LaserScannerFeatures
+ void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
+ void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
+
+} // 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 b224149c4aeb3c4858530f19ed051700060fc179..3a55ffc42e32b532d98a759e81cafa7033228beb 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
new file mode 100644
index 0000000000000000000000000000000000000000..c7640bdc8af9beadb0132c2fcee5ca4a9260f081
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.cpp
@@ -0,0 +1,219 @@
+#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/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#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 <armarx/navigation/memory/aron_conversions.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+ Reader::~Reader() = default;
+
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ // 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(armem::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 armem::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 armem::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 armem::wm::EntityInstance& entityInstance)
+ {
+ if (const auto o =
+ armem::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 armem::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::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
new file mode 100644
index 0000000000000000000000000000000000000000..4e08755e357910af451ba548d9617d3ac2b7e58b
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Reader.h
@@ -0,0 +1,114 @@
+/*
+ * 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 <armarx/navigation/memory/constants.h>
+#include <armarx/navigation/memory/types.h>
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+
+ /**
+ * @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 = memory::constants::NavigationMemoryName;
+ std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
+ } properties;
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..425345e3e12131c82a40d7d970505db1e21ce95f
--- /dev/null
+++ b/source/armarx/navigation/memory/client/laser_scanner_features/Writer.cpp
@@ -0,0 +1,109 @@
+#include "Writer.h"
+
+#include <RobotAPI/libraries/armem/core/error.h>
+
+#include <armarx/navigation/memory/aron/LaserScannerFeatures.aron.generated.h>
+#include <armarx/navigation/memory/aron_conversions.h>
+
+namespace armarx::navigation::memory::client::laser_scanner_features
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+
+ Writer::~Writer() = default;
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ 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 '" << properties.memoryName
+ << "' ...";
+ try
+ {
+ memoryWriter =
+ memoryNameSystem.useWriter(armem::MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << properties.memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << properties.memoryName;
+ }
+
+ bool
+ Writer::store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const armem::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(properties.memoryName)
+ .withCoreSegmentName(properties.coreSegmentName)
+ .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::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
new file mode 100644
index 0000000000000000000000000000000000000000..bd4cad88956e5dc4d99d2f0c49ac105273439455
--- /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/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
+{
+
+ /**
+ * @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 armem::Time& timestamp);
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Writer memoryWriter;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = memory::constants::NavigationMemoryName;
+ std::string coreSegmentName = memory::constants::LaserScannerFeaturesCoreSegment;
+ } properties;
+
+ std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.nav.laser_scanner_features.";
+ };
+
+} // namespace armarx::navigation::memory::client::laser_scanner_features
diff --git a/source/armarx/navigation/memory/constants.h b/source/armarx/navigation/memory/constants.h
index 177f9730e047da14810c1d7ad5dcad2be60ef7ab..0df19cc8d2139d617f2283e806f39e42551b77c4 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 0000000000000000000000000000000000000000..73935f916f4a5a513f420106c608d43aa675422a
--- /dev/null
+++ b/source/armarx/navigation/memory/types.h
@@ -0,0 +1,75 @@
+/*
+ * 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::navigation::memory
+{
+
+ 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::navigation::memory
diff --git a/source/armarx/navigation/server/scene_provider/SceneProvider.cpp b/source/armarx/navigation/server/scene_provider/SceneProvider.cpp
index 85e0d6236157edfd07c907367abafa3de69b7863..3fa2eb0179e30da481c3897e2f6dbc62cea8bcf9 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 1f21e85ead9be31ca561a925e2bcceff0be53567..df8a9e77e861a730f8a3dedbcc75fd6d54f3300a 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);