ObjectMemory.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotAPI::ArmarXObjects::ObjectMemory
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2021
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "ObjectMemory.h"
24 #include <VirtualRobot/VirtualRobot.h>
25 
29 
34 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
35 #include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
37 
38 
40 {
41 
43  {
45 
46  const std::string prefix = "mem.";
48 
49  classSegment.defineProperties(defs, prefix + "cls.");
50  instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
51 
52  attachmentSegment.defineProperties(defs, prefix + "attachments.");
53 
54 
55  // Publish
56  defs->topic(debugObserver);
57 
58  // Subscribe
59  defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
60 
61  // Use
62  // defs->component(kinematicUnitObserver); // Optional dependency.
63  defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
64  "Name of the kinematic unit observer.");
65  defs->optional(predictionTimeWindow, "prediction.TimeWindow",
66  "Duration of time window into the past to use for predictions"
67  " when requested via the PredictingMemoryInterface (in seconds).");
68 
69  defs->optional(p.markerMemoryName, prefix + ".marker.Name", "Marker Memory Name");
70  defs->optional(p.maxMarkerHistorySize, prefix + ".marker.maxHistorySize", "Maximum marker memory history size");
71 
72  return defs;
73  }
74 
75 
77  instance::SegmentAdapter(iceAdapter()),
78  classSegment(iceAdapter()),
79  attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
80  {
81  addPlugin(virtualRobotReaderPlugin);
82  }
83 
84 
86  {
87  }
88 
89 
90  std::string ObjectMemory::getDefaultName() const
91  {
92  return "ObjectMemory";
93  }
94 
95 
97  {
98  const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
99  {
100  try
101  {
102  fn();
103  }
104  catch (const LocalException& e)
105  {
106  ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
107  }
108  catch (const std::exception& e)
109  {
110  ARMARX_ERROR << "Failed to init `" << segmentName << "` segment. Reason: \n" << e.what();
111  }
112  catch (...)
113  {
114  ARMARX_ERROR << "Failed to init `" << segmentName << "` segment for unknown reason.";
115  }
116  };
117 
118  // Class segment needs to be initialized before instance segment,
119  // as the instance segment may refer to and search through the class segment.
120  initSegmentWithCatch("class", [&]()
121  {
122  classSegment.init();
123  });
124 
126 
127  initSegmentWithCatch("attachment", [&]()
128  {
129  attachmentSegment.init();
130  });
131 
132  initSegmentWithCatch(p.markerMemoryName, [&]()
133  {
134  workingMemory()
135  .addCoreSegment(p.markerMemoryName, arondto::Marker::ToAronType())
136  .setMaxHistorySize(p.maxMarkerHistorySize);
137  });
138  }
139 
140 
142  {
143  ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
144 
145  getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
146 
147  // Create first to use the original values.
149 
150  // ARMARX_CHECK(virtualRobotReaderPlugin->isAvailable());
151 
153  &virtualRobotReaderPlugin->get(),
154  kinematicUnitObserver,
156  debugObserver
157  );
158  classSegment.connect(
160  );
161 
162  attachmentSegment.connect();
163 
165  }
166 
167 
169  {
170  }
171 
172 
174  {
175  }
176 
177  armem::actions::GetActionsOutputSeq ObjectMemory::getActions(
178  const armem::actions::GetActionsInputSeq& inputs)
179  {
180  using namespace armem::actions;
181  GetActionsOutputSeq outputs;
182  for (const auto& input : inputs)
183  {
184  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
185  if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
186  and memoryID.hasEntityName() and not memoryID.hasGap())
187  {
188  Menu menu {
189  Action{"vis", "Visualize object"},
190  };
191  // For strange C++ reasons, this works, but emplace_back doesn't.
192  outputs.push_back({ menu.toIce() });
193  }
194  }
195 
196  return outputs;
197  }
198 
199  armem::actions::ExecuteActionOutputSeq ObjectMemory::executeActions(
200  const armem::actions::ExecuteActionInputSeq& inputs)
201  {
202  using namespace armem::actions;
203  ExecuteActionOutputSeq outputs;
204 
205  for (const auto& input : inputs)
206  {
207  auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
208  if (input.actionPath == ActionPath{"vis"})
209  {
210  if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
211  and memoryID.hasEntityName() and not memoryID.hasGap())
212  {
213  classSegment.visualizeClass(memoryID);
214  outputs.emplace_back(true, "");
215  }
216  else
217  {
218  std::stringstream sstream;
219  sstream << "MemoryID " << memoryID
220  << " does not refer to a valid object class.";
221  outputs.emplace_back(false, sstream.str());
222  }
223  }
224  else
225  {
226  std::stringstream sstream;
227  sstream << "Action path " << input.actionPath << " is not defined.";
228  outputs.emplace_back(false, sstream.str());
229  }
230  }
231  return outputs;
232  }
233 
234  armem::prediction::data::PredictionResultSeq
235  ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
236  {
237  std::vector<armem::prediction::data::PredictionResult> results;
238  for (const auto& request : requests)
239  {
240  auto boRequest = armarx::fromIce<armem::PredictionRequest>(request);
242  result.snapshotID = boRequest.snapshotID;
243  if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"), boRequest.snapshotID)
244  and not boRequest.snapshotID.hasGap()
245  and boRequest.snapshotID.hasTimestamp())
246  {
247  objpose::ObjectPosePredictionRequest objPoseRequest;
248  toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
249  objPoseRequest.objectID = toIce(ObjectID(request.snapshotID.entityName));
250  objPoseRequest.settings = request.settings;
251  toIce(objPoseRequest.timestamp, boRequest.snapshotID.timestamp);
252 
253  objpose::ObjectPosePredictionResult objPoseResult =
254  predictObjectPoses({objPoseRequest}).at(0);
255  result.success = objPoseResult.success;
256  result.errorMessage = objPoseResult.errorMessage;
257 
258  if (objPoseResult.success)
259  {
261  builder.latestEntitySnapshot(boRequest.snapshotID);
262  auto queryResult = armarx::fromIce<armem::client::QueryResult>(
263  query(builder.buildQueryInputIce()));
264  std::string instanceError =
265  "Could not find instance '" + boRequest.snapshotID.str() + "' in memory";
266  if (!queryResult.success)
267  {
268  result.success = false;
269  result.errorMessage << instanceError << ":\n" << queryResult.errorMessage;
270  }
271  else
272  {
273  if (not boRequest.snapshotID.hasInstanceIndex())
274  {
275  boRequest.snapshotID.instanceIndex = 0;
276  }
277  auto* aronInstance = queryResult.memory.findLatestInstance(
278  boRequest.snapshotID, boRequest.snapshotID.instanceIndex);
279  if (aronInstance == nullptr)
280  {
281  result.success = false;
282  result.errorMessage << instanceError << ": No latest instance found.";
283  }
284  else
285  {
286  auto instance =
287  armem::arondto::ObjectInstance::FromAron(aronInstance->data());
289  instance.pose,
290  armarx::fromIce<objpose::ObjectPose>(objPoseResult.prediction));
291  result.prediction = instance.toAron();
292  }
293  }
294  }
295  }
296  else
297  {
298  result.success = false;
299  result.errorMessage << "No predictions are supported for MemoryID "
300  << boRequest.snapshotID;
301  }
302  results.push_back(result.toIce());
303  }
304 
305  return results;
306  }
307 
309  {
310  using namespace armarx::RemoteGui::Client;
311 
312  tab.reset(new RemoteGuiTab);
313 
314  tab->instance.setup(*this);
315  tab->clazz.setup(classSegment);
316 
317  HBoxLayout segments =
318  {
319  tab->instance.group,
320  tab->clazz.group
321  };
322  VBoxLayout root =
323  {
324  segments,
325  VSpacer()
326  };
327  RemoteGui_createTab(Component::getName(), root, tab.get());
328  }
329 
330 
332  {
333  tab->instance.update(*this);
334  tab->clazz.update(classSegment);
335 
336  if (tab->clazz.data.rebuild)
337  {
339  }
340  }
341 
342 }
armarx::armem::server::obj::ObjectMemory::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: ObjectMemory.cpp:173
memory_ids.h
armarx::armem::server::obj::ObjectMemory::executeActions
armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq &inputs) override
Definition: ObjectMemory.cpp:199
armarx::ArVizComponentPluginUser::getArvizClient
armarx::viz::Client & getArvizClient()
Definition: ArVizComponentPlugin.h:45
armarx::armem::server::obj::instance::SegmentAdapter::predictObjectPoses
virtual objpose::ObjectPosePredictionResultSeq predictObjectPoses(const objpose::ObjectPosePredictionRequestSeq &requests, ICE_CURRENT_ARG) override
Definition: SegmentAdapter.cpp:434
armarx::armem::server::obj::ObjectMemory::getDefaultName
std::string getDefaultName() const override
Definition: ObjectMemory.cpp:90
armarx::armem::server::obj::ObjectMemory::getActions
armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq &inputs) override
Definition: ObjectMemory.cpp:177
armarx::armem::PredictionResult::snapshotID
armem::MemoryID snapshotID
Definition: Prediction.h:63
ice_conversions.h
armarx::armem::server::plugins::ReadWritePluginUser::query
virtual armem::query::data::Result query(const armem::query::data::Input &input, const Ice::Current &=Ice::emptyCurrent) override
Definition: ReadWritePluginUser.cpp:56
query.h
armarx::armem::server::obj::ObjectMemory::ObjectMemory
ObjectMemory()
Definition: ObjectMemory.cpp:76
armarx::RemoteGui::Client::VBoxLayout
Definition: Widgets.h:167
armarx::armem::attachment::ObjectID
armem::MemoryID ObjectID
Definition: types.h:79
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
Prediction.h
armarx::armem::server::obj::instance::SegmentAdapter::init
void init()
Definition: SegmentAdapter.cpp:68
armarx::armem::toIce
void toIce(data::MemoryID &ice, const MemoryID &id)
Definition: ice_conversions.cpp:21
armarx::armem::server::obj::ObjectMemory::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: ObjectMemory.cpp:141
armarx::armem::base::MemoryBase::name
std::string & name()
Definition: MemoryBase.h:92
ice_conversions.h
armarx::ManagedIceObject::addPlugin
PluginT * addPlugin(const std::string prefix="", ParamsT &&...params)
Definition: ManagedIceObject.h:182
armarx::armem::client::query::Builder::latestEntitySnapshot
void latestEntitySnapshot(const MemoryID &entityID)
Definition: Builder.cpp:107
armarx::armem::PredictionResult
Definition: Prediction.h:58
armarx::armem::contains
bool contains(const MemoryID &general, const MemoryID &specific)
Indicates whether general is "less specific" than, or equal to, specific, i.e.
Definition: MemoryID.cpp:558
armarx::armem::server::plugins::ReadWritePluginUser::workingMemory
server::wm::Memory & workingMemory()
Definition: ReadWritePluginUser.cpp:106
armarx::armem::server::obj
Definition: ObjectMemory.cpp:39
armarx::RemoteGui::Client::VSpacer
Definition: Widgets.h:204
armarx::armem::server::obj::ObjectMemory::onDisconnectComponent
void onDisconnectComponent() override
Hook for subclass.
Definition: ObjectMemory.cpp:168
Clock.h
armarx::armem::server::obj::clazz::Segment::init
void init() override
Definition: Segment.cpp:53
armarx::armem::objects::memoryID
const MemoryID memoryID
Definition: memory_ids.cpp:29
armarx::Component::getProxyFromProperty
ProxyType getProxyFromProperty(const std::string &propertyName, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true)
Get a proxy whose name is specified by the given property.
Definition: Component.h:236
armarx::armem::client::query::Builder::buildQueryInputIce
armem::query::data::Input buildQueryInputIce() const
Definition: Builder.cpp:18
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::armem::PredictionResult::prediction
aron::data::DictPtr prediction
Definition: Prediction.h:64
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:19
armarx::core::time::Duration::SecondsDouble
static Duration SecondsDouble(double seconds)
Constructs a duration in seconds.
Definition: Duration.cpp:90
armarx::objpose::toAron
void toAron(arondto::ObjectAttachmentInfo &dto, const ObjectAttachmentInfo &bo)
Definition: objpose.cpp:22
armarx::armem::server::obj::clazz::Segment::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="") override
Definition: Segment.cpp:38
armarx::armem::PredictionResult::toIce
armem::prediction::data::PredictionResult toIce() const
Definition: Prediction.cpp:74
armarx::armem::server::obj::attachments::Segment::init
void init()
Definition: Segment.cpp:41
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::MemoryID::hasGap
bool hasGap() const
Indicate whether this ID has a gap such as in 'Memory//MyProvider' (no core segment name).
Definition: MemoryID.cpp:158
armarx::armem::server::obj::ObjectMemory::predict
armem::prediction::data::PredictionResultSeq predict(const armem::prediction::data::PredictionRequestSeq &requests) override
Definition: ObjectMemory.cpp:235
armarx::armem::MemoryID::hasEntityName
bool hasEntityName() const
Definition: MemoryID.h:121
armarx::armem::MemoryID::memoryName
std::string memoryName
Definition: MemoryID.h:50
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_startRunningTask
void RemoteGui_startRunningTask()
Definition: LightweightRemoteGuiComponentPlugin.cpp:110
armarx::RemoteGui::Client::HBoxLayout
Definition: Widgets.h:160
ObjectMemory.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::armem::server::obj::attachments::Segment::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="")
Definition: Segment.cpp:35
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::armem::PredictionResult::errorMessage
std::string errorMessage
Definition: Prediction.h:61
armarx::armem::server::obj::attachments::Segment::connect
void connect()
Definition: Segment.cpp:49
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_createTab
void RemoteGui_createTab(std::string const &name, RemoteGui::Client::Widget const &rootWidget, RemoteGui::Client::Tab *tab)
Definition: LightweightRemoteGuiComponentPlugin.cpp:95
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::armem::index::memoryID
const MemoryID memoryID
Definition: memory_ids.cpp:29
armarx::armem::server::obj::ObjectMemory::createRemoteGuiTab
void createRemoteGuiTab()
Definition: ObjectMemory.cpp:308
armarx::armem::server::obj::clazz::Segment::connect
void connect(viz::Client arviz)
Definition: Segment.cpp:65
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
ice_conversions_templates.h
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::armem::PredictionResult::success
bool success
Definition: Prediction.h:60
armarx::armem::server::obj::ObjectMemory::~ObjectMemory
virtual ~ObjectMemory() override
Definition: ObjectMemory.cpp:85
armarx::armem::server::obj::ObjectMemory::RemoteGui_update
void RemoteGui_update() override
Definition: ObjectMemory.cpp:331
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
armarx::armem::server::obj::ObjectMemory::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: ObjectMemory.cpp:96
aron_conversions.h
armarx::armem::server::obj::ObjectMemory::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ObjectMemory.cpp:42
armarx::armem::server::obj::instance::SegmentAdapter::connect
void connect(robot_state::VirtualRobotReader *virtualRobotReader, KinematicUnitObserverInterfacePrx kinematicUnitObserver, viz::Client arviz, DebugObserverInterfacePrx debugObserver)
Definition: SegmentAdapter.cpp:80
armarx::armem::server::obj::instance::SegmentAdapter::defineProperties
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string &prefix="")
Definition: SegmentAdapter.cpp:59