Reader.cpp
Go to the documentation of this file.
1 #include "Reader.h"
2 
8 
9 #include "../frame_conversions.h"
10 
12 {
13  void
15  {
16  const std::string prefix = propertyPrefix;
17 
18  def->optional(p.memoryName, prefix + "MemoryName");
19  objReader.registerPropertyDefinitions(def);
20  robotReader.registerPropertyDefinitions(def);
21  }
22 
23  void
25  {
26  // Wait for the memory to become available and add it as dependency.
27  ARMARX_IMPORTANT << "Waiting for memory '" << p.memoryName << "' ...";
28  try
29  {
30  // simply wait until memory is ready. Do nothing with reader but get prx
31  locationReader = memoryNameSystem.useReader(p.memoryName);
32 
33  ARMARX_IMPORTANT << "Connected to Memory '" << p.memoryName << "'";
34  }
36  {
37  ARMARX_ERROR << e.what();
38  return;
39  }
40 
41  objReader.connect(memoryNameSystem);
42  robotReader.connect(memoryNameSystem);
43  }
44 
45  std::map<std::string, armarx::navigation::location::arondto::Location>
47  {
48  auto mId = armarx::armem::MemoryID{p.memoryName, "Location"};
49  auto res = locationReader.getLatestSnapshotsIn(mId);
50 
51  if (res.success)
52  {
53  std::map<std::string, armarx::navigation::location::arondto::Location> ret;
54  res.memory.forEachEntity(
55  [&ret](const armarx::armem::wm::Entity& e)
56  {
57  auto i = e.findLatestInstance();
58  if (i)
59  {
60  auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
61  ret[i->id().providerSegmentName + "/" + i->id().entityName] = loc;
62  }
63  });
64  return ret;
65  }
66 
67  throw error::ArMemError(res.errorMessage);
68  }
69 
70  std::map<std::string, armarx::navigation::location::arondto::Location>
72  {
73  auto locations = this->getAllLocations();
74  auto objects = objReader.queryLatestObjectInstances();
75  auto robotDecs = robotReader.queryDescriptions(armarx::core::time::DateTime::Now());
76 
77  for (auto& [locName, location] : locations)
78  {
79  (void)locName;
80  if (location.framedPose.header.frame == armarx::GlobalFrame)
81  {
82  location.framedPose.header.agent = ""; //sanity set
83  continue;
84  }
85  // check if we have an object that matches the relative location
86  for (const auto& [objectInstanceId, object] : objects)
87  {
88  if (simox::alg::starts_with(objectInstanceId, location.framedPose.header.agent))
89  {
90  toGlobal(location, object);
91  }
92  }
93 
94  // TODO: check if we have an robot that matches the relative location
95  }
96 
97  return locations;
98  }
99 
100 
101 } // namespace armarx::armem::locations::client
armarx::armem::locations::client::Reader::getAllLocations
std::map< std::string, armarx::navigation::location::arondto::Location > getAllLocations()
Definition: Reader.cpp:46
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
armarx::armem::locations::client::Reader::getAllLocationsInGlobalFrame
std::map< std::string, armarx::navigation::location::arondto::Location > getAllLocationsInGlobalFrame()
Definition: Reader.cpp:71
armarx::armem::client::Reader::getLatestSnapshotsIn
QueryResult getLatestSnapshotsIn(const MemoryID &id, armem::query::DataMode dataMode=armem::query::DataMode::WithData) const
Get the latest snapshots under the given memory ID.
Definition: Reader.cpp:343
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::obj::instance::Reader::queryLatestObjectInstances
std::map< std::string, objpose::ObjectPose > queryLatestObjectInstances(const ObjectID &classId)
Definition: ObjectReader.cpp:128
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::armem::robot_state::RobotReader::registerPropertyDefinitions
virtual void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr &def)
Definition: RobotReader.cpp:42
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::armem::error::ArMemError
Base class for all exceptions thrown by the armem library.
Definition: ArMemError.h:18
armarx::armem::robot_state::RobotReader::connect
virtual void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: RobotReader.cpp:48
armarx::starts_with
bool starts_with(const std::string &haystack, const std::string &needle)
Definition: StringHelpers.cpp:43
armarx::armem::locations::client::Reader::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: Reader.cpp:14
armarx::armem::locations::client
Definition: Reader.cpp:11
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::armem::locations::toGlobal
void toGlobal(armarx::navigation::location::arondto::Location &framedLoc, const Eigen::Matrix4f &framedToGlobal)
Definition: frame_conversions.h:14
error.h
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::obj::instance::Reader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: ObjectReader.cpp:33
armarx::armem::base::EntityBase::findLatestInstance
auto * findLatestInstance(int instanceIndex=0)
Definition: EntityBase.h:355
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::locations::client::Reader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: Reader.cpp:24
memory_definitions.h
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::robot_state::RobotReader::queryDescriptions
std::vector< description::RobotDescription > queryDescriptions(const armem::Time &timestamp) const
Definition: RobotReader.cpp:850
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
Reader.h
util.h
armarx::armem::wm::Entity
Client-side working memory entity.
Definition: memory_definitions.h:93
armarx::armem::locations::client::Reader::Properties::memoryName
std::string memoryName
Definition: Reader.h:23
armarx::armem::obj::instance::Reader::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: ObjectReader.cpp:25