LaserScannerSelfLocalisation.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::LaserScannerSelfLocalisation
17  * @author Fabian Paus ( fabian dot paus at kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
27 
32 
33 // These object factories are required, otherwise a runtime error will occur (static global object registration...)
37 
38 #include <SimoxUtility/json/json.hpp>
39 
40 #include <IceUtil/UUID.h>
41 
42 #include <Eigen/Geometry>
43 #include <fstream>
44 #include <iomanip>
45 #include <chrono>
46 #include <cfloat>
47 
48 
49 using namespace armarx;
50 
51 using Line = Eigen::ParametrizedLine<float, 2>;
52 
53 namespace
54 {
55  bool anyIsNaN(Eigen::Vector2f const& v)
56  {
57  return std::isnan(v.x())
58  || std::isnan(v.y());
59  }
60 
61  bool anyIsNaN(Eigen::Vector3f const& v)
62  {
63  return std::isnan(v.x())
64  || std::isnan(v.y())
65  || std::isnan(v.z());
66  }
67 
68  std::string readWholeFile(std::string const& filename)
69  {
70  std::ifstream t(filename.c_str());
71  std::string str;
72 
73  t.seekg(0, std::ios::end);
74  str.reserve(t.tellg());
75  t.seekg(0, std::ios::beg);
76 
77  str.assign(std::istreambuf_iterator<char>(t),
78  std::istreambuf_iterator<char>());
79  return str;
80  }
81 
82  Eigen::Vector2f parsePosition(Json::Value const& pos)
83  {
84  Eigen::Vector2f result = Eigen::Vector2f::Zero();
85  if (pos.isArray() && pos.size() == 2)
86  {
87  Json::Value const& x = pos[0];
88  Json::Value const& y = pos[1];
89  if (x.isNumeric() && y.isNumeric())
90  {
91  result[0] = (float)x.asDouble();
92  result[1] = (float)y.asDouble();
93  }
94  else
95  {
96  throw std::runtime_error("Map format error: Unexpected types in 2D vector");
97  }
98  }
99  else
100  {
101  throw std::runtime_error("Map format error: Unexpected number of elements in 2D vector");
102  }
103  return result;
104  }
105 
106  std::vector<LineSegment2Df> loadMapFromFile(std::string const& filename)
107  {
108  std::string fullFilename;
109  if (!ArmarXDataPath::getAbsolutePath(filename, fullFilename))
110  {
111  throw std::runtime_error("Could not find map file: " + filename);
112  }
113 
114  std::string fileContent = readWholeFile(fullFilename);
115  JSONObject o;
116  o.fromString(fileContent);
117  Json::Value const& j = o.getJsonValue();
118  if (!j.isObject())
119  {
120  throw std::runtime_error("Map format error: expected a JSON object at root level");
121  }
122 
123  float lengthUnit = 1.0f; // Default: mm
124  Json::Value const& unitValue = j["LengthUnit"];
125  if (unitValue.isString())
126  {
127  std::string unit = unitValue.asString();
128  if (unit == "m")
129  {
130  lengthUnit = 1000.0f;
131  }
132  else if (unit == "mm")
133  {
134  lengthUnit = 1.0f;
135  }
136  else
137  {
138  throw std::runtime_error("Unknown length unit: " + unit);
139  }
140  }
141 
142  Json::Value const& map = j["Map"];
143  if (!map.isArray())
144  {
145  throw std::runtime_error("Map format error: Expected an array at property 'Map'");
146  }
147 
148  std::vector<LineSegment2Df> result;
149 
150  for (Json::ArrayIndex i = 0; i < map.size(); ++i)
151  {
152  Json::Value const& p = map[i];
153  Json::Value const& position = p["Position"];
154  Eigen::Vector2f pos = parsePosition(position);
155  Json::Value const& strip = p["RelativeLineStrip"];
156  if (!strip.isArray())
157  {
158  throw std::runtime_error("Map format error: Expected an error at property 'RelativeLineStrip'");
159  }
160 
161  Eigen::Vector2f startPos = pos;
162  for (Json::ArrayIndex j = 0; j < strip.size(); ++j)
163  {
164  Eigen::Vector2f nextPos = startPos + parsePosition(strip[j]);
165 
166  result.push_back(LineSegment2Df {lengthUnit * startPos, lengthUnit * nextPos});
167  startPos = nextPos;
168  }
169  }
170 
171  return result;
172  }
173 
174  float lineSegmentToPointDistanceSquared(LineSegment2Df const& segment, Eigen::Vector2f point)
175  {
176  // Return minimum distance between line segment and a point
177  Eigen::Vector2f dir = segment.end - segment.start;
178  float l2 = dir.squaredNorm();
179  if (l2 <= 0.0)
180  {
181  return (point - segment.start).squaredNorm();
182  }
183 
184  // Consider the line extending the segment, parameterized as v + t (w - v).
185  // We find projection of point p onto the line.
186  // It falls where t = [(p-v) . (w-v)] / |w-v|^2
187  // We clamp t from [0,1] to handle points outside the segment vw.
188  float t = std::max(0.0f, std::min(1.0f, (point - segment.start).dot(dir) / l2));
189  Eigen::Vector2f projection = segment.start + t * dir; // Projection falls on the segment
190  return (point - projection).squaredNorm();
191  }
192 
193  float distance(Line const& line, Eigen::Vector2f point)
194  {
195  Eigen::Vector2f normalizedDir = line.direction();
196  Eigen::Vector2f diff = point - line.origin();
197  float signedDistance = normalizedDir.x() * diff.y() - normalizedDir.y() * diff.x();
198  return std::fabs(signedDistance);
199  }
200 
201  Line leastSquareLine(Eigen::Vector2f* begin, Eigen::Vector2f* end,
202  float* maxError = nullptr,
203  float* averageError = nullptr,
204  float* as = nullptr,
205  float* bs = nullptr,
206  float* det = nullptr)
207  {
208  float sumx = 0.0f;
209  float sumy = 0.0f;
210  float sumxx = 0.0f;
211  float sumyy = 0.0f;
212  float sumxy = 0.0f;
213  for (Eigen::Vector2f* point = begin; point != end; ++point)
214  {
215  float x = point->x();
216  float y = point->y();
217  sumx += x;
218  sumy += y;
219  sumxx += x * x;
220  sumyy += y * y;
221  sumxy += x * y;
222  }
223 
224  float numx = (float)(end - begin);
225  float calc_det = (numx * sumxx) - (sumx * sumx);
226  float calc_dety = (numx * sumyy) - (sumy * sumy);
227 
228  float calc_as = (sumxx * sumy) - (sumx * sumxy);
229  float calc_bs = (numx * sumxy) - (sumx * sumy);
230 
231  Line zeroLine;
232  zeroLine.origin() = Eigen::Vector2f::Zero();
233  zeroLine.direction() = Eigen::Vector2f::UnitX();
234  if (std::abs(calc_as) < 0.001f && std::abs(calc_bs) < 0.001f)
235  {
236  ARMARX_VERBOSE << "Vector is close to zero "
237  << VAROUT(calc_as)
238  << " " << VAROUT(calc_bs);
239  return zeroLine;
240  }
241  if (numx == 0.0f)
242  {
243  ARMARX_VERBOSE << "numx is zero "
244  << VAROUT(begin)
245  << " " << VAROUT(end);
246  return zeroLine;
247  }
248 
249 
250  Line line;
251  if (calc_det > calc_dety)
252  {
253  line.direction() = Eigen::Vector2f(calc_det, calc_bs).normalized();
254  }
255  else
256  {
257  line.direction() = Eigen::Vector2f(calc_bs, calc_dety).normalized();
258  }
259 
260  line.origin() = Eigen::Vector2f(sumx / numx, sumy / numx);
261 
262  // the error is calculated only if demanded
263  if (numx > 0 && maxError)
264  {
265  float this_average_error = 0.0f;
266  float this_max_error = 0.0f;
267  for (Eigen::Vector2f* point = begin; point != end; ++point)
268  {
269  float point_error = distance(line, *point);
270  this_average_error += point_error;
271  this_max_error = std::max(point_error, this_max_error);
272  }
273  this_average_error = this_average_error / numx;
274 
275  *maxError = this_max_error;
276 
277  if (averageError)
278  {
279  *averageError = this_average_error;
280  }
281  }
282 
283  if (as)
284  {
285  *as = calc_as;
286  }
287  if (bs)
288  {
289  *bs = calc_bs;
290  }
291  if (det)
292  {
293  *det = calc_det;
294  }
295 
296  return line;
297  }
298 
299  LineSegment2Df leastSquareLineSegmentInOrder(Eigen::Vector2f* begin, Eigen::Vector2f* end, float* maxError, float* averageError)
300  {
301  LineSegment2Df segment;
302  if ((end - begin) < 2)
303  {
304  segment.start = Eigen::Vector2f::Zero();
305  segment.end = Eigen::Vector2f(5000.0f, 5000.0f);
306  if (maxError)
307  {
308  *maxError = FLT_MAX;
309  }
310  if (averageError)
311  {
312  *averageError = FLT_MAX;
313  }
314  return segment;
315  }
316 
317  Line line = leastSquareLine(begin, end, maxError, averageError, nullptr, nullptr, nullptr);
318  if (anyIsNaN(line.direction()))
319  {
320  segment.start = Eigen::Vector2f::Zero();
321  segment.end = Eigen::Vector2f(5000.0f, 5000.0f);
322  if (maxError)
323  {
324  *maxError = FLT_MAX;
325  }
326  if (averageError)
327  {
328  *averageError = FLT_MAX;
329  }
330  return segment;
331  }
332  segment.start = line.projection(*begin);
333  segment.end = line.projection(*(end - 1));
334 
335  if (anyIsNaN(segment.start))
336  {
337  ARMARX_WARNING << "Segment start is NaN: " << VAROUT(segment.start)
338  << VAROUT(line.origin())
339  << VAROUT(line.direction());
340  }
341  if (anyIsNaN(segment.end))
342  {
343  ARMARX_WARNING << "Segment end is NaN: " << VAROUT(segment.end)
344  << VAROUT(line.origin())
345  << VAROUT(line.direction());
346  }
347 
348  return segment;
349  }
350 
351  float leastSquareEdge(Eigen::Vector2f* begin, Eigen::Vector2f* end, LineSegment2Df& segment)
352  {
353  float maxError {0};
354  float averageError {0};
355  segment = leastSquareLineSegmentInOrder(begin, end, &maxError, &averageError);
356 
357  return averageError;
358  }
359 }
360 
361 
363  : s(0.1) // This frequency will not be used
364 {
365 }
366 
368 {
369  reportTopicName = getProperty<std::string>("ReportTopicName");
370  robotStateComponentName = getProperty<std::string>("RobotStateComponentName");
371  platformName = getProperty<std::string>("PlatformName");
372  laserScannerUnitName = getProperty<std::string>("LaserScannerUnitName");
373  workingMemoryName = getProperty<std::string>("WorkingMemoryName");
374  longtermMemoryName = getProperty<std::string>("LongtermMemoryName");
375  mapFilename = getProperty<std::string>("MapFilename");
376  agentName = getProperty<std::string>("AgentName");
377  emergencyStopMasterName = getProperty<std::string>("EmergencyStopMasterName");
378  debugObserverName = getProperty<std::string>("DebugObserverName");
379  updateWorkingMemory = getProperty<bool>("UpdateWorkingMemory");
380  int updatePeriodWorkingMemoryInMs = getProperty<int>("UpdatePeriodWorkingMemoryInMs");
381  workingMemoryUpdateFrequency = 1000.0f / updatePeriodWorkingMemoryInMs;
382  int updatePeriodLongtermMemory = getProperty<int>("UpdatePeriodLongtermMemoryInMs");
383  longtermMemoryUpdateFrequency = 1000.0f / updatePeriodLongtermMemory;
384  robotPositionZ = getProperty<float>("RobotPositionZ");
385  maximalLaserScannerDelay = getProperty<float>("MaximalLaserScannerDelay").getValue();
386 
387  updateProperties(true);
388 
389  map = loadMapFromFile(mapFilename);
390 
391  {
392  const auto min = getPropertyAsCSV<float>("PlatformRectangleMin");
393  const auto max = getPropertyAsCSV<float>("PlatformRectangleMax");
394  std::tie(platformRectMin(0), platformRectMax(0)) = std::minmax(min.at(0), max.at(0));
395  std::tie(platformRectMin(1), platformRectMax(1)) = std::minmax(min.at(1), max.at(1));
396  ARMARX_INFO << "using platform rectangle min / max " << platformRectMin.transpose() << " / " << platformRectMax.transpose();
397  }
398 
399 
400  usingProxy(robotStateComponentName);
401  usingProxy(laserScannerUnitName);
402  usingProxy(workingMemoryName);
403  usingProxy(longtermMemoryName);
404 }
405 
406 
408 {
409  robotState = getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName);
410  laserScannerUnit = getProxy<LaserScannerUnitInterfacePrx>(laserScannerUnitName);
411  SharedRobotInterfacePrx sharedRobot = robotState->getSynchronizedRobot();
412  for (LaserScannerInfo const& info : laserScannerUnit->getConnectedDevices())
413  {
414  FramedPoseBasePtr basePose = sharedRobot->getRobotNode(info.frame)->getPoseInRootFrame();
415  FramedPose& pose = dynamic_cast<FramedPose&>(*basePose);
416 
418  data.pose = pose.toEigen();
419  data.info = info;
420  data.mutex = std::make_shared<std::mutex>();
421  scanData.push_back(data);
422  }
423 
424  debugObserver = getTopic<DebugObserverInterfacePrx>(debugObserverName);
425 
426  robotRootFrame = sharedRobot->getRootNode()->getName();
427 
428  workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
429  agentsMemory = workingMemory->getAgentInstancesSegment();
430  if (agentName.empty())
431  {
432  agentName = robotState->getRobotName();
433  }
434  agentInstance = new memoryx::AgentInstance();
435  agentInstance->setSharedRobot(sharedRobot);
436  agentInstance->setAgentFilePath(robotState->getRobotFilename());
437  agentInstance->setName(agentName);
438 
439  longtermMemory = getProxy<memoryx::LongtermMemoryInterfacePrx>(longtermMemoryName);
440  selfLocalisationSegment = longtermMemory->getSelfLocalisationSegment();
441 
442  offeringTopic(reportTopicName);
443  reportTopic = getTopic<LaserScannerSelfLocalisationListenerPrx>(reportTopicName);
444  offeringTopic("PlatformState");
445  platformUnitTopic = getTopic<PlatformUnitListenerPrx>("PlatformState");
446  // Load estimatedPose from longterm memory
447  estimatedPose = Eigen::Vector3f::Zero();
448  memoryx::EntityBasePtr poseEntity = selfLocalisationSegment->getEntityByName(agentName);
449  if (poseEntity)
450  {
451  poseEntityId = poseEntity->getId();
452  ARMARX_INFO << "Using pose entity from LTM with ID: " << poseEntityId;
453  memoryx::EntityAttributeBasePtr poseAttribute = poseEntity->getAttribute("pose");
454  if (poseAttribute)
455  {
456  // So this is how you access a typed attribute, fancy
457  FramedPosePtr framedPose = armarx::VariantPtr::dynamicCast(poseAttribute->getValueAt(0))->getClass<armarx::FramedPose>();
458  if (framedPose)
459  {
460  Eigen::Matrix4f globalPose = framedPose->toGlobalEigen(sharedRobot);
461  Eigen::Matrix3f robotRot = globalPose.block<3, 3>(0, 0);
462  Eigen::Vector2f robotPos = globalPose.block<2, 1>(0, 3);
463  Eigen::Vector2f yWorld(0.0f, 1.0f);
464  Eigen::Vector2f yRobot = robotRot.col(1).head<2>();
465  float robotTheta = acos(yWorld.dot(yRobot));
466  if (yRobot.x() >= 0.0f)
467  {
468  robotTheta = -robotTheta;
469  }
470 
471  estimatedPose = Eigen::Vector3f(robotPos.x(), robotPos.y(), robotTheta);
472  }
473  else
474  {
475  ARMARX_WARNING << "'pose' attribute does not contain a framed pose";
476  }
477  }
478  else
479  {
480  ARMARX_WARNING << "No attribute 'pose' in entity for agent '" << agentName << "' found";
481  }
482  }
483  else
484  {
485  ARMARX_WARNING << "No pose in longterm memory found for agent: " << agentName;
486  ARMARX_WARNING << "Self localisation will start at origin";
487 
490  poseEntity->setName(agentName);
491  poseEntity->putAttribute("pose", pose);
492  //poseEntity->setId(poseEntityId);
493 
494  poseEntityId = selfLocalisationSegment->addEntity(poseEntity);
495  ARMARX_INFO << "New entity ID: " << poseEntityId;
496  }
497  ARMARX_INFO << "Initial pose: (" << estimatedPose[0] << ", " << estimatedPose[1] << ", " << estimatedPose[2] << ")";
498  resetKalmanFilter(estimatedPose);
499  lastVelocityUpdate = TimeUtil::GetTime();
500  lastVelocity = Eigen::Vector3f::Zero();
501 
502  usingTopic(laserScannerUnit->getReportTopicName());
503  usingTopic(platformName + "State");
504 
505 
506  int updatePeriod = getProperty<int>("UpdatePeriodInMs");
507  task = new PeriodicTask<LaserScannerSelfLocalisation>(this, &LaserScannerSelfLocalisation::updateLocalisation, updatePeriod, "UpdateLocalisation");
508  task->start();
509 }
510 
511 
513 {
514  task->stop();
515  ARMARX_INFO << "Task stopped";
516  if (laserScannerFileLoggingData)
517  {
518  writeLogFile();
519  }
520 }
521 
522 
524 {
525 
526 }
527 
529 {
532 
533  prop->topic(globalRobotPoseTopic);
534 
535  return prop;
536 }
537 
538 std::string LaserScannerSelfLocalisation::getReportTopicName(const Ice::Current&)
539 {
540  return reportTopicName;
541 }
542 
544 {
545  {
546  std::unique_lock lock(setPoseMutex);
547  setPose = Eigen::Vector3f(x, y, theta);
548  resetKalmanFilter(*setPose);
549 
550  }
551 }
552 
553 void LaserScannerSelfLocalisation::reportSensorValues(const std::string& device, const std::string& name, const LaserScan& scan, const TimestampBasePtr& timestamp, const Ice::Current&)
554 {
555  for (LaserScanData& data : scanData)
556  {
557  if (data.info.device == device)
558  {
559  std::unique_lock lock(*data.mutex);
560  data.scan = scan;
561  data.measurementTime = IceUtil::Time::microSeconds(timestamp->timestamp);
562  }
563  }
564 }
565 
566 
567 static Eigen::Vector3f addPose(Eigen::Vector3f pose, Eigen::Vector3f add)
568 {
569  Eigen::Vector3f result = pose + add;
570  // Make sure that theta is in the range [-PI,PI]
571  result[2] = armarx::math::MathUtils::angleModPI(result[2]);
572  return result;
573 }
574 
575 static Eigen::Vector3f integratePose(Eigen::Vector3f pose, Eigen::Vector3f v0, Eigen::Vector3f v1, float dt)
576 {
577  // Assumption: Linear acceleration between v0 and v1 in dt seconds
578  // Travelled distance d = v0*dt + 0.5*(v1-v0)*dt = 0.5*dt*(v0+v1)
579 
580  float dTheta = 0.5f * dt * (v0[2] + v1[2]);
581  // This is an approximation (the right solution would have to integrate over changing theta)
582  float thetaMid = pose[2] + 0.5f * dTheta;
583  Eigen::Matrix2f rotation = Eigen::Rotation2Df(thetaMid).matrix();
584  Eigen::Vector2f globalV0 = rotation * v0.head<2>();
585  Eigen::Vector2f globalV1 = rotation * v1.head<2>();
586  Eigen::Vector2f globalDeltaPos = 0.5f * dt * (globalV0 + globalV1);
587 
588  Eigen::Vector3f d(globalDeltaPos.x(), globalDeltaPos.y(), dTheta);
589  return addPose(pose, d);
590 }
591 
593 {
595  auto elapsedSeconds = now - lastVelocityUpdate;
596  lastVelocityUpdate = now;
597  {
598  std::unique_lock lock(kalmanMutex);
599  filter->predict(velX, velY, velTheta, elapsedSeconds);
600  }
601  // Store odometry reports here and calculate the estimated pose in the central update task
602  if (useOdometry)
603  {
604 
605  std::unique_lock lock(odometryMutex);
606 
607  reportedVelocities.push_back(ReportedVelocity {static_cast<float>(elapsedSeconds.toSecondsDouble()), velX, velY, velTheta});
608 
609  }
610 
611 
612 }
613 
614 
616 {
617  // nop
618 }
619 
620 void LaserScannerSelfLocalisation::updateLocalisation()
621 {
622  // Periodic task to update the self localisation
623 
624  // Get the current parameter values;
625  propertyMutex.lock();
626  int halfFrameSize = propSmoothFrameSize / 2;
627  int mergeDistance = propSmoothMergeDistance;
628  float maxDistanceSquared = propMatchingMaxDistance * propMatchingMaxDistance;
629  float minPointsForMatch = propMatchingMinPoints;
630  float matchingCorrectionFactor = propMatchingCorrectionFactor;
631  bool reportPoints = propReportPoints;
632  bool reportEdges = propReportEdges;
633  bool useMapCorrection = propUseMapCorrection;
634  int epsilon = propEdgeEpsilon;
635  int minPointsPerEdge = propEdgeMinPoints;
636  float maxDistanceForEdgeSquared = propEdgeMaxDistance * propEdgeMaxDistance;
637  float pointAddingThreshold = propEdgePointAddingThreshold;
638  float edgeMaxDeltaAngle = propEdgeMaxDeltaAngle;
639  propertyMutex.unlock();
640 
641  Eigen::Vector3f currentPose = estimatedPose;
642 
643  // Apply the collected odometry pose correction
644  if (useOdometry)
645  {
646  std::unique_lock lock(odometryMutex);
647  for (ReportedVelocity const& report : reportedVelocities)
648  {
649  Eigen::Vector3f currentVelocity(report.x, report.y, report.theta);
650  Eigen::Vector3f motionPrediction = integratePose(currentPose, lastVelocity, currentVelocity, report.dt);
651  if (anyIsNaN(motionPrediction))
652  {
653  ARMARX_WARNING << "Motion prediction is NaN: " << motionPrediction;
654  ARMARX_WARNING << VAROUT(currentPose)
655  << " " << VAROUT(lastVelocity)
656  << " " << VAROUT(currentVelocity)
657  << " " << VAROUT(report.dt);
658  }
659  else
660  {
661  currentPose = motionPrediction;
662  }
663 
664  lastVelocity = currentVelocity;
665  }
666  reportedVelocities.clear();
667  }
668 
669  // Use the laser scan data to make a global pose correction
670  std::size_t pointsSize = 0;
672  IceUtil::Time measurementTime = IceUtil::Time::seconds(0);
673  // No data available
674  if (scanData.empty())
675  {
676  measurementTime = now;
677  }
678  for (LaserScanData const& data : scanData)
679  {
680  measurementTime = std::max(measurementTime, data.measurementTime);
681 
682  std::unique_lock lock(*data.mutex);
683  if (data.scan.empty())
684  {
685  continue;
686  }
687 
688  pointsSize += data.scan.size();
689 
690  // Check whether the scan data is out of date
691  double deltaMeasurement = (now - data.measurementTime).toSecondsDouble();
692  if (deltaMeasurement < -0.005)
693  {
694  ARMARX_WARNING << deactivateSpam(1) << "Delta between now and measurement time is negative: " << deltaMeasurement
695  << " now: " << now.toDateTime() << " measurement: " << data.measurementTime.toDateTime();
696  }
697  else if (deltaMeasurement > maximalLaserScannerDelay)
698  {
699  ARMARX_WARNING << deactivateSpam(1) << "Delta between now and measurement time has exceeded the limit ("
700  << deltaMeasurement << "s > " << maximalLaserScannerDelay
701  << "s). Activating emergency stop!";
702 
703  EmergencyStopMasterInterfacePrx emergencyStopMaster = getProxy<EmergencyStopMasterInterfacePrx>(emergencyStopMasterName);
704  if (emergencyStopMaster)
705  {
706  emergencyStopMaster->setEmergencyStopState(eEmergencyStopActive);
707  }
708  else
709  {
710  ARMARX_ERROR << "Could not activate emergency stop because proxy for EmergencyStopMaster was not found '"
711  << emergencyStopMasterName << "'";
712  }
713  }
714  }
715  if (useMapCorrection && pointsSize > 0)
716  {
717  for (LaserScanData& data : scanData)
718  {
719  Eigen::Rotation2Df globalRotation = Eigen::Rotation2Df(currentPose[2]);
720  Eigen::Vector2f globalTranslation = currentPose.head<2>();
721 
722  std::unique_lock lock(*data.mutex);
723  data.points.clear();
724  data.points.reserve(data.scan.size());
725  for (int stepIndex = 0; stepIndex < (int)data.scan.size(); ++stepIndex)
726  {
727  LaserScanStep const& step = data.scan[stepIndex];
728 
729  // Smoothing in a frame neighborhood
730  float distanceSum = 0;
731  int distanceElements = 0;
732  int minFrameIndex = std::max(0, stepIndex - halfFrameSize);
733  int maxFrameIndex = std::min((int)(data.scan.size()) - 1, stepIndex + halfFrameSize);
734  for (int frameIndex = minFrameIndex; frameIndex <= maxFrameIndex; ++frameIndex)
735  {
736  float frameDistance = data.scan[frameIndex].distance;
737  int deltaDistance = std::abs(step.distance - frameDistance);
738  if (deltaDistance <= mergeDistance)
739  {
740  distanceSum += frameDistance;
741  ++distanceElements;
742  }
743  }
744  if (distanceElements == 0)
745  {
746  ARMARX_WARNING << "No elements with distance smaller than merge distance found.";
747  ARMARX_WARNING << VAROUT(stepIndex)
748  << " " << VAROUT(step.distance)
749  << " " << VAROUT(mergeDistance);
750  }
751  else
752  {
753  float distance = distanceSum / distanceElements;
754 
755  // Transform to global 2D point
756  float sinAngle, cosAngle;
757  sincosf(step.angle, &sinAngle, &cosAngle);
758  float x = -distance * sinAngle;
759  float y = distance * cosAngle;
760 
761  Eigen::Vector2f localPoint = (data.pose * Eigen::Vector4f(x, y, 0, 1)).head<2>();
762 
763  if (anyIsNaN(localPoint))
764  {
765  ARMARX_WARNING << "Local point is NaN";
766  ARMARX_WARNING << VAROUT(localPoint)
767  << " " << VAROUT(x)
768  << " " << VAROUT(y)
769  << " " << VAROUT(step.angle);
770  continue;
771  }
772 
773  if (
774  platformRectMin(0) < localPoint(0) && localPoint(0) < platformRectMax(0) &&
775  platformRectMin(1) < localPoint(1) && localPoint(1) < platformRectMax(1)
776  )
777  {
778  ARMARX_INFO << deactivateSpam() << "discarding point inside of the platform rectangle: " << localPoint.transpose();
779  continue;
780  }
781 
782  Eigen::Vector2f globalPoint = globalRotation * localPoint + globalTranslation;
783 
784  if (anyIsNaN(globalPoint))
785  {
786  ARMARX_WARNING << "Global point is NaN";
787  ARMARX_WARNING << VAROUT(globalPoint)
788  << " " << VAROUT(x)
789  << " " << VAROUT(y)
790  << " " << VAROUT(step.angle);
791  }
792  else
793  {
794  data.points.push_back(globalPoint);
795  }
796  }
797  }
798  ARMARX_DEBUG << deactivateSpam() << "using " << data.points.size() << "/" << data.scan.size();
799  }
800  if (reportPoints)
801  {
802  std::vector<armarx::Vector2f> pointsA;
803  pointsA.reserve(pointsSize);
804  for (LaserScanData const& data : scanData)
805  {
806  for (auto& p : data.points)
807  {
808  pointsA.push_back(armarx::Vector2f {p.x(), p.y()});
809  }
810  }
811  if (reportTopic)
812  {
813  reportTopic->begin_reportLaserScanPoints(pointsA);
814  }
815  else
816  {
817  ARMARX_WARNING << "Report topic is not set";
818  }
819  }
820 
821  std::size_t edgeCount = 0;
822 
823  for (LaserScanData& data : scanData)
824  {
825  auto& points = data.points;
826 
827  std::vector<float> angles;
828  angles.reserve(points.size());
829  int pointsSize = (int)points.size();
830  for (int pointIndex = 0; pointIndex < pointsSize; ++pointIndex)
831  {
832  int beginIndex = std::max(pointIndex - epsilon, 0);
833  int endIndex = std::min(pointIndex + epsilon + 1, pointsSize - 1);
834  Eigen::Vector2f* beginPoint = &data.points[beginIndex];
835  Eigen::Vector2f* endPoint = &data.points[endIndex];
836  Line line(leastSquareLine(beginPoint, endPoint));
837  float angle = std::atan2(line.direction().x(), line.direction().y());
838  if (std::isnan(angle))
839  {
840  ARMARX_VERBOSE << "Edge angle is NaN"
841  << VAROUT(beginIndex)
842  << " " << VAROUT(endIndex);
843  }
844  else
845  {
846  angles.push_back(angle);
847  }
848  }
849 
850  int beginIndex = 0;
851  int endIndex = beginIndex;
852  int maxEndIndex = pointsSize - 1;
853  int minBeginIndex = 0;
854 
855  int maximumPointsInEdge = 0;
856  LineSegment2Df edgeLineSegment[2];
857  int resultIndex = 0;
858  data.edges.clear();
859  while (endIndex < maxEndIndex)
860  {
861  float yaw = angles[endIndex];
862 
863  // We compare the angles of following points
864  // If they stay within parametrized limits (angle and distance),
865  // we add them to the edge
866  while (endIndex < maxEndIndex &&
867  std::fabs(yaw - angles[endIndex + 1]) < edgeMaxDeltaAngle &&
868  (points[endIndex + 1] - points[endIndex]).squaredNorm() < maxDistanceForEdgeSquared)
869  {
870  endIndex++;
871  }
872 
873  // Corners have different angles but still are part of an edge
874  // Therefore we must extend both ends of the edge
875  // The extension continues until the least squares error exceeds a parameterized limit.
876 
877  bool frontExtended = false;
878  bool backExtended = false;
879 
880  // Extend the front
881  while (beginIndex > minBeginIndex &&
882  (points[beginIndex - 1] - points[beginIndex]).squaredNorm() < maxDistanceForEdgeSquared &&
883  leastSquareEdge(points.data() + beginIndex - 1, points.data() + endIndex + 1, edgeLineSegment[resultIndex]) < pointAddingThreshold)
884  {
885  beginIndex--;
886  resultIndex = !resultIndex;
887  frontExtended = true;
888  }
889 
890  // Extend the back
891  while (endIndex < maxEndIndex &&
892  (points[endIndex] - points[endIndex + 1]).squaredNorm() < maxDistanceForEdgeSquared &&
893  leastSquareEdge(points.data() + beginIndex, points.data() + endIndex + 1, edgeLineSegment[resultIndex]) < pointAddingThreshold)
894  {
895  endIndex++;
896  resultIndex = !resultIndex;
897  backExtended = true;
898  }
899 
900  if (!frontExtended && !backExtended)
901  {
902  leastSquareEdge(points.data() + beginIndex, points.data() + endIndex + 1, edgeLineSegment[!resultIndex]);
903  }
904 
905  // Check for minimum number of points in the edge
906  int numberOfPoints = endIndex - beginIndex + 1;
907  if (numberOfPoints > minPointsPerEdge)
908  {
909  // Get the extended line segment which did not exceed the error limit
910  // Index = resultIndex: contains the line segment which exceeded the error limit
911  LineSegment2Df& segment = edgeLineSegment[!resultIndex];
912  Eigen::Vector2f* pointsBegin = points.data() + beginIndex;
913  ExtractedEdge edge {segment, pointsBegin, pointsBegin + numberOfPoints};
914  data.edges.push_back(edge);
915  ++edgeCount;
916  maximumPointsInEdge = std::max(numberOfPoints, maximumPointsInEdge);
917  minBeginIndex = endIndex + 1;
918  }
919 
920  // Search for the next edge
921  beginIndex = endIndex + 1;
922  endIndex = beginIndex;
923  }
924  }
925 
926  if (reportEdges)
927  {
928  std::vector<armarx::LineSegment2D> segments;
929  segments.reserve(edgeCount);
930  for (LaserScanData const& data : scanData)
931  {
932  for (ExtractedEdge const& e : data.edges)
933  {
934  armarx::LineSegment2D s;
935  s.start = armarx::Vector2f {e.segment.start.x(), e.segment.start.y()};
936  s.end = armarx::Vector2f {e.segment.end.x(), e.segment.end.y()};
937  segments.push_back(s);
938  }
939  }
940  reportTopic->begin_reportExtractedEdges(segments);
941  }
942 
943  std::size_t rowCount = 0;
944  Eigen::Vector2f robotPos = currentPose.head<2>();
945 
946  Eigen::MatrixX3f X(pointsSize, 3);
947  Eigen::VectorXf Y(pointsSize);
948  for (LaserScanData const& data : scanData)
949  {
950  for (ExtractedEdge const& edge : data.edges)
951  {
952  for (auto* p = edge.pointsBegin; p != edge.pointsEnd; ++p)
953  {
954  auto& globalPoint = *p;
955  // Find the line segment closest to this point
956  LineSegment2Df* target = nullptr;
957  float minDistanceSquared = FLT_MAX;
958  for (LineSegment2Df& segment : map)
959  {
960  float distanceSquared = lineSegmentToPointDistanceSquared(segment, globalPoint);
961  if (distanceSquared < minDistanceSquared)
962  {
963  minDistanceSquared = distanceSquared;
964  target = &segment;
965  }
966  }
967  if (minDistanceSquared > maxDistanceSquared)
968  {
969  continue;
970  }
971  if (!target)
972  {
973  ARMARX_WARNING << "No line segment closest to point found: " << globalPoint;
974  continue;
975  }
976 
977  Eigen::Vector2f const& v = globalPoint;
978  Eigen::Vector2f targetDir = target->end - target->start;
979  Eigen::Vector2f u = Eigen::Vector2f(-targetDir.y(), targetDir.x()).normalized();
980  float r = target->start.dot(u);
981  Eigen::Vector2f cToV(v - robotPos);
982  Eigen::Vector2f cToVOrth(-cToV.y(), cToV.x());
983  float x3 = u.dot(cToVOrth);
984  float y = r - u.dot(v);
985 
986  X(rowCount, 0) = u(0);
987  X(rowCount, 1) = u(1);
988  X(rowCount, 2) = x3;
989  Y(rowCount) = y;
990  ++rowCount;
991  }
992  }
993  }
994 
995  Eigen::Vector3f b = Eigen::Vector3f::Zero();
996  float matched = static_cast<float>(rowCount) / pointsSize;
997  debugObserver->setDebugDatafield(getName(), "MatchedDataPointRatio", new Variant(matched));
998  if (matched < minPointsForMatch)
999  {
1000  ARMARX_WARNING << deactivateSpam(5) << "Too few data points could be associated with edges of the map (associated: "
1001  << rowCount << "/" << pointsSize << " = " << matched << ")";
1002  }
1003  else if (rowCount > 10)
1004  {
1005  b = X.block(0, 0, rowCount, 3).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Y.head(rowCount));
1006  b *= matchingCorrectionFactor;
1007  if (anyIsNaN(b))
1008  {
1009  ARMARX_WARNING << deactivateSpam(1) << "Equation solution is NaN "
1010  << VAROUT(b) << " " << VAROUT(rowCount);
1011  }
1012  }
1013 
1014  Eigen::Vector3f newEstimatedPose = addPose(currentPose, b);
1015  if (anyIsNaN(newEstimatedPose))
1016  {
1017  ARMARX_WARNING << "New estimated pose is NaN "
1018  << VAROUT(currentPose) << " " << VAROUT(b);
1019  }
1020  else
1021  {
1022  estimatedPose = newEstimatedPose;
1023  }
1024  {
1025  std::unique_lock lock(kalmanMutex);
1026  if (propSensorStdDev > 0)
1027  {
1028  filter->update(estimatedPose(0), estimatedPose(1), estimatedPose(2));
1029  }
1030  }
1031  }
1032  else
1033  {
1034  estimatedPose = currentPose;
1035  }
1036  // ARMARX_INFO << deactivateSpam(1) << VAROUT(estimatedPose) << VAROUT(propSensorStdDev);
1037  Eigen::Vector3f filteredPose;
1038  if (propSensorStdDev > 0)
1039  {
1040  Eigen::Matrix3d cov = filter->getCovariance();
1041  reportTopic->begin_reportPoseUncertainty(cov(0, 0), cov(1, 1), cov(2, 2));
1042  // ARMARX_INFO << deactivateSpam(1) << VAROUT(cov);
1043  filteredPose = filter->getPose().cast<float>();
1044  }
1045  else
1046  {
1047  filteredPose = estimatedPose;
1048  }
1049  {
1050  std::unique_lock lock(setPoseMutex);
1051  if (setPose)
1052  {
1053  estimatedPose = *setPose;
1054  resetKalmanFilter(estimatedPose);
1055  setPose.reset();
1056  }
1057  }
1058  reportTopic->begin_reportCorrectedPose(filteredPose[0], filteredPose[1], filteredPose[2]);
1059  PlatformPose newPose;
1060  newPose.timestampInMicroSeconds = measurementTime.toMicroSeconds();
1061  newPose.x = filteredPose[0];
1062  newPose.y = filteredPose[1];
1063  newPose.rotationAroundZ = filteredPose[2];
1064  // platformUnitTopic->reportPlatformPose(newPose);
1065 
1067  orientation.block<2, 2>(0, 0) = Eigen::Rotation2Df(filteredPose[2]).matrix();
1068  Eigen::Vector3f position(filteredPose[0], filteredPose[1], robotPositionZ);
1069  FramedPosePtr reportPose = new FramedPose(orientation, position, GlobalFrame, "");
1070 
1071 
1072  FrameHeader header;
1073  header.parentFrame = GlobalFrame;
1074  header.frame = robotRootFrame;
1075  header.agent = agentName;
1076  header.timestampInMicroSeconds = measurementTime.toMicroSeconds();
1077 
1078  TransformStamped globalRobotPose;
1079  globalRobotPose.header = header;
1080  globalRobotPose.transform = reportPose->toEigen();
1081 
1082  globalRobotPoseTopic->reportGlobalRobotPose(globalRobotPose);
1083 
1084  try
1085  {
1086  if (updateWorkingMemory && s.checkFrequency("workingMemoryUpdate", workingMemoryUpdateFrequency))
1087  {
1088  // This seems to be the way to update the current pose
1089  // Since in simulation another component (SelfLocalizationDynamicSimulation) updates this memory location
1090  // it is a bad idea to enable the working memory update in simulation
1091  if (agentInstance)
1092  {
1093  agentInstance->setPose(reportPose);
1094  if (agentsMemory)
1095  {
1096  std::string robotAgentId = agentsMemory->upsertEntityByName(agentInstance->getName(), agentInstance);
1097  agentInstance->setId(robotAgentId);
1098  }
1099  else
1100  {
1101  ARMARX_WARNING << "Agents memory not set";
1102  }
1103  }
1104  else
1105  {
1106  ARMARX_WARNING << "Agent instance not set";
1107  }
1108  }
1109 
1110  if (s.checkFrequency("longtermMemoryUpdate", longtermMemoryUpdateFrequency))
1111  {
1113  poseEntity->setName(agentName);
1114  poseEntity->putAttribute("pose", reportPose);
1115  poseEntity->setId(poseEntityId);
1116  // We just update the 'pose' attribute since ID and name have been set by the inital insert
1117  if (selfLocalisationSegment)
1118  {
1119  selfLocalisationSegment->upsertEntity(poseEntityId, poseEntity);
1120  }
1121  else
1122  {
1123  ARMARX_WARNING << "Self localisation memory segment is not set";
1124  }
1125  }
1126  }
1127  catch (IceUtil::Exception const& ex)
1128  {
1129  ARMARX_WARNING << "Caught exception while updating the memory:\n" << ex;
1130  }
1131 
1132  if (laserScannerFileLoggingData)
1133  {
1134  laserScannerFileLoggingData->scanDataHistory[now] = (scanData);
1135  }
1136 
1137 }
1138 
1140 {
1141 #define ARMARX_LSSL_UPDATE_PROPERTY(name) \
1142  { \
1143  auto prop = getProperty<decltype(prop ## name)>(#name); \
1144  if (initial || prop.isSet()) \
1145  { \
1146  prop ## name = prop.getValue(); \
1147  ARMARX_VERBOSE << VAROUT(prop ## name); \
1148  } \
1149  }
1150  try
1151  {
1152  ARMARX_VERBOSE << "Updating properties:";
1153  std::unique_lock lock(propertyMutex);
1154 
1155 
1156  if (std::abs(propSensorStdDev - getProperty<float>("SensorStdDev").getValue()) > 0.001)
1157  {
1158  propSensorStdDev = getProperty<float>("SensorStdDev");
1159  ARMARX_VERBOSE << "new " << VAROUT(propSensorStdDev);
1160  if (!initial)
1161  {
1162  resetKalmanFilter(estimatedPose);
1163  }
1164  }
1165  if (std::abs(propVelSensorStdDev - getProperty<float>("VelSensorStdDev").getValue()) > 0.00001)
1166  {
1167  propVelSensorStdDev = getProperty<float>("VelSensorStdDev");
1168  ARMARX_VERBOSE << "new " << VAROUT(propVelSensorStdDev);
1169  if (!initial)
1170  {
1171  resetKalmanFilter(estimatedPose);
1172  }
1173  }
1174 
1175 
1176 
1177 
1178  ARMARX_LSSL_UPDATE_PROPERTY(SmoothFrameSize);
1179  ARMARX_LSSL_UPDATE_PROPERTY(SmoothMergeDistance);
1180  ARMARX_LSSL_UPDATE_PROPERTY(MatchingMaxDistance);
1181  ARMARX_LSSL_UPDATE_PROPERTY(MatchingMinPoints);
1182  ARMARX_LSSL_UPDATE_PROPERTY(MatchingCorrectionFactor);
1183  ARMARX_LSSL_UPDATE_PROPERTY(EdgeMaxDistance);
1184  ARMARX_LSSL_UPDATE_PROPERTY(EdgeMaxDeltaAngle);
1185  ARMARX_LSSL_UPDATE_PROPERTY(EdgePointAddingThreshold);
1186  ARMARX_LSSL_UPDATE_PROPERTY(EdgeEpsilon);
1187  ARMARX_LSSL_UPDATE_PROPERTY(EdgeMinPoints);
1188  ARMARX_LSSL_UPDATE_PROPERTY(UseOdometry);
1189  ARMARX_LSSL_UPDATE_PROPERTY(UseMapCorrection);
1190  ARMARX_LSSL_UPDATE_PROPERTY(ReportPoints);
1191  ARMARX_LSSL_UPDATE_PROPERTY(ReportEdges);
1192  ARMARX_LSSL_UPDATE_PROPERTY(LoggingFilePath);
1193  // ARMARX_LSSL_UPDATE_PROPERTY(SensorStdDev);
1194  // ARMARX_LSSL_UPDATE_PROPERTY(VelSensorStdDev);
1195 
1196 
1197  if (!propLoggingFilePath.empty())
1198  {
1199  if (laserScannerFileLoggingData && propRecordData && !getProperty<bool>("RecordData").getValue())
1200  {
1201  writeLogFile();
1202 
1203  }
1204  if (!propRecordData && getProperty<bool>("RecordData").getValue())
1205  {
1206  // setup data
1207  laserScannerFileLoggingData.reset(new LaserScannerFileLoggingData());
1208  laserScannerFileLoggingData->filePath = propLoggingFilePath;
1209  }
1210  propRecordData = getProperty<bool>("RecordData").getValue();
1211  }
1212 
1213  useOdometry = propUseOdometry;
1214 
1215  }
1216  catch (std::exception const& ex)
1217  {
1218  ARMARX_WARNING << "Could not update properties. " << ex.what();
1219  }
1220 
1221 #undef ARMARX_LSSL_UPDATE_PROPERTY
1222 }
1223 
1224 
1225 
1226 void LaserScannerSelfLocalisation::resetKalmanFilter(const Eigen::Vector3f& pose)
1227 {
1228  ARMARX_INFO << "Position Std Dev: " << propSensorStdDev << " Vel Std Dev: " << propVelSensorStdDev;
1229  std::unique_lock lock(kalmanMutex);
1230  filter.reset(new memoryx::PlatformKalmanFilter(pose.head<2>().cast<double>(), pose(2),
1231  propSensorStdDev, propSensorStdDev / 100,
1232  propVelSensorStdDev, propVelSensorStdDev /* no scaling needed since motion noise scaling is done with rad/s*/));
1233 }
1234 
1235 void LaserScannerSelfLocalisation::writeLogFile()
1236 {
1237  if (!laserScannerFileLoggingData)
1238  {
1239  ARMARX_INFO << deactivateSpam(1) << "laserScannerFileLoggingData is NULL - cannot write log file";
1240  return;
1241  }
1242  std::map<IceUtil::Time, std::vector<LaserScanData>> dataMap;
1243  std::string filePath;
1244  {
1245  std::unique_lock lock(laserScannerFileLoggingData->loggingMutex);
1246  dataMap.swap(laserScannerFileLoggingData->scanDataHistory);
1247  filePath = laserScannerFileLoggingData->filePath;
1248  }
1249  ARMARX_INFO << "Writing " << dataMap.size() << " laser scans";
1250  laserScannerFileLoggingData.reset();
1251  nlohmann::json json;
1252  std::ofstream file(filePath, std::ofstream::out | std::ofstream::trunc);
1253  ARMARX_CHECK_EXPRESSION(file.is_open()) << filePath;
1254  // if(!dataMap.empty() && !dataMap.begin()->second.empty())
1255  // {
1256  // auto & LaserScanData = dataMap.begin()->second.empty();
1257  // }
1258  int skipLineAmount = 10;
1259  int i = 0;
1260  for (auto& pair : dataMap)
1261  {
1262  if (i % skipLineAmount != 0)
1263  {
1264  continue;
1265  }
1266  const std::vector<LaserScanData>& scanData = pair.second;
1267  for (auto& elem : scanData)
1268  {
1269  const LaserScanData& scan = elem;
1270  Eigen::Quaternionf quat(scan.pose.block<3, 3>(0, 0));
1271  json["DeviceInfo"][scan.info.device] =
1272  {
1273  {"frame", scan.info.frame},
1274  {"minAngle", scan.info.minAngle},
1275  {"maxAngle", scan.info.maxAngle},
1276  {"stepSize", scan.info.stepSize},
1277  {
1278  "sensorPosition", {
1279  {"x", scan.pose(0, 3)},
1280  {"y", scan.pose(1, 3)},
1281  {"z", scan.pose(2, 3)}
1282  }
1283  },
1284  {
1285  "sensorOrientation", {{"w", quat.w()},
1286  {"x", quat.x()},
1287  {"y", quat.y()},
1288  {"z", quat.z()}
1289  }
1290  }
1291 
1292  };
1293  Ice::IntSeq dataPts;
1294  for (const auto& pt : scan.points)
1295  {
1296  dataPts.push_back(pt(0));
1297  dataPts.push_back(pt(1));
1298  }
1299  json["Data"][scan.info.device]["GlobalPoints"] += dataPts;
1300  Ice::IntSeq scanPts;
1301  for (const LaserScanStep& pt : scan.scan)
1302  {
1303  scanPts.push_back(pt.distance);
1304  }
1305  json["Data"][scan.info.device]["ScanDistances"] += scanPts;
1306  }
1307  i++;
1308  }
1309  file << std::setw(1) << json << std::endl;
1310 }
1311 
1312 
1313 LineSegment2DSeq LaserScannerSelfLocalisation::getMap(const Ice::Current&)
1314 {
1315  LineSegment2DSeq result;
1316  for (LineSegment2Df const& segment : map)
1317  {
1318  armarx::Vector2f start {segment.start.x(), segment.start.y()};
1319  armarx::Vector2f end {segment.end.x(), segment.end.y()};
1320  result.push_back(LineSegment2D {start, end});
1321  }
1322  return result;
1323 }
1324 
1325 
1326 void LaserScannerSelfLocalisation::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
1327 {
1328  updateProperties();
1329 }
armarx::LaserScanData::scan
LaserScan scan
Definition: LaserScannerSelfLocalisation.h:67
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::LineSegment2Df::end
Eigen::Vector2f end
Definition: LaserScannerSelfLocalisation.h:51
ColorMap::Value
Value
Color maps that associate a color to every float from [0..1].
Definition: color.h:16
armarx::LaserScanData
Definition: LaserScannerSelfLocalisation.h:62
str
std::string str(const T &t)
Definition: UserAssistedSegmenterGuiWidgetController.cpp:42
MathUtils.h
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
JSONObject.h
armarx::LaserScanData::points
std::vector< Eigen::Vector2f > points
Definition: LaserScannerSelfLocalisation.h:68
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
armarx::JSONObject
The JSONObject class is used to represent and (de)serialize JSON objects.
Definition: JSONObject.h:43
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
armarx::LaserScanData::info
LaserScannerInfo info
Definition: LaserScannerSelfLocalisation.h:65
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:267
armarx::ExtractedEdge::pointsEnd
Eigen::Vector2f * pointsEnd
Definition: LaserScannerSelfLocalisation.h:58
armarx::LaserScannerSelfLocalisation::onDisconnectComponent
void onDisconnectComponent() override
Definition: LaserScannerSelfLocalisation.cpp:512
armarx::LaserScannerSelfLocalisationPropertyDefinitions
Definition: LaserScannerSelfLocalisation.h:85
armarx::LineSegment2Df::start
Eigen::Vector2f start
Definition: LaserScannerSelfLocalisation.h:50
armarx::LaserScannerSelfLocalisation::LaserScannerSelfLocalisation
LaserScannerSelfLocalisation()
Definition: LaserScannerSelfLocalisation.cpp:362
armarx::LaserScannerSelfLocalisation::componentPropertiesUpdated
void componentPropertiesUpdated(const std::set< std::string > &changedProperties) override
Implement this function if you would like to react to changes in the properties.
Definition: LaserScannerSelfLocalisation.cpp:1326
armarx::LaserScannerSelfLocalisation::setAbsolutePose
void setAbsolutePose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:543
armarx::ReportedVelocity
Definition: LaserScannerSelfLocalisation.h:73
IceInternal::Handle< FramedPose >
armarx::JSONObject::getJsonValue
const Json::Value & getJsonValue() const
Definition: JSONObject.cpp:93
memoryx::PlatformKalmanFilter
This class is a convenience class for a holonomic platform using a Kalman filter.
Definition: PlatformKalmanFilter.h:36
armarx::PropertyUser::updateProperties
void updateProperties()
Definition: PropertyUser.cpp:154
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
armarx::JSONObject::fromString
void fromString(const ::std::string &jsonString, const ::Ice::Current &=Ice::emptyCurrent) override
Definition: JSONObject.cpp:86
memoryx::SimpleEntity
Simple untyped entity class which allows adding arbitrary attributes.
Definition: SimpleEntity.h:35
GfxTL::Matrix2f
MatrixXX< 2, 2, float > Matrix2f
Definition: MatrixXX.h:599
FramedPose.h
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::LineSegment2Df
Definition: LaserScannerSelfLocalisation.h:48
ARMARX_LSSL_UPDATE_PROPERTY
#define ARMARX_LSSL_UPDATE_PROPERTY(name)
armarx::Pose::toEigen
virtual Eigen::Matrix4f toEigen() const
Definition: Pose.cpp:334
MemoryXCoreObjectFactories.h
filename
std::string filename
Definition: VisualizationRobot.cpp:84
max
T max(T t1, T t2)
Definition: gdiam.h:48
armarx::LaserScannerSelfLocalisation::reportPlatformVelocity
void reportPlatformVelocity(Ice::Float velX, Ice::Float velY, Ice::Float velTheta, const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:592
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::math::MathUtils::angleModPI
static float angleModPI(float value)
Definition: MathUtils.h:140
armarx::LaserScannerSelfLocalisation::getReportTopicName
std::string getReportTopicName(const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:538
armarx::LaserScannerSelfLocalisation::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: LaserScannerSelfLocalisation.cpp:528
Gaussian.h
IceReportSkipper.h
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::LaserScannerSelfLocalisation::reportSensorValues
void reportSensorValues(const std::string &device, const std::string &name, const LaserScan &scan, const TimestampBasePtr &timestamp, const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:553
armarx::TimeUtil::GetTime
static IceUtil::Time GetTime(TimeMode timeMode=TimeMode::VirtualTime)
Get the current time.
Definition: TimeUtil.cpp:42
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:55
armarx::LaserScannerSelfLocalisation::onExitComponent
void onExitComponent() override
Definition: LaserScannerSelfLocalisation.cpp:523
armarx::ManagedIceObject::usingTopic
void usingTopic(const std::string &name, bool orderedPublishing=false)
Registers a proxy for subscription after initialization.
Definition: ManagedIceObject.cpp:248
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::ExtractedEdge::pointsBegin
Eigen::Vector2f * pointsBegin
Definition: LaserScannerSelfLocalisation.h:57
armarx::LaserScannerSelfLocalisation::onInitComponent
void onInitComponent() override
Definition: LaserScannerSelfLocalisation.cpp:367
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
LaserScannerSelfLocalisation.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
TimeUtil.h
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
armarx::Quaternion< float, 0 >
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::min
std::vector< T > min(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:294
IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface >
MemoryXTypesObjectFactories.h
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
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:92
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::LaserScannerSelfLocalisation::reportPlatformOdometryPose
void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:615
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::PeriodicTask
Definition: ArmarXManager.h:70
memoryx::AgentInstance
Definition: AgentInstance.h:44
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::LaserScanData::pose
Eigen::Matrix4f pose
Definition: LaserScannerSelfLocalisation.h:64
ArmarXDataPath.h
armarx::LaserScannerSelfLocalisation::onConnectComponent
void onConnectComponent() override
Definition: LaserScannerSelfLocalisation.cpp:407
armarx::ExtractedEdge::segment
LineSegment2Df segment
Definition: LaserScannerSelfLocalisation.h:56
armarx::ExtractedEdge
Definition: LaserScannerSelfLocalisation.h:54
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:151
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::LaserScannerSelfLocalisation::getMap
LineSegment2DSeq getMap(const Ice::Current &) override
Definition: LaserScannerSelfLocalisation.cpp:1313
dt
constexpr T dt
Definition: UnscentedKalmanFilterTest.cpp:42
Line
Eigen::ParametrizedLine< float, 2 > Line
Definition: LaserScannerSelfLocalisation.cpp:51