From 9f1329e51057209368d55454e88ba71d8a668657 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 1 Jul 2021 11:18:09 +0200
Subject: [PATCH] visu for articulated objects now complete
---
.../client/articulated_object/Writer.cpp | 36 +++++++++++++++++--
.../armem_objects/server/instance/Segment.cpp | 10 ++++--
2 files changed, 41 insertions(+), 5 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 1c5134842..3f9cb647a 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -10,12 +10,16 @@
#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectID.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
@@ -203,7 +207,6 @@ namespace armarx::armem::articulated_object
arondto::ObjectInstance objectInstance;
toAron(objectInstance, obj.config);
-
const auto classId = storeOrGetClass(obj);
if (not classId)
@@ -212,12 +215,38 @@ namespace armarx::armem::articulated_object
return false;
}
-
+ ARMARX_INFO << "Class id is " << classId->str();
// install memory link
// toAron(aronArticulatedObject.description, *descriptionId);
toAron(objectInstance.classID, *classId);
+ // armarx::arondto::ObjectID& objectId = objectInstance.pose.objectID;
+
+ armem::MemoryID id;
+ id.setEntityID(classId->getEntityID());
+
+ // auto classId2 = classId->getEntityID();
+ // classId2.setEntityID(const MemoryID &id)
+
+ ARMARX_INFO << "manual object id: " << id.providerSegmentName + "/" + id.entityName;
+
+ armarx::ObjectID objectId(id.entityName);
+
+ armarx::arondto::ObjectID cs;
+ // objectInstance.pose.objectID
+ // toAron(objectInstance.pose.objectID, objectId);
+
+ cs.className = objectId.className();
+ cs.instanceName = objectId.instanceName();
+ cs.dataset = objectId.dataset();
+
+ objectInstance.pose.objectID = cs;
+
+ ARMARX_INFO << "class entity id" << classId->getEntityID().str();
+
+ // toAron(objectInstance.pose.objectID, objectInstance.classID);
+
update.instancesData = {objectInstance.toAron()};
update.timeCreated = timestamp;
@@ -244,6 +273,7 @@ namespace armarx::armem::articulated_object
<< "Known classes are " << simox::alg::get_keys(knownObjects);
return false;
}
+ ARMARX_INFO << "Object classId is " << classId->str();
// TODO(fabian.reister): integrate memory link
return storeInstance(obj);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 611ec0a8f..5fd9b28ee 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -482,8 +482,11 @@ namespace armarx::armem::server::obj::instance
articulatedObject.timestamp = objectPose.timestamp;
+ ARMARX_INFO << "Object id is " << objectId.str();
+ ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
+
// Search for object class.
- if (auto classInstance = findClassInstance(objectId))
+ if (auto classInstance = findClassInstance(objectPose.objectID))
{
arondto::ObjectClass dto;
@@ -780,11 +783,14 @@ namespace armarx::armem::server::obj::instance
return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
}
}
+
+ ARMARX_WARNING << "No provider segment for classID " << classID.str() << " found";
return std::nullopt;
}
- catch (const armem::error::ArMemError&)
+ catch (const armem::error::ArMemError& e)
{
// Some segment or entity did not exist.
+ ARMARX_WARNING << e.what();
return std::nullopt;
}
}
--
GitLab