From cbd8d96eae7f277461effd81c36bb3b95b936e92 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 22 Feb 2021 15:30:23 +0100
Subject: [PATCH] Visualize in own thread
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 168 +++++++++++++-----
.../ObjectPoseObserver/ObjectPoseObserver.h | 15 +-
2 files changed, 137 insertions(+), 46 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 7e0631906..6a317ab27 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -96,6 +96,14 @@ namespace armarx
});
decay.updateTask->start();
}
+ if (!visu.updateTask)
+ {
+ visu.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ visu.updateTask->start();
+ }
createRemoteGuiTab();
RemoteGui_startRunningTask();
@@ -120,6 +128,7 @@ namespace armarx
const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
+ TIMING_START(ReportObjectPoses);
std::optional<IceUtil::Time> discardUpdatesUntil;
bool discardAll = false;
@@ -246,6 +255,15 @@ namespace armarx
data.objectPoses[providerName] = newObjectPoses;
handleProviderUpdate(providerName);
+
+ TIMING_END_STREAM(ReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "ReportObjectPoses [ms]", new Variant(ReportObjectPoses.toMilliSecondsDouble()) },
+ });
+ }
}
}
@@ -647,73 +665,118 @@ namespace armarx
if (visu.enabled)
{
- visProviderUpdate(providerName);
+ // Trigger a visu update (we are holding dataMutex).
+ std::vector<viz::Layer> layers;
+ visualizeProvider(providerName, data.objectPoses.at(providerName), data.robot, layers);
+ arviz.commit(layers);
}
}
- void ObjectPoseObserver::visProviderUpdate(const std::string& providerName)
+ void ObjectPoseObserver::visualizeRun()
{
- viz::Layer layer = arviz.layer(providerName);
+ CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+ while (visu.updateTask && !visu.updateTask->isStopped())
+ {
+ TIMING_START(Visu);
+ if (visu.enabled)
+ {
+ std::scoped_lock lock(dataMutex);
+ visualizeCommit(data.objectPoses, data.robot);
+ }
+ if (visu.enabled)
+ {
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
+ }
+ cycle.waitForCycleDuration();
+ }
+ }
- bool synchronized = false;
- for (objpose::ObjectPose& objectPose : data.objectPoses.at(providerName))
+ void ObjectPoseObserver::visualizeCommit(std::map<std::string, objpose::ObjectPoseSeq>& objectPoses, VirtualRobot::RobotPtr robot)
+ {
+ std::vector<viz::Layer> layers;
+ for (auto& [name, poses] : objectPoses)
{
- const armarx::ObjectID id = objectPose.objectID;
- std::string key = id.str();
+ visualizeProvider(name, poses, robot, layers);
+ }
+ arviz.commit(layers);
+ }
+
+ void ObjectPoseObserver::visualizeProvider(
+ const std::string& providerName, objpose::ObjectPoseSeq& objectPoses,
+ VirtualRobot::RobotPtr robot, std::vector<viz::Layer>& layers)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer(providerName));
+ for (objpose::ObjectPose& objectPose : objectPoses)
+ {
+ visualizeObjectPose(layer, objectPose, robot);
+ }
+ }
+
+ void ObjectPoseObserver::visualizeObjectPose(viz::Layer& layer, objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot)
+ {
+ bool synchronized = false;
- Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+ const armarx::ObjectID id = objectPose.objectID;
+ std::string key = id.str();
- if (data.isAttachedCached(objectPose))
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ // TODO: Get this out of visu function.
+ if (data.isAttachedCached(objectPose))
+ {
+ if (!synchronized)
{
- if (!synchronized)
- {
- RobotState::synchronizeLocalClone(data.robot);
- synchronized = true;
- }
- objpose::ObjectPose updated = objectPose.getAttached(data.robot);
- pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
+ RobotState::synchronizeLocalClone(robot);
+ synchronized = true;
}
- else
+ objpose::ObjectPose updated = objectPose.getAttached(robot);
+ pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
+ }
+ else
+ {
+ pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ }
+
+ {
+ viz::Object object = viz::Object(key).pose(pose);
+ if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(id))
{
- pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
}
-
+ else
{
- viz::Object object = viz::Object(key).pose(pose);
- if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(id))
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
- if (visu.alphaByConfidence && objectPose.confidence < 1.0f)
- {
- object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
- }
- else if (visu.alpha < 1)
- {
- object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
- }
- layer.add(object);
+ object.fileByObjectFinder(id);
}
-
- if (visu.oobbs && objectPose.localOOBB)
+ if (visu.alphaByConfidence && objectPose.confidence < 1.0f)
{
- const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
- layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
- .color(simox::Color::lime(255, 64)));
+ object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
}
- if (visu.objectFrames)
+ else if (visu.alpha < 1)
{
- layer.add(viz::Pose(key + " Pose").pose(pose).scale(visu.objectFramesScale));
+ object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
}
+ layer.add(object);
}
- arviz.commit(layer);
+ if (visu.oobbs && objectPose.localOOBB)
+ {
+ const simox::OrientedBoxf& oobb = *objectPose.localOOBB;
+ layer.add(viz::Box(key + " OOBB").set(oobb.transformed(pose))
+ .color(simox::Color::lime(255, 64)));
+ }
+ if (visu.objectFrames)
+ {
+ layer.add(viz::Pose(key + " Pose").pose(pose).scale(visu.objectFramesScale));
+ }
}
objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
@@ -749,6 +812,8 @@ namespace armarx
CycleUtil cycle(static_cast<int>(1000 / decay.updateFrequencyHz));
while (decay.updateTask && !decay.updateTask->isStopped())
{
+ TIMING_START(Decay);
+ if (decay.enabled)
{
std::scoped_lock lock(dataMutex);
IceUtil::Time now = TimeUtil::GetTime();
@@ -760,6 +825,18 @@ namespace armarx
}
}
}
+ if (decay.enabled)
+ {
+ TIMING_END_STREAM(Decay, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Decay Update [ms]", new Variant(Decay.toMilliSecondsDouble()) },
+ });
+ }
+ }
+
cycle.waitForCycleDuration();
}
}
@@ -923,6 +1000,7 @@ namespace armarx
void ObjectPoseObserver::Visu::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(enabled, prefix + "enabled", "Enable or disable visualization of objects.");
+ defs->optional(frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
defs->optional(inGlobalFrame, prefix + "inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
defs->optional(alpha, prefix + "alpha", "Alpha of objects (1 = solid, 0 = transparent).");
defs->optional(alphaByConfidence, prefix + "alphaByConfidence", "If true, use the pose confidence as alpha (if < 1.0).");
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index be40690be..1c4f032ff 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -145,7 +145,6 @@ namespace armarx
void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
void handleProviderUpdate(const std::string& providerName);
- void visProviderUpdate(const std::string& providerName);
objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
@@ -155,6 +154,16 @@ namespace armarx
void decayUpdateRun();
+ // Visualization
+
+ void visualizeRun();
+
+ void visualizeCommit(std::map<std::string, objpose::ObjectPoseSeq>& objectPoses, VirtualRobot::RobotPtr robot);
+ void visualizeProvider(const std::string& providerName, objpose::ObjectPoseSeq& objectPoses,
+ VirtualRobot::RobotPtr robot, std::vector<viz::Layer>& layers);
+ void visualizeObjectPose(viz::Layer& layer, objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot);
+
+
private:
DebugObserverInterfacePrx debugObserver;
@@ -197,6 +206,8 @@ namespace armarx
struct Visu
{
bool enabled = false;
+ float frequencyHz = 10;
+
bool inGlobalFrame = true;
float alpha = 1.0;
bool alphaByConfidence = false;
@@ -204,6 +215,8 @@ namespace armarx
bool objectFrames = false;
float objectFramesScale = 1.0;
+ SimpleRunningTask<>::pointer_type updateTask;
+
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
};
Visu visu;
--
GitLab