From 4016734d2eeb5e8a2066ee8528ec221c09e469b1 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 1 Jul 2021 09:33:59 +0200
Subject: [PATCH] visu for articulated objects
---
.../libraries/armem_objects/CMakeLists.txt | 12 +--
.../ArticulatedObjectVisu.cpp} | 65 ++++++++--------
.../ArticulatedObjectVisu.h} | 32 ++++----
.../armem_objects/server/instance/Segment.cpp | 77 +++++++++++++++++--
.../armem_objects/server/instance/Segment.h | 12 +++
.../server/instance/SegmentAdapter.cpp | 2 +
6 files changed, 135 insertions(+), 65 deletions(-)
rename source/RobotAPI/libraries/armem_objects/server/{articulated_object_instance/Visu.cpp => instance/ArticulatedObjectVisu.cpp} (73%)
rename source/RobotAPI/libraries/armem_objects/server/{articulated_object_instance/Visu.h => instance/ArticulatedObjectVisu.h} (81%)
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index d78bddfa8..54eb919e1 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -30,11 +30,12 @@ armarx_add_library(
server/instance/Decay.h
server/instance/RobotHeadMovement.h
server/instance/Visu.h
+ server/instance/ArticulatedObjectVisu.h
- server/articulated_object_class/Segment.h
- server/articulated_object_instance/Segment.h
+ # server/articulated_object_class/Segment.h
+ # server/articulated_object_instance/Segment.h
# server/articulated_object/SegmentAdapter.h
- server/articulated_object_instance/Visu.h
+ # server/articulated_object_instance/Visu.h
server/attachments/Segment.h
@@ -61,12 +62,13 @@ armarx_add_library(
server/instance/Decay.cpp
server/instance/RobotHeadMovement.cpp
server/instance/Visu.cpp
+ server/instance/ArticulatedObjectVisu.cpp
server/articulated_object_class/Segment.cpp
- server/articulated_object_instance/Segment.cpp
+ # server/articulated_object_instance/Segment.cpp
# server/articulated_object/SegmentAdapter.cpp
- server/articulated_object_instance/Visu.cpp
+ # server/articulated_object_instance/Visu.cpp
server/attachments/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
similarity index 73%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
index d1ee962ac..f1b90e5da 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
@@ -1,31 +1,30 @@
-#include "Visu.h"
+#include "ArticulatedObjectVisu.h"
#include <algorithm>
+#include <SimoxUtility/math/pose.h>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <SimoxUtility/math/pose.h>
-
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include "Segment.h"
-namespace armarx::armem::server::obj::articulated_object_instance
+namespace armarx::armem::server::obj::instance
{
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void ArticulatedObjectVisu::defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix)
{
- defs->optional(p.enabled, prefix + "enabled",
- "Enable or disable visualization of objects.");
- defs->optional(p.frequencyHz, prefix + "frequenzyHz",
- "Frequency of visualization.");
+ defs->optional(
+ p.enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
-
- viz::Layer Visu::visualizeProvider(
+ viz::Layer ArticulatedObjectVisu::visualizeProvider(
const std::string& providerName,
const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
@@ -36,12 +35,13 @@ namespace armarx::armem::server::obj::articulated_object_instance
return layer;
}
-
- void Visu::visualizeObjects(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
+ void ArticulatedObjectVisu::visualizeObjects(
+ viz::Layer& layer,
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const
{
- const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
+ const auto visualizeObject =
+ [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
{
-
const auto xmlPath = obj.description.xml.serialize();
// clang-format off
@@ -58,40 +58,40 @@ namespace armarx::armem::server::obj::articulated_object_instance
};
std::for_each(objects.begin(), objects.end(), visualizeObject);
-
}
- void Visu::init()
+ void ArticulatedObjectVisu::init()
{
- updateTask = new PeriodicTask<Visu>(this, &Visu::visualizeRun, 1000 / p.frequencyHz);
+ updateTask = new PeriodicTask<ArticulatedObjectVisu>(this, &ArticulatedObjectVisu::visualizeRun, 1000 / p.frequencyHz);
+
+ ARMARX_INFO << "ArticulatedObjectVisu: init";
updateTask->start();
}
-
- void Visu::visualizeRun()
+ void ArticulatedObjectVisu::visualizeRun()
{
// std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
+ ARMARX_INFO << "Update task";
- if (not p.enabled)
- {
- return;
- }
+ // if (not p.enabled)
+ // {
+ // return;
+ // }
// TIMING_START(Visu);
const auto articulatedObjects = segment.getArticulatedObjects();
- ARMARX_DEBUG << "Found " << articulatedObjects.size() << " articulated objects";
+ ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
- ARMARX_DEBUG << "visualizing objects";
+ ARMARX_INFO << "visualizing objects";
visualizeObjects(layer, articulatedObjects);
- ARMARX_DEBUG << "Committing objects";
+ ARMARX_INFO << "Committing objects";
arviz.commit({layer});
- ARMARX_DEBUG << "Done committing";
+ ARMARX_INFO << "Done committing";
// TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
@@ -102,7 +102,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
// });
// }
-
}
// void Visu::RemoteGui::setup(const Visu& visu)
@@ -155,6 +154,4 @@ namespace armarx::armem::server::obj::articulated_object_instance
// visu.objectFramesScale = objectFramesScale.getValue();
// }
-
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
rename to source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
index 3590270fe..9c66fd7fb 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.h
@@ -27,56 +27,53 @@
// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
-
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx
{
class ObjectFinder;
}
-namespace armarx::armem::server::obj::articulated_object_instance
+namespace armarx::armem::server::obj::instance
{
class Segment;
/**
* @brief Visualizes articulated objects
*/
- class Visu : public armarx::Logging
+ class ArticulatedObjectVisu : public armarx::Logging
{
public:
+ ArticulatedObjectVisu(const viz::Client& arviz, Segment& segment) :
+ arviz(arviz), segment(segment)
+ {
+ }
- Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "visu.");
void init();
protected:
viz::Layer visualizeProvider(
const std::string& providerName,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
void visualizeObjects(
viz::Layer& layer,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
-
+ const armarx::armem::articulated_object::ArticulatedObjects& objects) const;
private:
viz::Client arviz;
- const Segment& segment;
+ Segment& segment;
struct Properties
{
- bool enabled = true;
+ bool enabled = true;
float frequencyHz = 25;
} p;
-
- PeriodicTask<Visu>::pointer_type updateTask;
+ PeriodicTask<ArticulatedObjectVisu>::pointer_type updateTask;
void visualizeRun();
// struct RemoteGui
@@ -95,7 +92,6 @@ namespace armarx::armem::server::obj::articulated_object_instance
// // void setup(const Visu& visu);
// // void update(Visu& visu);
// };
-
};
-} // namespace armarx::armem::server::obj::articulated_object_instance
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 31f25a68a..611ec0a8f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,5 +1,6 @@
#include "Segment.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
@@ -18,6 +19,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -30,6 +33,7 @@
#include <Eigen/Geometry>
+#include <IceUtil/Time.h>
#include <sstream>
@@ -111,6 +115,13 @@ namespace armarx::armem::server::obj::instance
}
}
+ void Segment::connect(viz::Client arviz)
+ {
+ ARMARX_INFO << "ArticulatedObjectVisu";
+ this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
+ visu->init();
+ }
+
Segment::CommitStats Segment::commitObjectPoses(
const std::string& providerName,
@@ -216,18 +227,13 @@ namespace armarx::armem::server::obj::instance
update.confidence = pose.confidence;
arondto::ObjectInstance dto;
- toAron(dto, pose);
// Search for object class.
if (auto instance = findClassInstance(pose.objectID))
{
- toAron(dto.classID, instance->id());
- }
- else
- {
- toAron(dto.classID, MemoryID());
+ fromAron(dto.classID, instance->id());
+
}
- toAron(dto.sourceID, MemoryID());
- update.instancesData.push_back(dto.toAron());
+
}
iceMemory.commit(commit);
}
@@ -459,6 +465,61 @@ namespace armarx::armem::server::obj::instance
}
+ ::armarx::armem::articulated_object::ArticulatedObjects Segment::getArticulatedObjects()
+ {
+ objpose::ObjectPoseMap objectPoses = getObjectPoses(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
+
+ ::armarx::armem::articulated_object::ArticulatedObjects objects;
+ for (const auto&[objectId, objectPose] : objectPoses)
+ {
+ armem::articulated_object::ArticulatedObject articulatedObject;
+ articulatedObject.config.jointMap = objectPose.objectJointValues;
+ articulatedObject.config.globalPose = objectPose.objectPoseGlobal;
+ articulatedObject.config.timestamp = objectPose.timestamp;
+ articulatedObject.instance = objectPose.objectID.instanceName();
+ articulatedObject.timestamp = objectPose.timestamp;
+
+
+ // Search for object class.
+ if (auto classInstance = findClassInstance(objectId))
+ {
+ arondto::ObjectClass dto;
+
+ try
+ {
+ dto.fromAron(classInstance->data());
+ robot::RobotDescription description;
+
+ fromAron(dto, description);
+ articulatedObject.description = description;
+
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion failed!";
+ continue;
+ }
+ }
+ else
+ {
+ ARMARX_WARNING << "Class instance not found!";
+ continue;
+ }
+
+
+
+ if (not articulatedObject.config.jointMap.empty())
+ {
+ objects.push_back(articulatedObject);
+ }
+ }
+
+ return objects;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 3d7af768b..3ec6a2f22 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -10,6 +10,7 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/components/ArViz/Client/Client.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
@@ -23,6 +24,8 @@
#include "Decay.h"
+#include "ArticulatedObjectVisu.h"
+
namespace armarx::armem::obj
{
@@ -55,6 +58,9 @@ namespace armarx::armem::server::obj::instance
void init();
+ void connect(viz::Client arviz);
+
+
CommitStats commitObjectPoses(
const std::string& providerName,
@@ -104,6 +110,9 @@ namespace armarx::armem::server::obj::instance
static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
+ ::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
+
+
private:
@@ -218,6 +227,9 @@ namespace armarx::armem::server::obj::instance
void update(Segment& data);
};
+ private:
+ std::unique_ptr<ArticulatedObjectVisu> visu;
+
};
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 54b298b6c..e0577877f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -93,6 +93,8 @@ namespace armarx::armem::server::obj::instance
});
visu.updateTask->start();
}
+
+ segment.connect(arviz);
}
void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
--
GitLab