Reader.cpp
Go to the documentation of this file.
1 #include "Reader.h"
2 
3 #include <SimoxUtility/algorithm/get_map_keys_values.h>
4 #include <SimoxUtility/algorithm/string/fuzzy_multi_match.hpp>
5 
7 
8 #include <RobotAPI/interface/armem/memory.h>
16 
18 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
19 #include <armarx/navigation/core/aron/Location.aron.generated.h>
22 
24 {
25  std::string
27  {
28  return "mem.nav.graph.";
29  }
30 
33  {
36  }
37 
38  std::map<std::string, core::Location>
40  {
41  std::map<std::string, core::Location> locations;
42 
43  const auto locs = queryLocations();
44  for (const auto& [id, location] : locs)
45  {
46  const std::string locationName = id.providerSegmentName + "/" + id.entityName;
47  ARMARX_DEBUG << "Location: " << locationName;
48 
50  armarx::fromAron(location.framedPose, fp);
51 
52  core::Location& loc = locations[locationName];
53  loc.name = locationName;
54  loc.framedPose = fp;
55  }
56 
57  return locations;
58  }
59 
60  std::vector<armarx::navigation::core::Graph>
62  {
64 
65  std::map<armem::MemoryID, armarx::navigation::core::Graph> graphs;
66 
67  const auto graphSegment = allGraphs();
68  graphSegment.forEachEntity(
69  [&](const armem::wm::Entity& entity)
70  {
71  navigation::core::Graph& graph = graphs[entity.id()];
72  if (const armem::wm::EntityInstance* instance = entity.findLatestInstance())
73  {
74  navigation::core::arondto::Graph aron;
75  aron.fromAron(instance->data());
76  fromAron(aron, graph);
77  }
78  // else: empty layer
79  });
80 
81 
82  const auto locSegment = allLocations();
83 
84  for (auto& [_, graph] : graphs)
85  {
87  }
88 
89  return simox::get_values(graphs);
90  }
91 
94  {
96  // Query all entities from provider.
98 
99  // clang-format off
100  qb
102  .providerSegments().all()
103  .entities().all()
104  .snapshots().latest();
105  // clang-format on
106 
107  const armem::client::QueryResult qResult =
108  memoryReaderLocations.query(qb.buildQueryInput());
109  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
110 
111  if (not qResult.success)
112  {
113  ARMARX_WARNING << "Failure ...";
114  return {};
115  }
116 
117  const auto& locSegment =
118  qResult.memory.getCoreSegment(navigation::location::coreSegmentID.coreSegmentName);
119  return locSegment;
120  }
121 
124  {
125  ARMARX_TRACE;
126 
127  // Query all entities from provider.
129 
130  // clang-format off
131  qb
133  .providerSegments().all()
134  .entities().all()
135  .snapshots().latest();
136  // clang-format on
137 
138  const armem::client::QueryResult qResult = memoryReaderGraphs.query(qb.buildQueryInput());
139  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
140 
141  if (not qResult.success)
142  {
143  ARMARX_WARNING << "Failure ...";
144  return {};
145  }
146 
147  const auto& graphSegment =
148  qResult.memory.getCoreSegment(navigation::graph::coreSegmentID.coreSegmentName);
149  return graphSegment;
150  }
151 
152  std::map<armem::MemoryID, location::arondto::Location>
154  {
155  // Query all entities from provider.
157 
158  // clang-format off
159  qb
161  .providerSegments().all()
162  .entities().all()
163  .snapshots().latest();
164  // clang-format on
165 
166  const armem::client::QueryResult qResult =
167  memoryReaderLocations.query(qb.buildQueryInput());
168  ARMARX_DEBUG << "Lookup result in reader: " << qResult;
169 
170  if (not qResult.success)
171  {
172  return {};
173  }
174 
175  std::map<armem::MemoryID, location::arondto::Location> locations;
176 
177  const auto& locSegment =
178  qResult.memory.getCoreSegment(navigation::location::coreSegmentID.coreSegmentName);
179  locSegment.forEachEntity(
180  [&](const armem::wm::Entity& entity)
181  {
182  if (const armem::wm::EntityInstance* instance = entity.findLatestInstance())
183  {
184  locations[entity.id()].fromAron(instance->data());
185  }
186  });
187 
188  ARMARX_VERBOSE << "Retrieved " << locations.size() << " locations.";
189  return locations;
190  }
191 
193  Reader::resolveLocationId(const std::string& locationId)
194  {
195  const auto locs = locations();
196  return locs.at(locationId);
197  }
198 
200  Reader::resolveLocationName(const std::string& locationName,
201  double minMatchRatio,
202  std::ostream* log)
203  {
204  const std::map<armem::MemoryID, location::arondto::Location> locations = queryLocations();
205  return ResolveLocationName(locations, locationName, minMatchRatio, log);
206  }
207 
210  const std::map<armem::MemoryID, location::arondto::Location>& locations,
211  const std::string& locationName,
212  double minMatchRatio,
213  std::ostream* log)
214  {
215  std::map<armarx::armem::MemoryID, std::vector<std::string>> locationIdsToWords;
216  for (const auto& [id, location] : locations)
217  {
218  if (location.names.has_value())
219  {
220  locationIdsToWords[id] = location.names->recognized;
221  }
222  locationIdsToWords[id].push_back(id.entityName);
223  // TODO: Split entityName by "/" and add components?
224  }
225 
226  ResolveLocationNameResult resolved;
227 
228  if (locationIdsToWords.empty())
229  {
230  std::stringstream msg;
231  msg << "Found no locations in core segment "
232  << armarx::navigation::location::coreSegmentID << " the name '" << locationName
233  << "' could be matched against.";
234 
235  resolved.errorMessage = msg.str();
236  return resolved;
237  }
238 
239  auto match = log ? simox::alg::fuzzy_multi_match(locationName, locationIdsToWords, *log)
240  : simox::alg::fuzzy_multi_match(locationName, locationIdsToWords);
241  ARMARX_CHECK(match.has_value());
242 
243  if (match->ratio < minMatchRatio)
244  {
245  std::stringstream msg;
246  msg << "Match ratio " << match->ratio << " of best match '" << *match->word
247  << "' (location ID " << match->option
248  << ") is below the requested minimum match ratio (" << minMatchRatio << ").";
249 
250  resolved.errorMessage = msg.str();
251  return resolved;
252  }
253 
254  resolved.resolved = ResolvedLocationName{
255  .locationId = *match->option,
256  .locationName = *match->word,
257  .matchRatio = match->ratio,
258  };
259 
260  return resolved;
261  }
262 
263  void
265  {
266  // for the sake of completion ...
267  // armem::client::util::SimpleReaderBase::connect();
268 
269  // Wait for the memory to become available and add it as dependency.
270  ARMARX_INFO << "Reader: Waiting for memory '" << navigation::graph::coreSegmentID.memoryName
271  << "' ...";
272 
273  try
274  {
275  ARMARX_INFO << "graphs ...";
276  memoryReaderGraphs = mns.useReader(
278 
279  ARMARX_INFO << "locations ...";
280  memoryReaderLocations = mns.useReader(
282 
283  ARMARX_INFO << "Reader: Connected to memory '"
285  }
287  {
288  ARMARX_ERROR << e.what();
289  return;
290  }
291  }
292 
293 } // namespace armarx::navigation::memory::client::graph
armarx::armem::client::query::EntitySelector::all
EntitySelector & all() override
Definition: selectors.cpp:96
armarx::navigation::graph::coreSegmentID
const armem::MemoryID coreSegmentID
Definition: constants.h:30
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::armem::client::query::SnapshotSelector::latest
SnapshotSelector & latest()
Definition: selectors.cpp:28
armarx::navigation::core::resolveLocations
void resolveLocations(Graph &graph, const MemoryContainerT &locationContainer)
Definition: Graph.h:140
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::navigation::memory::client::graph::Reader::resolveLocationName
ResolveLocationNameResult resolveLocationName(const std::string &locationName, double minMatchRatio, std::ostream *log=nullptr)
Resolve a location by human-readable name.
Definition: Reader.cpp:200
armarx::navigation::memory::client::graph::ResolvedLocationName
Definition: Reader.h:33
armarx::navigation::memory::client::graph::Reader::allLocations
armem::wm::CoreSegment allLocations()
Definition: Reader.cpp:93
framed.h
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:258
armarx::navigation::memory::client::graph::Reader::ResolveLocationName
static ResolveLocationNameResult ResolveLocationName(const std::map< armem::MemoryID, location::arondto::Location > &locations, const std::string &locationName, double minMatchRatio, std::ostream *log=nullptr)
Resolve a location by human-readable name.
Definition: Reader.cpp:209
armarx::armem::client::query::Builder::buildQueryInput
QueryInput buildQueryInput() const
Definition: Builder.cpp:11
armarx::navigation::core::Location::name
std::string name
Definition: location.h:32
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
mns.h
armarx::navigation::memory::client::graph::Reader::queryLocations
std::map< armem::MemoryID, location::arondto::Location > queryLocations()
Definition: Reader.cpp:153
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
armarx::navigation::memory::client::graph::ResolveLocationNameResult
Definition: Reader.h:40
armarx::armem::client::util::SimpleReaderBase::Properties::memoryName
std::string memoryName
Definition: SimpleReaderBase.h:45
armarx::armem::MemoryID::coreSegmentName
std::string coreSegmentName
Definition: MemoryID.h:51
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
constants.h
memory_definitions.h
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::armem::client::query::Builder::coreSegments
CoreSegmentSelector & coreSegments()
Start specifying core segments.
Definition: Builder.cpp:38
armarx::navigation::memory::client::graph::ResolvedLocationName::locationId
armarx::armem::MemoryID locationId
Definition: Reader.h:35
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::navigation::memory::client::graph::Reader::propertyPrefix
std::string propertyPrefix() const override
Definition: Reader.cpp:26
armarx::armem::wm::CoreSegment
Client-side working memory core segment.
Definition: memory_definitions.h:119
armarx::navigation::core::Graph
Definition: Graph.h:88
armarx::armem::client::query::CoreSegmentSelector::withName
CoreSegmentSelector & withName(const std::string &name) override
Definition: selectors.cpp:177
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::armem::base::EntityBase::findLatestInstance
auto * findLatestInstance(int instanceIndex=0)
Definition: EntityBase.h:355
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::navigation::memory::client::graph::ResolveLocationNameResult::resolved
std::optional< ResolvedLocationName > resolved
Definition: Reader.h:42
armarx::armem::base::detail::MemoryItem::id
MemoryID & id()
Definition: MemoryItem.h:27
armarx::navigation::memory::client::graph
This file is part of ArmarX.
Definition: Reader.cpp:23
memory_definitions.h
armarx::navigation::memory::client::graph::Reader::connect
void connect(armarx::armem::client::MemoryNameSystem &mns) override
Definition: Reader.cpp:264
armarx::armem::MemoryID::memoryName
std::string memoryName
Definition: MemoryID.h:50
ExpressionException.h
armarx::navigation::memory::client::graph::Reader::locations
std::map< std::string, core::Location > locations()
Definition: Reader.cpp:39
armarx::navigation::memory::client::graph::Reader::graphs
std::vector< armarx::navigation::core::Graph > graphs()
Definition: Reader.cpp:61
Graph.h
armarx::armem::client::util::SimpleReaderBase::Properties
Definition: SimpleReaderBase.h:43
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::navigation::core::Location
Definition: location.h:29
armarx::armem::laser_scans::constants::memoryName
const std::string memoryName
Definition: constants.h:28
armarx::navigation::memory::client::graph::Reader::allGraphs
armem::wm::CoreSegment allGraphs()
Definition: Reader.cpp:123
Builder.h
armarx::fromAron
void fromAron(const arondto::PackagePath &dto, PackageFileLocation &bo)
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::navigation::memory::client::graph::Reader::defaultProperties
Properties defaultProperties() const override
Definition: Reader.cpp:32
armarx::armem::error::CouldNotResolveMemoryServer
Indicates that a query to the Memory Name System failed.
Definition: mns.h:26
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
armarx::navigation::location::coreSegmentID
const armem::MemoryID coreSegmentID
Definition: constants.h:30
armarx::armem::client::query::CoreSegmentSelector::providerSegments
ProviderSegmentSelector & providerSegments()
Start specifying provider segments.
Definition: selectors.cpp:160
MemoryNameSystem.h
armarx::armem::wm::Entity
Client-side working memory entity.
Definition: memory_definitions.h:93
armarx::navigation::memory::client::graph::Reader::resolveLocationId
core::Location resolveLocationId(const std::string &locationId)
Definition: Reader.cpp:193
armarx::navigation::memory::client::graph::ResolveLocationNameResult::errorMessage
std::optional< std::string > errorMessage
Definition: Reader.h:43
armarx::navigation::core::Location::framedPose
armarx::FramedPose framedPose
Definition: location.h:33
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
SimpleReaderBase.h
armarx::armem::client::query::ProviderSegmentSelector::all
ProviderSegmentSelector & all() override
Definition: selectors.cpp:133
Reader.h
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33
constants.h