diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 03da4c2061d786b22fd1f126d9e8c2983e28faec..98a9587804881ac07f7290053704e26028c23002 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,33 +1,15 @@
#include "Segment.h"
-#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
-#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
-
-#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/memory_ids.h>
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/client/query/query_fns.h>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <sstream>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
@@ -36,24 +18,36 @@
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-#include <SimoxUtility/algorithm/string.h>
-#include <SimoxUtility/json.h>
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/regression/linear.h>
-
-#include <Eigen/Geometry>
-#include <Eigen/Dense>
-
-#include <sstream>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/memory_ids.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx::armem::server::obj::instance
{
- void Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::Calibration::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
{
- defs->optional(robotName, prefix + "robotName",
+ defs->optional(robotName,
+ prefix + "robotName",
"Name of robot whose note can be calibrated.\n"
"If not given, the 'fallbackName' is used.");
defs->optional(robotNode, prefix + "robotNode", "Robot node which can be calibrated.");
@@ -66,86 +60,96 @@ namespace armarx::armem::server::obj::instance
arondto::ObjectInstance::ToAronType(),
64)
{
- oobbCache.setFetchFn([this](const ObjectID & id) -> std::optional<simox::OrientedBoxf>
- {
- // Try to get OOBB from repository.
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
+ oobbCache.setFetchFn(
+ [this](const ObjectID& id) -> std::optional<simox::OrientedBoxf>
{
- try
+ // Try to get OOBB from repository.
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id))
{
- objectInfo->setLogError(false); // Don't log missing files
- return objectInfo->loadOOBB();
+ try
+ {
+ objectInfo->setLogError(false); // Don't log missing files
+ 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;
+ }
}
- catch (const std::ios_base::failure& e)
+ else
{
- // Give up - no OOBB information.
- ARMARX_WARNING << "Could not get OOBB of object " << id << ".\n- " << e.what();
return std::nullopt;
}
- }
- else
- {
- return std::nullopt;
- }
- });
+ });
- classNameToDatasetCache.setFetchFn([this](const std::string & className)
- {
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
- return objectInfo ? objectInfo->dataset() : "";
- });
+ classNameToDatasetCache.setFetchFn(
+ [this](const std::string& className)
+ {
+ std::optional<ObjectInfo> objectInfo = objectFinder.findObject(className);
+ return objectInfo ? objectInfo->dataset() : "";
+ });
}
-
Segment::~Segment()
{
}
-
- void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ void
+ Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
SpecializedCoreSegment::defineProperties(defs, prefix);
- defs->optional(p.discardSnapshotsWhileAttached, prefix + "DiscardSnapshotsWhileAttached",
- "If true, no new snapshots are stored while an object is attached to a robot node.\n"
- "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+ defs->optional(
+ p.discardSnapshotsWhileAttached,
+ prefix + "DiscardSnapshotsWhileAttached",
+ "If true, no new snapshots are stored while an object is attached to a robot node.\n"
+ "If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
- defs->optional(robots.fallbackName, prefix + "robots.FallbackName",
+ defs->optional(robots.fallbackName,
+ prefix + "robots.FallbackName",
"Robot name to use as fallback if the robot name is not specified "
"in a provided object pose.");
- defs->optional(p.sceneSnapshotsPackage, prefix + "scene.10_Package",
- "ArmarX package containing the scene snapshots.\n"
- "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
- defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
+ defs->optional(
+ p.sceneSnapshotsPackage,
+ prefix + "scene.10_Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotsDirectory,
+ prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
- std::vector<std::string> sceneSnapshotToLoadDescription =
- {
+ std::vector<std::string> sceneSnapshotToLoadDescription = {
"Scene(s) to load on startup.",
"Specify multiple scenes in a ; separated list.",
"Each entry must be one of the following:",
"(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
"e.g. 'MyScene', 'MyScene.json'",
- "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), "
+ "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' "
+ "extension), "
"e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
"(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
};
- defs->optional(p.sceneSnapshotsToLoad, prefix + "scene.12_SnapshotToLoad",
+ defs->optional(p.sceneSnapshotsToLoad,
+ prefix + "scene.12_SnapshotToLoad",
simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
decay.defineProperties(defs, prefix + "decay.");
}
-
- void Segment::init()
+ void
+ Segment::init()
{
SpecializedCoreSegment::init();
if (not p.sceneSnapshotsToLoad.empty())
{
bool trim = true;
- const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
+ const std::vector<std::string> scenes =
+ simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
for (const std::string& scene : scenes)
{
const bool lockMemory = false;
@@ -156,21 +160,20 @@ namespace armarx::armem::server::obj::instance
robots.setTag(Logging::tag);
}
-
- void Segment::connect(viz::Client arviz)
+ void
+ Segment::connect(viz::Client arviz)
{
- (void) arviz;
+ (void)arviz;
// ARMARX_INFO << "ArticulatedObjectVisu";
// this->visu = std::make_unique<ArticulatedObjectVisu>(arviz, *this);
// visu->init();
}
-
- Segment::CommitStats Segment::commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<armem::Time> discardUpdatesUntil)
+ Segment::CommitStats
+ Segment::commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<armem::Time> discardUpdatesUntil)
{
CommitStats stats;
@@ -187,7 +190,8 @@ namespace armarx::armem::server::obj::instance
// Check whether we have an old snapshot for this object.
std::optional<objpose::ObjectPose> previousPose;
- const wm::Entity* entity = findObjectEntity(armarx::fromIce(provided.objectID), providerName);
+ const wm::Entity* entity =
+ findObjectEntity(armarx::fromIce(provided.objectID), providerName);
if (entity)
{
const arondto::ObjectInstance data = getLatestInstanceData(*entity);
@@ -221,11 +225,13 @@ namespace armarx::armem::server::obj::instance
// Update the entity.
stats.numUpdated++;
- VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(provided.robotName, provided.providerName);
- if(not robot)
+ if (not robot)
{
- ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `" << provided.robotName << "`.";
+ ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
+ << provided.robotName << "`.";
}
// robot may be null!
@@ -233,10 +239,10 @@ namespace armarx::armem::server::obj::instance
// Update the robot to obtain correct local -> global transformation
if (robot and robotSyncTimestamp != timestamp)
{
- ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
- << "Failed to synchronize robot to timestamp " << timestamp
- << ". This is " << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
-
+ ARMARX_CHECK(robots.reader->synchronizeRobot(*robot, timestamp))
+ << "Failed to synchronize robot to timestamp " << timestamp << ". This is "
+ << (Clock::Now() - timestamp).toSecondsDouble() << " seconds in the past.";
+
robotSyncTimestamp = timestamp;
@@ -244,7 +250,8 @@ namespace armarx::armem::server::obj::instance
{
if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
{
- VirtualRobot::RobotNodePtr robotNode = robot->getRobotNode(calibration.robotNode);
+ VirtualRobot::RobotNodePtr robotNode =
+ robot->getRobotNode(calibration.robotNode);
float value = robotNode->getJointValue();
float newValue = value + calibration.offset;
@@ -255,7 +262,8 @@ namespace armarx::armem::server::obj::instance
* for the calibrated value.
* As this is just for perception (and not for controlling the robot), this should be fine^TM.
*/
- VirtualRobot::RobotNode::JointLimits limits = robotNode->getJointLimits();
+ VirtualRobot::RobotNode::JointLimits limits =
+ robotNode->getJointLimits();
bool limitsChanged = false;
if (newValue < limits.low)
{
@@ -282,11 +290,11 @@ namespace armarx::armem::server::obj::instance
{
objpose::data::ProvidedObjectPose copy = provided;
copy.objectPoseFrame = armarx::GlobalFrame;
- newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
}
else
{
- newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
+ newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
}
if (previousPose && previousPose->attachment)
@@ -299,10 +307,12 @@ namespace armarx::armem::server::obj::instance
if (newPose.objectID.dataset().empty())
{
// Try to find the data set.
- const std::string dataset = classNameToDatasetCache.get(newPose.objectID.className());
+ const std::string dataset =
+ classNameToDatasetCache.get(newPose.objectID.className());
if (!dataset.empty())
{
- newPose.objectID = { dataset, newPose.objectID.className(), newPose.objectID.instanceName() };
+ newPose.objectID = {
+ dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
}
}
if (!provided.localOOBB)
@@ -318,8 +328,8 @@ namespace armarx::armem::server::obj::instance
return stats;
}
-
- void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
+ void
+ Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
Time now = Time::Now();
@@ -332,7 +342,7 @@ namespace armarx::armem::server::obj::instance
EntityUpdate& update = commit.updates.emplace_back();
const MemoryID providerID = coreSegmentID.withProviderSegmentName(
- providerName.empty() ? pose.providerName : providerName);
+ providerName.empty() ? pose.providerName : providerName);
update.entityID = providerID.withEntityName(pose.objectID.str());
update.arrivedTime = now;
@@ -352,13 +362,11 @@ namespace armarx::armem::server::obj::instance
}
toAron(dto.sourceID, MemoryID());
update.instancesData.push_back(dto.toAron());
-
}
// Commit non-locking.
iceMemory.commit(commit);
}
-
objpose::ObjectPoseMap
Segment::getObjectPoses(const DateTime& now)
{
@@ -367,20 +375,16 @@ namespace armarx::armem::server::obj::instance
return filterObjectPoses(objectPoses);
}
-
-
objpose::ObjectPoseMap
- Segment::getObjectPosesByProvider(
- const std::string& providerName,
- const DateTime& now)
+ Segment::getObjectPosesByProvider(const std::string& providerName, const DateTime& now)
{
ARMARX_CHECK_NOT_NULL(segmentPtr);
- ObjectPoseMap objectPoses = getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
+ ObjectPoseMap objectPoses =
+ getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
updateObjectPoses(objectPoses, now);
return filterObjectPoses(objectPoses);
}
-
wm::Entity*
Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
@@ -389,15 +393,16 @@ namespace armarx::armem::server::obj::instance
if (providerName.empty())
{
wm::Entity* result = nullptr;
- segmentPtr->forEachProviderSegment([&result, &entityID](wm::ProviderSegment & prov)
- {
- if (prov.hasEntity(entityID.entityName))
+ segmentPtr->forEachProviderSegment(
+ [&result, &entityID](wm::ProviderSegment& prov)
{
- result = &prov.getEntity(entityID);
- return false;
- }
- return true;
- });
+ if (prov.hasEntity(entityID.entityName))
+ {
+ result = &prov.getEntity(entityID);
+ return false;
+ }
+ return true;
+ });
return result;
}
else
@@ -415,24 +420,24 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now)
{
bool agentSynchronized = false;
for (auto& [id, objectPose] : objectPoses)
{
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
updateObjectPose(objectPose, now, robot, agentSynchronized);
}
}
-
- void Segment::updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
for (auto& [id, objectPose] : objectPoses)
{
@@ -440,12 +445,11 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized) const
+ void
+ Segment::updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const
{
updateAttachement(objectPose, agent, agentSynchronized);
@@ -455,8 +459,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- Segment::ObjectPoseMap Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
+ Segment::ObjectPoseMap
+ Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
{
ObjectPoseMap result;
for (const auto& [id, objectPose] : objectPoses)
@@ -469,9 +473,10 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::updateAttachement(
- ObjectPose& objectPose, VirtualRobot::RobotPtr agent, bool& synchronized) const
+ void
+ Segment::updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
+ bool& synchronized) const
{
if (not objectPose.attachment.has_value())
{
@@ -487,7 +492,7 @@ namespace armarx::armem::server::obj::instance
}
ARMARX_CHECK_NOT_NULL(agent);
- if (not synchronized) // Synchronize only once.
+ if (not synchronized) // Synchronize only once.
{
ARMARX_CHECK_NOT_NULL(robots.reader);
@@ -499,7 +504,6 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses() const
{
@@ -507,7 +511,6 @@ namespace armarx::armem::server::obj::instance
return getLatestObjectPoses(*segmentPtr);
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
{
@@ -516,7 +519,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPoseMap
Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
{
@@ -525,7 +527,6 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
objpose::ObjectPose
Segment::getLatestObjectPose(const wm::Entity& entity)
{
@@ -534,51 +535,51 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- void Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
{
- coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment & provSegment)
- {
- getLatestObjectPoses(provSegment, out);
- });
+ coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment& provSegment)
+ { getLatestObjectPoses(provSegment, out); });
}
-
- void Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
+ void
+ Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
{
- provSegment.forEachEntity([&out](const wm::Entity & entity)
- {
- if (!entity.empty())
+ provSegment.forEachEntity(
+ [&out](const wm::Entity& entity)
{
- ObjectPose pose = getLatestObjectPose(entity);
- // Try to insert. Fails and returns false if an entry already exists.
- const auto [it, success] = out.insert({pose.objectID, pose});
- if (!success)
+ if (!entity.empty())
{
- // An entry with that ID already exists. We keep the newest.
- if (it->second.timestamp < pose.timestamp)
+ ObjectPose pose = getLatestObjectPose(entity);
+ // Try to insert. Fails and returns false if an entry already exists.
+ const auto [it, success] = out.insert({pose.objectID, pose});
+ if (!success)
{
- it->second = pose;
+ // An entry with that ID already exists. We keep the newest.
+ if (it->second.timestamp < pose.timestamp)
+ {
+ it->second = pose;
+ }
}
}
- }
- });
+ });
}
-
- void Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
+ void
+ Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
{
- entity.getLatestSnapshot().forEachInstance([&out](const wm::EntityInstance & instance)
- {
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
+ entity.getLatestSnapshot().forEachInstance(
+ [&out](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
- fromAron(dto, out);
- });
+ fromAron(dto, out);
+ });
}
-
- arondto::ObjectInstance Segment::getLatestInstanceData(const wm::Entity& entity)
+ arondto::ObjectInstance
+ Segment::getLatestInstanceData(const wm::Entity& entity)
{
ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
@@ -592,7 +593,6 @@ namespace armarx::armem::server::obj::instance
return data;
}
-
::armarx::armem::articulated_object::ArticulatedObjects
Segment::getArticulatedObjects()
{
@@ -601,7 +601,7 @@ namespace armarx::armem::server::obj::instance
ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
::armarx::armem::articulated_object::ArticulatedObjects objects;
- for (const auto&[objectId, objectPose] : objectPoses)
+ for (const auto& [objectId, objectPose] : objectPoses)
{
armem::articulated_object::ArticulatedObject articulatedObject;
articulatedObject.config.jointMap = objectPose.objectJointValues;
@@ -625,7 +625,6 @@ namespace armarx::armem::server::obj::instance
fromAron(dto, description);
articulatedObject.description = description;
-
}
catch (...)
{
@@ -648,7 +647,6 @@ namespace armarx::armem::server::obj::instance
return objects;
}
-
std::map<DateTime, objpose::ObjectPose>
Segment::getObjectPosesInRange(const wm::Entity& entity,
const DateTime& start,
@@ -677,14 +675,14 @@ namespace armarx::armem::server::obj::instance
return result;
}
-
- std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
+ std::optional<simox::OrientedBoxf>
+ Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
-
- objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
+ objpose::ProviderInfo
+ Segment::getProviderInfo(const std::string& providerName)
{
try
{
@@ -703,14 +701,13 @@ namespace armarx::armem::server::obj::instance
}
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
objpose::AttachObjectToRobotNodeOutput output;
- output.success = false; // We are not successful until proven otherwise.
+ output.success = false; // We are not successful until proven otherwise.
ObjectID objectID = armarx::fromIce(input.objectID);
@@ -718,7 +715,8 @@ namespace armarx::armem::server::obj::instance
if (not agent)
{
std::stringstream ss;
- ss << "Tried to attach object " << objectID << " to unknown agent '" << input.agentName << "'."
+ ss << "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: ";
for (const auto& [name, robot] : robots.loaded)
@@ -732,8 +730,8 @@ namespace armarx::armem::server::obj::instance
if (!agent->hasRobotNode(input.frameName))
{
- ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '" << input.frameName
- << "' of agent '" << agent->getName() << "'.";
+ ARMARX_WARNING << "Tried to attach object " << objectID << " to unknown node '"
+ << input.frameName << "' of agent '" << agent->getName() << "'.";
return output;
}
std::string frameName = input.frameName;
@@ -744,7 +742,8 @@ namespace armarx::armem::server::obj::instance
if (!objectEntity || objectEntity->empty())
{
ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- << "' of agent '" << agent->getName() << "', but object is currently not provided.";
+ << "' of agent '" << agent->getName()
+ << "', but object is currently not provided.";
return output;
}
arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
@@ -764,7 +763,8 @@ namespace armarx::armem::server::obj::instance
const auto timestamp = armarx::Clock::Now();
ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
- armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
+ armarx::FramedPose framed(
+ data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
if (frameName == armarx::GlobalFrame)
{
info.poseInFrame = framed.toGlobalEigen(agent);
@@ -779,21 +779,23 @@ namespace armarx::armem::server::obj::instance
// Store attachment in new entity snapshot.
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = objectEntity->id();
update.referencedTime = now;
{
arondto::ObjectInstance updated = data;
toAron(updated.pose.attachment, info);
updated.pose.attachmentValid = true;
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
- ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
+ ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName
+ << "' "
<< "to node '" << info.frameName << "' of agent '" << info.agentName << "'.\n"
- << "Object pose in frame: \n" << info.poseInFrame;
+ << "Object pose in frame: \n"
+ << info.poseInFrame;
output.success = true;
output.attachment = new objpose::data::ObjectAttachmentInfo();
@@ -804,10 +806,8 @@ namespace armarx::armem::server::obj::instance
return output;
}
-
objpose::DetachObjectFromRobotNodeOutput
- Segment::detachObjectFromRobotNode(
- const objpose::DetachObjectFromRobotNodeInput& input)
+ Segment::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input)
{
const armem::Time now = armem::Time::Now();
@@ -840,19 +840,20 @@ namespace armarx::armem::server::obj::instance
output.wasAttached = bool(attachment);
if (attachment)
{
- ARMARX_INFO << "Detached object " << objectID << " by provider '" << providerName << "' from robot node '"
- << attachment->frameName << "' of agent '" << attachment->agentName << "'.";
+ 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 << "' "
+ ARMARX_INFO << "Tried to detach object " << objectID << " by provider '" << providerName
+ << "' "
<< "from robot node, but it was not attached.";
}
return output;
}
-
objpose::DetachAllObjectsFromRobotNodesOutput
Segment::detachAllObjectsFromRobotNodes(
const objpose::DetachAllObjectsFromRobotNodesInput& input)
@@ -863,31 +864,31 @@ namespace armarx::armem::server::obj::instance
objpose::DetachAllObjectsFromRobotNodesOutput output;
output.numDetached = 0;
- segmentPtr->forEachEntity([this, now, &input, &output](wm::Entity & entity)
- {
- const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
- if (data.pose.attachmentValid)
+ segmentPtr->forEachEntity(
+ [this, now, &input, &output](wm::Entity& entity)
{
- ++output.numDetached;
- // Store non-attached pose in new snapshot.
- this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
- }
- });
+ const arondto::ObjectInstance data = this->getLatestInstanceData(entity);
+ if (data.pose.attachmentValid)
+ {
+ ++output.numDetached;
+ // Store non-attached pose in new snapshot.
+ this->storeDetachedSnapshot(entity, data, now, input.commitAttachedPose);
+ }
+ });
ARMARX_INFO << "Detached all objects (" << output.numDetached << ") from robot nodes.";
return output;
}
-
- void Segment::storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose)
+ void
+ Segment::storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose)
{
armem::Commit commit;
- armem::EntityUpdate & update = commit.add();
+ armem::EntityUpdate& update = commit.add();
update.entityID = entity.id();
update.referencedTime = now;
{
@@ -897,7 +898,8 @@ namespace armarx::armem::server::obj::instance
ObjectPose objectPose;
fromAron(data, objectPose);
- VirtualRobot::RobotPtr robot = robots.get(objectPose.robotName, objectPose.providerName);
+ VirtualRobot::RobotPtr robot =
+ robots.get(objectPose.robotName, objectPose.providerName);
bool agentSynchronized = false;
updateAttachement(objectPose, robot, agentSynchronized);
@@ -911,34 +913,34 @@ namespace armarx::armem::server::obj::instance
toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
}
- update.instancesData = { updated.toAron() };
+ update.instancesData = {updated.toAron()};
}
iceMemory.commit(commit);
}
-
std::optional<wm::EntityInstance>
Segment::findClassInstance(const ObjectID& objectID) const
{
- const ObjectID classID = { objectID.dataset(), objectID.className() };
+ const ObjectID classID = {objectID.dataset(), objectID.className()};
try
{
std::optional<wm::EntityInstance> result;
iceMemory.workingMemory->getCoreSegment("Class").forEachProviderSegment(
- [&classID, &result](const wm::ProviderSegment & provSeg)
- {
- if (provSeg.hasEntity(classID.str()))
+ [&classID, &result](const wm::ProviderSegment& provSeg)
{
- result = provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
- return false;
- }
- return true;
- });
+ if (provSeg.hasEntity(classID.str()))
+ {
+ result =
+ provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ return false;
+ }
+ return true;
+ });
if (not result.has_value())
{
- ARMARX_WARNING << deactivateSpam(120)
- << "No provider segment for classID " << classID.str() << " found";
+ ARMARX_WARNING << deactivateSpam(120) << "No provider segment for classID "
+ << classID.str() << " found";
}
return result;
}
@@ -950,8 +952,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
+ void
+ Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
{
@@ -968,7 +970,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::string& filename)
{
@@ -982,7 +983,6 @@ namespace armarx::armem::server::obj::instance
}
}
-
std::optional<armarx::objects::Scene>
Segment::loadScene(const std::filesystem::path& path)
{
@@ -997,17 +997,15 @@ namespace armarx::armem::server::obj::instance
}
}
-
const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
-
std::optional<std::filesystem::path>
Segment::resolveSceneFilepath(const std::string& _filename)
{
std::string filepath = _filename;
- filepath = simox::alg::replace_all(filepath, timestampPlaceholder,
- Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
+ filepath = simox::alg::replace_all(
+ filepath, timestampPlaceholder, Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
if (not simox::alg::ends_with(filepath, ".json"))
{
filepath += ".json";
@@ -1025,8 +1023,8 @@ namespace armarx::armem::server::obj::instance
if (finder->packageFound())
{
std::filesystem::path absDataDir = finder->getDataDir();
- std::filesystem::path absPath = absDataDir / p.sceneSnapshotsPackage
- / p.sceneSnapshotsDirectory / filepath;
+ std::filesystem::path absPath =
+ absDataDir / p.sceneSnapshotsPackage / p.sceneSnapshotsDirectory / filepath;
if (std::filesystem::is_regular_file(absPath))
{
return absPath;
@@ -1046,8 +1044,8 @@ namespace armarx::armem::server::obj::instance
return std::nullopt;
}
-
- armarx::objects::Scene Segment::getSceneSnapshot() const
+ armarx::objects::Scene
+ Segment::getSceneSnapshot() const
{
using armarx::objects::SceneObject;
@@ -1060,36 +1058,42 @@ namespace armarx::armem::server::obj::instance
};
std::map<ObjectID, StampedSceneObject> objects;
- segmentPtr->forEachEntity([&objects](wm::Entity & entity)
- {
- const wm::EntityInstance* entityInstance = entity.findLatestInstance();
- if (entityInstance)
+ segmentPtr->forEachEntity(
+ [&objects](wm::Entity& entity)
{
- std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(*entityInstance);
- if (objectInstance)
+ const wm::EntityInstance* entityInstance = entity.findLatestInstance();
+ if (entityInstance)
{
- const ObjectID objectID = ObjectID::FromString(objectInstance->classID.entityName);
-
- auto it = objects.find(objectID);
- if (it == objects.end() or objectInstance->pose.timestamp > it->second.timestamp)
+ std::optional<arondto::ObjectInstance> objectInstance =
+ tryCast<arondto::ObjectInstance>(*entityInstance);
+ if (objectInstance)
{
- StampedSceneObject& stamped = objects[objectID];
- stamped.timestamp = objectInstance->pose.timestamp;
+ const ObjectID objectID =
+ ObjectID::FromString(objectInstance->classID.entityName);
+
+ auto it = objects.find(objectID);
+ if (it == objects.end() or
+ objectInstance->pose.timestamp > it->second.timestamp)
+ {
+ StampedSceneObject& stamped = objects[objectID];
+ stamped.timestamp = objectInstance->pose.timestamp;
- SceneObject& object = stamped.object;
- object.className = objectID.getClassID().str();
- object.instanceName = objectID.instanceName();
- object.collection = "";
+ SceneObject& object = stamped.object;
+ object.className = objectID.getClassID().str();
+ object.instanceName = objectID.instanceName();
+ object.collection = "";
- object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
- object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ object.position =
+ simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation =
+ simox::math::orientation(objectInstance->pose.objectPoseGlobal);
- object.isStatic = objectInstance->pose.isStatic;
- object.jointValues = objectInstance->pose.objectJointValues;
+ object.isStatic = objectInstance->pose.isStatic;
+ object.jointValues = objectInstance->pose.objectJointValues;
+ }
}
}
- }
- });
+ });
armarx::objects::Scene scene;
for (const auto& [id, stamped] : objects)
@@ -1099,8 +1103,8 @@ namespace armarx::armem::server::obj::instance
return scene;
}
-
- void Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
+ void
+ Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
{
const Time now = Time::Now();
std::map<ObjectID, int> idCounters;
@@ -1117,11 +1121,9 @@ namespace armarx::armem::server::obj::instance
pose.objectType = objpose::ObjectType::KnownObject;
// If not specified, assume loaded objects are static.
pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
- pose.objectID = classID.withInstanceName(
- object.instanceName.empty()
- ? std::to_string(idCounters[classID]++)
- : object.instanceName
- );
+ pose.objectID = classID.withInstanceName(object.instanceName.empty()
+ ? std::to_string(idCounters[classID]++)
+ : object.instanceName);
pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
pose.objectPoseRobot = pose.objectPoseGlobal;
@@ -1142,8 +1144,8 @@ namespace armarx::armem::server::obj::instance
commitObjectPoses(objectPoses);
}
-
- void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ void
+ Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
{
std::stringstream ss;
ss << "Loading scene '" << filename << "' ...";
@@ -1156,7 +1158,7 @@ namespace armarx::armem::server::obj::instance
auto makeSceneName = [](const std::filesystem::path& path)
{
std::filesystem::path filename = path.filename();
- filename.replace_extension(); // Removes extension
+ filename.replace_extension(); // Removes extension
return filename.string();
};
std::string sceneName = makeSceneName(path.value());
@@ -1164,10 +1166,8 @@ namespace armarx::armem::server::obj::instance
// The check seems useless?
if (lockMemory)
{
- segmentPtr->doLocked([this,&snapshot, &sceneName]()
- {
- commitSceneSnapshot(snapshot.value(), sceneName);
- });
+ segmentPtr->doLocked([this, &snapshot, &sceneName]()
+ { commitSceneSnapshot(snapshot.value(), sceneName); });
}
else
{
@@ -1181,8 +1181,8 @@ namespace armarx::armem::server::obj::instance
}
}
-
- void Segment::RemoteGui::setup(const Segment& data)
+ void
+ Segment::RemoteGui::setup(const Segment& data)
{
using namespace armarx::RemoteGui::Client;
@@ -1196,11 +1196,9 @@ namespace armarx::armem::server::obj::instance
storeButton.setLabel("Store Scene");
HBoxLayout storeLoadLineLayout(
- {
- Label(data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsDirectory + "/"),
- storeLoadLine,
- Label(".json")
- });
+ {Label(data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsDirectory + "/"),
+ storeLoadLine,
+ Label(".json")});
HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
GridLayout grid;
@@ -1215,15 +1213,16 @@ namespace armarx::armem::server::obj::instance
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
row++;
- grid.add(Label("Discard Snapshots while Attached"), {row, 0}).add(discardSnapshotsWhileAttached, {row, 1});
+ grid.add(Label("Discard Snapshots while Attached"), {row, 0})
+ .add(discardSnapshotsWhileAttached, {row, 1});
row++;
group.setLabel("Data");
group.addChild(grid);
}
-
- void Segment::RemoteGui::update(Segment& segment)
+ void
+ Segment::RemoteGui::update(Segment& segment)
{
if (loadButton.wasClicked())
{
@@ -1234,41 +1233,41 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
armarx::objects::Scene scene;
- segment.doLocked([&scene, &segment]()
- {
- scene = segment.getSceneSnapshot();
- });
+ segment.doLocked([&scene, &segment]() { scene = segment.getSceneSnapshot(); });
segment.storeScene(storeLoadLine.getValue(), scene);
}
- if (infiniteHistory.hasValueChanged()
- || maxHistorySize.hasValueChanged()
- || discardSnapshotsWhileAttached.hasValueChanged())
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged() ||
+ discardSnapshotsWhileAttached.hasValueChanged())
{
- segment.doLocked([this, &segment]()
- {
- if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
+ segment.doLocked(
+ [this, &segment]()
{
- segment.properties.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- if (segment.segmentPtr)
+ if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
- segment.segmentPtr->setMaxHistorySize(long(segment.properties.maxHistorySize));
+ segment.properties.maxHistorySize =
+ infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
+ if (segment.segmentPtr)
+ {
+ segment.segmentPtr->setMaxHistorySize(
+ long(segment.properties.maxHistorySize));
+ }
}
- }
- segment.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
- });
+ segment.p.discardSnapshotsWhileAttached =
+ discardSnapshotsWhileAttached.getValue();
+ });
}
}
-
- VirtualRobot::RobotPtr Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
+ VirtualRobot::RobotPtr
+ Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
{
std::string robotName = _robotName;
if (robotName.empty())
{
- auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
+ auto antiSpam = deactivateSpam(10 * 60); // 10 minutes.
std::stringstream ss;
if (providerName.empty())
@@ -1285,8 +1284,7 @@ namespace armarx::armem::server::obj::instance
{
ss << ", and no fallback robot name was configured (e.g. via the properties).\n"
<< "For these object poses, the object instance segment is not able "
- << "to provide transformed object poses (global and in robot root frame)."
- ;
+ << "to provide transformed object poses (global and in robot root frame).";
ARMARX_INFO << antiSpam << ss.str();
return nullptr;
@@ -1312,8 +1310,7 @@ namespace armarx::armem::server::obj::instance
// Try to fetch the robot.
ARMARX_CHECK_NOT_NULL(reader);
VirtualRobot::RobotPtr robot = reader->getRobot(
- robotName, Clock::Now(),
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
if (robot)
{
@@ -1321,14 +1318,15 @@ namespace armarx::armem::server::obj::instance
if (not synchronized)
{
ARMARX_INFO << "The robot '" << robotName << "' could be loaded, but not"
- << " synchronized successfully (e.g., the global localization could be missing). "
+ << " synchronized successfully (e.g., the global localization "
+ "could be missing). "
<< "Make sure to synchronize it before use if necessary.";
}
// Store robot if valid.
loaded.emplace(robotName, robot);
}
- return robot; // valid or nullptr
+ return robot; // valid or nullptr
}
}
-}
+} // namespace armarx::armem::server::obj::instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index c5e62d975ca161773c9ae4e8b7d7f5352f0f1922..0def06e72312e97acd457aec5820a98fd290cf21 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -9,24 +9,20 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
-
#include <RobotAPI/libraries/armem/core/Prediction.h>
-#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include "ArticulatedObjectVisu.h"
#include "Decay.h"
-
namespace armarx::armem::arondto
{
class ObjectInstance;
@@ -38,11 +34,11 @@ namespace armarx::armem::server::obj::instance
class Segment : public segment::SpecializedCoreSegment
{
public:
-
struct CommitStats
{
int numUpdated = 0;
};
+
using ObjectPose = objpose::ObjectPose;
using ObjectPoseSeq = objpose::ObjectPoseSeq;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
@@ -53,41 +49,46 @@ namespace armarx::armem::server::obj::instance
std::string robotNode = "Neck_2_Pitch";
float offset = 0.0f;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "calibration.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "calibration.");
};
public:
-
Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment() override;
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "") override;
+ void defineProperties(armarx::PropertyDefinitionsPtr defs,
+ const std::string& prefix = "") override;
void init() override;
void connect(viz::Client arviz);
-
- CommitStats commitObjectPoses(
- const std::string& providerName,
- const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- const Calibration& calibration,
- std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
+ CommitStats commitObjectPoses(const std::string& providerName,
+ const objpose::data::ProvidedObjectPoseSeq& providedPoses,
+ const Calibration& calibration,
+ std::optional<Time> discardUpdatesUntil = std::nullopt);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses,
+ const std::string& providerName = "");
objpose::ObjectPoseMap getObjectPoses(const DateTime& now);
- objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName, const DateTime& now);
+ objpose::ObjectPoseMap getObjectPosesByProvider(const std::string& providerName,
+ const DateTime& now);
- wm::Entity* findObjectEntity(const ObjectID& objectID, const std::string& providerName = "");
+ wm::Entity* findObjectEntity(const ObjectID& objectID,
+ const std::string& providerName = "");
std::optional<simox::OrientedBoxf> getObjectOOBB(const ObjectID& id);
objpose::ProviderInfo getProviderInfo(const std::string& providerName);
- objpose::AttachObjectToRobotNodeOutput attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
- objpose::DetachObjectFromRobotNodeOutput detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
- objpose::DetachAllObjectsFromRobotNodesOutput detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
+ objpose::AttachObjectToRobotNodeOutput
+ attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input);
+ objpose::DetachObjectFromRobotNodeOutput
+ detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input);
+ objpose::DetachAllObjectsFromRobotNodesOutput
+ detachAllObjectsFromRobotNodes(const objpose::DetachAllObjectsFromRobotNodesInput& input);
/**
@@ -100,7 +101,8 @@ namespace armarx::armem::server::obj::instance
*
* @param synchronized Indicates whether the agent is already synchronized to the current time.
*/
- void updateAttachement(ObjectPose& objectPose, VirtualRobot::RobotPtr agent,
+ void updateAttachement(ObjectPose& objectPose,
+ VirtualRobot::RobotPtr agent,
bool& synchronized) const;
@@ -120,34 +122,26 @@ namespace armarx::armem::server::obj::instance
getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
private:
-
ObjectPoseMap getLatestObjectPoses() const;
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now);
- void updateObjectPoses(
- ObjectPoseMap& objectPoses,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
- void updateObjectPose(
- ObjectPose& objectPose,
- const DateTime& now,
- VirtualRobot::RobotPtr agent,
- bool& agentSynchronized
- ) const;
+ void updateObjectPoses(ObjectPoseMap& objectPoses, const DateTime& now);
+ void updateObjectPoses(ObjectPoseMap& objectPoses,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
+ void updateObjectPose(ObjectPose& objectPose,
+ const DateTime& now,
+ VirtualRobot::RobotPtr agent,
+ bool& agentSynchronized) const;
ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
- void storeDetachedSnapshot(
- wm::Entity& entity,
- const arondto::ObjectInstance& data,
- Time now,
- bool commitAttachedPose);
+ void storeDetachedSnapshot(wm::Entity& entity,
+ const arondto::ObjectInstance& data,
+ Time now,
+ bool commitAttachedPose);
std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
@@ -156,7 +150,6 @@ namespace armarx::armem::server::obj::instance
private:
-
void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
@@ -168,7 +161,6 @@ namespace armarx::armem::server::obj::instance
public:
-
/// Loaded robot models identified by the robot name.
struct RobotsCache : public armarx::Logging
{
@@ -177,8 +169,10 @@ namespace armarx::armem::server::obj::instance
std::map<std::string, VirtualRobot::RobotPtr> loaded;
- VirtualRobot::RobotPtr get(const std::string& robotName, const std::string& providerName = "");
+ VirtualRobot::RobotPtr get(const std::string& robotName,
+ const std::string& providerName = "");
};
+
RobotsCache robots;
@@ -192,7 +186,6 @@ namespace armarx::armem::server::obj::instance
private:
-
struct Properties
{
bool discardSnapshotsWhileAttached = true;
@@ -202,6 +195,7 @@ namespace armarx::armem::server::obj::instance
std::string sceneSnapshotsDirectory = "scenes";
std::string sceneSnapshotsToLoad = "";
};
+
Properties p;
@@ -216,7 +210,6 @@ namespace armarx::armem::server::obj::instance
static const std::string timestampPlaceholder;
public:
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
@@ -235,9 +228,7 @@ namespace armarx::armem::server::obj::instance
private:
-
std::unique_ptr<ArticulatedObjectVisu> visu;
-
};
-}
+} // namespace armarx::armem::server::obj::instance