diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index 96c82550e62586d3ab6e13c2a152e756206810fe..fd526dd81804ccfa68fa288d576831b58e1e5bce 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -12,6 +12,8 @@ set(COMPONENT_LIBS
set(SOURCES
ObjectPoseObserver.cpp
+ detail/ObjectPoseDecay.cpp
+ detail/RobotHeadMovement.cpp
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
@@ -19,6 +21,8 @@ set(SOURCES
)
set(HEADERS
ObjectPoseObserver.h
+ detail/ObjectPoseDecay.h
+ detail/RobotHeadMovement.h
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 760af325a522618218a7d39ceaf45d9fa20fa65a..5ee3be0e75093e3a948e21ce1c7604b64f15d699 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -31,6 +31,7 @@
#include <VirtualRobot/RobotConfig.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -54,19 +55,10 @@ namespace armarx
defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
defs->topic(debugObserver);
- defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
- defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
- defs->optional(visu.alpha, "visu.alpha", "Alpha of objects (1 = solid, 0 = transparent).");
- defs->optional(visu.oobbs, "visu.oobbs", "Enable showing oriented bounding boxes.");
- defs->optional(visu.objectFrames, "visu.objectFrames", "Enable showing object frames.");
- defs->optional(visu.objectFramesScale, "visu.objectFramesScale", "Scaling of object frames.");
-
- defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated.");
- defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated.");
-
- defs->optional(robotHead.checkHeadVelocity, "head.checkHeadVelocity", "If true, check whether the head is moving and discard updates in the meantime.");
- defs->optional(robotHead.maxJointVelocity, "head.maxJointVelocity", "If a head joint's velocity is higher, the head is considered moving.");
- defs->optional(robotHead.discardIntervalAfterMoveMS, "head.discardIntervalAfterMoveMS", "For how long new updates are ignored after moving the head.");
+ visu.defineProperties(defs, "visu.");
+ calibration.defineProperties(defs, "calibration.");
+ robotHead.defineProperties(defs, "head.");
+ decay.defineProperties(defs, "decay.");
return defs;
}
@@ -78,6 +70,7 @@ namespace armarx
void ObjectPoseObserver::onInitObserver()
{
+ data.setTag(getName());
robotHead.setTag(getName());
usingTopicFromProperty("ObjectPoseTopicName");
@@ -89,13 +82,30 @@ namespace armarx
// So we need to always make sure to guard a call to addRobot
if (!RobotState::hasRobot("robot"))
{
- robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
+ data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
}
getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
robotHead.debugObserver = debugObserver;
robotHead.fetchDatafields();
+ if (!decay.updateTask)
+ {
+ decay.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->decayUpdateRun();
+ });
+ decay.updateTask->start();
+ }
+ if (!visu.updateTask)
+ {
+ visu.updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ visu.updateTask->start();
+ }
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
@@ -119,6 +129,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;
@@ -161,18 +172,18 @@ namespace armarx
}
{
std::scoped_lock lock(dataMutex);
- RobotState::synchronizeLocalClone(robot);
+ RobotState::synchronizeLocalClone(data.robot);
- if (robot->hasRobotNode(calibration.robotNode))
+ if (data.robot->hasRobotNode(calibration.robotNode))
{
- VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode = data.robot->getRobotNode(calibration.robotNode);
float value = robotNode->getJointValue();
robotNode->setJointValue(value + calibration.offset);
}
// This stays empty on the first report.
objpose::ObjectPoseSeq previousPoses;
- if (auto it = this->objectPoses.find(providerName); it != this->objectPoses.end())
+ if (auto it = data.objectPoses.find(providerName); it != data.objectPoses.end())
{
previousPoses = it->second;
}
@@ -217,11 +228,11 @@ namespace armarx
{
numUpdated++;
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- newPose.fromProvidedPose(provided, robot);
+ newPose.fromProvidedPose(provided, data.robot);
if (newPose.objectID.dataset().empty())
{
// Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(newPose.objectID))
+ if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(newPose.objectID))
{
newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
}
@@ -229,7 +240,7 @@ namespace armarx
if (!provided.localOOBB)
{
// Try to load oobb from disk.
- newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ newPose.localOOBB = data.getObjectOOBB(newPose.objectID);
}
}
}
@@ -243,8 +254,17 @@ namespace armarx
});
}
- this->objectPoses[providerName] = newObjectPoses;
+ data.objectPoses[providerName] = newObjectPoses;
handleProviderUpdate(providerName);
+
+ TIMING_END_STREAM(ReportObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "ReportObjectPoses [ms]", new Variant(ReportObjectPoses.toMilliSecondsDouble()) },
+ });
+ }
}
}
@@ -266,11 +286,11 @@ namespace armarx
}
ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
<< "Supported objects: \n" << ss.str();
- providers[providerName] = info;
+ data.providers[providerName] = info;
- if (updateCounters.count(providerName) == 0)
+ if (data.updateCounters.count(providerName) == 0)
{
- updateCounters[providerName] = 0;
+ data.updateCounters[providerName] = 0;
}
}
@@ -278,33 +298,48 @@ namespace armarx
{
offerChannel(providerName, "Channel of provider '" + providerName + "'.");
}
- offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType), "The object type (known or unknown)");
- offerOrUpdateDataField(providerName, "numSupportedObjects", int(info.supportedObjects.size()), "Number of requestable objects.");
+ offerOrUpdateDataField(providerName, "objectType", objpose::ObjectTypeEnumNames.to_name(info.objectType),
+ "The object type (known or unknown)");
+ offerOrUpdateDataField(providerName, "numSupportedObjects", int(info.supportedObjects.size()),
+ "Number of requestable objects.");
}
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
- // IceUtil::Time start = IceUtil::Time::now();
- std::scoped_lock lock(dataMutex);
- // ARMARX_INFO << "Locking took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
- // start = IceUtil::Time::now();
+ TIMING_START(GetObjectPosesLock);
+ std::scoped_lock lock(dataMutex, decayMutex);
+ TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+ TIMING_START(GetObjectPoses);
+
bool synchronized = false;
objpose::data::ObjectPoseSeq result;
- for (auto& [name, poses] : objectPoses)
+ for (auto& [name, poses] : data.objectPoses)
{
- toIceWithAttachments(poses, robot, result, synchronized);
+ toIceWithAttachments(poses, data.robot, result, synchronized, decay);
}
- // ARMARX_INFO << "getObjectPoses() took " << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
+
+ TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "getObjectPoses() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+ { "getObjectPoses() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
+
return result;
}
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex);
+ std::scoped_lock lock(dataMutex, decayMutex);
+
bool synchronized = false;
objpose::data::ObjectPoseSeq result;
- toIceWithAttachments(objectPoses.at(providerName), robot, result, synchronized);
+ toIceWithAttachments(data.objectPoses.at(providerName), data.robot, result, synchronized, decay);
return result;
}
@@ -321,7 +356,7 @@ namespace armarx
{
if (proxies.count(providerName) == 0)
{
- if (auto it = providers.find(providerName); it != providers.end())
+ if (auto it = data.providers.find(providerName); it != data.providers.end())
{
proxies[providerName] = it->second.proxy;
}
@@ -344,7 +379,7 @@ namespace armarx
for (const auto& objectID : input.request.objectIDs)
{
bool found = true;
- for (const auto& [providerName, info] : providers)
+ for (const auto& [providerName, info] : data.providers)
{
// ToDo: optimize look up.
if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
@@ -387,27 +422,28 @@ namespace armarx
objpose::ProviderInfoMap ObjectPoseObserver::getAvailableProvidersInfo(const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- return providers;
+ return data.providers;
}
Ice::StringSeq ObjectPoseObserver::getAvailableProviderNames(const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- return simox::alg::get_keys(providers);
+ return simox::alg::get_keys(data.providers);
}
objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
try
{
- return providers.at(providerName);
+ return data.providers.at(providerName);
}
catch (const std::out_of_range&)
{
std::stringstream ss;
ss << "No provider with name '" << providerName << "' available.\n";
ss << "Available are:\n";
- for (const auto& [name, _] : providers)
+ for (const auto& [name, _] : data.providers)
{
ss << "- '" << name << "'\n";
}
@@ -417,8 +453,7 @@ namespace armarx
bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex);
- return providers.count(providerName) > 0;
+ return data.providers.count(providerName) > 0;
}
@@ -433,14 +468,14 @@ namespace armarx
ObjectID objectID = armarx::fromIce(input.objectID);
- if (input.agentName != "" && input.agentName != robot->getName())
+ if (input.agentName != "" && input.agentName != data.robot->getName())
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
<< "\n(You can leave the agent name empty if there is only one agent.)\n"
- << "\nKnown agents: " << std::vector<std::string> {robot->getName()};
+ << "\nKnown agents: " << std::vector<std::string> {data.robot->getName()};
return output;
}
- VirtualRobot::RobotPtr agent = this->robot;
+ VirtualRobot::RobotPtr agent = data.robot;
if (!agent->hasRobotNode(input.frameName))
{
@@ -452,7 +487,7 @@ namespace armarx
// Find object pose.
- objpose::ObjectPose* currentObjectPose = findObjectPose(input.providerName, objectID);
+ objpose::ObjectPose* currentObjectPose = data.findObjectPose(input.providerName, objectID);
if (!currentObjectPose)
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
@@ -470,20 +505,20 @@ namespace armarx
}
else
{
- RobotState::synchronizeLocalClone(robot);
+ RobotState::synchronizeLocalClone(data.robot);
armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
if (frameName == armarx::GlobalFrame)
{
- info.poseInFrame = framed.toGlobalEigen(robot);
+ info.poseInFrame = framed.toGlobalEigen(data.robot);
}
else
{
- framed.changeFrame(robot, info.frameName);
+ framed.changeFrame(data.robot, info.frameName);
info.poseInFrame = framed.toEigen();
}
}
- attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
+ data.attachments[std::make_pair(currentObjectPose->providerName, objectID)] = info;
ARMARX_INFO << "Attached object " << objectID << " by provider '" << currentObjectPose->providerName << "' "
<< "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
@@ -509,7 +544,7 @@ namespace armarx
std::scoped_lock lock(dataMutex);
// Remove from latest pose (if it was cached).
- objpose::ObjectPose* objectPose = findObjectPose(input.providerName, objectID);
+ objpose::ObjectPose* objectPose = data.findObjectPose(input.providerName, objectID);
if (objectPose)
{
objectPose->attachment = std::nullopt;
@@ -522,23 +557,23 @@ namespace armarx
// Remove from attachment map.
if (input.providerName.size() > 0)
{
- auto it = attachments.find(std::make_pair(input.providerName, objectID));
- if (it != attachments.end())
+ auto it = data.attachments.find(std::make_pair(input.providerName, objectID));
+ if (it != data.attachments.end())
{
attachment = it->second;
- attachments.erase(it);
+ data.attachments.erase(it);
}
}
else
{
// Search for entry with matching object ID.
- for (auto it = attachments.begin(); it != attachments.end(); ++it)
+ for (auto it = data.attachments.begin(); it != data.attachments.end(); ++it)
{
const ObjectID& id = it->first.second;
if (id == objectID)
{
attachment = it->second;
- attachments.erase(it);
+ data.attachments.erase(it);
break;
}
}
@@ -570,12 +605,12 @@ namespace armarx
std::scoped_lock lock(dataMutex);
// Clear attachment map.
- size_t num = attachments.size();
- attachments.clear();
+ size_t num = data.attachments.size();
+ data.attachments.clear();
output.numDetached = int(num);
// Remove from poses (if it was cached).
- for (auto& [prov, poses] : objectPoses)
+ for (auto& [prov, poses] : data.objectPoses)
{
for (auto& pose : poses)
{
@@ -594,7 +629,7 @@ namespace armarx
std::scoped_lock lock(dataMutex);
objpose::AgentFramesSeq output;
- std::vector<VirtualRobot::RobotPtr> agents = { robot };
+ std::vector<VirtualRobot::RobotPtr> agents = { data.robot };
for (VirtualRobot::RobotPtr agent : agents)
{
objpose::AgentFrames& frames = output.emplace_back();
@@ -631,200 +666,172 @@ namespace armarx
void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
{
// Initialized to 0 on first access.
- updateCounters[providerName]++;
- if (providers.count(providerName) == 0)
+ data.updateCounters[providerName]++;
+ if (data.providers.count(providerName) == 0)
{
- providers[providerName] = objpose::ProviderInfo();
+ data.providers[providerName] = objpose::ProviderInfo();
}
if (!existsChannel(providerName))
{
offerChannel(providerName, "Channel of provider '" + providerName + "'.");
}
- offerOrUpdateDataField(providerName, "updateCounter", Variant(updateCounters.at(providerName)), "Counter incremented for each update.");
- offerOrUpdateDataField(providerName, "objectCount", Variant(int(objectPoses.at(providerName).size())), "Number of provided object poses.");
+ offerOrUpdateDataField(providerName, "updateCounter", Variant(data.updateCounters.at(providerName)), "Counter incremented for each update.");
+ offerOrUpdateDataField(providerName, "objectCount", Variant(int(data.objectPoses.at(providerName).size())), "Number of provided object poses.");
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, decay);
+ arviz.commit(layers);
}
}
- void ObjectPoseObserver::visProviderUpdate(const std::string& providerName)
+ void ObjectPoseObserver::visualizeRun()
{
- viz::Layer layer = arviz.layer(providerName);
-
- bool synchronized = false;
-
- for (objpose::ObjectPose& objectPose : objectPoses.at(providerName))
+ CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
+ while (visu.updateTask && !visu.updateTask->isStopped())
{
- const armarx::ObjectID id = objectPose.objectID;
- std::string key = id.str();
-
- Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
-
- if (isAttachedCached(objectPose))
{
- if (!synchronized)
- {
- RobotState::synchronizeLocalClone(robot);
- synchronized = true;
- }
- objpose::ObjectPose updated = applyAttachment(objectPose, robot);
- pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
- }
- else
- {
- pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
- }
+ std::scoped_lock lock(visuMutex);
- {
- viz::Object object = viz::Object(key).pose(pose);
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
+ TIMING_START(Visu);
+ if (visu.enabled)
{
- object.fileByObjectFinder(id);
+ std::scoped_lock lock(dataMutex, decayMutex);
+ visualizeCommit(data.objectPoses, data.robot, decay);
}
- if (visu.alpha < 1)
+ if (visu.enabled)
{
- object.overrideColor(simox::Color::white().with_alpha(visu.alpha));
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
+ });
+ }
}
- layer.add(object);
- }
-
- 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));
}
+ cycle.waitForCycleDuration();
}
-
- arviz.commit(layer);
}
- objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
+
+ void ObjectPoseObserver::visualizeCommit(
+ std::map<std::string, objpose::ObjectPoseSeq>& objectPoses, VirtualRobot::RobotPtr robot,
+ const objpose::ObjectPoseDecay& decay)
{
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
+ std::vector<viz::Layer> layers;
+ for (auto& [name, poses] : objectPoses)
+ {
+ visualizeProvider(name, poses, robot, layers, decay);
+ }
+ arviz.commit(layers);
}
- std::optional<simox::OrientedBoxf> ObjectPoseObserver::getObjectOOBB(const ObjectID& id)
+ void ObjectPoseObserver::visualizeProvider(
+ const std::string& providerName, objpose::ObjectPoseSeq& objectPoses,
+ VirtualRobot::RobotPtr robot, std::vector<viz::Layer>& layers,
+ const objpose::ObjectPoseDecay& decay)
{
- return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ viz::Layer& layer = layers.emplace_back(arviz.layer(providerName));
+ for (objpose::ObjectPose& objectPose : objectPoses)
{
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ bool show = decay.enabled and objectPose.confidence < decay.removeObjectsBelowConfidence;
+ if (show)
{
- try
- {
- return objectInfo->loadOOBB();
- }
- catch (const std::ios_base::failure& e)
- {
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- return std::nullopt;
- }
+ visualizeObjectPose(layer, objectPose, robot);
}
- else
- {
- return std::nullopt;
- }
- });
+ }
}
- objpose::ObjectPose* ObjectPoseObserver::findObjectPose(const std::string& providerName, ObjectID& objectID)
+ void ObjectPoseObserver::visualizeObjectPose(viz::Layer& layer, objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot)
{
- objpose::ObjectPose* pose = nullptr;
- if (!providerName.empty())
+ bool synchronized = false;
+
+ const armarx::ObjectID id = objectPose.objectID;
+ std::string key = id.str();
+
+ Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
+
+ // TODO: Get this out of visu function.
+ if (data.isAttachedCached(objectPose))
{
- pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
+ if (!synchronized)
+ {
+ RobotState::synchronizeLocalClone(robot);
+ synchronized = true;
+ }
+ objpose::ObjectPose updated = objectPose.getAttached(robot);
+ pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
}
else
{
- for (auto& [_, poses] : objectPoses)
- {
- pose = findObjectPoseByID(poses, objectID);
- if (pose)
- {
- break;
- }
- }
+ pose = visu.inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
}
- return pose;
- }
- objpose::ObjectPose* ObjectPoseObserver::findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id)
- {
- for (objpose::ObjectPose& pose : objectPoses)
{
- if (pose.objectID == id)
+ 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)
{
- return &pose;
+ 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);
}
- return nullptr;
- }
- bool ObjectPoseObserver::isAttachedCached(objpose::ObjectPose& objectPose) const
- {
- if (!objectPose.attachment)
+ if (visu.oobbs && objectPose.localOOBB)
{
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- objectPose.attachment = it->second;
- }
+ 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));
}
- return bool(objectPose.attachment);
}
- objpose::ObjectPose ObjectPoseObserver::applyAttachment(
- const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr agent)
+ objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
{
- ARMARX_CHECK(objectPose.attachment);
-
- objpose::ObjectPose updated = objectPose;
- const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
-
- ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
-
- VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment.frameName);
- ARMARX_CHECK_NOT_NULL(robotNode);
-
- // Overwrite object pose
- updated.objectPoseRobot = robotNode->getPoseInRootFrame() * attachment.poseInFrame;
- updated.objectPoseGlobal = agent->getGlobalPose() * updated.objectPoseRobot;
-
- updated.robotPose = agent->getGlobalPose();
- updated.robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
-
- return updated;
+ return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
}
void ObjectPoseObserver::toIceWithAttachments(
objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
- objpose::data::ObjectPoseSeq& result, bool& synchronized)
+ objpose::data::ObjectPoseSeq& result, bool& synchronized,
+ const objpose::ObjectPoseDecay& decay)
{
for (objpose::ObjectPose& pose : objectPoses)
{
- if (isAttachedCached(pose))
+ if (decay.enabled && pose.confidence < decay.removeObjectsBelowConfidence)
+ {
+ // Skip.
+ continue;
+ }
+ if (data.isAttachedCached(pose))
{
if (!synchronized) // Synchronize only once.
{
RobotState::synchronizeLocalClone(agent);
synchronized = true;
}
- result.push_back(applyAttachment(pose, agent).toIce());
+ result.push_back(pose.getAttached(agent).toIce());
}
else
{
@@ -833,84 +840,99 @@ namespace armarx
}
}
- void ObjectPoseObserver::RobotHeadMovement::fetchDatafields()
+ void ObjectPoseObserver::decayUpdateRun()
{
- if (kinematicUnitObserver)
+ CycleUtil cycle(static_cast<int>(1000 / decay.updateFrequencyHz));
+ while (decay.updateTask && !decay.updateTask->isStopped())
{
- for (const std::string& jointName : jointNames)
{
- std::string error = "";
- try
+ std::scoped_lock lock(decayMutex);
+
+ TIMING_START(Decay);
+ if (decay.enabled)
{
- DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
- DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
- if (casted)
+ std::scoped_lock lock(dataMutex);
+ IceUtil::Time now = TimeUtil::GetTime();
+ for (auto& [providerName, objectPoses] : data.objectPoses)
{
- jointVelocitiesDatafields.push_back(casted);
+ decay.updateConfidences(objectPoses, now);
}
}
- catch (const InvalidDatafieldException& e)
- {
- error = e.what();
- }
- catch (const InvalidChannelException& e)
- {
- error = e.what();
- }
- if (error.size() > 0)
+ if (decay.enabled)
{
- ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
- << error;
+ TIMING_END_STREAM(Decay, ARMARX_VERBOSE);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Decay Update [ms]", new Variant(Decay.toMilliSecondsDouble()) },
+ });
+ }
}
}
- }
- else
- {
- ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
+
+ cycle.waitForCycleDuration();
}
}
- bool ObjectPoseObserver::RobotHeadMovement::isMoving() const
+
+ objpose::ObjectPose* ObjectPoseObserver::Data::findObjectPose(const std::string& providerName, ObjectID& objectID)
{
- for (DatafieldRefPtr df : jointVelocitiesDatafields)
+ objpose::ObjectPose* pose = nullptr;
+ if (!providerName.empty())
+ {
+ pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
+ }
+ else
{
- if (df)
+ for (auto& [_, poses] : objectPoses)
{
- float jointVelocity = df->getFloat();
- // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
- if (std::abs(jointVelocity) > maxJointVelocity)
+ pose = findObjectPoseByID(poses, objectID);
+ if (pose)
{
- return true;
+ break;
}
}
}
- return false;
+ return pose;
}
- void ObjectPoseObserver::RobotHeadMovement::movementStarts(long discardIntervalMs)
- {
- return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
- }
- void ObjectPoseObserver::RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
+ std::optional<simox::OrientedBoxf> ObjectPoseObserver::Data::getObjectOOBB(const ObjectID& id)
{
- discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
- }
- void ObjectPoseObserver::RobotHeadMovement::movementStops(long discardIntervalMs)
- {
- return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
+ return oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ {
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ try
+ {
+ return objectInfo->loadOOBB();
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ // Give up - no OOBB information.
+ ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
+ return std::nullopt;
+ }
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ });
}
- void ObjectPoseObserver::RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
+
+ bool ObjectPoseObserver::Data::isAttachedCached(objpose::ObjectPose& objectPose) const
{
- if (discardInterval.toMilliSeconds() < 0)
- {
- // Stop discarding.
- discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
- }
- else
+ if (!objectPose.attachment)
{
- // Basically the same as starting.
- discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
+ if (it != attachments.end())
+ {
+ objectPose.attachment = it->second;
+ }
}
+ return bool(objectPose.attachment);
}
@@ -924,6 +946,7 @@ namespace armarx
tab.visu.inGlobalFrame.setValue(visu.inGlobalFrame);
tab.visu.alpha.setRange(0, 1.0);
tab.visu.alpha.setValue(visu.alpha);
+ tab.visu.alphaByConfidence.setValue(visu.alphaByConfidence);
tab.visu.oobbs.setValue(visu.oobbs);
tab.visu.objectFrames.setValue(visu.objectFrames);
{
@@ -942,6 +965,8 @@ namespace armarx
row++;
grid.add(Label("Alpha"), {row, 0}).add(tab.visu.alpha, {row, 1}, {1, 3});
row++;
+ grid.add(Label("Alpha By Confidence"), {row, 0}).add(tab.visu.alphaByConfidence, {row, 1});
+ row++;
grid.add(Label("OOBB"), {row, 0}).add(tab.visu.oobbs, {row, 1});
row++;
grid.add(Label("Object Frames"), {row, 0}).add(tab.visu.objectFrames, {row, 1});
@@ -981,7 +1006,9 @@ namespace armarx
robotHeadGroup.addChild(grid);
}
- VBoxLayout root = {visuGroup, robotHeadGroup, VSpacer()};
+ tab.decay.setup(this->decay);
+
+ VBoxLayout root = {visuGroup, tab.decay.group, robotHeadGroup, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
@@ -989,7 +1016,7 @@ namespace armarx
{
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
{
- std::scoped_lock lock(dataMutex);
+ std::scoped_lock lock(visuMutex);
visu.enabled = tab.visu.enabled.getValue();
visu.inGlobalFrame = tab.visu.inGlobalFrame.getValue();
@@ -1000,10 +1027,35 @@ namespace armarx
}
{
std::scoped_lock lock(robotHeadMutex);
+
robotHead.checkHeadVelocity = tab.robotHead.checkHeadVelocity.getValue();
robotHead.maxJointVelocity = tab.robotHead.maxJointVelocity.getValue();
robotHead.discardIntervalAfterMoveMS = tab.robotHead.discardIntervalAfterMoveMS.getValue();
}
+ {
+ std::scoped_lock lock(decayMutex);
+ tab.decay.update(this->decay);
+ }
+ }
+
+ 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).");
+ defs->optional(oobbs, prefix + "oobbs", "Enable showing oriented bounding boxes.");
+ defs->optional(objectFrames, prefix + "objectFrames", "Enable showing object frames.");
+ defs->optional(objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
+ }
+
+ void ObjectPoseObserver::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
+ defs->optional(offset, prefix + "offset", "Offset for the node to be calibrated.");
}
+
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 5022afbf2c92b7cba7a7c2455dbe26d0e3c1f88f..9b1b93cbf8da428369e95aa57a09f332ed537350 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -27,6 +27,7 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -39,8 +40,10 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
+#include <RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h>
+#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
+#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
namespace armarx
@@ -142,87 +145,93 @@ 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);
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+ void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
+ objpose::data::ObjectPoseSeq& result, bool& synchronized,
+ const objpose::ObjectPoseDecay& decay);
+ void decayUpdateRun();
- objpose::ObjectPose* findObjectPose(const std::string& providerName, ObjectID& objectID);
- static objpose::ObjectPose* findObjectPoseByID(objpose::ObjectPoseSeq& objectPoses, const ObjectID& id);
+ // Visualization
- bool isAttachedCached(objpose::ObjectPose& objectPose) const;
- static objpose::ObjectPose applyAttachment(const objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot);
+ void visualizeRun();
- void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
- objpose::data::ObjectPoseSeq& result, bool& synchronized);
+ void visualizeCommit(std::map<std::string, objpose::ObjectPoseSeq>& objectPoses, VirtualRobot::RobotPtr robot,
+ const objpose::ObjectPoseDecay& decay);
+ void visualizeProvider(const std::string& providerName, objpose::ObjectPoseSeq& objectPoses,
+ VirtualRobot::RobotPtr robot, std::vector<viz::Layer>& layers,
+ const objpose::ObjectPoseDecay& decay);
+ void visualizeObjectPose(viz::Layer& layer, objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot);
- private:
- std::mutex dataMutex;
+ private:
- VirtualRobot::RobotPtr robot;
DebugObserverInterfacePrx debugObserver;
+ class Data : public armarx::Logging
+ {
+ public:
+ VirtualRobot::RobotPtr robot;
- objpose::ProviderInfoMap providers;
+ objpose::ProviderInfoMap providers;
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
- std::map<std::string, int> updateCounters;
+ std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
+ std::map<std::string, int> updateCounters;
- std::map<std::pair<std::string, ObjectID>, objpose::ObjectAttachmentInfo> attachments;
+ std::map<std::pair<std::string, ObjectID>, objpose::ObjectAttachmentInfo> attachments;
- ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+ ObjectFinder objectFinder;
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
- class RobotHeadMovement : public armarx::Logging
- {
public:
+ objpose::ObjectPose* findObjectPose(const std::string& providerName, ObjectID& objectID);
+ std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
- bool checkHeadVelocity = true;
-
- std::string jointVelocitiesChannelName = "jointvelocities";
- std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
- float maxJointVelocity = 0.05f;
- int discardIntervalAfterMoveMS = 100;
+ bool isAttachedCached(objpose::ObjectPose& objectPose) const;
+ };
+ Data data;
+ std::mutex dataMutex;
- KinematicUnitObserverInterfacePrx kinematicUnitObserver;
- std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
- IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
- DebugObserverInterfacePrx debugObserver;
+ objpose::ObjectPoseDecay decay;
+ std::mutex decayMutex;
- void fetchDatafields();
- bool isMoving() const;
- void movementStarts(long discardIntervalMs);
- void movementStarts(IceUtil::Time discardInterval);
- void movementStops(long discardIntervalMs);
- void movementStops(IceUtil::Time discardInterval);
- };
- RobotHeadMovement robotHead;
+ objpose::RobotHeadMovement robotHead;
std::mutex robotHeadMutex;
+
struct Visu
{
bool enabled = false;
+ float frequencyHz = 10;
+
bool inGlobalFrame = true;
float alpha = 1.0;
+ bool alphaByConfidence = false;
bool oobbs = false;
bool objectFrames = false;
float objectFramesScale = 1.0;
+
+ SimpleRunningTask<>::pointer_type updateTask;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
};
Visu visu;
+ std::mutex visuMutex;
struct Calibration
{
std::string robotNode = "Neck_2_Pitch";
float offset = 0.0f;
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
};
Calibration calibration;
@@ -232,8 +241,10 @@ namespace armarx
struct Visu
{
RemoteGui::Client::CheckBox enabled;
+
RemoteGui::Client::CheckBox inGlobalFrame;
RemoteGui::Client::FloatSlider alpha;
+ RemoteGui::Client::CheckBox alphaByConfidence;
RemoteGui::Client::CheckBox oobbs;
RemoteGui::Client::CheckBox objectFrames;
RemoteGui::Client::FloatSpinBox objectFramesScale;
@@ -247,6 +258,8 @@ namespace armarx
RemoteGui::Client::IntSpinBox discardIntervalAfterMoveMS;
};
RobotHead robotHead;
+
+ objpose::ObjectPoseDecay::RemoteGui decay;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3eab8c7ee5395da08f7dd920e67ced953c3dab76
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
@@ -0,0 +1,126 @@
+#include "ObjectPoseDecay.h"
+
+#include <SimoxUtility/math/scale_value.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+
+namespace armarx::objpose
+{
+
+ void ObjectPoseDecay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(enabled, prefix + "enabled",
+ "If true, object poses decay over time when not localized anymore.");
+ defs->optional(delaySeconds, prefix + "delaySeconds",
+ "Duration after latest localization before decay starts.");
+ defs->optional(durationSeconds, prefix + "durationSeconds",
+ "How long to reach minimal confidence.");
+ defs->optional(maxConfidence, prefix + "maxConfidence",
+ "Confidence when decay starts.");
+ defs->optional(minConfidence, prefix + "minConfidence",
+ "Confidence after decay duration.");
+ defs->optional(removeObjectsBelowConfidence, prefix + "removeObjectsBelowConfidence",
+ "Remove objects whose confidence is lower than this value.");
+ }
+
+ void ObjectPoseDecay::updateConfidence(ObjectPose& pose, IceUtil::Time now)
+ {
+ float confidence = calculateConfidence(pose.timestamp, now);
+ pose.confidence = confidence;
+ }
+
+ void ObjectPoseDecay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ {
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ updateConfidence(pose, now);
+ }
+ }
+
+ float ObjectPoseDecay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now)
+ {
+ float duration = (now - localization).toSeconds();
+ if (duration < delaySeconds)
+ {
+ return maxConfidence;
+ }
+ else if (duration > delaySeconds + this->durationSeconds)
+ {
+ return minConfidence;
+ }
+ else
+ {
+ return simox::math::scale_value_from_to(
+ duration,
+ delaySeconds, delaySeconds + this->durationSeconds,
+ maxConfidence, minConfidence);
+ }
+ }
+
+
+
+ void ObjectPoseDecay::RemoteGui::setup(const ObjectPoseDecay& decay)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ enabled.setValue(decay.enabled);
+ {
+ float max = 1e6;
+ delaySeconds.setRange(0, max);
+ delaySeconds.setDecimals(2);
+ delaySeconds.setSteps(int(10 * max)); // = 0.1 steps
+ delaySeconds.setValue(decay.delaySeconds);
+ }
+ {
+ float max = 1e6;
+ durationSeconds.setRange(0, max);
+ durationSeconds.setDecimals(2);
+ durationSeconds.setSteps(int(10 * max)); // = 0.1 steps
+ durationSeconds.setValue(decay.durationSeconds);
+ }
+ {
+ maxConfidence.setRange(0, 1);
+ maxConfidence.setSteps(20);
+ maxConfidence.setValue(decay.maxConfidence);
+ }
+ {
+ minConfidence.setRange(0, 1);
+ minConfidence.setSteps(20);
+ minConfidence.setValue(decay.minConfidence);
+ }
+ {
+ removeObjectsBelowConfidence.setRange(0, 1);
+ removeObjectsBelowConfidence.setSteps(20);
+ removeObjectsBelowConfidence.setValue(decay.removeObjectsBelowConfidence);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ row++;
+ grid.add(Label("Delay [sec]"), {row, 0}).add(delaySeconds, {row, 1});
+ row++;
+ grid.add(Label("Duration [sec]"), {row, 0}).add(durationSeconds, {row, 1});
+ row++;
+ grid.add(Label("Max Confidence"), {row, 0}).add(maxConfidence, {row, 1});
+ row++;
+ grid.add(Label("Min Confidence"), {row, 0}).add(minConfidence, {row, 1});
+ row++;
+ grid.add(Label("Remove Objects with Confidence < "), {row, 0}).add(removeObjectsBelowConfidence, {row, 1});
+ row++;
+
+ group.setLabel("Decay");
+ group.addChild(grid);
+ }
+
+ void ObjectPoseDecay::RemoteGui::update(ObjectPoseDecay& decay)
+ {
+ decay.enabled = enabled.getValue();
+ decay.delaySeconds = delaySeconds.getValue();
+ decay.durationSeconds = durationSeconds.getValue();
+ decay.maxConfidence = maxConfidence.getValue();
+ decay.minConfidence = minConfidence.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
new file mode 100644
index 0000000000000000000000000000000000000000..22a4940da95f94edbe5abc35dd4f14efc36d20a3
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
@@ -0,0 +1,72 @@
+#pragma once
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+
+namespace armarx::objpose
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class ObjectPoseDecay : public armarx::Logging
+ {
+ public:
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay.");
+
+ void updateConfidence(ObjectPose& pose, IceUtil::Time now);
+ void updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now);
+
+ private:
+
+ float calculateConfidence(IceUtil::Time localization, IceUtil::Time now);
+
+
+ public:
+
+ bool enabled = false;
+
+ /// Duration after latest localization before decay starts.
+ float delaySeconds = 5.0;
+ /// How long to reach minConfidence.
+ float durationSeconds = 20.0;
+
+ float maxConfidence = 1.0;
+ float minConfidence = 0.0f;
+
+ float removeObjectsBelowConfidence = 0.1f;
+
+ /// Frequency of updating the current decay.
+ float updateFrequencyHz = 10.0;
+ SimpleRunningTask<>::pointer_type updateTask;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::CheckBox enabled;
+
+ armarx::RemoteGui::Client::FloatSpinBox delaySeconds;
+ armarx::RemoteGui::Client::FloatSpinBox durationSeconds;
+ armarx::RemoteGui::Client::FloatSlider maxConfidence;
+ armarx::RemoteGui::Client::FloatSlider minConfidence;
+
+ armarx::RemoteGui::Client::FloatSlider removeObjectsBelowConfidence;
+
+ void setup(const ObjectPoseDecay& decay);
+ void update(ObjectPoseDecay& decay);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b9ac721ca1ade44a8667b28667520093169d0770
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
@@ -0,0 +1,100 @@
+#include "RobotHeadMovement.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+
+namespace armarx::objpose
+{
+
+ void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ defs->optional(checkHeadVelocity, prefix + "checkHeadVelocity",
+ "If true, check whether the head is moving and discard updates in the meantime.");
+ defs->optional(maxJointVelocity, prefix + "maxJointVelocity",
+ "If a head joint's velocity is higher, the head is considered moving.");
+ defs->optional(discardIntervalAfterMoveMS, prefix + "discardIntervalAfterMoveMS",
+ "For how long new updates are ignored after moving the head.");
+ }
+
+ void RobotHeadMovement::fetchDatafields()
+ {
+ if (kinematicUnitObserver)
+ {
+ for (const std::string& jointName : jointNames)
+ {
+ std::string error = "";
+ try
+ {
+ DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
+ DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
+ if (casted)
+ {
+ jointVelocitiesDatafields.push_back(casted);
+ }
+ }
+ catch (const InvalidDatafieldException& e)
+ {
+ error = e.what();
+ }
+ catch (const InvalidChannelException& e)
+ {
+ error = e.what();
+ }
+ if (error.size() > 0)
+ {
+ ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
+ << error;
+ }
+ }
+ }
+ else
+ {
+ ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
+ }
+ }
+
+ bool RobotHeadMovement::isMoving() const
+ {
+ for (DatafieldRefPtr df : jointVelocitiesDatafields)
+ {
+ if (df)
+ {
+ float jointVelocity = df->getFloat();
+ // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
+ if (std::abs(jointVelocity) > maxJointVelocity)
+ {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ void RobotHeadMovement::movementStarts(long discardIntervalMs)
+ {
+ return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
+ {
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ void RobotHeadMovement::movementStops(long discardIntervalMs)
+ {
+ return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
+ {
+ if (discardInterval.toMilliSeconds() < 0)
+ {
+ // Stop discarding.
+ discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
+ }
+ else
+ {
+ // Basically the same as starting.
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd06a1b2d83831642b4abcfe6fc16784c105c554
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
+
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+
+
+namespace armarx
+{
+ class PropertyDefinitionContainer;
+ using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
+}
+
+namespace armarx::objpose
+{
+ class RobotHeadMovement : public armarx::Logging
+ {
+ public:
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "head.");
+
+ void fetchDatafields();
+ bool isMoving() const;
+
+ void movementStarts(long discardIntervalMs);
+ void movementStarts(IceUtil::Time discardInterval);
+ void movementStops(long discardIntervalMs);
+ void movementStops(IceUtil::Time discardInterval);
+
+
+ public:
+
+ bool checkHeadVelocity = true;
+
+ std::string jointVelocitiesChannelName = "jointvelocities";
+ std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
+ float maxJointVelocity = 0.05f;
+ int discardIntervalAfterMoveMS = 100;
+
+ KinematicUnitObserverInterfacePrx kinematicUnitObserver;
+ std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
+ IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
+
+ DebugObserverInterfacePrx debugObserver;
+
+ };
+
+
+
+
+}
diff --git a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
index 0d9a2b0be8a1f938308ea52d42ce087d55386bd5..80404797fa4176aff345a9acc6946024ff04574b 100644
--- a/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ObjectPoseGui/ObjectPoseGuiWidgetController.cpp
@@ -168,7 +168,8 @@ namespace armarx
{
if (!objectPoseObserver)
{
- ARMARX_WARNING << "No object pose observer.";
+ // Probably disconnected.
+ ARMARX_VERBOSE << "No object pose observer.";
return;
}
@@ -263,7 +264,8 @@ namespace armarx
{
if (!objectPoseObserver)
{
- ARMARX_WARNING << "No object pose observer.";
+ // Probably disconnected.
+ ARMARX_VERBOSE << "No object pose observer.";
return;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index ddecbed52ac6a14640dc6546bbe8398ec286a5fc..02b04bf6f3961074891472deed1ade625850b467 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -119,6 +119,28 @@ namespace armarx::objpose
return std::nullopt;
}
+ objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent)
+ {
+ ARMARX_CHECK(attachment);
+
+ objpose::ObjectPose updated = *this;
+ const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
+
+ ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
+
+ VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment.frameName);
+ ARMARX_CHECK_NOT_NULL(robotNode);
+
+ // Overwrite object pose
+ updated.objectPoseRobot = robotNode->getPoseInRootFrame() * attachment.poseInFrame;
+ updated.objectPoseGlobal = agent->getGlobalPose() * updated.objectPoseRobot;
+
+ updated.robotPose = agent->getGlobalPose();
+ updated.robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
+
+ return updated;
+ }
+
}
namespace armarx
@@ -239,5 +261,31 @@ namespace armarx
return ice;
}
+
+ objpose::ObjectPose* objpose::findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id)
+ {
+ for (objpose::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
+
+ const objpose::ObjectPose* objpose::findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id)
+ {
+ for (const objpose::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
}
+
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index cdfd1cfdece0570cf77162549de5da7d443c0e26..4f107d549e87d1b523d032349e1c1743071df4b5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -73,6 +73,8 @@ namespace armarx::objpose
/// Get the OOBB in the global frame (according to `objectPoseGlobal`).
std::optional<simox::OrientedBoxf> oobbGlobal() const;
+ /// Get `*this` with applied attachment.
+ objpose::ObjectPose getAttached(VirtualRobot::RobotPtr agent);
};
using ObjectPoseSeq = std::vector<ObjectPose>;
@@ -98,4 +100,17 @@ namespace armarx::objpose
void toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses);
data::ObjectPoseSeq toIce(const ObjectPoseSeq& poses);
+
+ // Operations on ObjectPoseSeq
+
+ /**
+ * @brief Find an object pose by the object ID. Return nullptr if not found.
+ * @param objectPoses The object poses.
+ * @param id The object ID.
+ * @return A pointer to the (first) object pose with objectID == id, or nullptr if none is found.
+ */
+ ObjectPose* findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id);
+ const ObjectPose* findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id);
+
+
}