37 #include <Eigen/Geometry>
39 #include <IceUtil/Time.h>
41 #include <SimoxUtility/algorithm/apply.hpp>
42 #include <SimoxUtility/color/Color.h>
43 #include <VirtualRobot/MathTools.h>
58 #include <RobotAPI/interface/units/LaserScannerUnit.h>
73 #include "conversions/eigen.h"
78 const std::string LaserScannerFeatureExtraction::defaultName =
"LaserScannerFeatureExtraction";
86 def->required(properties.robotName,
"p.robotName");
90 properties.taskPeriodMs,
"p.taskPeriodMs",
"Update rate of the running task.");
92 def->required(properties.robotName,
"p.robotName",
"Name of the robot");
94 def->optional(properties.robotHull.shape,
"p.robotHull.shape",
"Shape of the robot area.")
97 def->optional(properties.robotHull.radius,
99 "The radius of the robot when using the circle shape.");
101 properties.robotHull.robotConvexHullMargin,
102 "p.robotHull.robotConvexHullMargin",
103 "Parameter to increase the robot's convex hull when using the rectangle shape.");
105 def->optional(properties.cableFix.enabled,
106 "p.cableFix.enabled",
107 "Try to supress clusters belonging to the power supply cable.");
108 def->optional(properties.cableFix.cableAreaWidth,
109 "p.cableFix.cableAreaWidth",
110 "Width of the area where to search for the cable.");
113 properties.cableFix.maxAreaTh,
114 "p.cableFix.maxAreaTh",
115 "The cable will only be removed if the cluster area is below this threshold.");
118 properties.chainApproximationParams.distanceTh,
"p.chainApproximation.distanceTh",
"");
119 def->optional(properties.chainApproximationParams.maxIterations,
120 "p.chainApproximation.maxIterations",
123 def->optional(properties.scanClusteringParams.angleThreshold,
124 "p.scanClustering.angleThreshold",
125 "Angular distance between consecutive points in laser scan for clustering.");
126 def->optional(properties.scanClusteringParams.distanceThreshold,
127 "p.scanClustering.distanceThreshold",
128 "Radial distance between consecutive points in laser scan for clustering.");
129 def->optional(properties.scanClusteringParams.maxDistance,
130 "p.scanClustering.maxDistance",
131 "Maximum radius around sensors to detect clusters.");
133 def->optional(properties.arviz.drawRawPoints,
134 "p.arviz.drawRawPoints",
135 "If true, the laser scans will be drawn.");
137 def->optional(properties.arviz.visualizeSeparateFeatures,
138 "p.arviz.visualizeSeparateFeatures",
139 "If true, the features from each sensor will be drawn.");
141 def->optional(properties.arviz.visualizeMergedFeatures,
142 "p.arviz.visualizeMergedFeatures",
143 "If true, the merged features from all sensors will be drawn.");
145 def->optional(properties.filtering.filterLaserScansWithCostmap,
146 "p.filtering.filterLaserScansWithCostmap",
147 "It true, the laser scans are filtered and anything too close to an obstacle "
151 properties.filtering.distance,
152 "p.filtering.distance",
153 "The minimum distance of laser scanner features to obstacles so they are not removed.");
156 properties.filtering.costmapProviderName,
"p.filtering.costmapProviderName",
"");
158 def->optional(properties.filtering.costmapName,
"p.filtering.costmapName",
"");
160 def->optional(properties.arviz.arvizDrawer.drawCircles,
"p.arviz.drawer.drawCircles",
"");
162 properties.arviz.arvizDrawer.drawConvexHulls,
"p.arviz.drawer.drawConvexHulls",
"");
164 properties.arviz.arvizDrawer.drawEllipsoids,
"p.arviz.drawer.drawEllipsoids",
"");
165 def->optional(properties.arviz.arvizDrawer.drawChains,
"p.arviz.drawer.drawChains",
"");
173 addPlugin(laserScannerFeatureWriterPlugin);
189 LaserScannerFeatureExtraction::fetchDistanceToObstacleCostmapSmallMargin(
197 .
providerName = properties.filtering.costmapProviderName,
198 .name = properties.filtering.costmapName,
199 .timestamp = timestamp};
202 costmapReader.
query(costmapQuery);
205 if (costmapResult.
status !=
217 return costmapResult.
costmap.value();
226 frequencyReporterPublish = std::make_unique<FrequencyReporter>(
228 frequencyReporterRun = std::make_unique<FrequencyReporter>(
231 arVizDrawer = std::make_unique<ArVizDrawer>(
getArvizClient(), properties.arviz.arvizDrawer);
233 featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
234 properties.chainApproximationParams);
239 robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName);
242 switch (properties.robotHull.shape)
248 robot->getRobotNode(
"PlatformCornerFrontLeft")->getPositionInRootFrame());
250 robot->getRobotNode(
"PlatformCornerFrontRight")->getPositionInRootFrame());
252 robot->getRobotNode(
"PlatformCornerBackRight")->getPositionInRootFrame());
254 robot->getRobotNode(
"PlatformCornerBackLeft")->getPositionInRootFrame());
256 const auto addMargin = [&](
const Eigen::Vector2f& pt) -> Eigen::Vector2f
258 Eigen::Vector2f ptWithMargin = pt;
260 std::copysignf(properties.robotHull.robotConvexHullMargin, pt.x());
262 std::copysignf(properties.robotHull.robotConvexHullMargin, pt.y());
268 std::vector<Eigen::Vector2f> hullPoints;
269 hullPoints.push_back(addMargin(ptFrontLeft));
270 hullPoints.push_back(addMargin(ptFrontRight));
271 hullPoints.push_back(addMargin(ptBackRight));
272 hullPoints.push_back(addMargin(ptBackLeft));
274 robotHull = VirtualRobot::MathTools::createConvexHull2D(hullPoints);
278 if (properties.cableFix.enabled)
283 std::vector<Eigen::Vector2f> cableAreaPoints;
284 cableAreaPoints.emplace_back(addMargin(ptBackRight) +
285 100 * Eigen::Vector2f::UnitY());
286 cableAreaPoints.emplace_back(addMargin(ptBackLeft) +
287 100 * Eigen::Vector2f::UnitY());
288 cableAreaPoints.emplace_back(addMargin(ptBackRight) -
289 properties.cableFix.cableAreaWidth *
290 Eigen::Vector2f::UnitY());
291 cableAreaPoints.emplace_back(addMargin(ptBackLeft) -
292 properties.cableFix.cableAreaWidth *
293 Eigen::Vector2f::UnitY());
295 cableArea = VirtualRobot::MathTools::createConvexHull2D(cableAreaPoints);
303 if (properties.cableFix.enabled)
305 ARMARX_ERROR <<
"Cable fix is not supported for circular robots!";
307 const Eigen::Vector2f root =
315 obstacleDistancesCostmapSmallMargin.emplace(
316 fetchDistanceToObstacleCostmapSmallMargin(costmapReaderPlugin->get()));
321 &LaserScannerFeatureExtraction::runPeriodically,
322 properties.taskPeriodMs,
361 toArmemFeatures(
const std::vector<Features>& features,
362 const Eigen::Isometry3f& global_T_sensor,
363 const std::string& sensorFrame)
366 armemFeatures.
frame = sensorFrame;
372 return armemFeatures;
375 std::vector<Features>
376 removeInvalidFeatures(std::vector<Features> features)
378 const auto isInvalid = [](
const Features& features) ->
bool
380 if (features.points.size() < 2)
385 if (not features.convexHull.has_value())
404 features.erase(std::remove_if(features.begin(), features.end(), isInvalid),
411 std::vector<Features>
412 LaserScannerFeatureExtraction::onFeatures(
const armem::laser_scans::LaserScanStamped&
data,
413 const std::vector<Features>& featuresFromExtractor)
418 const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
420 ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*robot,
data.header.timestamp));
422 const Eigen::Isometry3f global_T_sensor(
423 robot->getRobotNode(
data.header.frame)->getGlobalPose());
424 const Eigen::Isometry3f robot_T_sensor(
425 robot->getRobotNode(
data.header.frame)->getPoseInRootFrame());
430 const auto transformPoints =
431 [](
const std::vector<Eigen::Vector2f>& points,
const Eigen::Isometry3f& tf)
433 std::vector<Eigen::Vector2f> out;
434 out.reserve(points.size());
438 std::back_inserter(out),
439 [&tf](
const auto& pt)
440 { return conversions::to2D(tf * conversions::to3D(pt)); });
445 const auto isPointInsideRobot = [&](
const Eigen::Vector2f& pt) ->
bool
447 if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
449 return VirtualRobot::MathTools::isInside(
450 pt, std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull));
452 else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
454 return (std::get<Eigen::Vector2f>(robotHull) - pt).norm() <
455 properties.robotHull.radius;
459 const auto isPointInsideCableArea = [&](
const Eigen::Vector2f& pt) ->
bool
460 {
return VirtualRobot::MathTools::isInside(pt, cableArea); };
461 const auto isClusterInvalid = [&](
const Features& features) ->
bool
464 const auto points = [&]()
466 if (features.convexHull)
468 return transformPoints(features.convexHull->vertices, robot_T_sensor);
471 return transformPoints(features.points, robot_T_sensor);
474 const bool allPointsInsideRobot =
475 std::all_of(points.begin(), points.end(), isPointInsideRobot);
476 if (allPointsInsideRobot)
481 if (properties.cableFix.enabled)
484 const bool allPointsInCableArea =
485 std::all_of(points.begin(), points.end(), isPointInsideCableArea);
486 if (allPointsInCableArea)
488 if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
498 const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
500 features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
505 ARMARX_VERBOSE << featuresFromExtractor.size() <<
" features from extractor";
507 const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
508 ARMARX_VERBOSE << features.size() <<
" features without cable region";
510 const auto validFeatures = removeInvalidFeatures(features);
511 ARMARX_VERBOSE << validFeatures.size() <<
" valid features without cable region";
513 const auto armemFeatures =
514 toArmemFeatures(validFeatures, global_T_sensor,
data.header.frame);
516 ARMARX_VERBOSE <<
"Reporting " << armemFeatures.features.size() <<
" features";
519 publishFeatures(armemFeatures,
data.header.timestamp);
521 if (properties.arviz.visualizeSeparateFeatures)
524 features, &
data,
data.header.frame,
data.header.timestamp, global_T_sensor);
527 return validFeatures;
531 LaserScannerFeatureExtraction::getOrCreateThrottler(std::string frame)
533 const auto it = throttlers.find(frame);
534 if (it == throttlers.end())
536 throttlers.emplace(frame, Throttler(10.
F));
539 return throttlers.at(frame);
543 LaserScannerFeatureExtraction::drawRobot(
const armem::Time& timestamp)
546 if (getOrCreateThrottler(robot->getName()).
check(timestamp))
548 const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
550 if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
552 VirtualRobot::MathTools::ConvexHull2DPtr& convexHullPtr =
553 std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull);
554 if (convexHullPtr !=
nullptr)
556 arVizDrawer->draw(
"robot_convex_hull",
559 simox::Color::azure(255, 80));
562 else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
564 Eigen::Vector2f& root = std::get<Eigen::Vector2f>(robotHull);
565 arVizDrawer->draw(
"robot_circle_hull",
566 Circle{root, properties.robotHull.radius},
568 simox::Color::azure(255, 80));
571 if (cableArea !=
nullptr)
574 "cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
580 LaserScannerFeatureExtraction::drawFeatures(
581 const std::vector<Features>& features,
582 const armem::laser_scans::LaserScanStamped* laserPoints,
583 const std::string frame,
585 const Eigen::Isometry3f& global_T_sensor)
588 if (getOrCreateThrottler(frame).check(timestamp))
590 arVizDrawer->draw(features, frame, global_T_sensor);
592 if (properties.arviz.drawRawPoints && laserPoints !=
nullptr)
594 arVizDrawer->draw(*laserPoints, global_T_sensor, simox::Color::magenta(255, 100));
600 LaserScannerFeatureExtraction::publishFeatures(
const memory::LaserScannerFeatures& features,
605 laserScannerFeatureWriterPlugin->get().store(features,
getName(), timestamp);
650 LaserScannerFeatureExtraction::runPeriodically()
653 .
agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
656 laserScannerReaderPlugin->get().queryData(laserScanQuery);
673 frequencyReporterRun->add(IceUtil::Time::now().toMicroSeconds());
675 std::vector<FramedFeatures> allFeatures;
676 allFeatures.reserve(laserScanResult.
laserScans.size());
681 mostRecentTimestamp =
std::max(mostRecentTimestamp, scan.header.timestamp);
684 if (properties.filtering.filterLaserScansWithCostmap)
686 if (not filterLaserScannerData(scan))
688 ARMARX_INFO <<
"Failed to filter laser scanner data.";
695 const auto features = featureExtractor->onData(scan);
697 auto& framedFeatures = allFeatures.emplace_back();
700 framedFeatures.features = onFeatures(scan, features);
701 framedFeatures.frame = scan.header.frame;
704 if (!allFeatures.empty())
706 mergeFeatures(allFeatures, mostRecentTimestamp);
710 drawRobot(mostRecentTimestamp);
713 frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
717 LaserScannerFeatureExtraction::filterLaserScannerData(
718 armem::laser_scans::LaserScanStamped& laserScan)
724 const auto size_before_filtering = laserScan.data.size();
726 const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
727 if (not virtualRobotReader.synchronizeRobot(*robot, laserScan.header.timestamp))
729 ARMARX_INFO <<
"Failed to synchronize robot to timestamp `"
730 << laserScan.header.timestamp <<
"`.";
734 const Eigen::Isometry3f global_T_sensor(
735 robot->getRobotNode(laserScan.header.frame)->getGlobalPose());
739 const auto converted = toCartesian<Eigen::Vector2f>(laserScan.data);
740 std::vector<Eigen::Vector2f> laserScanPositionsGlobal;
744 std::back_inserter(laserScanPositionsGlobal),
745 [&global_T_sensor](
const Eigen::Vector2f& pt)
746 { return conversions::to2D(global_T_sensor * conversions::to3D(pt)); });
748 ARMARX_CHECK(obstacleDistancesCostmapSmallMargin.has_value());
749 const auto& distance_to_obstacle_costmap = obstacleDistancesCostmapSmallMargin.value();
752 std::vector<armarx::LaserScanStep> filteredSteps;
753 for (
size_t i = 0; i < laserScanPositionsGlobal.size(); ++i)
755 const auto& pt = laserScanPositionsGlobal[i];
756 const auto vertex = distance_to_obstacle_costmap.toVertex(pt);
757 if (not distance_to_obstacle_costmap.isValid(vertex.index))
761 const auto&
distance = distance_to_obstacle_costmap.value(vertex.index);
765 filteredSteps.push_back(laserScan.data[i]);
769 laserScan.data = filteredSteps;
770 ARMARX_VERBOSE <<
"Filtered points: " << size_before_filtering <<
" -> "
771 << laserScan.data.size();
777 LaserScannerFeatureExtraction::mergeFeatures(
const std::vector<FramedFeatures>& framedFeatures,
781 std::vector<Features> converted;
782 for (
const auto& features : framedFeatures)
784 converted.reserve(converted.size() + features.features.size());
785 Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
787 for (
const auto& singleFeature : features.features)
794 FeatureMerger merger{std::move(converted), properties.chainApproximationParams};
795 std::vector<Features> merged = merger.merge();
798 std::string frame =
"global";
801 const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
803 laserScannerFeatureWriterPlugin->get().store(armemFeatures,
getName(), timestamp);
807 if (properties.arviz.visualizeMergedFeatures)
810 auto global_T_vis = global_T_global;
811 global_T_vis.translation() = Eigen::Vector3f{0, 0, 20};
812 drawFeatures(merged,
nullptr, frame, timestamp, global_T_vis);