KnownGraspCandidateReader.cpp
Go to the documentation of this file.
2 
3 #include <mutex>
4 #include <optional>
5 
8 
14 #include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
16 #include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
20 
22 {
23  void
25  {
26  ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
27 
28  const std::string prefix = propertyPrefix;
29 
30  def->optional(properties.memoryName, prefix + "MemoryName");
31 
32  def->optional(properties.coreSegmentName,
33  prefix + "CoreSegment",
34  "Name of the memory core segment to use for object instances.");
35  }
36 
37  void
39  {
40  // Wait for the memory to become available and add it as dependency.
41  ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
42  try
43  {
44  memoryReader = memoryNameSystem.useReader(properties.memoryName);
45  ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName << "'";
46  }
48  {
49  ARMARX_ERROR << e.what();
50  return;
51  }
52  }
53 
54  std::optional<armem::grasping::arondto::KnownGraspInfo>
56  {
57  // clang-format off
59  .getCoreSegment(properties.coreSegmentName);
60  // clang-format on
61 
62  const armem::wm::EntityInstance* instance = nullptr;
63  s.forEachInstance([&instance](const wm::EntityInstance& i) { instance = &i; });
64  if (instance == nullptr)
65  {
66  ARMARX_VERBOSE << "No entity snapshots found";
67  return std::nullopt;
68  }
69  return armem::grasping::arondto::KnownGraspInfo::FromAron(instance->data());
70  }
71 
72  std::optional<armarx::armem::grasping::arondto::KnownGraspInfo>
73  Reader::queryKnownGraspInfoByEntityName(const std::string& entityName,
74  const armem::Time& timestamp)
75  {
76  // Query all entities from all provider.
78 
79  // clang-format off
80  qb
81  .coreSegments().withName(properties.coreSegmentName)
83  .entities().withName(entityName) // first, we search for the input entity, which may be dataset/class/index
84  .snapshots().beforeOrAtTime(timestamp);
85  // clang-format on
86 
87  const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
88 
89  ARMARX_VERBOSE << "Lookup result in reader: " << qResult;
90 
91  if (not qResult.success) /* c++20 [[unlikely]] */
92  {
93  return std::nullopt;
94  }
95 
96  auto ret = queryKnownGraspInfo(qResult.memory, timestamp);
97 
98  if (not ret)
99  {
100  // No grasp info was found. Try if we find a grasp without the object entity index
101  auto split = simox::alg::split(entityName, "/");
102  if (split.size() > 2) // there is more than just dataset/class
103  {
104  ARMARX_INFO << "No grasp found for object entity " << entityName
105  << ". Search for grasp without the index";
106  return queryKnownGraspInfoByEntityName(split[0] + "/" + split[1], timestamp);
107  }
108  }
109 
110  return ret;
111  }
112 
113 } // namespace armarx::armem::grasping::known_grasps
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::armem::grasping::known_grasps::Reader::queryKnownGraspInfoByEntityName
std::optional< armem::grasping::arondto::KnownGraspInfo > queryKnownGraspInfoByEntityName(const std::string &, const armem::Time &)
Definition: KnownGraspCandidateReader.cpp:73
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::armem::wm::EntityInstance
Client-side working entity instance.
Definition: memory_definitions.h:32
armarx::armem::client::query::Builder::buildQueryInput
QueryInput buildQueryInput() const
Definition: Builder.cpp:11
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
armarx::armem::grasping::known_grasps::Reader::queryKnownGraspInfo
std::optional< armem::grasping::arondto::KnownGraspInfo > queryKnownGraspInfo(const armem::wm::Memory &memory, const armem::Time &)
Definition: KnownGraspCandidateReader.cpp:55
armarx::memory
Brief description of class memory.
Definition: memory.h:39
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::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::armem::grasping::known_grasps::Reader::registerPropertyDefinitions
void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr &def)
Definition: KnownGraspCandidateReader.cpp:24
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
aron_conversions.h
memory_definitions.h
aron_conversions.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::client::query::EntitySelector::withName
EntitySelector & withName(const std::string &name) override
Definition: selectors.cpp:103
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 >
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::armem::grasping::known_grasps
Definition: KnownGraspCandidateReader.cpp:21
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::armem::grasping::known_grasps::Reader::connect
void connect(armem::client::MemoryNameSystem &memoryNameSystem)
Definition: KnownGraspCandidateReader.cpp:38
util.h
Logging.h
robot_conversions.h
aron_conversions.h
armarx::armem::base::EntityInstanceBase::data
const DataT & data() const
Definition: EntityInstanceBase.h:129
armarx::armem::client::query::ProviderSegmentSelector::all
ProviderSegmentSelector & all() override
Definition: selectors.cpp:133
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
KnownGraspCandidateReader.h
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36
PackagePath.h