Component.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
17  * @author Fabian Reister ( fabian dot reister at kit dot edu )
18  * @date 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "Component.h"
24 
25 #include <algorithm>
26 #include <cmath>
27 #include <cstddef>
28 #include <iterator>
29 #include <memory>
30 #include <optional>
31 #include <string>
32 #include <utility>
33 #include <variant>
34 #include <vector>
35 
36 #include <Eigen/Core>
37 #include <Eigen/Geometry>
38 
39 #include <IceUtil/Time.h>
40 
41 #include <SimoxUtility/algorithm/apply.hpp>
42 #include <SimoxUtility/color/Color.h>
43 #include <VirtualRobot/MathTools.h>
44 
57 
58 #include <RobotAPI/interface/units/LaserScannerUnit.h>
62 
70 
71 #include "ArVizDrawer.h"
72 #include "FeatureExtractor.h"
73 #include "conversions/eigen.h"
74 
76 {
77 
78  const std::string LaserScannerFeatureExtraction::defaultName = "LaserScannerFeatureExtraction";
79 
82  {
85 
86  def->required(properties.robotName, "p.robotName");
87 
88  // // Add an optional property.
89  def->optional(
90  properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
91 
92  def->required(properties.robotName, "p.robotName", "Name of the robot");
93 
94  def->optional(properties.robotHull.shape, "p.robotHull.shape", "Shape of the robot area.")
95  .map({std::make_pair("Rectangle", Properties::RobotHull::RECTANGLE),
96  std::make_pair("Circle", Properties::RobotHull::CIRCLE)});
97  def->optional(properties.robotHull.radius,
98  "p.robotHull.radius",
99  "The radius of the robot when using the circle shape.");
100  def->optional(
101  properties.robotHull.robotConvexHullMargin,
102  "p.robotHull.robotConvexHullMargin",
103  "Parameter to increase the robot's convex hull when using the rectangle shape.");
104 
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.");
111 
112  def->optional(
113  properties.cableFix.maxAreaTh,
114  "p.cableFix.maxAreaTh",
115  "The cable will only be removed if the cluster area is below this threshold.");
116 
117  def->optional(
118  properties.chainApproximationParams.distanceTh, "p.chainApproximation.distanceTh", "");
119  def->optional(properties.chainApproximationParams.maxIterations,
120  "p.chainApproximation.maxIterations",
121  "");
122 
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.");
132 
133  def->optional(properties.arviz.drawRawPoints,
134  "p.arviz.drawRawPoints",
135  "If true, the laser scans will be drawn.");
136 
137  def->optional(properties.arviz.visualizeSeparateFeatures,
138  "p.arviz.visualizeSeparateFeatures",
139  "If true, the features from each sensor will be drawn.");
140 
141  def->optional(properties.arviz.visualizeMergedFeatures,
142  "p.arviz.visualizeMergedFeatures",
143  "If true, the merged features from all sensors will be drawn.");
144 
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 "
148  "is removed.");
149 
150  def->optional(
151  properties.filtering.distance,
152  "p.filtering.distance",
153  "The minimum distance of laser scanner features to obstacles so they are not removed.");
154 
155  def->optional(
156  properties.filtering.costmapProviderName, "p.filtering.costmapProviderName", "");
157 
158  def->optional(properties.filtering.costmapName, "p.filtering.costmapName", "");
159 
160  def->optional(properties.arviz.arvizDrawer.drawCircles, "p.arviz.drawer.drawCircles", "");
161  def->optional(
162  properties.arviz.arvizDrawer.drawConvexHulls, "p.arviz.drawer.drawConvexHulls", "");
163  def->optional(
164  properties.arviz.arvizDrawer.drawEllipsoids, "p.arviz.drawer.drawEllipsoids", "");
165  def->optional(properties.arviz.arvizDrawer.drawChains, "p.arviz.drawer.drawChains", "");
166 
167  return def;
168  }
169 
171  {
172  addPlugin(laserScannerReaderPlugin);
173  addPlugin(laserScannerFeatureWriterPlugin);
174  addPlugin(virtualRobotReaderPlugin);
175  addPlugin(costmapReaderPlugin);
176  }
177 
178  void
180  {
181  // Topics and properties defined above are automagically registered.
182 
183  // Keep debug observer data until calling `sendDebugObserverBatch()`.
184  // (Requies the armarx::DebugObserverComponentPluginUser.)
185  // setDebugObserverBatchModeEnabled(true);
186  }
187 
189  LaserScannerFeatureExtraction::fetchDistanceToObstacleCostmapSmallMargin(
191  {
192  while (true)
193  {
194  const auto timestamp = Clock::Now();
195 
197  .providerName = properties.filtering.costmapProviderName,
198  .name = properties.filtering.costmapName,
199  .timestamp = timestamp};
200 
202  costmapReader.query(costmapQuery);
203 
204  // if costmap is not available yet, wait
205  if (costmapResult.status !=
207  {
209  continue;
210  }
211 
212  ARMARX_CHECK_EQUAL(costmapResult.status,
214 
215  ARMARX_TRACE;
216  ARMARX_CHECK(costmapResult.costmap.has_value());
217  return costmapResult.costmap.value();
218  };
219  }
220 
221  void
223  {
224  ARMARX_INFO << "Connecting";
225 
226  frequencyReporterPublish = std::make_unique<FrequencyReporter>(
227  getDebugObserver(), "LaserScannerFeatureExtraction_publish");
228  frequencyReporterRun = std::make_unique<FrequencyReporter>(
229  getDebugObserver(), "LaserScannerFeatureExtraction_run");
230 
231  arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient(), properties.arviz.arvizDrawer);
232 
233  featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
234  properties.chainApproximationParams);
235 
236  ARMARX_INFO << "Connected";
237 
238  ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
239  robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName);
240  ARMARX_CHECK_NOT_NULL(robot);
241 
242  switch (properties.robotHull.shape)
243  {
245  {
246  // initialize robot platform convex hull
247  const Eigen::Vector2f ptFrontLeft = conversions::to2D(
248  robot->getRobotNode("PlatformCornerFrontLeft")->getPositionInRootFrame());
249  const Eigen::Vector2f ptFrontRight = conversions::to2D(
250  robot->getRobotNode("PlatformCornerFrontRight")->getPositionInRootFrame());
251  const Eigen::Vector2f ptBackRight = conversions::to2D(
252  robot->getRobotNode("PlatformCornerBackRight")->getPositionInRootFrame());
253  const Eigen::Vector2f ptBackLeft = conversions::to2D(
254  robot->getRobotNode("PlatformCornerBackLeft")->getPositionInRootFrame());
255 
256  const auto addMargin = [&](const Eigen::Vector2f& pt) -> Eigen::Vector2f
257  {
258  Eigen::Vector2f ptWithMargin = pt;
259  ptWithMargin.x() +=
260  std::copysignf(properties.robotHull.robotConvexHullMargin, pt.x());
261  ptWithMargin.y() +=
262  std::copysignf(properties.robotHull.robotConvexHullMargin, pt.y());
263 
264  return ptWithMargin;
265  };
266 
267  {
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));
273 
274  robotHull = VirtualRobot::MathTools::createConvexHull2D(hullPoints);
275  }
276 
277  // cable fix is only applicable when the robot shape is a rectangle
278  if (properties.cableFix.enabled)
279  {
280  // the cable area and the robot hull overlap slightly. as all points must be within one region,
281  // the cable area should start at approx. the position of the wheels
282 
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());
294 
295  cableArea = VirtualRobot::MathTools::createConvexHull2D(cableAreaPoints);
296  }
297 
298  break;
299  }
301  {
302  // cable fix is not supported for circular robots
303  if (properties.cableFix.enabled)
304  {
305  ARMARX_ERROR << "Cable fix is not supported for circular robots!";
306  }
307  const Eigen::Vector2f root =
308  conversions::to2D(robot->getRootNode()->getPositionInRootFrame());
309  robotHull = root;
310  break;
311  }
312  }
313 
314  // initialize costmap
315  obstacleDistancesCostmapSmallMargin.emplace(
316  fetchDistanceToObstacleCostmapSmallMargin(costmapReaderPlugin->get()));
317 
318 
320  this,
321  &LaserScannerFeatureExtraction::runPeriodically,
322  properties.taskPeriodMs,
323  false,
324  "runningTask");
325  task->start();
326  }
327 
329  toArmemFeature(const Features& features)
330  {
331  memory::LaserScannerFeature armemFeature;
332 
333  if (features.chain)
334  {
335  armemFeature.chain = features.chain.value();
336  }
337 
338  if (features.circle)
339  {
340  armemFeature.circle = features.circle.value();
341  }
342 
343  if (features.convexHull)
344  {
345  armemFeature.convexHull = features.convexHull->vertices;
346  }
347 
348  if (features.ellipsoid)
349  {
350  armemFeature.ellipsoid = features.ellipsoid.value();
351  }
352 
353  armemFeature.points = features.points;
354 
355  return armemFeature;
356  }
357 
358  namespace
359  {
361  toArmemFeatures(const std::vector<Features>& features,
362  const Eigen::Isometry3f& global_T_sensor,
363  const std::string& sensorFrame)
364  {
365  memory::LaserScannerFeatures armemFeatures;
366  armemFeatures.frame = sensorFrame;
367  armemFeatures.frameGlobalPose = global_T_sensor;
368 
369 
370  armemFeatures.features = simox::alg::apply(features, toArmemFeature);
371 
372  return armemFeatures;
373  }
374 
375  std::vector<Features>
376  removeInvalidFeatures(std::vector<Features> features)
377  {
378  const auto isInvalid = [](const Features& features) -> bool
379  {
380  if (features.points.size() < 2)
381  {
382  return true;
383  }
384 
385  if (not features.convexHull.has_value())
386  {
387  return true;
388  }
389 
390  // if(not features.circle.has_value())
391  // {
392  // return true;
393  // }
394 
395  // if(not features.ellipsoid.has_value())
396  // {
397  // return true;
398  // }
399 
400  return false;
401  };
402 
403 
404  features.erase(std::remove_if(features.begin(), features.end(), isInvalid),
405  features.end());
406 
407  return features;
408  }
409  } // namespace
410 
411  std::vector<Features>
412  LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
413  const std::vector<Features>& featuresFromExtractor)
414  {
415  ARMARX_DEBUG << "Publishing data";
416 
417  // obtain sensor pose
418  const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
419 
420  ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*robot, data.header.timestamp));
421 
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());
426 
427  //Eigen::AlignedBox2f box;
428  //box.extend(pt1).extend(pt2).extend(pt3).extend(pt4);
429 
430  const auto transformPoints =
431  [](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& tf)
432  {
433  std::vector<Eigen::Vector2f> out;
434  out.reserve(points.size());
435 
436  std::transform(points.begin(),
437  points.end(),
438  std::back_inserter(out),
439  [&tf](const auto& pt)
440  { return conversions::to2D(tf * conversions::to3D(pt)); });
441 
442  return out;
443  };
444 
445  const auto isPointInsideRobot = [&](const Eigen::Vector2f& pt) -> bool
446  {
447  if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
448  {
449  return VirtualRobot::MathTools::isInside(
450  pt, std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull));
451  }
452  else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
453  {
454  return (std::get<Eigen::Vector2f>(robotHull) - pt).norm() <
455  properties.robotHull.radius;
456  }
457  return false;
458  };
459  const auto isPointInsideCableArea = [&](const Eigen::Vector2f& pt) -> bool
460  { return VirtualRobot::MathTools::isInside(pt, cableArea); };
461  const auto isClusterInvalid = [&](const Features& features) -> bool
462  {
463  // either use convex hull (compact representation) or raw points as fallbacck
464  const auto points = [&]()
465  {
466  if (features.convexHull)
467  {
468  return transformPoints(features.convexHull->vertices, robot_T_sensor);
469  }
470 
471  return transformPoints(features.points, robot_T_sensor);
472  }();
473 
474  const bool allPointsInsideRobot =
475  std::all_of(points.begin(), points.end(), isPointInsideRobot);
476  if (allPointsInsideRobot)
477  {
478  return true;
479  }
480 
481  if (properties.cableFix.enabled)
482  {
483 
484  const bool allPointsInCableArea =
485  std::all_of(points.begin(), points.end(), isPointInsideCableArea);
486  if (allPointsInCableArea)
487  {
488  if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
489  {
490  return true;
491  }
492  }
493  }
494 
495  return false;
496  };
497 
498  const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
499  {
500  features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
501  features.end());
502  return features;
503  };
504 
505  ARMARX_VERBOSE << featuresFromExtractor.size() << " features from extractor";
506 
507  const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
508  ARMARX_VERBOSE << features.size() << " features without cable region";
509 
510  const auto validFeatures = removeInvalidFeatures(features);
511  ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
512 
513  const auto armemFeatures =
514  toArmemFeatures(validFeatures, global_T_sensor, data.header.frame);
515 
516  ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
517 
518  // report the features
519  publishFeatures(armemFeatures, data.header.timestamp);
520 
521  if (properties.arviz.visualizeSeparateFeatures)
522  {
523  drawFeatures(
524  features, &data, data.header.frame, data.header.timestamp, global_T_sensor);
525  }
526 
527  return validFeatures;
528  }
529 
530  Throttler&
531  LaserScannerFeatureExtraction::getOrCreateThrottler(std::string frame)
532  {
533  const auto it = throttlers.find(frame);
534  if (it == throttlers.end())
535  {
536  throttlers.emplace(frame, Throttler(10.F));
537  }
538 
539  return throttlers.at(frame);
540  }
541 
542  void
543  LaserScannerFeatureExtraction::drawRobot(const armem::Time& timestamp)
544  {
545  // check if arviz should be triggered
546  if (getOrCreateThrottler(robot->getName()).check(timestamp))
547  {
548  const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
549 
550  if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
551  {
552  VirtualRobot::MathTools::ConvexHull2DPtr& convexHullPtr =
553  std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull);
554  if (convexHullPtr != nullptr)
555  {
556  arVizDrawer->draw("robot_convex_hull",
557  *convexHullPtr,
558  global_T_robot,
559  simox::Color::azure(255, 80));
560  }
561  }
562  else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
563  {
564  Eigen::Vector2f& root = std::get<Eigen::Vector2f>(robotHull);
565  arVizDrawer->draw("robot_circle_hull",
566  Circle{root, properties.robotHull.radius},
567  global_T_robot,
568  simox::Color::azure(255, 80));
569  }
570 
571  if (cableArea != nullptr)
572  {
573  arVizDrawer->draw(
574  "cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
575  }
576  }
577  }
578 
579  void
580  LaserScannerFeatureExtraction::drawFeatures(
581  const std::vector<Features>& features,
582  const armem::laser_scans::LaserScanStamped* laserPoints,
583  const std::string frame,
584  const armem::Time& timestamp,
585  const Eigen::Isometry3f& global_T_sensor)
586  {
587  // check if arviz should be triggered
588  if (getOrCreateThrottler(frame).check(timestamp))
589  {
590  arVizDrawer->draw(features, frame, global_T_sensor);
591 
592  if (properties.arviz.drawRawPoints && laserPoints != nullptr)
593  {
594  arVizDrawer->draw(*laserPoints, global_T_sensor, simox::Color::magenta(255, 100));
595  }
596  }
597  }
598 
599  void
600  LaserScannerFeatureExtraction::publishFeatures(const memory::LaserScannerFeatures& features,
601  const armem::Time& timestamp)
602  {
603  // store in memory
604  ARMARX_CHECK_NOT_NULL(laserScannerFeatureWriterPlugin);
605  laserScannerFeatureWriterPlugin->get().store(features, getName(), timestamp);
606 
607  // // legacy - topic
608  // LineSegment2DChainSeq chains;
609  // for (const auto& feature : features.features)
610  // {
611  // if (not feature.chain.empty())
612  // {
613  // LineSegment2DChain chain;
614  // for (const auto& pt : feature.chain)
615  // {
616  // chain.push_back(
617  // conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
618  // }
619  // chains.push_back(chain);
620  // }
621  // }
622 
623  // featuresTopic->reportExtractedLineSegments(chains);
624  }
625 
626  void
628  {
629  task->stop();
630  }
631 
632  void
634  {
635  }
636 
637  std::string
639  {
640  return defaultName;
641  }
642 
643  std::string
645  {
646  return defaultName;
647  }
648 
649  void
650  LaserScannerFeatureExtraction::runPeriodically()
651  {
652  const armem::laser_scans::client::Reader::Query laserScanQuery{
653  .agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
654 
656  laserScannerReaderPlugin->get().queryData(laserScanQuery);
657 
659  {
660  ARMARX_INFO << deactivateSpam(10) << "Failed to read laser scanner data. Reason: "
661  << laserScanResult.errorMessage;
662  return;
663  }
664 
665  // "Happy" case: data is available
666  ARMARX_CHECK_EQUAL(laserScanResult.status,
668  << laserScanResult.errorMessage;
669 
670  ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
671  << " sensors";
672 
673  frequencyReporterRun->add(IceUtil::Time::now().toMicroSeconds());
674 
675  std::vector<FramedFeatures> allFeatures;
676  allFeatures.reserve(laserScanResult.laserScans.size());
677  armem::Time mostRecentTimestamp{0};
678 
679  for (auto& scan : laserScanResult.laserScans)
680  {
681  mostRecentTimestamp = std::max(mostRecentTimestamp, scan.header.timestamp);
682 
683  // Remove scan steps that belong to the environment (see distance_to_obstacle_costmap_small_margin)
684  if (properties.filtering.filterLaserScansWithCostmap)
685  {
686  if (not filterLaserScannerData(scan))
687  {
688  ARMARX_INFO << "Failed to filter laser scanner data.";
689  return;
690  }
691  }
692 
693  // run clustering for every sensor
694  setDebugObserverDatafield(getDefaultName() + scan.header.frame, scan.data.size());
695  const auto features = featureExtractor->onData(scan);
696 
697  auto& framedFeatures = allFeatures.emplace_back();
698 
699  // onFeatures filters out some clusters (cable area, inside robot hull etc.), we only want to keep the filtered points
700  framedFeatures.features = onFeatures(scan, features);
701  framedFeatures.frame = scan.header.frame;
702  }
703 
704  if (!allFeatures.empty())
705  {
706  mergeFeatures(allFeatures, mostRecentTimestamp);
707  }
708 
709  // visualize robot with bounding box/cable area etc.
710  drawRobot(mostRecentTimestamp);
711 
712  // some debugobserver reporting
713  frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
714  }
715 
716  bool
717  LaserScannerFeatureExtraction::filterLaserScannerData(
718  armem::laser_scans::LaserScanStamped& laserScan)
719  {
720  // filter laser scanner data
721  // Problem: Distance to obstacle costmap uses robot radius to invalidate points that are too close to an obstacle.
722  // But we want to keep a larger distance
723 
724  const auto size_before_filtering = laserScan.data.size();
725  // obtain sensor pose
726  const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
727  if (not virtualRobotReader.synchronizeRobot(*robot, laserScan.header.timestamp))
728  {
729  ARMARX_INFO << "Failed to synchronize robot to timestamp `"
730  << laserScan.header.timestamp << "`.";
731  return false;
732  }
733 
734  const Eigen::Isometry3f global_T_sensor(
735  robot->getRobotNode(laserScan.header.frame)->getGlobalPose());
736  /*const Eigen::Isometry3f robot_T_sensor(
737  robot->getRobotNode(scan.header.frame)->getPoseInRootFrame());*/
738 
739  const auto converted = toCartesian<Eigen::Vector2f>(laserScan.data);
740  std::vector<Eigen::Vector2f> laserScanPositionsGlobal;
741 
742  std::transform(converted.begin(),
743  converted.end(),
744  std::back_inserter(laserScanPositionsGlobal),
745  [&global_T_sensor](const Eigen::Vector2f& pt)
746  { return conversions::to2D(global_T_sensor * conversions::to3D(pt)); });
747 
748  ARMARX_CHECK(obstacleDistancesCostmapSmallMargin.has_value());
749  const auto& distance_to_obstacle_costmap = obstacleDistancesCostmapSmallMargin.value();
750 
751  // filter points that are too close to obstacles
752  std::vector<armarx::LaserScanStep> filteredSteps;
753  for (size_t i = 0; i < laserScanPositionsGlobal.size(); ++i)
754  {
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))
758  {
759  continue;
760  }
761  const auto& distance = distance_to_obstacle_costmap.value(vertex.index);
762 
763  if (distance.has_value() && *distance > properties.filtering.distance)
764  {
765  filteredSteps.push_back(laserScan.data[i]);
766  }
767  }
768 
769  laserScan.data = filteredSteps;
770  ARMARX_VERBOSE << "Filtered points: " << size_before_filtering << " -> "
771  << laserScan.data.size();
772 
773  return true;
774  }
775 
776  void
777  LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
778  const armem::Time& timestamp)
779  {
780  // convert features to global coordinate system
781  std::vector<Features> converted;
782  for (const auto& features : framedFeatures)
783  {
784  converted.reserve(converted.size() + features.features.size());
785  Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
786 
787  for (const auto& singleFeature : features.features)
788  {
789  converted.push_back(conversions::transformFeature(singleFeature, global_T_frame));
790  }
791  }
792 
793  // merge overlapping features
794  FeatureMerger merger{std::move(converted), properties.chainApproximationParams};
795  std::vector<Features> merged = merger.merge();
796 
797  // save merged features in another entity with name 'global'
798  std::string frame = "global";
799  const Eigen::Isometry3f global_T_global = Eigen::Isometry3f::Identity();
800 
801  const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
802 
803  laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
804 
805  setDebugObserverDatafield("numMergedClusters", merged.size());
806 
807  if (properties.arviz.visualizeMergedFeatures)
808  {
809  // visualize the features above the floor
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);
813  }
814  }
815 
816  ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
818 
819 } // namespace armarx::navigation::components::laser_scanner_feature_extraction
armarx::armem::laser_scans::client::Reader::Result::Success
@ Success
Definition: Reader.h:90
armarx::ArVizComponentPluginUser::getArvizClient
armarx::viz::Client & getArvizClient()
Definition: ArVizComponentPlugin.h:45
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
features.h
armarx::navigation::memory::LaserScannerFeatures
Definition: types.h:61
armarx::armem::laser_scans::client::Reader::Result
Definition: Reader.h:80
geometry.h
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::GetDefaultName
static std::string GetDefaultName()
Get the component's default name.
Definition: Component.cpp:644
armarx::navigation::components::laser_scanner_feature_extraction::Features
Definition: FeatureExtractor.h:42
armarx::core::time::Clock::WaitFor
static void WaitFor(const Duration &duration)
Wait for a certain duration on the virtual clock.
Definition: Clock.cpp:99
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:638
DateTime.h
ARMARX_CHECK_NOT_NULL
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
Definition: ExpressionException.h:206
armarx::Throttler::check
bool check(const armarx::DateTime &timestamp) noexcept
Check if enough time has passed since this function last returned true.
Definition: Throttler.cpp:13
armarx::navigation::memory::LaserScannerFeature::ellipsoid
Ellipsoid ellipsoid
Definition: types.h:54
forward_declarations.h
armarx::DebugObserverComponentPluginUser::setDebugObserverDatafield
void setDebugObserverDatafield(Ts &&... ts) const
Definition: DebugObserverComponentPlugin.h:86
trace.h
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::Properties::RobotHull::RECTANGLE
@ RECTANGLE
Definition: Component.h:182
Duration.h
Reader.h
PeriodicTask.h
armarx::navigation::components::laser_scanner_feature_extraction::Circle
memory::Circle Circle
Definition: FeatureExtractor.h:40
armarx::navigation::memory::LaserScannerFeature::points
Points points
Definition: types.h:58
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&... params)
Definition: ManagedIceObject.h:186
types.h
armarx::navigation::memory::client::costmap::Reader::query
Result query(const Query &query) const
Definition: Reader.cpp:80
armarx::DebugObserverComponentPluginUser::getDebugObserver
const DebugObserverInterfacePrx & getDebugObserver() const
Definition: DebugObserverComponentPlugin.cpp:123
FeatureMerger.h
Reader.h
armarx::navigation::components::laser_scanner_feature_extraction::Features::ellipsoid
std::optional< Ellipsoid > ellipsoid
Definition: FeatureExtractor.h:50
armarx::navigation::memory::LaserScannerFeatures::features
std::vector< LaserScannerFeature > features
Definition: types.h:67
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::navigation::components::laser_scanner_feature_extraction::Features::chain
std::optional< Chain > chain
Definition: FeatureExtractor.h:52
Clock.h
armarx::navigation::memory::client::costmap::Reader::Query::providerName
std::string providerName
Definition: Reader.h:44
Costmap.h
FeatureExtractor.h
armarx::navigation::memory::LaserScannerFeatures::frameGlobalPose
Eigen::Isometry3f frameGlobalPose
Definition: types.h:65
armarx::navigation::memory::LaserScannerFeature::chain
Chain chain
Definition: types.h:56
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
armarx::navigation::components::laser_scanner_feature_extraction::ARMARX_REGISTER_COMPONENT_EXECUTABLE
ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction, LaserScannerFeatureExtraction::GetDefaultName())
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::navigation::memory::client::costmap::Reader
Definition: Reader.h:36
armarx::navigation::memory::client::costmap::Reader::Result::costmap
std::optional< algorithms::Costmap > costmap
Definition: Reader.h:51
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:222
armarx::armem::laser_scans::client::Reader::Result::errorMessage
std::string errorMessage
Definition: Reader.h:93
armarx::conversions::to2D
Eigen::Vector2f to2D(const Eigen::Vector3f &v2)
Definition: eigen.h:33
armarx::conversions::transformFeature
Features transformFeature(const Features &feature, const Eigen::Isometry3f &transformation)
Definition: features.cpp:15
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::Properties::RobotHull::CIRCLE
@ CIRCLE
Definition: Component.h:183
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::LaserScannerFeatureExtraction
LaserScannerFeatureExtraction()
Definition: Component.cpp:170
armarx::armem::laser_scans::client::Reader::Query
Definition: Reader.h:69
armarx::armem::laser_scans::client::Reader::Result::status
enum armarx::armem::laser_scans::client::Reader::Result::Status status
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::onInitComponent
void onInitComponent() override
Definition: Component.cpp:179
max
T max(T t1, T t2)
Definition: gdiam.h:51
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::navigation::memory::client::costmap::Reader::Result::status
enum armarx::navigation::memory::client::costmap::Reader::Result::Status status
armarx::navigation::memory::client::costmap::Reader::Query
Definition: Reader.h:42
Throttler.h
Component.h
types.h
armarx::navigation::components::laser_scanner_feature_extraction::Features::convexHull
std::optional< VirtualRobot::MathTools::ConvexHull2D > convexHull
Definition: FeatureExtractor.h:47
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
ExpressionException.h
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:627
armarx::navigation::memory::LaserScannerFeature::circle
Circle circle
Definition: types.h:53
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
armarx::navigation::components::laser_scanner_feature_extraction::toArmemFeature
memory::LaserScannerFeature toArmemFeature(const Features &features)
Definition: Component.cpp:329
Decoupled.h
armarx::navigation::memory::client::costmap::Reader::Result
Definition: Reader.h:49
armarx::conversions::Features
armarx::navigation::components::laser_scanner_feature_extraction::Features Features
Definition: features.cpp:12
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
PropertyDefinitionContainer.h
armarx::navigation::components::laser_scanner_feature_extraction::Features::points
Points points
Definition: FeatureExtractor.h:54
IceUtil::Handle
Definition: forward_declarations.h:30
FrequencyReporter.h
armarx::navigation::memory::LaserScannerFeatures::frame
std::string frame
Definition: types.h:64
F
Definition: ExportDialogControllerTest.cpp:18
armarx::armem::laser_scans::client::Reader::Result::laserScans
std::vector< LaserScanStamped > laserScans
Definition: Reader.h:83
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:93
armarx::Logging::deactivateSpam
SpamFilterDataPtr deactivateSpam(float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const
disables the logging for the current line for the given amount of seconds.
Definition: Logging.cpp:99
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:95
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:108
armarx::navigation::components::laser_scanner_feature_extraction::Features::circle
std::optional< Circle > circle
Definition: FeatureExtractor.h:49
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:81
armarx::navigation::memory::LaserScannerFeature
Definition: types.h:46
armarx::PeriodicTask
Definition: ArmarXManager.h:70
Logging.h
ArVizDrawer.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
laser_scanner_conversion.h
armarx::navigation::components::laser_scanner_feature_extraction::LaserScannerFeatureExtraction::onExitComponent
void onExitComponent() override
Definition: Component.cpp:633
armarx::navigation::components::laser_scanner_feature_extraction
Definition: ArVizDrawer.cpp:28
armarx::armem::laser_scans::client::Reader::Query::agent
std::string agent
Definition: Reader.h:71
armarx::navigation::algorithms::Costmap
Definition: Costmap.h:16
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:48
armarx::navigation::memory::client::costmap::Reader::Result::Success
@ Success
Definition: Reader.h:55
armarx::navigation::memory::LaserScannerFeature::convexHull
Points convexHull
Definition: types.h:51