Component.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 MemoryX::ArmarXObjects::GraphImportExport
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 "Component.h"
24 
25 #include <iomanip>
26 
27 #include <VirtualRobot/VirtualRobot.h>
28 
31 
35 
37 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
38 #include <armarx/navigation/core/aron/Location.aron.generated.h>
42 
45 
47 {
48 
50  {
51  properties.locationCoreSegmentID = location::coreSegmentID;
52  properties.graphCoreSegmentID = graph::coreSegmentID;
53  }
54 
57  {
60 
61  def->component(proxies.priorKnowledge);
62  def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver");
63 
64  return def;
65  }
66 
67  void
69  {
70  // Topics and properties defined above are automagically registered.
71 
72  // Keep debug observer data until calling `sendDebugObserverBatch()`.
74  }
75 
76  void
78  {
80 
81  // Get proxies.
82  if (proxies.priorKnowledge->hasGraphSegment())
83  {
84  proxies.graphSegment = proxies.priorKnowledge->getGraphSegment();
85  }
86  proxies.locationWriter = memoryNameSystem().useWriter(properties.locationCoreSegmentID);
87  proxies.graphWriter = memoryNameSystem().useWriter(properties.graphCoreSegmentID);
88 
89 
90  // Setup and start the remote GUI.
91  refreshScenes();
93  }
94 
95  void
97  {
98  }
99 
100  void
102  {
103  }
104 
105  std::string
107  {
108  return GetDefaultName();
109  }
110 
111  std::string
113  {
114  return "graph_import_export";
115  }
116 
117  void
118  Component::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
119  {
120  using namespace armarx::RemoteGui::Client;
121 
122  tab.sceneComboBox.setOptions(sceneNames);
123  tab.sceneRefreshButton.setLabel("Refresh");
124 
125  tab.providerSegmentLine.setValue(getName());
126 
127  tab.dryRun.setValue(false);
128  tab.visuEnabled.setValue(true);
129 
130  tab.locationsMemoryxToArmemButton.setLabel("Locations MemoryX -> ArMem");
131  tab.locationsArmemToMemoryxButton.setLabel("Locations ArMem -> MemoryX (WIP)");
132  tab.locationsClearArMemButton.setLabel("Clear ArMem Locations");
133 
134  tab.graphMemoryxToArmemButton.setLabel("Graph MemoryX -> ArMem");
135  tab.graphArmemToMemoryxButton.setLabel("Graph ArMem -> MemoryX (WIP)");
136  tab.graphClearArMemButton.setLabel("Clear ArMem Graphs");
137 
138 
139  GridLayout grid;
140  int row = 0;
141  {
142  grid.add(Label("Scene:"), {row, 0})
143  .add(tab.sceneComboBox, {row, 1})
144  .add(tab.sceneRefreshButton, {row, 2});
145  ++row;
146 
147  grid.add(Label("Provider Segment:"), {row, 0}).add(tab.providerSegmentLine, {row, 1});
148  ++row;
149 
150  grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
151  ++row;
152 
153  grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
154  ++row;
155 
156  grid.add(tab.locationsMemoryxToArmemButton, {row, 0})
157  .add(tab.locationsArmemToMemoryxButton, {row, 1})
158  .add(tab.locationsClearArMemButton, {row, 2});
159  ++row;
160 
161  grid.add(tab.graphMemoryxToArmemButton, {row, 0})
162  .add(tab.graphArmemToMemoryxButton, {row, 1})
163  .add(tab.graphClearArMemButton, {row, 2});
164  ++row;
165  }
166 
167  VBoxLayout root = {grid, VSpacer()};
168  RemoteGui_createTab(getName(), root, &tab);
169  }
170 
171  void
173  {
174  if (tab.sceneRefreshButton.wasClicked())
175  {
176  refreshScenes();
177  }
178 
179  if (tab.locationsMemoryxToArmemButton.wasClicked())
180  {
181  locationsMemoryxToArmem(tab.sceneComboBox.getValue());
182  }
183  if (tab.locationsArmemToMemoryxButton.wasClicked())
184  {
185  locationsArmemToMemoryx(tab.sceneComboBox.getValue());
186  }
187  if (tab.graphMemoryxToArmemButton.wasClicked())
188  {
189  graphMemoryxToArmem(tab.sceneComboBox.getValue());
190  }
191  if (tab.graphArmemToMemoryxButton.wasClicked())
192  {
193  graphArmemToMemoryx(tab.sceneComboBox.getValue());
194  }
195 
196  if (tab.locationsClearArMemButton.wasClicked())
197  {
198  clearArMemProviderSegment(proxies.locationWriter, getLocationProviderSegmentID());
199  }
200  if (tab.graphClearArMemButton.wasClicked())
201  {
202  clearArMemProviderSegment(proxies.graphWriter, getGraphProviderSegmentID());
203  }
204  }
205 
206  void
207  Component::refreshScenes()
208  {
209  const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes();
210  createRemoteGuiTab(sceneNames);
211  }
212 
213  void
214  Component::locationsMemoryxToArmem(const std::string& sceneName)
215  {
216  const armem::Time time = armem::Time::Now();
217  armem::Commit commit;
218 
219  memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
220  for (memoryx::GraphNodeBasePtr& node : graphNodes)
221  {
222  ARMARX_CHECK_NOT_NULL(node);
223  if (not node->isMetaEntity())
224  {
225  // ID is just some random MongoDB hash
226  const std::string nodeId = node->getId();
227  // This is the readable name entered in the GUI.
228  const std::string name = node->getName();
229 
230  armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
231  ARMARX_CHECK_NOT_NULL(pose);
232 
233  FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(
234  proxies.graphNodePoseResolver->resolveToGlobalPose(node));
235  ARMARX_CHECK_NOT_NULL(globalNodePose);
236 
237  // `pose` and `globalNodePose` seem to be identical. Is the last step necessary?
238  // Maybe `pose` could be non-global.
239 
240  ARMARX_VERBOSE << std::setprecision(2) << std::fixed << "Processing node "
241  << (commit.updates.size() + 1) << "\n- ID: \t" << nodeId
242  << "\n- Name: \t" << name << "\n- Pose: \n"
243  << pose->toEigen() << "\n- Resolved global pose: \n"
244  << globalNodePose->toEigen();
245 
246  navigation::location::arondto::Location data;
247  data.framedPose.header.frame = GlobalFrame; // TODO is that correct?
248  data.framedPose.pose = globalNodePose->toEigen();
249 
250  armem::EntityUpdate& update = commit.add();
251  update.entityID = getLocationProviderSegmentID().withEntityName(name);
252  update.referencedTime = time;
253  update.instancesData = {data.toAron()};
254  }
255  }
256 
257  if (not tab.dryRun.getValue())
258  {
259  armem::CommitResult result = proxies.locationWriter.commit(commit);
260  if (result.allSuccess())
261  {
262  ARMARX_IMPORTANT << "Successfully exported " << result.results.size()
263  << " locations from MemoryX to ArMem.";
264  }
265  else
266  {
267  ARMARX_WARNING << result.allErrorMessages();
268  }
269  }
270  else
271  {
272  ARMARX_VERBOSE << "Dry Run - skipping commit.";
273  }
274  }
275 
276  void
277  Component::locationsArmemToMemoryx(const std::string& sceneName)
278  {
279  ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
280  (void)sceneName;
281  }
282 
283  void
284  Component::graphMemoryxToArmem(const std::string& sceneName)
285  {
286  memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
287  navigation::core::Graph graph = toArmemGraph(graphNodes);
288 
289  if (tab.visuEnabled.getValue())
290  {
291  navigation::graph::GraphVisu visu;
292  viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
293 
294  graph::visu::ObjectParserInfo info{objClient.fetchObjectPosesAsMap(),
295  objClient.getObjectFinder().findAllObjects()};
296  visu.draw(layer, graph, info);
297 
298  ARMARX_VERBOSE << "Visualize graph '" << sceneName << "' ...";
299  arviz.commit(layer);
300  }
301 
302  // Build ARON Graph
304  toAron(aron, graph);
305 
306  // Build commit
307  const armem::Time time = armem::Time::Now();
309  update.entityID = getGraphProviderSegmentID().withEntityName(sceneName);
310  update.referencedTime = time;
311  update.instancesData = {aron.toAron()};
312 
313  if (not tab.dryRun.getValue())
314  {
315  armem::EntityUpdateResult result = proxies.graphWriter.commit(update);
316  if (result.success)
317  {
318  ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName
319  << "' from MemoryX to ArMem.";
320  }
321  else
322  {
323  ARMARX_WARNING << result.errorMessage;
324  }
325  }
326  else
327  {
328  ARMARX_VERBOSE << "Dry Run - skipping commit.";
329  }
330  }
331 
332  void
333  Component::graphArmemToMemoryx(const std::string& sceneName)
334  {
335  ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
336  (void)sceneName;
337  }
338 
339  void
340  Component::clearArMemProviderSegment(armem::client::Writer& writer,
341  const armem::MemoryID& providerSegmentID)
342  {
343  const bool clearWhenExists = true;
344  auto result = writer.addSegment(providerSegmentID, clearWhenExists);
345  if (result.success)
346  {
347  ARMARX_IMPORTANT << "Cleared ArMem provider segment " << providerSegmentID << ".";
348  }
349  else
350  {
351  ARMARX_WARNING << result.errorMessage;
352  }
353  }
354 
356  Component::getLocationProviderSegmentID()
357  {
358  return properties.locationCoreSegmentID.withProviderSegmentName(
359  tab.providerSegmentLine.getValue());
360  }
361 
363  Component::getGraphProviderSegmentID()
364  {
365  return properties.graphCoreSegmentID.withProviderSegmentName(
366  tab.providerSegmentLine.getValue());
367  }
368 
370  Component::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
371  {
373  std::map<std::string, navigation::core::Graph::Vertex> vertexMap;
374 
375  // Add nodes
376  semrel::ShapeID nextVertexID{0};
377  for (memoryx::GraphNodeBasePtr node : graphNodes)
378  {
379  ARMARX_CHECK_NOT_NULL(node);
380  if (not node->isMetaEntity())
381  {
382  // This is the readable name entered in the GUI.
383  const std::string name = node->getName();
384 
385  FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(
386  proxies.graphNodePoseResolver->resolveToGlobalPose(node));
387  ARMARX_CHECK_NOT_NULL(globalNodePose);
388 
389  ARMARX_VERBOSE << "\n- Adding node: \t" << name;
390 
391  navigation::core::Graph::Vertex vertex = graph.addVertex(nextVertexID);
392  vertexMap.emplace(name, vertex);
393 
394  vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
395  toAron(vertex.attrib().aron.locationID,
396  getLocationProviderSegmentID().withEntityName(name));
397  vertex.attrib().setPose(*globalNodePose);
398 
399  nextVertexID++;
400  }
401  }
402  ARMARX_CHECK_EQUAL(graph.numVertices(), graphNodes.size());
403  for (auto v : graph.vertices())
404  {
405  ARMARX_CHECK(v.attrib().hasName())
406  << VAROUT(v.objectID()) << " | " << VAROUT(v.attrib().getName());
407  ARMARX_CHECK(v.attrib().hasPose())
408  << VAROUT(v.objectID()) << " | " << VAROUT(v.attrib().getName());
409  }
410 
411  // Add edges
412  for (memoryx::GraphNodeBasePtr node : graphNodes)
413  {
414  const auto& sourceVertex = vertexMap.at(node->getName());
415  for (int i = 0; i < node->getOutdegree(); i++)
416  {
417  auto adjacent =
418  memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
419  ARMARX_CHECK_NOT_NULL(adjacent);
420  const auto& targetVertex = vertexMap.at(adjacent->getName());
421 
422  ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> "
423  << adjacent->getName();
424  navigation::core::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
425  edge.attrib().aron.sourceVertexID =
426  static_cast<long>(sourceVertex.attrib().aron.vertexID);
427  edge.attrib().aron.targetVertexID =
428  static_cast<long>(targetVertex.attrib().aron.vertexID);
429  }
430  }
431  ARMARX_CHECK_EQUAL(graph.numVertices(), graphNodes.size());
432 
433  return graph;
434  }
435 
437 
438 
439 } // namespace armarx::navigation::components::graph_import_export
Component.h
armarx::navigation::components::graph_import_export
Definition: Component.cpp:46
armarx::viz::Client::commit
CommitResult commit(StagedCommit const &commit)
Definition: Client.cpp:80
armarx::navigation::graph::coreSegmentID
const armem::MemoryID coreSegmentID
Definition: constants.h:30
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::navigation::components::graph_import_export::Component::onConnectComponent
void onConnectComponent() override
Definition: Component.cpp:77
armarx::armem::CommitResult::allErrorMessages
std::vector< std::string > allErrorMessages() const
Definition: Commit.cpp:72
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::navigation::components::graph_import_export::ARMARX_REGISTER_COMPONENT_EXECUTABLE
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName())
armarx::navigation::components::graph_import_export::Component::getDefaultName
std::string getDefaultName() const override
Definition: Component.cpp:106
armarx::armem::Commit
A bundle of updates to be sent to the memory.
Definition: Commit.h:89
armarx::ObjectFinder::findAllObjects
std::vector< ObjectInfo > findAllObjects(bool checkPaths=true) const
Definition: ObjectFinder.cpp:144
armarx::RemoteGui::Client::VBoxLayout
Definition: Widgets.h:167
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
armarx::armem::EntityUpdateResult
Result of an EntityUpdate.
Definition: Commit.h:72
armarx::DebugObserverComponentPluginUser::setDebugObserverBatchModeEnabled
void setDebugObserverBatchModeEnabled(bool enable)
Definition: DebugObserverComponentPlugin.cpp:118
armarx::armem::client::plugins::PluginUser::memoryNameSystem
MemoryNameSystem & memoryNameSystem()
Definition: PluginUser.cpp:22
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
armarx::RemoteGui::Client::GridLayout::add
GridLayout & add(Widget const &child, Pos pos, Span span=Span{1, 1})
Definition: Widgets.cpp:412
armarx::navigation::components::graph_import_export::Component::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: Component.cpp:56
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::FramedPosePtr
IceInternal::Handle< FramedPose > FramedPosePtr
Definition: FramedPose.h:250
armarx::armem::client::MemoryNameSystem::useWriter
Writer useWriter(const MemoryID &memoryID)
Use a memory server and get a writer for it.
Definition: MemoryNameSystem.cpp:276
armarx::RemoteGui::Client::VSpacer
Definition: Widgets.h:204
ARMARX_CHECK
#define ARMARX_CHECK(expression)
Shortcut for ARMARX_CHECK_EXPRESSION.
Definition: ExpressionException.h:82
constants.h
armarx::navigation::components::graph_import_export::Component::onInitComponent
void onInitComponent() override
Definition: Component.cpp:68
armarx::armem::Commit::updates
std::vector< EntityUpdate > updates
The entity updates.
Definition: Commit.h:97
armarx::objpose::ObjectPoseClient::getObjectFinder
const ObjectFinder & getObjectFinder() const
Get the internal object finder.
Definition: ObjectPoseClient.cpp:111
IceInternal::Handle< FramedPose >
visionx::voxelgrid::Label
uint32_t Label
Type of an object label.
Definition: types.h:7
armarx::RemoteGui::Client::GridLayout
Definition: Widgets.h:186
armarx::navigation::components::graph_import_export::Component::RemoteGui_update
void RemoteGui_update() override
After calling RemoteGui_startRunningTask, this function is called periodically in a separate thread.
Definition: Component.cpp:172
armarx::objpose::ObjectPoseClient::fetchObjectPosesAsMap
ObjectPoseMap fetchObjectPosesAsMap() const
Fetch all known object poses.
Definition: ObjectPoseClient.cpp:51
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
FramedPose.h
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
Visu.h
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:27
armarx::armem::CommitResult
Result of a Commit.
Definition: Commit.h:110
MemoryXCoreObjectFactories.h
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:54
armarx::navigation::components::graph_import_export::Component::GetDefaultName
static std::string GetDefaultName()
Definition: Component.cpp:112
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
aron_conversions.h
armarx::LightweightRemoteGuiComponentPluginUser::RemoteGui_startRunningTask
void RemoteGui_startRunningTask()
Definition: LightweightRemoteGuiComponentPlugin.cpp:110
ExpressionException.h
Commit.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
Decoupled.h
Graph.h
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx::armem::MemoryID::withEntityName
MemoryID withEntityName(const std::string &name) const
Definition: MemoryID.cpp:420
armarx::armem::Commit::add
EntityUpdate & add()
Definition: Commit.cpp:81
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::ArVizComponentPluginUser::arviz
armarx::viz::Client arviz
Definition: ArVizComponentPlugin.h:43
armarx::armem::CommitResult::results
std::vector< EntityUpdateResult > results
Definition: Commit.h:112
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
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::navigation::components::graph_import_export::Component::Component
Component()
Definition: Component.cpp:49
MemoryXTypesObjectFactories.h
armarx::armem::EntityUpdateResult::errorMessage
std::string errorMessage
Definition: Commit.h:79
armarx::navigation::location::coreSegmentID
const armem::MemoryID coreSegmentID
Definition: constants.h:30
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
armarx::toAron
void toAron(arondto::PackagePath &dto, const PackageFileLocation &bo)
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::ObjectPoseClientPluginUser::getClient
objpose::ObjectPoseClient getClient() const
Definition: ObjectPoseClientPlugin.cpp:64
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::viz::Client::layer
Layer layer(std::string const &name) const
Definition: Client.cpp:73
armarx::RemoteGui::Client
Definition: EigenWidgets.cpp:8
armarx::viz::Layer
Definition: Layer.h:12
armarx::armem::EntityUpdateResult::success
bool success
Definition: Commit.h:74
armarx::navigation::components::graph_import_export::Component::onDisconnectComponent
void onDisconnectComponent() override
Definition: Component.cpp:96
armarx::navigation::components::graph_import_export::Component::createRemoteGuiTab
void createRemoteGuiTab(const std::vector< std::string > &sceneNames)
This function should be called once in onConnect() or when you need to re-create the Remote GUI tab.
Definition: Component.cpp:118
armarx::armem::CommitResult::allSuccess
bool allSuccess() const
Definition: Commit.cpp:64
constants.h
armarx::navigation::components::graph_import_export::Component::onExitComponent
void onExitComponent() override
Definition: Component.cpp:101