RobotReader.cpp
Go to the documentation of this file.
1 #include "RobotReader.h"
2 
3 #include <chrono>
4 #include <mutex>
5 #include <optional>
6 #include <thread>
7 
13 
21 #include <RobotAPI/libraries/armem_robot_state/aron/Exteroception.aron.generated.h>
22 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
23 #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
27 
28 
29 namespace fs = ::std::filesystem;
30 
32 {
34  properties(r.properties),
35  propertyPrefix(r.propertyPrefix),
36  memoryReader(r.memoryReader),
37  transformReader(r.transformReader)
38  {
39  }
40 
41  void
43  {
44  transformReader.registerPropertyDefinitions(def);
45  }
46 
47  void
49  {
50  transformReader.connect(memoryNameSystem);
51 
52  // Wait for the memory to become available and add it as dependency.
53  ARMARX_INFO << "RobotReader: Waiting for memory '" << constants::memoryName << "' ...";
54  try
55  {
56  memoryReader = memoryNameSystem.useReader(constants::memoryName);
57  ARMARX_INFO << "RobotReader: Connected to memory '" << constants::memoryName << "'";
58  }
60  {
61  ARMARX_ERROR << e.what();
62  return;
63  }
64  }
65 
66  std::optional<Robot>
67  RobotReader::get(const std::string& name, const armem::Time& timestamp) const
68  {
69  const auto description = queryDescription(name, timestamp);
70 
71  if (not description)
72  {
73  ARMARX_ERROR << "Unknown object " << name;
74  return std::nullopt;
75  }
76 
77  return get(*description, timestamp);
78  }
79 
80  Robot
82  const armem::Time& timestamp) const
83  {
84  Robot robot{.description = description,
85  .instance = "", // TODO(fabian.reister):
86  .config = {}, // will be populated by synchronize
87  .timestamp = timestamp};
88 
89  ARMARX_CHECK(synchronize(robot, timestamp));
90 
91  return robot;
92  }
93 
94  void
96  {
97  syncTimeout = duration;
98  }
99 
100  void
102  {
104  sleepAfterFailure = duration;
105  }
106 
107  bool
108  RobotReader::synchronize(Robot& robot, const armem::Time& timestamp) const
109  {
110  const auto tsStartFunctionInvokation = armem::Time::Now();
111 
112  while (true)
113  {
114  auto state = queryState(robot.description.name, timestamp);
115 
116  if (not state) /* c++20 [[unlikely]] */
117  {
118  ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
119 
120  // if the syncTime is non-positive there will be no retry
121  const auto elapsedTime = armem::Time::Now() - tsStartFunctionInvokation;
122  if (elapsedTime > syncTimeout)
123  {
124  ARMARX_VERBOSE << "Could not synchronize object " << robot.description.name;
125  return false;
126  }
127 
128  ARMARX_INFO << "Retrying to query robot state after failure";
130  }
131 
132  robot.config = std::move(*state);
133  return true;
134  }
135  }
136 
137  std::optional<description::RobotDescription>
138  RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp) const
139  {
140 
141  const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
142 
143  // Query all entities from provider.
145 
146  // clang-format off
147  qb
149  .providerSegments().withName(name)
150  .entities().all()
151  .snapshots().beforeOrAtTime(sanitizedTimestamp);
152  // clang-format on
153 
154  ARMARX_DEBUG << "Lookup query in reader";
155 
156  if (not memoryReader)
157  {
158  ARMARX_WARNING << "Memory reader is null. Did you forget to call "
159  "RobotReader::connect() in onConnectComponent()?";
160  return std::nullopt;
161  }
162 
163  try
164  {
165  std::scoped_lock l(memoryReaderMutex);
166  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
167 
168  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
169 
170  if (not qResult.success) /* c++20 [[unlikely]] */
171  {
172  return std::nullopt;
173  }
174 
175  return getRobotDescription(qResult.memory, name);
176  }
177  catch (...)
178  {
179  ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
180  }
181 
182  return std::nullopt;
183  }
184 
185  std::optional<RobotState>
186  RobotReader::queryState(const std::string& robotName,
187  const armem::Time& timestamp) const
188  {
189  std::optional<RobotState> robotState = queryJointState(robotName, timestamp);
190 
191  if (robotState)
192  {
193  const auto globalPose = queryGlobalPose(robotName, timestamp);
194  if (not globalPose)
195  {
196  ARMARX_VERBOSE << "Failed to query global pose for robot " << robotName;
197  return std::nullopt;
198  }
199  robotState->globalPose = *globalPose;
200  }
201 
202  return robotState;
203  }
204 
205  std::optional<RobotState>
206  RobotReader::queryJointState(const std::string& robotName,
207  const armem::Time& timestamp) const
208  {
209  const auto proprioception = queryProprioception(robotName, timestamp);
210 
211  if (not proprioception.has_value())
212  {
213  ARMARX_VERBOSE << "Failed to query proprioception for robot '" << robotName
214  << "'.";
215  return std::nullopt;
216  }
217  const auto jointMap = proprioception->joints.position;
218 
219  return RobotState{.timestamp = timestamp,
220  .globalPose = RobotState::Pose::Identity(),
221  .jointMap = jointMap,
222  .proprioception = proprioception};
223  }
224 
225  std::optional<::armarx::armem::robot_state::localization::Transform>
226  RobotReader::queryOdometryPose(const std::string& robotName,
227  const armem::Time& timestamp) const
228  {
229 
233  .agent = robotName,
234  .timestamp = timestamp}};
235 
236  // try
237  {
238  const auto result = transformReader.lookupTransform(query);
239  if (not result)
240  {
241  return std::nullopt;
242  }
243  return result.transform;
244  }
245  // catch (...)
246  // {
247  // ARMARX_VERBOSE << GetHandledExceptionString();
248  // return std::nullopt;
249  // }
250  }
251 
252  std::optional<armarx::armem::arondto::Proprioception>
253  RobotReader::queryProprioception(const std::string& robotName,
254  const armem::Time& timestamp) const // Why timestamp?!?!
255  {
256  // TODO(fabian.reister): how to deal with multiple providers?
257 
258  // Query all entities from provider.
260 
261  ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
262 
263  // clang-format off
264  qb
266  .providerSegments().withName(robotName) // agent
267  .entities().all() // TODO
268  .snapshots().beforeOrAtTime(timestamp);
269  // clang-format on
270 
271  try
272  {
273  std::scoped_lock l(memoryReaderMutex);
274  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
275 
276  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
277 
278  if (not qResult.success) /* c++20 [[unlikely]] */
279  {
280  ARMARX_VERBOSE << qResult.errorMessage;
281  return std::nullopt;
282  }
283 
284  return getRobotProprioception(qResult.memory, robotName);
285  }
286  catch (...)
287  {
288  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint state. Reason: "
290  return std::nullopt;
291  }
292  }
293 
295  RobotReader::queryJointStates(const std::string& robotName,
296  const armem::Time& begin,
297  const armem::Time& end) const
298  {
300 
301  ARMARX_DEBUG << "Querying robot joint states for robot: `" << robotName
302  << "` on time interval [" << begin << "," << end << "]";
303 
304  // clang-format off
305  qb
307  .providerSegments().withName(robotName) // agent
308  .entities().all() // TODO
309  .snapshots().timeRange(begin, end);
310  // clang-format on
311 
312  try
313  {
314  std::scoped_lock l(memoryReaderMutex);
315  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
316 
317  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
318 
319  if (not qResult.success) /* c++20 [[unlikely]] */
320  {
321  ARMARX_VERBOSE << qResult.errorMessage;
322  return {};
323  }
324 
325  return getRobotJointStates(qResult.memory, robotName);
326  }
327  catch (...)
328  {
329  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query joint states. Reason: "
331  return {};
332  }
333  }
334 
335  std::optional<PlatformState>
336  RobotReader::queryPlatformState(const std::string& robotName,
337  const armem::Time& timestamp) const
338  {
339  // TODO(fabian.reister): how to deal with multiple providers?
340 
341  // Query all entities from provider.
343 
344  ARMARX_DEBUG << "Querying robot description for robot: " << robotName;
345 
346  // clang-format off
347  qb
349  .providerSegments().withName(robotName) // agent
350  .entities().all() // TODO
351  .snapshots().beforeOrAtTime(timestamp);
352  // clang-format on
353 
354  try
355  {
356  std::scoped_lock l(memoryReaderMutex);
357  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
358 
359  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
360 
361  if (not qResult.success) /* c++20 [[unlikely]] */
362  {
363  ARMARX_VERBOSE << qResult.errorMessage;
364  return std::nullopt;
365  }
366 
367  return getRobotPlatformState(qResult.memory, robotName);
368  }
369  catch (...)
370  {
371  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query platform state. Reason: "
373  return std::nullopt;
374  }
375  }
376 
377  std::optional<RobotState::Pose>
378  RobotReader::queryGlobalPose(const std::string& robotName,
379  const armem::Time& timestamp) const
380  {
381  try
382  {
383  const auto result = transformReader.getGlobalPose(
384  robotName, constants::robotRootNodeName, timestamp);
385  if (not result)
386  {
387  return std::nullopt;
388  }
389 
390  return result.transform.transform;
391  }
392  catch (...)
393  {
394  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query global pose. Reason: "
396  return std::nullopt;
397  }
398  }
399 
400  std::optional<RobotState>
401  RobotReader::getRobotState(const armarx::armem::wm::Memory& memory,
402  const std::string& name) const
403  {
404  // clang-format off
405  const armem::wm::ProviderSegment& providerSegment = memory
406  .getCoreSegment(constants::proprioceptionCoreSegment)
407  .getProviderSegment(name);
408 
409  // TODO entitiesToRobotState()
410 
411  if (providerSegment.empty())
412  {
413  ARMARX_VERBOSE << "No entity found";
414  return std::nullopt;
415  }
416 
417  // clang-format on
418 
419  const armem::wm::EntityInstance* instance = nullptr;
420  providerSegment.forEachInstance(
421  [&instance](const wm::EntityInstance& i)
422  {
423  instance = &i;
424  return false; // break
425  });
426 
427  if (instance == nullptr)
428  {
429  ARMARX_VERBOSE << "No entity snapshots found";
430  return std::nullopt;
431  }
432 
433  // Here, we access the RobotUnit streaming data stored in the proprioception segment.
434  return convertRobotState(*instance);
435  }
436 
437  // FIXME remove this, use armem/util/util.h
438  template <typename AronClass>
439  std::optional<AronClass>
441  {
442  static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
443  AronClass>::value);
444 
445  try
446  {
447  return AronClass::FromAron(item.data());
448  }
450  {
451  return std::nullopt;
452  }
453  }
454 
455  std::optional<armarx::armem::arondto::Proprioception>
456  RobotReader::getRobotProprioception(const armarx::armem::wm::Memory& memory,
457  const std::string& name) const
458  {
459  // clang-format off
460  const armem::wm::CoreSegment& coreSegment = memory
461  .getCoreSegment(constants::proprioceptionCoreSegment);
462  // clang-format on
463 
464  std::optional<armarx::armem::arondto::Proprioception> proprioception;
465 
466  coreSegment.forEachEntity(
467  [&proprioception](const wm::Entity& entity)
468  {
469  if (not entity.getLatestSnapshot().hasInstance(0))
470  {
471  return;
472  }
473 
474  const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
475 
476  proprioception = tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
477  });
478 
479  return proprioception;
480  }
481 
483  RobotReader::getRobotJointStates(const armarx::armem::wm::Memory& memory,
484  const std::string& name) const
485  {
486 
487  RobotReader::JointTrajectory jointTrajectory;
488 
489  // clang-format off
490  const armem::wm::CoreSegment& coreSegment = memory
492  // clang-format on
493 
494  coreSegment.forEachEntity(
495  [&jointTrajectory](const wm::Entity& entity)
496  {
497  entity.forEachSnapshot(
498  [&](const auto& snapshot)
499  {
500  if (not snapshot.hasInstance(0))
501  {
502  return;
503  }
504 
505  const auto& entityInstance = snapshot.getInstance(0);
506 
507  const auto proprioception =
508  tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
509  ARMARX_CHECK(proprioception.has_value());
510 
511  const armarx::armem::prop::arondto::Joints& joints = proprioception->joints;
512 
513  jointTrajectory.emplace(entityInstance.id().timestamp, joints.position);
514  });
515  });
516 
517  ARMARX_INFO << "Joint trajectory with " << jointTrajectory.size() << " elements";
518 
519  return jointTrajectory;
520  }
521 
522  // force torque for left and right
523  std::optional<std::map<RobotReader::Hand, proprioception::ForceTorque>>
524  RobotReader::queryForceTorque(const std::string& robotName,
525  const armem::Time& timestamp) const
526  {
527 
528  // Query all entities from provider.
530 
531  ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
532 
533  // clang-format off
534  qb
536  .providerSegments().withName(robotName) // agent
537  .entities().all() // TODO
538  .snapshots().beforeOrAtTime(timestamp);
539  // clang-format on
540 
541  try
542  {
543  std::scoped_lock l(memoryReaderMutex);
544  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
545 
546  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
547 
548  if (not qResult.success) /* c++20 [[unlikely]] */
549  {
550  ARMARX_VERBOSE << qResult.errorMessage;
551  return std::nullopt;
552  }
553 
554  return getForceTorque(qResult.memory, robotName);
555  }
556  catch (...)
557  {
558  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torque. Reason: "
560  return std::nullopt;
561  }
562  }
563 
564  // force torque for left and right
565  std::optional<std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>>
566  RobotReader::queryForceTorques(const std::string& robotName,
567  const armem::Time& start,
568  const armem::Time& end) const
569  {
570 
571  // Query all entities from provider.
573 
574  ARMARX_DEBUG << "Querying force torques description for robot: " << robotName;
575 
576  // clang-format off
577  qb
579  .providerSegments().withName(robotName) // agent
580  .entities().all() // TODO
581  .snapshots().timeRange(start, end);
582  // clang-format on
583 
584  try
585  {
586  std::scoped_lock l(memoryReaderMutex);
587  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
588 
589  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
590 
591  if (not qResult.success) /* c++20 [[unlikely]] */
592  {
593  ARMARX_VERBOSE << qResult.errorMessage;
594  return std::nullopt;
595  }
596 
597  return getForceTorques(qResult.memory, robotName);
598  }
599  catch (...)
600  {
601  ARMARX_VERBOSE << deactivateSpam(1) << "Failed to query force torques. Reason: "
603  return std::nullopt;
604  }
605  }
606 
607  std::optional<std::map<RobotReader::Hand, exteroception::ToF>>
608  RobotReader::queryToF(const std::string& robotName,
609  const armem::Time& timestamp) const
610  {
611  // Query all entities from provider.
613 
614  ARMARX_DEBUG << "Querying ToF data for robot: " << robotName;
615 
616  // clang-format off
617  qb
619  .providerSegments().withName(robotName) // agent
620  .entities().all() // TODO
621  .snapshots().beforeOrAtTime(timestamp);
622  // clang-format on
623 
624  try
625  {
626  std::scoped_lock l(memoryReaderMutex);
627  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
628 
629  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
630 
631  if (not qResult.success) /* c++20 [[unlikely]] */
632  {
633  ARMARX_VERBOSE << qResult.errorMessage;
634  return std::nullopt;
635  }
636 
637  return getToF(qResult.memory, robotName);
638  }
639  catch (...)
640  {
642  << "Failed to query ToF. Reason: " << GetHandledExceptionString();
643  return std::nullopt;
644  }
645  }
646 
647  std::optional<PlatformState>
648  RobotReader::getRobotPlatformState(const armarx::armem::wm::Memory& memory,
649  const std::string& name) const
650  {
651  std::optional<PlatformState> platformState;
652 
653  // clang-format off
654  const armem::wm::CoreSegment& coreSegment = memory
655  .getCoreSegment(constants::proprioceptionCoreSegment);
656  // clang-format on
657 
658  coreSegment.forEachEntity(
659  [&platformState](const wm::Entity& entity)
660  {
661  if (not entity.getLatestSnapshot().hasInstance(0))
662  {
663  return;
664  }
665 
666  const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
667 
668  const auto proprioception =
669  tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
670  ARMARX_CHECK(proprioception.has_value());
671 
672  platformState = PlatformState(); // initialize optional
673  fromAron(proprioception->platform, platformState.value());
674  });
675 
676  return platformState;
677  }
678 
679  inline RobotReader::Hand
680  fromHandName(const std::string& name)
681  {
682  if (name == "Left")
683  {
685  }
686 
687  if (name == "Right")
688  {
690  }
691 
692  throw LocalException("Unknown hand name `" + name + "`!");
693  }
694 
695  std::map<RobotReader::Hand, proprioception::ForceTorque>
696  RobotReader::getForceTorque(const armarx::armem::wm::Memory& memory,
697  const std::string& name) const
698  {
699  std::map<RobotReader::Hand, proprioception::ForceTorque> forceTorques;
700 
701  // clang-format off
702  const armem::wm::CoreSegment& coreSegment = memory
703  .getCoreSegment(constants::proprioceptionCoreSegment);
704  // clang-format on
705 
706  coreSegment.forEachEntity(
707  [&forceTorques](const wm::Entity& entity)
708  {
709  if (not entity.getLatestSnapshot().hasInstance(0))
710  {
711  return;
712  }
713 
714  const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
715 
716  const auto proprioception =
717  tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
718  ARMARX_CHECK(proprioception.has_value());
719 
720  for (const auto& [handName, dtoFt] : proprioception->forceTorque)
721  {
722  proprioception::ForceTorque forceTorque;
723  fromAron(dtoFt, forceTorque);
724 
725  const auto hand = fromHandName(handName);
726  forceTorques.emplace(hand, forceTorque);
727  }
728  });
729 
730  return forceTorques;
731  }
732 
733  std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>>
734  RobotReader::getForceTorques(const armarx::armem::wm::Memory& memory,
735  const std::string& name) const
736  {
737  std::map<RobotReader::Hand, std::map<armem::Time, proprioception::ForceTorque>> forceTorques;
738 
739  // clang-format off
740  const armem::wm::CoreSegment& coreSegment = memory
742  // clang-format on
743 
744  coreSegment.forEachInstance(
745  [&forceTorques](const wm::EntityInstance& entityInstance)
746  {
747  const auto proprioception =
748  tryCast<::armarx::armem::arondto::Proprioception>(entityInstance);
749  ARMARX_CHECK(proprioception.has_value());
750 
751 
752  for (const auto& [handName, dtoFt] : proprioception->forceTorque)
753  {
754  proprioception::ForceTorque forceTorque;
755  fromAron(dtoFt, forceTorque);
756 
757  const auto hand = fromHandName(handName);
758  forceTorques[hand].emplace(entityInstance.id().timestamp, forceTorque);
759  }
760  });
761 
762  return forceTorques;
763  }
764 
765  std::map<RobotReader::Hand, exteroception::ToF>
766  RobotReader::getToF(const armarx::armem::wm::Memory& memory, const std::string& name) const
767  {
768  std::map<RobotReader::Hand, exteroception::ToF> tofs;
769 
770  // clang-format off
771  const armem::wm::CoreSegment& coreSegment = memory
773  // clang-format on
774 
775  coreSegment.forEachEntity(
776  [&tofs](const wm::Entity& entity)
777  {
778  ARMARX_DEBUG << "Processing ToF element";
779 
780  if (not entity.getLatestSnapshot().hasInstance(0))
781  {
782  return;
783  }
784  const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
785 
786  const auto exteroception =
787  tryCast<::armarx::armem::arondto::Exteroception>(entityInstance);
788  ARMARX_CHECK(exteroception.has_value());
789 
790 
791  for (const auto& [handName, dtoFt] : exteroception->tof)
792  {
793  ARMARX_DEBUG << "Processing ToF element for hand `" << handName << "`";
794 
795  exteroception::ToF tof;
796  fromAron(dtoFt, tof);
797 
798  const auto hand = fromHandName(handName);
799  tofs.emplace(hand, tof);
800  }
801  });
802 
803  return tofs;
804  }
805 
806  std::optional<description::RobotDescription>
807  RobotReader::getRobotDescription(const armarx::armem::wm::Memory& memory,
808  const std::string& name) const
809  {
810  // clang-format off
811  const armem::wm::ProviderSegment& providerSegment = memory
813  .getProviderSegment(name);
814  // clang-format on
815 
816  const armem::wm::EntityInstance* instance = nullptr;
817  providerSegment.forEachInstance([&instance](const wm::EntityInstance& i)
818  { instance = &i; });
819  if (instance == nullptr)
820  {
821  ARMARX_VERBOSE << "No entity snapshots found in provider segment `" << name << "`";
822  return std::nullopt;
823  }
824 
825  return convertRobotDescription(*instance);
826  }
827 
828  std::vector<description::RobotDescription>
829  RobotReader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
830  {
831  const armem::wm::CoreSegment& coreSegment =
833 
834  std::vector<description::RobotDescription> descriptions;
835 
836  coreSegment.forEachInstance(
837  [&descriptions](const wm::EntityInstance& instance)
838  {
839  if (const std::optional<description::RobotDescription> desc =
840  convertRobotDescription(instance))
841  {
842  descriptions.push_back(desc.value());
843  }
844  });
845 
846  return descriptions;
847  }
848 
849  std::vector<description::RobotDescription>
851  {
852  const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
853 
854  // Query all entities from provider.
856 
857  // clang-format off
858  qb
860  .providerSegments().all()
861  .entities().all()
862  .snapshots().beforeOrAtTime(sanitizedTimestamp);
863  // clang-format on
864 
865  ARMARX_DEBUG << "Lookup query in reader";
866 
867  if (not memoryReader)
868  {
869  ARMARX_WARNING << "Memory reader is null. Did you forget to call "
870  "RobotReader::connect() in onConnectComponent()?";
871  return {};
872  }
873 
874  try
875  {
876  std::scoped_lock l(memoryReaderMutex);
877  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
878 
879  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
880 
881  if (not qResult.success) /* c++20 [[unlikely]] */
882  {
883  return {};
884  }
885 
886  return getRobotDescriptions(qResult.memory);
887  }
888  catch (...)
889  {
890  ARMARX_VERBOSE << "Query description failure" << GetHandledExceptionString();
891  }
892 
893  return {};
894  }
895 } // namespace armarx::armem::robot_state::client::common
armarx::armem::robot_state::RobotReader::queryGlobalPose
std::optional< RobotState::Pose > queryGlobalPose(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:378
armarx::armem::robot_state::RobotReader::queryJointState
std::optional< RobotState > queryJointState(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:206
armarx::armem::client::query::EntitySelector::all
EntitySelector & all() override
Definition: selectors.cpp:96
armarx::aron::error::AronException
A base class for aron exceptions.
Definition: Exception.h:42
armarx::armem::base::detail::MemoryContainerBase::empty
bool empty() const
Definition: MemoryContainerBase.h:44
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::core::time::DateTime::isValid
bool isValid() const
Definition: DateTime.cpp:135
ARMARX_CHECK_NONNEGATIVE
#define ARMARX_CHECK_NONNEGATIVE(number)
Check whether number is nonnegative (>= 0). If it is not, throw an ExpressionException with the expre...
Definition: ExpressionException.h:152
armarx::armem::robot_state::client::localization::TransformReader::getGlobalPose
TransformResult getGlobalPose(const std::string &agentName, const std::string &robotRootFrame, const armem::Time &timestamp) const override
Definition: TransformReader.cpp:102
armarx::armem::server::wm::EntityInstance
armem::wm::EntityInstance EntityInstance
Definition: forward_declarations.h:64
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::armem::robot_state::fromAron
void fromAron(const arondto::ObjectInstance &dto, RobotState &bo)
Definition: aron_conversions.cpp:27
armarx::armem::client::query::SnapshotSelector::timeRange
SnapshotSelector & timeRange(Time min, Time max)
Definition: selectors.cpp:42
armarx::armem::robot_state::RobotReader::queryPlatformState
std::optional< PlatformState > queryPlatformState(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:336
armarx::human::pose::model::k4a_bt_body_32::Joints
Joints
Joints with index as defined in the body model.
Definition: k4a_bt_body_32.h:39
armarx::armem::robot_state::description::RobotDescription
Definition: types.h:44
LocalException.h
armarx::armem::wm::ProviderSegment
Client-side working memory provider segment.
Definition: memory_definitions.h:105
armarx::armem::robot_state::RobotReader::setSyncTimeout
void setSyncTimeout(const armem::Duration &duration)
Definition: RobotReader.cpp:95
armarx::armem::robot_state::Robot
Definition: types.h:126
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
armarx::armem::robot_state::client::localization::TransformReader::lookupTransform
TransformResult lookupTransform(const TransformQuery &query) const override
Definition: TransformReader.cpp:137
armarx::armem::client::query::Builder::buildQueryInput
QueryInput buildQueryInput() const
Definition: Builder.cpp:11
armarx::armem::base::detail::GetLatestSnapshotMixin::getLatestSnapshot
auto & getLatestSnapshot(int snapshotIndex=0)
Retrieve the latest entity snapshot.
Definition: lookup_mixins.h:199
armarx::core::time::Clock::WaitFor
static void WaitFor(const Duration &duration)
Wait for a certain duration on the virtual clock.
Definition: Clock.cpp:104
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
armarx::armem::base::MemoryBase::getCoreSegment
CoreSegmentT & getCoreSegment(const std::string &name)
Definition: MemoryBase.h:132
armarx::armem::robot_state::RobotState
Definition: types.h:113
armarx::armem::robot_state::RobotReader::registerPropertyDefinitions
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr &def)
Definition: RobotReader.cpp:42
armarx::armem::robot_state::RobotReader::Hand
Hand
Definition: RobotReader.h:91
armarx::armem::robot_state::RobotReader::Hand::Right
@ Right
armarx::armem::robot_state::constants::memoryName
const std::string memoryName
Definition: constants.h:26
constants.h
armarx::armem::robot_state::constants::proprioceptionCoreSegment
const std::string proprioceptionCoreSegment
Definition: constants.h:30
armarx::armem::robot_state
Definition: RobotStatePredictionClient.cpp:42
types.h
armarx::armem::robot_state::RobotReader::connect
virtual void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: RobotReader.cpp:48
armarx::armem::robot_state::tryCast
std::optional< AronClass > tryCast(const wm::EntityInstance &item)
Definition: RobotReader.cpp:440
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
armarx::armem::robot_state::constants::robotRootNodeName
const std::string robotRootNodeName
Definition: constants.h:36
armarx::armem::robot_state::RobotState::timestamp
DateTime timestamp
Definition: types.h:118
armarx::armem::robot_state::localization::TransformQuery
Definition: types.h:74
armarx::armem::server::wm::Entity
Definition: memory_definitions.h:30
armarx::memory
Brief description of class memory.
Definition: memory.h:39
armarx::armem::base::detail::ForEachEntityMixin::forEachEntity
bool forEachEntity(FunctionT &&func)
Definition: iteration_mixins.h:258
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::armem::robot_state::RobotReader::queryForceTorques
std::optional< std::map< RobotReader::Hand, std::map< armem::Time, proprioception::ForceTorque > > > queryForceTorques(const std::string &robotName, const armem::Time &start, const armem::Time &end) const
Definition: RobotReader.cpp:566
Clock.h
armarx::armem::robot_state::localization::TransformHeader::parentFrame
std::string parentFrame
Definition: types.h:146
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:147
armarx::armem::robot_state::client::localization::TransformReader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem) override
Definition: TransformReader.cpp:85
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
armarx::armem::robot_state::RobotReader::Hand::Left
@ Left
FramedPose.h
armarx::armem::robot_state::Robot::config
RobotState config
Definition: types.h:131
armarx::armem::client::query::Builder::coreSegments
CoreSegmentSelector & coreSegments()
Start specifying core segments.
Definition: Builder.cpp:38
armarx::armem::robot_state::RobotReader::queryForceTorque
std::optional< std::map< Hand, proprioception::ForceTorque > > queryForceTorque(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:524
armarx::armem::robot_state::RobotReader::setSleepAfterSyncFailure
void setSleepAfterSyncFailure(const armem::Duration &duration)
Definition: RobotReader.cpp:101
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::armem::wm::CoreSegment
Client-side working memory core segment.
Definition: memory_definitions.h:119
armarx::armem::client::query::CoreSegmentSelector::withName
CoreSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:177
error.h
armarx::armem::wm::Memory
Client-side working memory.
Definition: memory_definitions.h:133
armarx::armem::client::MemoryNameSystem::useReader
Reader useReader(const MemoryID &memoryID)
Use a memory server and get a reader for it.
Definition: MemoryNameSystem.cpp:184
armarx::aron::cpp::AronGeneratedClass
Definition: AronGeneratedClass.h:46
aron_conversions.h
armarx::armem::robot_state::localization::Transform::transform
Eigen::Isometry3f transform
Definition: types.h:158
armarx::OdometryFrame
const std::string OdometryFrame
Definition: FramedPose.h:63
RobotReader.h
armarx::armem::robot_state::RobotReader::queryDescription
std::optional< description::RobotDescription > queryDescription(const std::string &name, const armem::Time &timestamp) const
Definition: RobotReader.cpp:138
armarx::armem::robot_state::convertRobotDescription
std::optional< description::RobotDescription > convertRobotDescription(const armem::wm::EntityInstance &instance)
Definition: robot_conversions.cpp:20
armarx::armem::robot_state::constants::descriptionCoreSegment
const std::string descriptionCoreSegment
Definition: constants.h:28
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::robot_state::RobotReader
The RobotReader class.
Definition: RobotReader.h:43
armarx::armem::robot_state::localization::TransformQuery::header
::armarx::armem::robot_state::localization::TransformHeader header
Definition: types.h:76
types.h
armarx::armem::robot_state::PlatformState
Definition: types.h:71
armarx::armem::robot_state::convertRobotState
std::optional< RobotState > convertRobotState(const armem::wm::EntityInstance &instance)
Definition: robot_conversions.cpp:46
armarx::armem::robot_state::RobotReader::synchronize
bool synchronize(Robot &obj, const armem::Time &timestamp) const override
Definition: RobotReader.cpp:108
memory_definitions.h
for
for(;yybottom<=yytop;yybottom++)
Definition: Grammar.cpp:790
armarx::armem::robot_state::client::localization::TransformReader::registerPropertyDefinitions
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr &def) override
Definition: TransformReader.cpp:71
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::robot_state::RobotReader::queryJointStates
JointTrajectory queryJointStates(const std::string &robotName, const armem::Time &begin, const armem::Time &end) const
Definition: RobotReader.cpp:295
armarx::armem::robot_state::description::RobotDescription::name
std::string name
Definition: types.h:48
armarx::armem::client::query::ProviderSegmentSelector::withName
ProviderSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:140
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::armem::client::query::SnapshotSelector::beforeOrAtTime
SnapshotSelector & beforeOrAtTime(Time timestamp)
Definition: selectors.cpp:68
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::robot_state::RobotReader::queryProprioception
std::optional< armarx::armem::arondto::Proprioception > queryProprioception(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:253
armarx::core::time::Duration
Represents a duration.
Definition: Duration.h:17
armarx::armem::base::detail::ForEachEntityInstanceMixin::forEachInstance
bool forEachInstance(InstanceFunctionT &&func)
Definition: iteration_mixins.h:146
armarx::armem::robot_state::RobotReader::queryDescriptions
std::vector< description::RobotDescription > queryDescriptions(const armem::Time &timestamp) const
Definition: RobotReader.cpp:850
armarx::armem::robot_state::RobotReader::get
std::optional< Robot > get(const std::string &name, const armem::Time &timestamp) const override
Definition: RobotReader.cpp:67
Time.h
Builder.h
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:97
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
armarx::armem::robot_state::RobotReader::queryToF
std::optional< std::map< Hand, exteroception::ToF > > queryToF(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:608
armarx::armem::client::query::CoreSegmentSelector::providerSegments
ProviderSegmentSelector & providerSegments()
Start specifying provider segments.
Definition: selectors.cpp:160
armarx::core::time::Duration::toMicroSeconds
std::int64_t toMicroSeconds() const
Returns the amount of microseconds.
Definition: Duration.cpp:41
armarx::armem::robot_state::RobotReader::JointTrajectory
std::map< armem::Time, RobotState::JointMap > JointTrajectory
Definition: RobotReader.h:76
Logging.h
robot_conversions.h
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::armem::base::EntityInstanceBase::data
const DataT & data() const
Definition: EntityInstanceBase.h:129
armarx::armem::robot_state::RobotReader::queryState
std::optional< RobotState > queryState(const std::string &robotName, const armem::Time &timestamp) const
Definition: RobotReader.cpp:186
armarx::armem::client::query::ProviderSegmentSelector::all
ProviderSegmentSelector & all() override
Definition: selectors.cpp:133
armarx::armem::robot_state::constants::exteroceptionCoreSegment
const std::string exteroceptionCoreSegment
Definition: constants.h:31
armarx::armem::robot_state::RobotReader::RobotReader
RobotReader()=default
armarx::armem::robot_state::RobotReader::sleepAfterFailure
armem::Duration sleepAfterFailure
Definition: RobotReader.h:125
armarx::armem::robot_state::RobotReader::queryOdometryPose
std::optional<::armarx::armem::robot_state::localization::Transform > queryOdometryPose(const std::string &robotName, const armem::Time &timestamp) const
retrieve the robot's pose in the odometry frame.
Definition: RobotReader.cpp:226
armarx::armem::robot_state::localization::TransformResult::transform
::armarx::armem::robot_state::localization::Transform transform
Definition: types.h:35
armarx::armem::robot_state::RobotReader::syncTimeout
armem::Duration syncTimeout
Definition: RobotReader.h:124
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33
armarx::armem::robot_state::Robot::description
description::RobotDescription description
Definition: types.h:128
armarx::armem::robot_state::fromHandName
RobotReader::Hand fromHandName(const std::string &name)
Definition: RobotReader.cpp:680
PackagePath.h