diff --git a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx b/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
index 197fe23ade0643213c3b6e9ca444098bdc5cb8ed..1aa5238bfbb09c201635d6504393e499e7e21593 100644
--- a/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
+++ b/scenarios/ObjectPoseObserverExample/ObjectPoseObserverExample.scx
@@ -5,5 +5,7 @@
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg b/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
index d14b730f3ab022ddb4bfc9bc0fea2038d5cd03ab..279193cc2f6feef27a705bf30dbae8451f7bfe14 100644
--- a/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
+++ b/scenarios/ObjectPoseObserverExample/config/ObjectPoseObserver.cfg
@@ -118,6 +118,14 @@
# ArmarX.ObjectPoseObserver.EnableProfiling = false
+# ArmarX.ObjectPoseObserver.KinematicUnitObserverName: Name of the kinematic unit observer.
+# Attributes:
+# - Default: KinematicUnitObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.KinematicUnitObserverName = KinematicUnitObserver
+
+
# ArmarX.ObjectPoseObserver.MaxHistoryRecordFrequency: The Observer history is written with this maximum frequency. Everything faster is being skipped.
# Attributes:
# - Default: 50
@@ -175,7 +183,7 @@
# ArmarX.ObjectPoseObserver.RemoteStateComponentName = RobotStateComponent
-# ArmarX.ObjectPoseObserver.calibration.offset: Offset for the node to be calibrated
+# ArmarX.ObjectPoseObserver.calibration.offset: Offset for the node to be calibrated.
# Attributes:
# - Default: 0
# - Case sensitivity: yes
@@ -183,7 +191,7 @@
# ArmarX.ObjectPoseObserver.calibration.offset = 0
-# ArmarX.ObjectPoseObserver.calibration.robotNode: Robot node which can be calibrated
+# ArmarX.ObjectPoseObserver.calibration.robotNode: Robot node which can be calibrated.
# Attributes:
# - Default: Neck_2_Pitch
# - Case sensitivity: yes
@@ -191,6 +199,88 @@
# ArmarX.ObjectPoseObserver.calibration.robotNode = Neck_2_Pitch
+# ArmarX.ObjectPoseObserver.decay.delaySeconds: Duration after latest localization before decay starts.
+# Attributes:
+# - Default: 5
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.decay.delaySeconds = 5
+
+
+# ArmarX.ObjectPoseObserver.decay.durationSeconds: How long to reach minimal confidence.
+# Attributes:
+# - Default: 20
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.decay.durationSeconds = 20
+
+
+# ArmarX.ObjectPoseObserver.decay.enabled: If true, object poses decay over time when not localized anymore.
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseObserver.decay.enabled = false
+
+
+# ArmarX.ObjectPoseObserver.decay.maxConfidence: Confidence when decay starts.
+# Attributes:
+# - Default: 1
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.decay.maxConfidence = 1
+
+
+# ArmarX.ObjectPoseObserver.decay.minConfidence: Confidence after decay duration.
+# Attributes:
+# - Default: 0
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.decay.minConfidence = 0
+
+
+# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence: Remove objects whose confidence is lower than this value.
+# Attributes:
+# - Default: 0.100000001
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.decay.removeObjectsBelowConfidence = 0.100000001
+
+
+# ArmarX.ObjectPoseObserver.head.checkHeadVelocity: If true, check whether the head is moving and discard updates in the meantime.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseObserver.head.checkHeadVelocity = true
+
+
+# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS: For how long new updates are ignored after moving the head.
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.head.discardIntervalAfterMoveMS = 100
+
+
+# ArmarX.ObjectPoseObserver.head.maxJointVelocity: If a head joint's velocity is higher, the head is considered moving.
+# Attributes:
+# - Default: 0.0500000007
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.head.maxJointVelocity = 0.0500000007
+
+
+# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.tpc.pub.DebugObserver = DebugObserver
+
+
# ArmarX.ObjectPoseObserver.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
# Attributes:
# - Default: 1
@@ -199,6 +289,15 @@
# ArmarX.ObjectPoseObserver.visu.alpha = 1
+# ArmarX.ObjectPoseObserver.visu.alphaByConfidence: If true, use the pose confidence as alpha (if < 1.0).
+# Attributes:
+# - Default: false
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectPoseObserver.visu.alphaByConfidence = false
+
+
# ArmarX.ObjectPoseObserver.visu.enabled: Enable or disable visualization of objects.
# Attributes:
# - Default: false
@@ -208,6 +307,14 @@
ArmarX.ObjectPoseObserver.visu.enabled = true
+# ArmarX.ObjectPoseObserver.visu.frequenzyHz: Frequency of visualization.
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectPoseObserver.visu.frequenzyHz = 25
+
+
# ArmarX.ObjectPoseObserver.visu.inGlobalFrame: If true, show global poses. If false, show poses in robot frame.
# Attributes:
# - Default: true
diff --git a/source/RobotAPI/components/ArViz/Client/Client.h b/source/RobotAPI/components/ArViz/Client/Client.h
index 658a1c2fd5475175b36a1412a933e5439ee6e7b0..cc8e7289a57f4302b65fae48574d04f8931d4bc5 100644
--- a/source/RobotAPI/components/ArViz/Client/Client.h
+++ b/source/RobotAPI/components/ArViz/Client/Client.h
@@ -51,13 +51,13 @@ namespace armarx::viz
// ////////////////////////////////////////////////////////////////// //
//layer
- Layer layer(std::string const& name)
+ Layer layer(std::string const& name) const
{
return Layer(componentName, name);
}
template<class...Ts>
- Layer layerContaining(std::string const& name, Ts&& ...elems)
+ Layer layerContaining(std::string const& name, Ts&& ...elems) const
{
auto l = layer(name);
l.add(std::forward<Ts>(elems)...);
diff --git a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
index fd526dd81804ccfa68fa288d576831b58e1e5bce..74da78adf1af125b3ccab6669b1b47a2a9d0d9e8 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
+++ b/source/RobotAPI/components/ObjectPoseObserver/CMakeLists.txt
@@ -12,8 +12,11 @@ set(COMPONENT_LIBS
set(SOURCES
ObjectPoseObserver.cpp
- detail/ObjectPoseDecay.cpp
+
+ detail/Data.cpp
+ detail/Decay.cpp
detail/RobotHeadMovement.cpp
+ detail/Visu.cpp
plugins/ObjectPoseProviderPlugin.cpp
plugins/ObjectPoseClientPlugin.cpp
@@ -21,8 +24,11 @@ set(SOURCES
)
set(HEADERS
ObjectPoseObserver.h
- detail/ObjectPoseDecay.h
+
+ detail/Data.h
+ detail/Decay.h
detail/RobotHeadMovement.h
+ detail/Visu.h
plugins/ObjectPoseProviderPlugin.h
plugins/ObjectPoseClientPlugin.h
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 2bc0726ed9a169e73fc91ceba560a4c09313ad2b..62a56ebf248e9601ea4e281f84a0d043bef68a61 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -22,21 +22,18 @@
#include "ObjectPoseObserver.h"
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/meta/EnumNames.hpp>
-#include <SimoxUtility/shapes/OrientedBox.h>
-
-#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/RobotConfig.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.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>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotConfig.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/meta/EnumNames.hpp>
namespace armarx
@@ -55,10 +52,10 @@ namespace armarx
defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
defs->topic(debugObserver);
- visu.defineProperties(defs, "visu.");
calibration.defineProperties(defs, "calibration.");
+ data.defineProperties(defs);
robotHead.defineProperties(defs, "head.");
- decay.defineProperties(defs, "decay.");
+ visu.defineProperties(defs, "visu.");
return defs;
}
@@ -71,7 +68,9 @@ namespace armarx
void ObjectPoseObserver::onInitObserver()
{
data.setTag(getName());
+ data.decay.setTag(getName());
robotHead.setTag(getName());
+ visu.setTag(getName());
usingTopicFromProperty("ObjectPoseTopicName");
}
@@ -84,19 +83,13 @@ namespace armarx
{
data.robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
}
+ data.robotStateComponent = getRobotStateComponent();
getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
robotHead.debugObserver = debugObserver;
robotHead.fetchDatafields();
- if (!decay.updateTask)
- {
- decay.updateTask = new SimpleRunningTask<>([this]()
- {
- this->decayUpdateRun();
- });
- decay.updateTask->start();
- }
+ visu.arviz = arviz;
if (!visu.updateTask)
{
visu.updateTask = new SimpleRunningTask<>([this]()
@@ -128,7 +121,44 @@ namespace armarx
void ObjectPoseObserver::reportObjectPoses(
const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
{
- ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
+ ARMARX_VERBOSE << "Received object " << providedPoses.size() << " poses from provider '" << providerName << "'.";
+ updateObjectPoses(providerName, providedPoses);
+ }
+
+
+ void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ {
+ if (!info.proxy)
+ {
+ ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
+ << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
+ return;
+ }
+ {
+ std::scoped_lock lock(dataMutex);
+ std::stringstream ss;
+ for (const auto& id : info.supportedObjects)
+ {
+ ss << "- " << id << "\n";
+ }
+ ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
+ << "Supported objects: \n" << ss.str();
+ data.providers[providerName] = info;
+ }
+
+ if (!existsChannel(providerName))
+ {
+ 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.");
+ }
+
+
+ void ObjectPoseObserver::updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses)
+ {
TIMING_START(ReportObjectPoses);
std::optional<IceUtil::Time> discardUpdatesUntil;
@@ -156,20 +186,20 @@ namespace armarx
}
if (debugObserver)
{
- StringVariantBaseMap map =
- {
- { "Discarding All Updates", new Variant(discardAll ? 1.f : 0.f) },
- };
+ StringVariantBaseMap map;
+ map["Discarding All Updates"] = new Variant(discardAll ? 1.f : 0.f);
if (discardAll)
{
map["Proportion Updated Poses"] = new Variant(0.f);
}
debugObserver->setDebugChannel(getName(), map);
}
+
if (discardAll)
{
return;
}
+
{
std::scoped_lock lock(dataMutex);
RobotState::synchronizeLocalClone(data.robot);
@@ -269,58 +299,35 @@ namespace armarx
}
- void ObjectPoseObserver::updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info)
+ void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
{
- if (!info.proxy)
- {
- ARMARX_WARNING << "Received availability signal by provider '" << providerName << "' "
- << "with invalid provider proxy.\nIgnoring provider '" << providerName << "'.";
- return;
- }
+ // Initialized to 0 on first access.
+ if (data.providers.count(providerName) == 0)
{
- std::scoped_lock lock(dataMutex);
- std::stringstream ss;
- for (const auto& id : info.supportedObjects)
- {
- ss << "- " << id << "\n";
- }
- ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- << "Supported objects: \n" << ss.str();
- data.providers[providerName] = info;
-
- if (data.updateCounters.count(providerName) == 0)
- {
- data.updateCounters[providerName] = 0;
- }
+ data.providers[providerName] = objpose::ProviderInfo();
}
if (!existsChannel(providerName))
{
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, "objectCount", Variant(int(data.objectPoses.at(providerName).size())), "Number of provided object poses.");
}
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPoses(const Ice::Current&)
{
+ TIMING_START(GetObjectPoses);
+
TIMING_START(GetObjectPosesLock);
- std::scoped_lock lock(dataMutex, decayMutex);
+ std::scoped_lock lock(dataMutex);
TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
- TIMING_START(GetObjectPoses);
-
- bool synchronized = false;
- objpose::data::ObjectPoseSeq result;
- for (auto& [name, poses] : data.objectPoses)
- {
- toIceWithAttachments(poses, data.robot, result, synchronized, decay);
- }
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPoses(now));
TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
if (debugObserver)
{
debugObserver->setDebugChannel(getName(),
@@ -335,11 +342,26 @@ namespace armarx
objpose::data::ObjectPoseSeq ObjectPoseObserver::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
{
- std::scoped_lock lock(dataMutex, decayMutex);
+ TIMING_START(GetObjectPoses);
+
+ TIMING_START(GetObjectPosesLock);
+ std::scoped_lock lock(dataMutex);
+ TIMING_END_STREAM(GetObjectPosesLock, ARMARX_VERBOSE);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ const objpose::data::ObjectPoseSeq result = objpose::toIce(data.getObjectPosesByProvider(providerName, now));
+
+ TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
+
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "getObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
+ { "getObjectPosesByProvider() lock [ms]", new Variant(GetObjectPosesLock.toMilliSecondsDouble()) }
+ });
+ }
- bool synchronized = false;
- objpose::data::ObjectPoseSeq result;
- toIceWithAttachments(data.objectPoses.at(providerName), data.robot, result, synchronized, decay);
return result;
}
@@ -434,193 +456,34 @@ namespace armarx
objpose::ProviderInfo ObjectPoseObserver::getProviderInfo(const std::string& providerName, const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
- try
- {
- 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, _] : data.providers)
- {
- ss << "- '" << name << "'\n";
- }
- throw std::out_of_range(ss.str());
- }
+ return data.getProviderInfo(providerName);
}
bool ObjectPoseObserver::hasProvider(const std::string& providerName, const Ice::Current&)
{
+ std::scoped_lock lock(dataMutex);
return data.providers.count(providerName) > 0;
}
-
objpose::AttachObjectToRobotNodeOutput ObjectPoseObserver::attachObjectToRobotNode(
const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
{
std::scoped_lock lock(dataMutex);
-
- objpose::AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
-
- ObjectID objectID = armarx::fromIce(input.objectID);
-
- 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> {data.robot->getName()};
- return output;
- }
- VirtualRobot::RobotPtr agent = data.robot;
-
- if (!agent->hasRobotNode(input.frameName))
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
- return output;
- }
- std::string frameName = input.frameName;
-
-
- // Find object pose provider name can be empty.
- objpose::ObjectPose* currentObjectPose = data.findObjectPose(objectID, input.providerName);
- if (!currentObjectPose)
- {
- ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
- return output;
- }
-
- objpose::ObjectAttachmentInfo info;
- info.agentName = agent->getName();
- info.frameName = frameName;
-
- if (input.poseInFrame)
- {
- info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
- }
- else
- {
- RobotState::synchronizeLocalClone(data.robot);
-
- armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
- if (frameName == armarx::GlobalFrame)
- {
- info.poseInFrame = framed.toGlobalEigen(data.robot);
- }
- else
- {
- framed.changeFrame(data.robot, info.frameName);
- info.poseInFrame = framed.toEigen();
- }
- }
- 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"
- << "Object pose in frame: \n" << info.poseInFrame;
-
- output.success = true;
- output.attachment = new objpose::data::ObjectAttachmentInfo();
- output.attachment->frameName = info.frameName;
- output.attachment->agentName = info.agentName;
- output.attachment->poseInFrame = new Pose(info.poseInFrame);
-
- return output;
+ return data.attachObjectToRobotNode(input);
}
objpose::DetachObjectFromRobotNodeOutput ObjectPoseObserver::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
{
- ObjectID objectID = armarx::fromIce(input.objectID);
- std::string providerName = input.providerName;
-
- std::optional<objpose::ObjectAttachmentInfo> attachment;
- {
- std::scoped_lock lock(dataMutex);
-
- // Remove from latest pose (if it was cached).
- objpose::ObjectPose* objectPose = data.findObjectPose(objectID, input.providerName);
- if (objectPose)
- {
- objectPose->attachment = std::nullopt;
- }
-
- if (providerName.empty() && objectPose)
- {
- providerName = objectPose->providerName;
- }
- // Remove from attachment map.
- if (input.providerName.size() > 0)
- {
- auto it = data.attachments.find(std::make_pair(input.providerName, objectID));
- if (it != data.attachments.end())
- {
- attachment = it->second;
- data.attachments.erase(it);
- }
- }
- else
- {
- // Search for entry with matching object ID.
- for (auto it = data.attachments.begin(); it != data.attachments.end(); ++it)
- {
- const ObjectID& id = it->first.second;
- if (id == objectID)
- {
- attachment = it->second;
- data.attachments.erase(it);
- break;
- }
- }
- }
- }
-
- objpose::DetachObjectFromRobotNodeOutput output;
- output.wasAttached = bool(attachment);
- if (attachment)
- {
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
- }
- else
- {
- ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
- << "from robot node, but it was not attached.";
- }
-
- return output;
+ std::scoped_lock lock(dataMutex);
+ return data.detachObjectFromRobotNode(input);
}
objpose::DetachAllObjectsFromRobotNodesOutput ObjectPoseObserver::detachAllObjectsFromRobotNodes(const Ice::Current&)
{
- objpose::DetachAllObjectsFromRobotNodesOutput output;
- output.numDetached = 0;
-
- {
- std::scoped_lock lock(dataMutex);
-
- // Clear attachment map.
- size_t num = data.attachments.size();
- data.attachments.clear();
- output.numDetached = int(num);
-
- // Remove from poses (if it was cached).
- for (auto& [prov, poses] : data.objectPoses)
- {
- for (auto& pose : poses)
- {
- pose.attachment = std::nullopt;
- }
- }
- }
- ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
-
- return output;
+ std::scoped_lock lock(dataMutex);
+ return data.detachAllObjectsFromRobotNodes();
}
@@ -643,49 +506,7 @@ namespace armarx
ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
{
std::scoped_lock lock(robotHeadMutex);
- objpose::SignalHeadMovementOutput output;
- switch (input.action)
- {
- case objpose::HeadMovementAction::HeadMovementAction_Starting:
- robotHead.movementStarts(input.discardUpdatesIntervalMilliSeconds < 0
- ? robotHead.discardIntervalAfterMoveMS
- : input.discardUpdatesIntervalMilliSeconds);
- break;
- case objpose::HeadMovementAction::HeadMovementAction_Stopping:
- robotHead.movementStops(input.discardUpdatesIntervalMilliSeconds);
- break;
- default:
- ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementaction, input.action);
- break;
- }
- output.discardUpdatesUntilMilliSeconds = robotHead.discardUpdatesUntil.toMilliSeconds();
- return output;
- }
-
-
- void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
- {
- // Initialized to 0 on first access.
- data.updateCounters[providerName]++;
- if (data.providers.count(providerName) == 0)
- {
- data.providers[providerName] = objpose::ProviderInfo();
- }
-
- if (!existsChannel(providerName))
- {
- offerChannel(providerName, "Channel of provider '" + providerName + "'.");
- }
- 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)
- {
- // 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);
- }
+ return robotHead.signalHeadMovement(input);
}
@@ -697,318 +518,58 @@ namespace armarx
{
std::scoped_lock lock(visuMutex);
- TIMING_START(Visu);
- if (visu.enabled)
- {
- std::scoped_lock lock(dataMutex, decayMutex);
- visualizeCommit(data.objectPoses, data.robot, decay);
- }
if (visu.enabled)
{
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
- if (debugObserver)
+ TIMING_START(Visu);
+
+ std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
+ ObjectFinder objectFinder;
+ float minConfidence = -1;
{
- debugObserver->setDebugChannel(getName(),
+ std::scoped_lock lock(dataMutex);
+
+ const IceUtil::Time now = TimeUtil::GetTime();
+ data.updateObjectPoses(now);
+ objectPoses = data.objectPoses;
+ objectFinder = data.objectFinder;
+ if (data.decay.enabled)
{
- { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
+ minConfidence = data.decay.removeObjectsBelowConfidence;
+ }
}
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
- void ObjectPoseObserver::visualizeCommit(
- std::map<std::string, objpose::ObjectPoseSeq>& objectPoses, VirtualRobot::RobotPtr robot,
- const objpose::ObjectPoseDecay& decay)
- {
- std::vector<viz::Layer> layers;
- for (auto& [name, poses] : objectPoses)
- {
- visualizeProvider(name, poses, robot, layers, decay);
- }
- arviz.commit(layers);
- }
-
- void ObjectPoseObserver::visualizeProvider(
- const std::string& providerName, objpose::ObjectPoseSeq& objectPoses,
- VirtualRobot::RobotPtr robot, std::vector<viz::Layer>& layers,
- const objpose::ObjectPoseDecay& decay)
- {
- viz::Layer& layer = layers.emplace_back(arviz.layer(providerName));
- for (objpose::ObjectPose& objectPose : objectPoses)
- {
- bool show = not(decay.enabled and objectPose.confidence < decay.removeObjectsBelowConfidence);
- if (show)
- {
- visualizeObjectPose(layer, objectPose, robot);
- }
- }
- }
-
- void ObjectPoseObserver::visualizeObjectPose(viz::Layer& layer, objpose::ObjectPose& objectPose, VirtualRobot::RobotPtr robot)
- {
- 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))
- {
- if (!synchronized)
- {
- RobotState::synchronizeLocalClone(robot);
- synchronized = true;
- }
- 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))
- {
- 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);
- }
+ const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, minConfidence, objectFinder);
+ arviz.commit(layers);
- 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)
- {
- return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
- }
-
-
- void ObjectPoseObserver::toIceWithAttachments(
- objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
- objpose::data::ObjectPoseSeq& result, bool& synchronized,
- const objpose::ObjectPoseDecay& decay)
- {
- for (objpose::ObjectPose& pose : objectPoses)
- {
- 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(pose.getAttached(agent).toIce());
- }
- else
- {
- result.push_back(pose.toIce());
- }
- }
- }
-
- void ObjectPoseObserver::decayUpdateRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / decay.updateFrequencyHz));
- while (decay.updateTask && !decay.updateTask->isStopped())
- {
- {
- std::scoped_lock lock(decayMutex);
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
- TIMING_START(Decay);
- if (decay.enabled)
- {
- std::scoped_lock lock(dataMutex);
- IceUtil::Time now = TimeUtil::GetTime();
- for (auto& [providerName, objectPoses] : data.objectPoses)
- {
- decay.updateConfidences(objectPoses, now);
- }
- }
- if (decay.enabled)
- {
- TIMING_END_STREAM(Decay, ARMARX_VERBOSE);
if (debugObserver)
{
debugObserver->setDebugChannel(getName(),
{
- { "Decay Update [ms]", new Variant(Decay.toMilliSecondsDouble()) },
+ { "Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
});
}
}
}
-
cycle.waitForCycleDuration();
}
}
- objpose::ObjectPose* ObjectPoseObserver::Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
- {
- objpose::ObjectPose* pose = nullptr;
- if (!providerName.empty())
- {
- pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
- }
- else
- {
- for (auto& [_, poses] : objectPoses)
- {
- pose = findObjectPoseByID(poses, objectID);
- if (pose)
- {
- break;
- }
- }
- }
- return pose;
- }
-
-
- std::optional<simox::OrientedBoxf> ObjectPoseObserver::Data::getObjectOOBB(const ObjectID& id)
- {
- 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;
- }
- });
- }
- bool ObjectPoseObserver::Data::isAttachedCached(objpose::ObjectPose& objectPose) const
+ objpose::ObjectPoseProviderPrx ObjectPoseObserver::getProviderProxy(const std::string& providerName)
{
- if (!objectPose.attachment)
- {
- auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != attachments.end())
- {
- objectPose.attachment = it->second;
- }
- }
- return bool(objectPose.attachment);
+ return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
}
-
void ObjectPoseObserver::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
- GroupBox visuGroup;
- {
- tab.visu.enabled.setValue(visu.enabled);
- 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);
- {
- float max = 10000;
- tab.visu.objectFramesScale.setRange(0, max);
- tab.visu.objectFramesScale.setDecimals(2);
- tab.visu.objectFramesScale.setSteps(int(10 * max));
- tab.visu.objectFramesScale.setValue(visu.objectFramesScale);
- }
-
- GridLayout grid;
- int row = 0;
- grid.add(Label("Enabled"), {row, 0}).add(tab.visu.enabled, {row, 1});
- row++;
- grid.add(Label("Global Frame"), {row, 0}).add(tab.visu.inGlobalFrame, {row, 1});
- 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});
- grid.add(Label("Scale:"), {row, 2}).add(tab.visu.objectFramesScale, {row, 3});
- row++;
-
- visuGroup.setLabel("Visualization");
- visuGroup.addChild(grid);
- }
-
- GroupBox robotHeadGroup;
- {
- tab.robotHead.checkHeadVelocity.setValue(robotHead.checkHeadVelocity);
- {
- float max = 10.0;
- tab.robotHead.maxJointVelocity.setRange(0, max);
- tab.robotHead.maxJointVelocity.setDecimals(3);
- tab.robotHead.maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
- tab.robotHead.maxJointVelocity.setValue(robotHead.maxJointVelocity);
- }
- {
- int max = 10 * 1000;
- tab.robotHead.discardIntervalAfterMoveMS.setRange(0, max);
- tab.robotHead.discardIntervalAfterMoveMS.setValue(robotHead.discardIntervalAfterMoveMS);
- }
-
- GridLayout grid;
- int row = 0;
- grid.add(Label("Check Head Motion"), {row, 0}).add(tab.robotHead.checkHeadVelocity, {row, 1});
- row++;
- grid.add(Label("Max Joint Velocity"), {row, 0}).add(tab.robotHead.maxJointVelocity, {row, 1});
- row++;
- grid.add(Label("Discard Interval after Move [ms]"), {row, 0}).add(tab.robotHead.discardIntervalAfterMoveMS, {row, 1});
- row++;
-
- robotHeadGroup.setLabel("Robot Head Motion");
- robotHeadGroup.addChild(grid);
- }
-
- tab.decay.setup(this->decay);
+ tab.visu.setup(this->visu);
+ tab.decay.setup(this->data.decay);
+ tab.robotHead.setup(this->robotHead);
- VBoxLayout root = {visuGroup, tab.decay.group, robotHeadGroup, VSpacer()};
+ VBoxLayout root = {tab.visu.group, tab.decay.group, tab.robotHead.group, VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
@@ -1017,45 +578,23 @@ namespace armarx
// Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
{
std::scoped_lock lock(visuMutex);
-
- visu.enabled = tab.visu.enabled.getValue();
- visu.inGlobalFrame = tab.visu.inGlobalFrame.getValue();
- visu.alpha = tab.visu.alpha.getValue();
- visu.oobbs = tab.visu.oobbs.getValue();
- visu.objectFrames = tab.visu.objectFrames.getValue();
- visu.objectFramesScale = tab.visu.objectFramesScale.getValue();
+ tab.visu.update(this->visu);
}
{
std::scoped_lock lock(robotHeadMutex);
-
- robotHead.checkHeadVelocity = tab.robotHead.checkHeadVelocity.getValue();
- robotHead.maxJointVelocity = tab.robotHead.maxJointVelocity.getValue();
- robotHead.discardIntervalAfterMoveMS = tab.robotHead.discardIntervalAfterMoveMS.getValue();
+ tab.robotHead.update(this->robotHead);
}
{
- std::scoped_lock lock(decayMutex);
- tab.decay.update(this->decay);
+ std::scoped_lock lock(dataMutex);
+ tab.decay.update(this->data.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 22fa6496f7f4a9db0cdede26939175be00657e62..8fb3369ceaed1f4880037c741bd74f8e2eb5625e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -24,23 +24,17 @@
#include <mutex>
-#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>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
-#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h>
+#include <RobotAPI/components/ObjectPoseObserver/detail/Data.h>
+#include <RobotAPI/components/ObjectPoseObserver/detail/Decay.h>
+#include <RobotAPI/components/ObjectPoseObserver/detail/Visu.h>
#include <RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h>
#define ICE_CURRENT_ARG const Ice::Current& = Ice::emptyCurrent
@@ -79,6 +73,8 @@ namespace armarx
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
{
+ class Data;
+
public:
using RobotState = armarx::RobotStateComponentPluginUser;
@@ -144,88 +140,33 @@ namespace armarx
private:
void updateProviderInfo(const std::string& providerName, const objpose::ProviderInfo& info);
+
+ void updateObjectPoses(const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses);
void handleProviderUpdate(const std::string& providerName);
objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
- void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
- objpose::data::ObjectPoseSeq& result, bool& synchronized,
- const objpose::ObjectPoseDecay& decay);
-
- void decayUpdateRun();
// Visualization
void visualizeRun();
- 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:
DebugObserverInterfacePrx debugObserver;
- class Data : public armarx::Logging
- {
- public:
- VirtualRobot::RobotPtr robot;
-
- objpose::ProviderInfoMap providers;
-
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
- std::map<std::string, int> updateCounters;
-
- 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;
-
-
- public:
- objpose::ObjectPose* findObjectPose(const ObjectID& objectID, const std::string& providerName = "");
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
-
- bool isAttachedCached(objpose::ObjectPose& objectPose) const;
- };
- Data data;
+ objpose::observer::Data data;
std::mutex dataMutex;
-
- objpose::ObjectPoseDecay decay;
- std::mutex decayMutex;
-
-
- objpose::RobotHeadMovement robotHead;
+ objpose::observer::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;
+ objpose::observer::Visu visu;
std::mutex visuMutex;
+
struct Calibration
{
std::string robotNode = "Neck_2_Pitch";
@@ -238,28 +179,9 @@ namespace armarx
struct RemoteGuiTab : RemoteGui::Client::Tab
{
- 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;
- };
- Visu visu;
-
- struct RobotHead
- {
- RemoteGui::Client::CheckBox checkHeadVelocity;
- RemoteGui::Client::FloatSpinBox maxJointVelocity;
- RemoteGui::Client::IntSpinBox discardIntervalAfterMoveMS;
- };
- RobotHead robotHead;
-
- objpose::ObjectPoseDecay::RemoteGui decay;
+ objpose::observer::Visu::RemoteGui visu;
+ objpose::observer::Decay::RemoteGui decay;
+ objpose::observer::RobotHeadMovement::RemoteGui robotHead;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be89f745d995ff82431d4b2fb285181a607730dc
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.cpp
@@ -0,0 +1,359 @@
+#include "Data.h"
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <sstream>
+
+
+namespace armarx::objpose::observer
+{
+
+ void Data::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ decay.defineProperties(defs, prefix + "decay.");
+ }
+
+ ObjectPoseSeq Data::getObjectPoses(IceUtil::Time now)
+ {
+ bool synchronized = false;
+ ObjectPoseSeq result;
+
+ for (auto& [providerName, objectPoses] : objectPoses)
+ {
+ // Update data.
+ updateObjectPoses(objectPoses, now, robot, synchronized);
+
+ // Collect results.
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ }
+ return result;
+ }
+
+
+ ObjectPoseSeq Data::getObjectPosesByProvider(
+ const std::string& providerName,
+ IceUtil::Time now)
+ {
+ bool synchronized = false;
+
+ // Update data.
+ ObjectPoseSeq& objectPoses = this->objectPoses.at(providerName);
+ updateObjectPoses(objectPoses, now, robot, synchronized);
+
+ // Collect results.
+ ObjectPoseSeq result;
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
+ {
+ result.push_back(objectPose);
+ }
+ }
+ return result;
+ }
+
+
+ void Data::updateObjectPoses(IceUtil::Time now)
+ {
+ bool synchronized = false;
+ for (auto& [providerName, objectPoses] : objectPoses)
+ {
+ updateObjectPoses(objectPoses, now, robot, synchronized);
+ }
+ }
+
+
+ void Data::updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ for (ObjectPose& pose : objectPoses)
+ {
+ updateObjectPose(pose, now, agent, agentSynchronized);
+ }
+ }
+
+
+ void Data::updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
+ {
+ updateAttachement(objectPose, agent, agentSynchronized);
+
+ if (decay.enabled)
+ {
+ decay.updateConfidence(objectPose, now);
+ }
+ }
+
+
+ void Data::updateAttachement(
+ ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ {
+ if (!objectPose.attachment)
+ {
+ // Fetch attachment info from internal data structure.
+ auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
+ if (it != attachments.end())
+ {
+ // Store it in the objectPose.
+ objectPose.attachment = it->second;
+ }
+ else
+ {
+ // No attachment, nothing to do.
+ return;
+ }
+ }
+ ARMARX_CHECK(objectPose.attachment);
+
+ if (!synchronized) // Synchronize only once.
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ synchronized = true;
+ }
+ objectPose.updateAttached(agent);
+ }
+
+
+ ObjectPose* Data::findObjectPose(const ObjectID& objectID, const std::string& providerName)
+ {
+ ObjectPose* pose = nullptr;
+ if (!providerName.empty())
+ {
+ pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
+ }
+ else
+ {
+ for (auto& [_, poses] : objectPoses)
+ {
+ pose = findObjectPoseByID(poses, objectID);
+ if (pose)
+ {
+ break;
+ }
+ }
+ }
+ return pose;
+ }
+
+
+ std::optional<simox::OrientedBoxf> Data::getObjectOOBB(const ObjectID& id)
+ {
+ 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;
+ }
+ });
+ }
+
+ ProviderInfo Data::getProviderInfo(const std::string& providerName)
+ {
+ try
+ {
+ return 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)
+ {
+ ss << "- '" << name << "'\n";
+ }
+ throw std::out_of_range(ss.str());
+ }
+ }
+
+
+
+ AttachObjectToRobotNodeOutput
+ Data::attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input)
+ {
+ AttachObjectToRobotNodeOutput output;
+ output.success = false; // We are not successful until proven otherwise.
+
+ ObjectID objectID = armarx::fromIce(input.objectID);
+
+ if (input.agentName != "" && input.agentName != this->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> {this->robot->getName()};
+ return output;
+ }
+ VirtualRobot::RobotPtr agent = this->robot;
+
+ if (!agent->hasRobotNode(input.frameName))
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
+ << "' of agent '" << agent->getName() << "'.";
+ return output;
+ }
+ std::string frameName = input.frameName;
+
+
+ // Find object pose provider name can be empty.
+ ObjectPose* currentObjectPose = this->findObjectPose(objectID, input.providerName);
+ if (!currentObjectPose)
+ {
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
+ << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ return output;
+ }
+
+ ObjectAttachmentInfo info;
+ info.agentName = agent->getName();
+ info.frameName = frameName;
+
+ if (input.poseInFrame)
+ {
+ info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
+ }
+ else
+ {
+ RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+
+ armarx::FramedPose framed(currentObjectPose->objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ if (frameName == armarx::GlobalFrame)
+ {
+ info.poseInFrame = framed.toGlobalEigen(this->robot);
+ }
+ else
+ {
+ framed.changeFrame(this->robot, info.frameName);
+ info.poseInFrame = framed.toEigen();
+ }
+ }
+ this->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"
+ << "Object pose in frame: \n" << info.poseInFrame;
+
+ output.success = true;
+ output.attachment = new data::ObjectAttachmentInfo();
+ output.attachment->frameName = info.frameName;
+ output.attachment->agentName = info.agentName;
+ output.attachment->poseInFrame = new Pose(info.poseInFrame);
+
+ return output;
+ }
+
+ DetachObjectFromRobotNodeOutput Data::detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input)
+ {
+ ObjectID objectID = armarx::fromIce(input.objectID);
+ std::string providerName = input.providerName;
+
+ std::optional<ObjectAttachmentInfo> attachment;
+ {
+ // Remove from latest pose (if it was cached).
+ ObjectPose* objectPose = this->findObjectPose(objectID, input.providerName);
+ if (objectPose)
+ {
+ objectPose->attachment = std::nullopt;
+ }
+
+ if (providerName.empty() && objectPose)
+ {
+ providerName = objectPose->providerName;
+ }
+ // Remove from attachment map.
+ if (input.providerName.size() > 0)
+ {
+ auto it = this->attachments.find(std::make_pair(input.providerName, objectID));
+ if (it != this->attachments.end())
+ {
+ attachment = it->second;
+ this->attachments.erase(it);
+ }
+ }
+ else
+ {
+ // Search for entry with matching object ID.
+ for (auto it = this->attachments.begin(); it != this->attachments.end(); ++it)
+ {
+ const ObjectID& id = it->first.second;
+ if (id == objectID)
+ {
+ attachment = it->second;
+ this->attachments.erase(it);
+ break;
+ }
+ }
+ }
+ }
+
+ DetachObjectFromRobotNodeOutput output;
+ output.wasAttached = bool(attachment);
+ if (attachment)
+ {
+ ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
+ << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ }
+ else
+ {
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName << "' "
+ << "from robot node, but it was not attached.";
+ }
+
+ return output;
+ }
+
+ DetachAllObjectsFromRobotNodesOutput Data::detachAllObjectsFromRobotNodes()
+ {
+ DetachAllObjectsFromRobotNodesOutput output;
+ output.numDetached = int(this->attachments.size());
+
+ // Clear attachment map.
+ this->attachments.clear();
+
+ // Remove from poses (if it was cached).
+ for (auto& [prov, poses] : this->objectPoses)
+ {
+ for (auto& pose : poses)
+ {
+ pose.attachment = std::nullopt;
+ }
+ }
+
+ ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
+
+ return output;
+ }
+
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
new file mode 100644
index 0000000000000000000000000000000000000000..e825a95340f71e4357392433ae16f8c3fec1ee30
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Data.h
@@ -0,0 +1,98 @@
+#pragma once
+
+#include <map>
+#include <string>
+#include <optional>
+
+#include <SimoxUtility/caching/CacheMap.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+#include "Decay.h"
+
+
+namespace armarx::objpose::observer
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Data : public armarx::Logging
+ {
+ public:
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
+ ObjectPoseSeq getObjectPoses(IceUtil::Time now);
+ ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName, IceUtil::Time now);
+
+ ObjectPose* findObjectPose(const ObjectID& objectID, const std::string& providerName = "");
+ std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+
+ ProviderInfo getProviderInfo(const std::string& providerName);
+
+
+ AttachObjectToRobotNodeOutput attachObjectToRobotNode(const AttachObjectToRobotNodeInput& input);
+ DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const DetachObjectFromRobotNodeInput& input);
+ DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes();
+
+
+ void updateObjectPoses(IceUtil::Time now);
+ void updateObjectPoses(
+ ObjectPoseSeq& objectPoses,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+ void updateObjectPose(
+ ObjectPose& objectPose,
+ IceUtil::Time now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized
+ ) const;
+
+ /**
+ * @brief If the object is attached to a robot node, update it according to the current robot state.
+ *
+ * If there is no attachement info in `objectPose` itself, the internal data
+ * structure `attachments` is queried. If an attachment is found there,
+ * it is written into the given `objectPose` (thus, it is "cached" in the
+ * info `objectPose`).
+ *
+ * @param synchronized Indicates whether the agent is already synchronized to the current time.
+ */
+ void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ bool& synchronized) const;
+
+
+ public:
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ VirtualRobot::RobotPtr robot;
+
+ ProviderInfoMap providers;
+
+ std::map<std::string, ObjectPoseSeq> objectPoses;
+
+ std::map<std::pair<std::string, ObjectID>, ObjectAttachmentInfo> attachments;
+
+
+ ObjectFinder objectFinder;
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ /// Decay model.
+ Decay decay;
+
+ };
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
similarity index 86%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
rename to source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
index 0df6db13c53aa295a8117c3845307375e95402c9..522a5cc13d83acf95a4efe1c3289c3aec2912487 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.cpp
@@ -1,14 +1,14 @@
-#include "ObjectPoseDecay.h"
+#include "Decay.h"
#include <SimoxUtility/math/scale_value.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose
+namespace armarx::objpose::observer
{
- void ObjectPoseDecay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void Decay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(enabled, prefix + "enabled",
"If true, object poses decay over time when not localized anymore.");
@@ -24,15 +24,15 @@ namespace armarx::objpose
"Remove objects whose confidence is lower than this value.");
}
- void ObjectPoseDecay::updateConfidence(ObjectPose& pose, IceUtil::Time now)
+ void Decay::updateConfidence(ObjectPose& pose, IceUtil::Time now) const
{
float confidence = calculateConfidence(pose.timestamp, now);
pose.confidence = confidence;
}
- void ObjectPoseDecay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now)
+ void Decay::updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const
{
- for (objpose::ObjectPose& pose : objectPoses)
+ for (ObjectPose& pose : objectPoses)
{
if (pose.attachment)
{
@@ -45,7 +45,7 @@ namespace armarx::objpose
}
}
- float ObjectPoseDecay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now)
+ float Decay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const
{
float duration = (now - localization).toSeconds();
if (duration < delaySeconds)
@@ -67,7 +67,7 @@ namespace armarx::objpose
- void ObjectPoseDecay::RemoteGui::setup(const ObjectPoseDecay& decay)
+ void Decay::RemoteGui::setup(const Decay& decay)
{
using namespace armarx::RemoteGui::Client;
@@ -121,7 +121,7 @@ namespace armarx::objpose
group.addChild(grid);
}
- void ObjectPoseDecay::RemoteGui::update(ObjectPoseDecay& decay)
+ void Decay::RemoteGui::update(Decay& decay)
{
decay.enabled = enabled.getValue();
decay.delaySeconds = delaySeconds.getValue();
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
similarity index 81%
rename from source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
rename to source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
index 22a4940da95f94edbe5abc35dd4f14efc36d20a3..ec64221f3e202395ff4f0f8c83d3bb737b6571be 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Decay.h
@@ -10,25 +10,25 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-namespace armarx::objpose
+namespace armarx::objpose::observer
{
/**
* @brief Models decay of object localizations by decreasing the confidence
* the longer the object was not localized.
*/
- class ObjectPoseDecay : public armarx::Logging
+ class Decay : 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);
+ void updateConfidence(ObjectPose& pose, IceUtil::Time now) const;
+ void updateConfidences(ObjectPoseSeq& objectPoses, IceUtil::Time now) const;
private:
- float calculateConfidence(IceUtil::Time localization, IceUtil::Time now);
+ float calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const;
public:
@@ -45,10 +45,6 @@ namespace armarx::objpose
float removeObjectsBelowConfidence = 0.1f;
- /// Frequency of updating the current decay.
- float updateFrequencyHz = 10.0;
- SimpleRunningTask<>::pointer_type updateTask;
-
struct RemoteGui
{
@@ -63,8 +59,8 @@ namespace armarx::objpose
armarx::RemoteGui::Client::FloatSlider removeObjectsBelowConfidence;
- void setup(const ObjectPoseDecay& decay);
- void update(ObjectPoseDecay& decay);
+ void setup(const Decay& decay);
+ void update(Decay& decay);
};
};
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
index b9ac721ca1ade44a8667b28667520093169d0770..a5e78b5942704a7e53a3aae34d10c32b07baaa73 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.cpp
@@ -1,10 +1,11 @@
#include "RobotHeadMovement.h"
-#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
-namespace armarx::objpose
+namespace armarx::objpose::observer
{
void RobotHeadMovement::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
@@ -97,4 +98,65 @@ namespace armarx::objpose
}
}
+ objpose::SignalHeadMovementOutput RobotHeadMovement::signalHeadMovement(const objpose::SignalHeadMovementInput& input)
+ {
+ objpose::SignalHeadMovementOutput output;
+ switch (input.action)
+ {
+ case objpose::HeadMovementAction::HeadMovementAction_Starting:
+ this->movementStarts(input.discardUpdatesIntervalMilliSeconds < 0
+ ? this->discardIntervalAfterMoveMS
+ : input.discardUpdatesIntervalMilliSeconds);
+ break;
+ case objpose::HeadMovementAction::HeadMovementAction_Stopping:
+ this->movementStops(input.discardUpdatesIntervalMilliSeconds);
+ break;
+ default:
+ ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementAction, input.action);
+ break;
+ }
+ output.discardUpdatesUntilMilliSeconds = this->discardUpdatesUntil.toMilliSeconds();
+ return output;
+ }
+
+
+
+ void RobotHeadMovement::RemoteGui::setup(const RobotHeadMovement& rhm)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ checkHeadVelocity.setValue(rhm.checkHeadVelocity);
+ {
+ float max = 10.0;
+ maxJointVelocity.setRange(0, max);
+ maxJointVelocity.setDecimals(3);
+ maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
+ maxJointVelocity.setValue(rhm.maxJointVelocity);
+ }
+ {
+ int max = 10 * 1000;
+ discardIntervalAfterMoveMS.setRange(0, max);
+ discardIntervalAfterMoveMS.setValue(rhm.discardIntervalAfterMoveMS);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Check Head Motion"), {row, 0}).add(checkHeadVelocity, {row, 1});
+ row++;
+ grid.add(Label("Max Joint Velocity"), {row, 0}).add(maxJointVelocity, {row, 1});
+ row++;
+ grid.add(Label("Discard Interval after Move [ms]"), {row, 0}).add(discardIntervalAfterMoveMS, {row, 1});
+ row++;
+
+ group.setLabel("Robot Head Movement");
+ group.addChild(grid);
+ }
+
+ void RobotHeadMovement::RemoteGui::update(RobotHeadMovement& rhm)
+ {
+ rhm.checkHeadVelocity = checkHeadVelocity.getValue();
+ rhm.maxJointVelocity = maxJointVelocity.getValue();
+ rhm.discardIntervalAfterMoveMS = discardIntervalAfterMoveMS.getValue();
+ }
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
index fd06a1b2d83831642b4abcfe6fc16784c105c554..fdd6078d9fcefeb619339c15f3e93029e90c3049 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/RobotHeadMovement.h
@@ -9,7 +9,10 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
+#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
namespace armarx
@@ -18,7 +21,7 @@ namespace armarx
using PropertyDefinitionsPtr = IceUtil::Handle<PropertyDefinitionContainer>;
}
-namespace armarx::objpose
+namespace armarx::objpose::observer
{
class RobotHeadMovement : public armarx::Logging
{
@@ -34,6 +37,8 @@ namespace armarx::objpose
void movementStops(long discardIntervalMs);
void movementStops(IceUtil::Time discardInterval);
+ objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input);
+
public:
@@ -50,9 +55,19 @@ namespace armarx::objpose
DebugObserverInterfacePrx debugObserver;
- };
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::CheckBox checkHeadVelocity;
+ armarx::RemoteGui::Client::FloatSpinBox maxJointVelocity;
+ armarx::RemoteGui::Client::IntSpinBox discardIntervalAfterMoveMS;
+ void setup(const RobotHeadMovement& rhm);
+ void update(RobotHeadMovement& rhm);
+ };
+
+ };
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..467dba24f947b38c02b47044986cf3086775b6a1
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.cpp
@@ -0,0 +1,156 @@
+#include "Visu.h"
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+
+namespace armarx::objpose::observer
+{
+
+ void Visu::defineProperties(armarx::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.");
+ }
+
+
+ std::vector<viz::Layer> Visu::visualizeCommit(
+ const std::map<std::string, ObjectPoseSeq>& objectPoses,
+ float minConfidence,
+ const ObjectFinder& objectFinder) const
+ {
+ std::vector<viz::Layer> layers;
+ for (const auto& [name, poses] : objectPoses)
+ {
+ layers.push_back(visualizeProvider(name, poses, minConfidence, objectFinder));
+ }
+ return layers;
+ }
+
+ viz::Layer Visu::visualizeProvider(
+ const std::string& providerName,
+ const ObjectPoseSeq& objectPoses,
+ float minConfidence,
+ const ObjectFinder& objectFinder) const
+ {
+ viz::Layer layer = arviz.layer(providerName);
+ for (const ObjectPose& objectPose : objectPoses)
+ {
+ const bool show = not(objectPose.confidence < minConfidence);
+ if (show)
+ {
+ visualizeObjectPose(layer, objectPose, objectFinder);
+ }
+ }
+ return layer;
+ }
+
+ void Visu::visualizeObjectPose(
+ viz::Layer& layer,
+ const ObjectPose& objectPose,
+ const ObjectFinder& objectFinder) const
+ {
+ const armarx::ObjectID id = objectPose.objectID;
+ const std::string key = id.str();
+
+ Eigen::Matrix4f pose = inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
+ {
+ viz::Object object = viz::Object(key).pose(pose);
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ {
+ object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
+ }
+ else
+ {
+ object.fileByObjectFinder(id);
+ }
+ if (alphaByConfidence && objectPose.confidence < 1.0f)
+ {
+ object.overrideColor(simox::Color::white().with_alpha(objectPose.confidence));
+ }
+ else if (alpha < 1)
+ {
+ object.overrideColor(simox::Color::white().with_alpha(alpha));
+ }
+ layer.add(object);
+ }
+
+ if (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 (objectFrames)
+ {
+ layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
+ }
+ }
+
+
+ void Visu::RemoteGui::setup(const Visu& visu)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ enabled.setValue(visu.enabled);
+ inGlobalFrame.setValue(visu.inGlobalFrame);
+ alpha.setRange(0, 1.0);
+ alpha.setValue(visu.alpha);
+ alphaByConfidence.setValue(visu.alphaByConfidence);
+ oobbs.setValue(visu.oobbs);
+ objectFrames.setValue(visu.objectFrames);
+ {
+ float max = 10000;
+ objectFramesScale.setRange(0, max);
+ objectFramesScale.setDecimals(2);
+ objectFramesScale.setSteps(int(10 * max));
+ objectFramesScale.setValue(visu.objectFramesScale);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
+ row++;
+ grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
+ row++;
+ grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
+ row++;
+ grid.add(Label("Alpha By Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
+ row++;
+ grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
+ row++;
+ grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
+ grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
+ row++;
+
+ group.setLabel("Visualization");
+ group.addChild(grid);
+ }
+
+ void Visu::RemoteGui::update(Visu& visu)
+ {
+ visu.enabled = enabled.getValue();
+ visu.inGlobalFrame = inGlobalFrame.getValue();
+ visu.alpha = alpha.getValue();
+ visu.alphaByConfidence = alphaByConfidence.getValue();
+ visu.oobbs = oobbs.getValue();
+ visu.objectFrames = objectFrames.getValue();
+ visu.objectFramesScale = objectFramesScale.getValue();
+ }
+
+}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..9677f6198adcd9f52918061925876860640ba999
--- /dev/null
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/Visu.h
@@ -0,0 +1,86 @@
+#pragma once
+
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
+
+namespace armarx
+{
+ class ObjectFinder;
+}
+namespace armarx::objpose::observer
+{
+
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class Visu : public armarx::Logging
+ {
+ public:
+
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+
+ std::vector<viz::Layer> visualizeCommit(
+ const std::map<std::string, ObjectPoseSeq>& objectPoses,
+ float minConfidence,
+ const ObjectFinder& objectFinder
+ ) const;
+
+ viz::Layer visualizeProvider(
+ const std::string& providerName,
+ const ObjectPoseSeq& objectPoses,
+ float minConfidence,
+ const ObjectFinder& objectFinder
+ ) const;
+
+ void visualizeObjectPose(
+ viz::Layer& layer,
+ const ObjectPose& objectPose,
+ const ObjectFinder& objectFinder
+ ) const;
+
+
+ public:
+
+ viz::Client arviz;
+
+ bool enabled = false;
+ float frequencyHz = 25;
+
+ bool inGlobalFrame = true;
+ float alpha = 1.0;
+ bool alphaByConfidence = false;
+ bool oobbs = false;
+ bool objectFrames = false;
+ float objectFramesScale = 1.0;
+
+ SimpleRunningTask<>::pointer_type updateTask;
+
+
+ struct RemoteGui
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::CheckBox enabled;
+
+ armarx::RemoteGui::Client::CheckBox inGlobalFrame;
+ armarx::RemoteGui::Client::FloatSlider alpha;
+ armarx::RemoteGui::Client::CheckBox alphaByConfidence;
+ armarx::RemoteGui::Client::CheckBox oobbs;
+ armarx::RemoteGui::Client::CheckBox objectFrames;
+ armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
+
+ void setup(const Visu& visu);
+ void update(Visu& visu);
+ };
+
+ };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 02b04bf6f3961074891472deed1ade625850b467..53da6a10a507a5a63c5205f22d8a6643bfb867c0 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -119,30 +119,35 @@ namespace armarx::objpose
return std::nullopt;
}
- objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent)
+ objpose::ObjectPose ObjectPose::getAttached(VirtualRobot::RobotPtr agent) const
{
ARMARX_CHECK(attachment);
objpose::ObjectPose updated = *this;
- const objpose::ObjectAttachmentInfo& attachment = *updated.attachment;
+ updated.updateAttached(agent);
+ return updated;
+ }
+
+ void ObjectPose::updateAttached(VirtualRobot::RobotPtr agent)
+ {
+ ARMARX_CHECK(attachment);
- ARMARX_CHECK_EQUAL(attachment.agentName, agent->getName());
+ ARMARX_CHECK_EQUAL(attachment->agentName, agent->getName());
- VirtualRobot::RobotNodePtr robotNode = agent->getRobotNode(attachment.frameName);
+ 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;
+ objectPoseRobot = robotNode->getPoseInRootFrame() * attachment->poseInFrame;
+ objectPoseGlobal = agent->getGlobalPose() * objectPoseRobot;
- updated.robotPose = agent->getGlobalPose();
- updated.robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
-
- return updated;
+ // Overwrite robot config.
+ robotPose = agent->getGlobalPose();
+ robotConfig = agent->getConfig()->getRobotNodeJointValueMap();
}
-
}
+
namespace armarx
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index f7fec6b7776d1638d6dbdbd9c043872c372c803b..36379f97d0d3d047058dbb9a9d57e9517a0caa71 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -74,8 +74,17 @@ 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);
+ /**
+ * @brief Get a copy of `*this` with updated attachment.
+ * @see `updateAttached()`
+ */
+ objpose::ObjectPose getAttached(VirtualRobot::RobotPtr agent) const;
+ /**
+ * @brief Update an the object pose and robot state of an attached
+ * object pose according to the given agent state (in-place).
+ * @param agent The agent/robot.
+ */
+ void updateAttached(VirtualRobot::RobotPtr agent);
};
using ObjectPoseSeq = std::vector<ObjectPose>;