From 8a0a9bc4b675fe7d7eae4cc35c21d36fd5beb21e Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 19 Feb 2021 18:34:21 +0100
Subject: [PATCH] Move functions to class
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 94 +++++++++----------
.../ObjectPoseObserver/ObjectPoseObserver.h | 18 ++--
2 files changed, 55 insertions(+), 57 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index ede848186..28fd1a789 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -78,6 +78,7 @@ namespace armarx
void ObjectPoseObserver::onInitObserver()
{
+ data.setTag(getName());
robotHead.setTag(getName());
usingTopicFromProperty("ObjectPoseTopicName");
@@ -229,7 +230,7 @@ namespace armarx
if (!provided.localOOBB)
{
// Try to load oobb from disk.
- newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ newPose.localOOBB = data.getObjectOOBB(newPose.objectID);
}
}
}
@@ -452,7 +453,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
@@ -509,7 +510,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;
@@ -664,14 +665,14 @@ namespace armarx
Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
- if (isAttachedCached(objectPose))
+ if (data.isAttachedCached(objectPose))
{
if (!synchronized)
{
RobotState::synchronizeLocalClone(data.robot);
synchronized = true;
}
- objpose::ObjectPose updated = applyAttachment(objectPose, data.robot);
+ objpose::ObjectPose updated = objectPose.getAttached(data.robot);
pose = visu.inGlobalFrame ? updated.objectPoseGlobal : updated.objectPoseRobot;
}
else
@@ -716,41 +717,40 @@ namespace armarx
return getProxy<objpose::ObjectPoseProviderPrx>(providerName, false, "", false);
}
- std::optional<simox::OrientedBoxf> ObjectPoseObserver::getObjectOOBB(const ObjectID& id)
+
+ void ObjectPoseObserver::toIceWithAttachments(
+ objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
+ objpose::data::ObjectPoseSeq& result, bool& synchronized)
{
- return data.oobbCache.get(id, [this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
+ for (objpose::ObjectPose& pose : objectPoses)
{
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = data.objectFinder.findObject(id))
+ if (data.isAttachedCached(pose))
{
- try
- {
- return objectInfo->loadOOBB();
- }
- catch (const std::ios_base::failure& e)
+ if (!synchronized) // Synchronize only once.
{
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
- return std::nullopt;
+ RobotState::synchronizeLocalClone(agent);
+ synchronized = true;
}
+ result.push_back(pose.getAttached(agent).toIce());
}
else
{
- return std::nullopt;
+ result.push_back(pose.toIce());
}
- });
+ }
}
- objpose::ObjectPose* ObjectPoseObserver::findObjectPose(const std::string& providerName, ObjectID& objectID)
+
+ objpose::ObjectPose* ObjectPoseObserver::Data::findObjectPose(const std::string& providerName, ObjectID& objectID)
{
objpose::ObjectPose* pose = nullptr;
if (!providerName.empty())
{
- pose = findObjectPoseByID(data.objectPoses.at(providerName), objectID);
+ pose = findObjectPoseByID(objectPoses.at(providerName), objectID);
}
else
{
- for (auto& [_, poses] : data.objectPoses)
+ for (auto& [_, poses] : objectPoses)
{
pose = findObjectPoseByID(poses, objectID);
if (pose)
@@ -762,18 +762,37 @@ namespace armarx
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::isAttachedCached(objpose::ObjectPose& objectPose) const
+ bool ObjectPoseObserver::Data::isAttachedCached(objpose::ObjectPose& objectPose) const
{
if (!objectPose.attachment)
{
- auto it = data.attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
- if (it != data.attachments.end())
+ auto it = attachments.find(std::make_pair(objectPose.providerName, objectPose.objectID));
+ if (it != attachments.end())
{
objectPose.attachment = it->second;
}
@@ -782,29 +801,6 @@ namespace armarx
}
-
- void ObjectPoseObserver::toIceWithAttachments(
- objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
- objpose::data::ObjectPoseSeq& result, bool& synchronized)
- {
- for (objpose::ObjectPose& pose : objectPoses)
- {
- if (isAttachedCached(pose))
- {
- if (!synchronized) // Synchronize only once.
- {
- RobotState::synchronizeLocalClone(agent);
- synchronized = true;
- }
- result.push_back(applyAttachment(pose, agent).toIce());
- }
- else
- {
- result.push_back(pose.toIce());
- }
- }
- }
-
void ObjectPoseObserver::RobotHeadMovement::fetchDatafields()
{
if (kinematicUnitObserver)
@@ -977,5 +973,7 @@ namespace armarx
robotHead.discardIntervalAfterMoveMS = tab.robotHead.discardIntervalAfterMoveMS.getValue();
}
}
+
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index ff26b59f7..190b0828e 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -146,14 +146,6 @@ namespace armarx
objpose::ObjectPoseProviderPrx getProviderProxy(const std::string& providerName);
- std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
-
-
- objpose::ObjectPose* findObjectPose(const std::string& providerName, ObjectID& objectID);
-
-
- bool isAttachedCached(objpose::ObjectPose& objectPose) const;
-
void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
objpose::data::ObjectPoseSeq& result, bool& synchronized);
@@ -161,8 +153,9 @@ namespace armarx
DebugObserverInterfacePrx debugObserver;
- struct Data
+ class Data : public armarx::Logging
{
+ public:
VirtualRobot::RobotPtr robot;
objpose::ProviderInfoMap providers;
@@ -176,6 +169,13 @@ namespace armarx
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 std::string& providerName, ObjectID& objectID);
+ std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
+
+ bool isAttachedCached(objpose::ObjectPose& objectPose) const;
};
std::mutex dataMutex;
Data data;
--
GitLab