Reader.cpp
Go to the documentation of this file.
1 #include "Reader.h"
3 
4 #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
7 
9 {
12  {
13  const auto qb = buildLocalPlannerResultQuery(query);
14  const auto& providerName = query.clientID;
15 
16  ARMARX_DEBUG << "[MappingDataReader] query ... ";
17 
18  const armem::client::QueryResult qResult = memoryReader().query(qb.buildQueryInput());
19  ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
20 
21  if (not qResult.success)
22  {
23  ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
24  return {.trajectory = {},
25  .status = LocalPlannerResult::Status::Error,
26  .errorMessage = qResult.errorMessage};
27  }
28 
29  const auto coreSegment = qResult.memory.getCoreSegment(properties().coreSegmentName);
30 
31  if (not coreSegment.hasProviderSegment(providerName))
32  {
33  ARMARX_DEBUG << "Provider segment `" << providerName << "` does not exist (yet).";
34  return {.trajectory = {}, .status = LocalPlannerResult::Status::NoData};
35  }
36 
37  const armem::wm::ProviderSegment& providerSegment =
38  coreSegment.getProviderSegment(providerName);
39 
40  if (providerSegment.empty())
41  {
42  ARMARX_DEBUG << "No entities.";
43  return {.trajectory = {},
44  .status = LocalPlannerResult::Status::NoData,
45  .errorMessage = "No entities"};
46  }
47 
48  try
49  {
50  return LocalPlannerResult{.trajectory = asLocalPlannerResult(providerSegment),
51  .status = LocalPlannerResult::Status::Success};
52  }
53  catch (...)
54  {
55  return LocalPlannerResult{.trajectory = {},
56  .status = LocalPlannerResult::Status::Error,
57  .errorMessage = GetHandledExceptionString()};
58  }
59  }
60 
61 
62  std::string
64  {
65  return "mem.nav.stack_result.";
66  }
67 
70  {
73  }
74 
76  Reader::asLocalPlannerResult(const armem::wm::ProviderSegment& providerSegment)
77  {
79 
80  ARMARX_CHECK(not providerSegment.empty()) << "No entities";
81  ARMARX_CHECK(providerSegment.size() == 1) << "There should be only one entity!";
82 
83  const armem::wm::EntityInstance* entityInstance = nullptr;
84 
85  providerSegment.forEachEntity(
86  [&entityInstance](const armem::wm::Entity& entity)
87  {
88  const auto& entitySnapshot = entity.getLatestSnapshot();
89  ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
90 
91  entityInstance = &entitySnapshot.getInstance(0);
92  });
93 
94  ARMARX_CHECK_NOT_NULL(entityInstance);
95 
96  const auto dto =
97  navigation::core::arondto::LocalTrajectory::FromAron(entityInstance->data());
98 
100  fromAron(dto, localTrajectory);
101 
102  return localTrajectory;
103  }
104 
105  armarx::armem::client::query::Builder Reader::buildLocalPlannerResultQuery(const Query& query) const
106  {
108 
109  // clang-format off
110  qb
111  .coreSegments().withName(properties().coreSegmentName)
112  .providerSegments().withName(query.clientID)
113  .entities().withName("trajectory")
114  .snapshots().beforeOrAtTime(query.timestamp);
115  // clang-format on
116 
117  return qb;
118  }
119 
120 } // namespace armarx::navigation::memory::client::stack_result
armarx::armem::base::detail::MemoryContainerBase::empty
bool empty() const
Definition: MemoryContainerBase.h:44
armarx::navigation::memory::client::stack_result::Reader::Query::clientID
std::string clientID
Definition: Reader.h:42
armarx::armem::client::util::SimpleReaderBase::properties
const Properties & properties() const
Definition: SimpleReaderBase.cpp:55
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::navigation::memory::client::stack_result::Reader::LocalPlannerResult::trajectory
armarx::navigation::core::LocalTrajectory trajectory
Definition: Reader.h:48
armarx::armem::wm::ProviderSegment
Client-side working memory provider segment.
Definition: memory_definitions.h:105
armarx::navigation::memory::fromAron
void fromAron(const arondto::Circle &dto, Circle &bo)
Definition: aron_conversions.cpp:60
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
armarx::armem::base::detail::GetLatestSnapshotMixin::getLatestSnapshot
auto & getLatestSnapshot(int snapshotIndex=0)
Retrieve the latest entity snapshot.
Definition: lookup_mixins.h:199
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
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
Reader.h
armarx::navigation::memory::constants::NavigationMemoryName
const std::string NavigationMemoryName
Definition: constants.h:27
armarx::navigation::memory::client::stack_result
This file is part of ArmarX.
Definition: Reader.cpp:8
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
armarx::armem::client::util::SimpleReaderBase::Properties::memoryName
std::string memoryName
Definition: SimpleReaderBase.h:45
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
armarx::armem::base::detail::MemoryContainerBase::size
std::size_t size() const
Definition: MemoryContainerBase.h:48
armarx::navigation::memory::client::stack_result::Reader::Query
Definition: Reader.h:39
armarx::GetHandledExceptionString
std::string GetHandledExceptionString()
Definition: Exception.cpp:147
armarx::armem::client::query::Builder::coreSegments
CoreSegmentSelector & coreSegments()
Start specifying core segments.
Definition: Builder.cpp:38
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::armem::client::query::CoreSegmentSelector::withName
CoreSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:177
armarx::navigation::core::LocalTrajectory
Definition: Trajectory.h:167
armarx::armem::client::util::SimpleReaderBase::memoryReader
const armem::client::Reader & memoryReader() const
Definition: SimpleReaderBase.cpp:49
armarx::navigation::memory::client::stack_result::Reader::defaultProperties
Properties defaultProperties() const override
Definition: Reader.cpp:69
armarx::armem::client::util::SimpleReaderBase::Properties
Definition: SimpleReaderBase.h:43
armarx::armem::client::query::EntitySelector::withName
EntitySelector & withName(const std::string &name) override
Definition: selectors.cpp:103
armarx::armem::client::query::ProviderSegmentSelector::withName
ProviderSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:140
aron_conversions.h
armarx::armem::base::ProviderSegmentBase::forEachEntity
bool forEachEntity(EntityFunctionT &&func)
Definition: ProviderSegmentBase.h:189
armarx::armem::client::query::SnapshotSelector::beforeOrAtTime
SnapshotSelector & beforeOrAtTime(Time timestamp)
Definition: selectors.cpp:68
Builder.h
constants.h
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
armarx::armem::client::query::CoreSegmentSelector::providerSegments
ProviderSegmentSelector & providerSegments()
Start specifying provider segments.
Definition: selectors.cpp:160
armarx::navigation::memory::client::stack_result::Reader::LocalPlannerResult
Definition: Reader.h:46
armarx::armem::wm::Entity
Client-side working memory entity.
Definition: memory_definitions.h:93
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::navigation::memory::client::stack_result::Reader::queryLocalPlannerResult
LocalPlannerResult queryLocalPlannerResult(const Query &query)
Definition: Reader.cpp:11
armarx::armem::base::EntityInstanceBase::data
const DataT & data() const
Definition: EntityInstanceBase.h:129
armarx::navigation::memory::client::stack_result::Reader::propertyPrefix
std::string propertyPrefix() const override
Definition: Reader.cpp:63
armarx::navigation::memory::constants::LocalPlannerResultCoreSegment
const std::string LocalPlannerResultCoreSegment
Definition: constants.h:39
armarx::navigation::human::HumanGroups
std::vector< HumanGroup > HumanGroups
Definition: types.h:58
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33