diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
index 226df0fffee073ca8bc5560bbdb118d095c1ebb3..a141b97e4e8b9d4b169ff5b9118c330b71080d5a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.cpp
@@ -143,423 +143,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
iceMemory.commit(commit);
}
- // Segment::CommitStats
- // Segment::commitObjectPoses(const std::string& providerName,
- // const objpose::data::ProvidedObjectPoseSeq& providedPoses,
- // const Calibration& calibration,
- // std::optional<armem::Time> discardUpdatesUntil)
- // {
- // CommitStats stats;
-
- // // Build new poses.
- // objpose::ObjectPoseSeq newObjectPoses;
- // stats.numUpdated = 0;
-
- // // timestamp used to reduce the rpc calls for robot sync
- // Time robotSyncTimestamp = Time::Invalid();
-
- // for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
- // {
- // const Time timestamp = armarx::fromIce<Time>(provided.timestamp);
-
- // // 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);
- // if (entity)
- // {
- // const arondto::ObjectInstance data = getLatestInstanceData(*entity);
-
- // previousPose = objpose::ObjectPose();
- // fromAron(data, *previousPose);
- // }
-
- // bool discard = false;
- // if (discardUpdatesUntil && timestamp < discardUpdatesUntil.value())
- // {
- // // Dicard updates temporarily (e.g. due to head movement).
- // discard = true;
- // }
- // else if (previousPose)
- // {
- // // if (p.discardSnapshotsWhileAttached && previousPose->attachment)
- // // {
- // // // Discard update due to active attachemnt.
- // // discard = true;
- // // }
- // else if (timestamp == previousPose->timestamp)
- // {
- // // Discard update as it is not new.
- // discard = true;
- // }
- // }
-
- // if (discard)
- // {
- // continue;
- // }
-
- // // Update the entity.
- // stats.numUpdated++;
-
- // VirtualRobot::RobotPtr robot = robots.get(provided.robotName, provided.providerName);
- // if (not robot)
- // {
- // ARMARX_INFO << deactivateSpam(1) << "Failed to retrieve robot `"
- // << provided.robotName << "`.";
- // }
-
- // // robot may be null!
-
- // bool synchronized = false;
-
- // // Update the robot to obtain correct local -> global transformation
- // if (robot and robotSyncTimestamp != timestamp)
- // {
- // synchronized = robots.reader->synchronizeRobot(*robot, timestamp);
- // if (not synchronized)
- // {
- // ARMARX_INFO << deactivateSpam(5) << "Failed to synchronize robot to timestamp "
- // << timestamp << " of provided object pose (" << provided.objectID
- // << "). This is " << (Clock::Now() - timestamp).toSecondsDouble()
- // << " seconds in the past.";
- // }
-
- // robotSyncTimestamp = timestamp;
-
- // // Apply calibration offset after synchronizing the robot.
- // {
- // if (calibration.offset != 0 and robot->hasRobotNode(calibration.robotNode))
- // {
- // VirtualRobot::RobotNodePtr robotNode =
- // robot->getRobotNode(calibration.robotNode);
-
- // float value = robotNode->getJointValue();
- // float newValue = value + calibration.offset;
-
- // /*
- // * If newValue is out of the joint's limits, then the calibration would be ignored.
- // * In that case, we expand the joint value limits of the local robot model to allow
- // * 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();
- // bool limitsChanged = false;
- // if (newValue < limits.low)
- // {
- // limits.low = newValue;
- // limitsChanged = true;
- // }
- // if (newValue > limits.high)
- // {
- // limits.high = newValue;
- // limitsChanged = true;
- // }
- // if (limitsChanged)
- // {
- // robotNode->setJointLimits(limits.low, limits.high);
- // }
-
- // robotNode->setJointValue(newValue);
- // }
- // }
- // }
- // else if (robot and timestamp == robotSyncTimestamp)
- // {
- // synchronized = true;
- // }
-
- // if (not synchronized)
- // {
- // /*
- // * We know nothing about the current robot configuration. So the behvaiour of the
- // * following code is the same as if we have no robot model at all.
- // */
- // ARMARX_INFO << deactivateSpam(5)
- // << "Robot could not be synchronized; discarding robot";
- // robot = nullptr;
- // }
-
- // objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
- // if (provided.objectPoseFrame.empty())
- // {
- // objpose::data::ProvidedObjectPose copy = provided;
- // copy.objectPoseFrame = armarx::GlobalFrame;
- // newPose.fromProvidedPose(copy, robot); // robot == nullptr is OK.
- // }
- // else
- // {
- // newPose.fromProvidedPose(provided, robot); // robot == nullptr is OK.
- // }
-
- // if (previousPose && previousPose->attachment)
- // {
- // // Keep current attachment.
- // ARMARX_CHECK(!p.discardSnapshotsWhileAttached);
- // newPose.attachment = previousPose->attachment;
- // }
-
- // if (newPose.objectID.dataset().empty())
- // {
- // // Try to find the data set.
- // // const std::string dataset =
- // // classNameToDatasetCache.get(newPose.objectID.className());
- // // if (!dataset.empty())
- // // {
-
-
- // newPose.objectID = {
- // dataset, newPose.objectID.className(), newPose.objectID.instanceName()};
- // // }
- // }
- // if (!provided.localOOBB)
- // {
- // // Try to load oobb from disk.
- // newPose.localOOBB = getObjectOOBB(newPose.objectID);
- // }
- // }
-
- // commitObjectPoses(newObjectPoses, providerName);
-
- // return stats;
- // }
-
- // void
- // Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
- // {
- // Time now = Time::Now();
-
- // ARMARX_CHECK_NOT_NULL(segmentPtr);
- // const MemoryID coreSegmentID = segmentPtr->id();
-
- // Commit commit;
- // for (const objpose::ObjectPose& pose : objectPoses)
- // {
- // EntityUpdate& update = commit.updates.emplace_back();
-
- // const MemoryID providerID = coreSegmentID.withProviderSegmentName(
- // providerName.empty() ? pose.providerName : providerName);
-
- // update.entityID = providerID.withEntityName(pose.objectID.str());
- // update.arrivedTime = now;
- // update.referencedTime = pose.timestamp;
- // update.confidence = pose.confidence;
-
- // arondto::ObjectInstance dto;
- // toAron(dto, pose);
- // // Search for object class.
- // if (auto instance = findClassInstance(pose.objectID))
- // {
- // toAron(dto.classID, instance->id());
- // }
- // else
- // {
- // toAron(dto.classID, MemoryID());
- // }
- // toAron(dto.sourceID, MemoryID());
- // update.instancesData.push_back(dto.toAron());
- // }
- // // Commit non-locking.
- // iceMemory.commit(commit);
- // }
-
- // objpose::ObjectPoseMap
- // Segment::getObjectPoses(const DateTime& now)
- // {
- // ObjectPoseMap objectPoses = getLatestObjectPoses();
- // updateObjectPoses(objectPoses, now);
- // return filterObjectPoses(objectPoses);
- // }
-
- // objpose::ObjectPoseMap
- // Segment::getObjectPosesByProvider(const std::string& providerName, const DateTime& now)
- // {
- // ARMARX_CHECK_NOT_NULL(segmentPtr);
- // ObjectPoseMap objectPoses =
- // getLatestObjectPoses(segmentPtr->getProviderSegment(providerName));
- // updateObjectPoses(objectPoses, now);
- // return filterObjectPoses(objectPoses);
- // }
-
- // wm::Entity*
- // Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
- // {
- // ARMARX_CHECK_NOT_NULL(segmentPtr);
- // armem::MemoryID entityID = armem::MemoryID().withEntityName(objectID.str());
- // if (providerName.empty())
- // {
- // wm::Entity* result = nullptr;
- // segmentPtr->forEachProviderSegment(
- // [&result, &entityID](wm::ProviderSegment& prov)
- // {
- // if (prov.hasEntity(entityID.entityName))
- // {
- // result = &prov.getEntity(entityID);
- // return false;
- // }
- // return true;
- // });
- // return result;
- // }
- // else
- // {
- // entityID.providerSegmentName = providerName;
- // if (segmentPtr->hasProviderSegment(providerName))
- // {
- // wm::ProviderSegment& prov = segmentPtr->getProviderSegment(providerName);
- // return prov.hasEntity(entityID.entityName) ? &prov.getEntity(entityID) : nullptr;
- // }
- // else
- // {
- // return nullptr;
- // }
- // }
- // }
-
- // 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);
- // updateObjectPose(objectPose, now, robot, agentSynchronized);
- // }
- // }
-
- // void
- // Segment::updateObjectPoses(ObjectPoseMap& objectPoses,
- // const DateTime& now,
- // VirtualRobot::RobotPtr agent,
- // bool& agentSynchronized) const
- // {
- // for (auto& [id, objectPose] : objectPoses)
- // {
- // updateObjectPose(objectPose, now, agent, agentSynchronized);
- // }
- // }
-
- // void
- // Segment::updateObjectPose(ObjectPose& objectPose,
- // const DateTime& now,
- // VirtualRobot::RobotPtr agent,
- // bool& agentSynchronized) const
- // {
- // updateAttachement(objectPose, agent, agentSynchronized);
-
- // if (decay.enabled)
- // {
- // decay.updateConfidence(objectPose, now);
- // }
- // }
-
- // Segment::ObjectPoseMap
- // Segment::filterObjectPoses(const ObjectPoseMap& objectPoses) const
- // {
- // ObjectPoseMap result;
- // for (const auto& [id, objectPose] : objectPoses)
- // {
- // if (!(decay.enabled && objectPose.confidence < decay.removeObjectsBelowConfidence))
- // {
- // result[id] = objectPose;
- // }
- // }
- // return result;
- // }
-
- // void
- // Segment::updateAttachement(ObjectPose& objectPose,
- // VirtualRobot::RobotPtr agent,
- // bool& synchronized) const
- // {
- // if (not objectPose.attachment.has_value())
- // {
- // // No attachment, nothing to do.
- // return;
- // }
- // ARMARX_CHECK(objectPose.attachment);
-
- // if (not agent)
- // {
- // // Without a robot model, we cannot update the object pose.
- // return;
- // }
- // ARMARX_CHECK_NOT_NULL(agent);
-
- // if (not synchronized) // Synchronize only once.
- // {
- // ARMARX_CHECK_NOT_NULL(robots.reader);
-
- // const armarx::DateTime timestamp = armarx::Clock::Now();
-
- // ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
- // synchronized = true;
- // }
- // objectPose.updateAttached(agent);
- // }
-
- // objpose::ObjectPoseMap
- // Segment::getLatestObjectPoses() const
- // {
- // ARMARX_CHECK_NOT_NULL(segmentPtr);
- // return getLatestObjectPoses(*segmentPtr);
- // }
-
- // objpose::ObjectPoseMap
- // Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg)
- // {
- // ObjectPoseMap result;
- // getLatestObjectPoses(coreSeg, result);
- // return result;
- // }
-
- // objpose::ObjectPoseMap
- // Segment::getLatestObjectPoses(const wm::ProviderSegment& provSeg)
- // {
- // ObjectPoseMap result;
- // getLatestObjectPoses(provSeg, result);
- // return result;
- // }
-
- // objpose::ObjectPose
- // Segment::getLatestObjectPose(const wm::Entity& entity)
- // {
- // ObjectPose result;
- // getLatestObjectPose(entity, result);
- // return result;
- // }
-
- // void
- // Segment::getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out)
- // {
- // coreSeg.forEachProviderSegment([&out](const wm::ProviderSegment& provSegment)
- // { getLatestObjectPoses(provSegment, out); });
- // }
-
- // void
- // Segment::getLatestObjectPoses(const wm::ProviderSegment& provSegment, ObjectPoseMap& out)
- // {
- // provSegment.forEachEntity(
- // [&out](const wm::Entity& entity)
- // {
- // if (!entity.empty())
- // {
- // 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)
- // {
- // // An entry with that ID already exists. We keep the newest.
- // if (it->second.timestamp < pose.timestamp)
- // {
- // it->second = pose;
- // }
- // }
- // }
- // });
- // }
std::vector<armarx::armem::arondto::FamiliarObjectInstance>
Segment::getFamiliarObjects(const DateTime& now)
@@ -618,737 +201,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
return familiarObjectsByProvider;
}
- // void
- // Segment::getLatestObjectPose(const wm::Entity& entity, ObjectPose& out)
- // {
- // entity.getLatestSnapshot().forEachInstance(
- // [&out](const wm::EntityInstance& instance)
- // {
- // arondto::ObjectInstance dto;
- // dto.fromAron(instance.data());
-
- // fromAron(dto, out);
- // });
- // }
-
- // arondto::ObjectInstance
- // Segment::getLatestInstanceData(const wm::Entity& entity)
- // {
- // ARMARX_CHECK_GREATER_EQUAL(entity.size(), 1);
- // const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
-
- // ARMARX_CHECK_EQUAL(snapshot.size(), 1);
- // const wm::EntityInstance& instance = snapshot.getInstance(0);
-
- // arondto::ObjectInstance data;
- // data.fromAron(instance.data());
-
- // return data;
- // }
-
- // ::armarx::armem::articulated_object::ArticulatedObjects
- // Segment::getArticulatedObjects()
- // {
- // objpose::ObjectPoseMap objectPoses = getObjectPoses(Time::Now());
-
- // ARMARX_INFO << "Found " << objectPoses.size() << " object poses";
-
- // ::armarx::armem::articulated_object::ArticulatedObjects objects;
- // for (const auto& [objectId, objectPose] : objectPoses)
- // {
- // armem::articulated_object::ArticulatedObject articulatedObject;
- // articulatedObject.config.jointMap = objectPose.objectJointValues;
- // articulatedObject.config.globalPose = objectPose.objectPoseGlobal;
- // articulatedObject.config.timestamp = objectPose.timestamp;
- // articulatedObject.instance = objectPose.objectID.instanceName();
- // articulatedObject.timestamp = objectPose.timestamp;
-
- // ARMARX_INFO << "Object id is " << objectId.str();
- // ARMARX_INFO << "Object id for objectPose is " << objectPose.objectID.str();
-
- // // Search for object class.
- // if (auto classInstance = findClassInstance(objectPose.objectID))
- // {
- // arondto::ObjectClass dto;
-
- // try
- // {
- // dto.fromAron(classInstance->data());
- // robot_state::description::RobotDescription description;
-
- // fromAron(dto, description);
- // articulatedObject.description = description;
- // }
- // catch (...)
- // {
- // ARMARX_WARNING << "Conversion failed!";
- // continue;
- // }
- // }
- // else
- // {
- // ARMARX_WARNING << "Class instance not found!";
- // continue;
- // }
-
- // if (not articulatedObject.config.jointMap.empty())
- // {
- // objects.push_back(articulatedObject);
- // }
- // }
-
- // return objects;
- // }
-
- // std::map<DateTime, objpose::ObjectPose>
- // Segment::getObjectPosesInRange(const wm::Entity& entity,
- // const DateTime& start,
- // const DateTime& end)
- // {
- // std::map<DateTime, objpose::ObjectPose> result;
-
- // entity.forEachSnapshotInTimeRange(
- // start,
- // end,
- // [&result](const wm::EntitySnapshot& snapshot)
- // {
- // snapshot.forEachInstance(
- // [&result, &snapshot](const wm::EntityInstance& instance)
- // {
- // arondto::ObjectInstance dto;
- // dto.fromAron(instance.data());
-
- // ObjectPose pose;
- // fromAron(dto, pose);
-
- // result.emplace(snapshot.time(), pose);
- // });
- // });
-
- // return result;
- // }
-
- // std::optional<simox::OrientedBoxf>
- // Segment::getObjectOOBB(const ObjectID& id)
- // {
- // return oobbCache.get(id);
- // }
-
- // objpose::ProviderInfo
- // Segment::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());
- // }
- // }
-
- // 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.
-
- // ObjectID objectID = armarx::fromIce(input.objectID);
-
- // VirtualRobot::RobotPtr agent = robots.get(input.agentName);
- // if (not agent)
- // {
- // std::stringstream ss;
- // 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)
- // {
- // ss << "\n- '" << name << "'";
- // }
- // ARMARX_WARNING << ss.str();
- // return output;
- // }
- // ARMARX_CHECK_NOT_NULL(agent);
-
- // 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).
- // wm::Entity* objectEntity = this->findObjectEntity(objectID, input.providerName);
- // if (!objectEntity || objectEntity->empty())
- // {
- // ARMARX_WARNING << "Tried to attach object " << objectID << " to node '" << frameName
- // << "' of agent '" << agent->getName()
- // << "', but object is currently not provided.";
- // return output;
- // }
- // arondto::ObjectInstance data = getLatestInstanceData(*objectEntity);
-
- // objpose::ObjectAttachmentInfo info;
- // info.agentName = agent->getName();
- // info.frameName = frameName;
-
- // if (input.poseInFrame)
- // {
- // info.poseInFrame = PosePtr::dynamicCast(input.poseInFrame)->toEigen();
- // }
- // else
- // {
- // ARMARX_CHECK_NOT_NULL(robots.reader);
-
- // const auto timestamp = armarx::Clock::Now();
- // ARMARX_CHECK(robots.reader->synchronizeRobot(*agent, timestamp));
-
- // armarx::FramedPose framed(
- // data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
- // if (frameName == armarx::GlobalFrame)
- // {
- // info.poseInFrame = framed.toGlobalEigen(agent);
- // }
- // else
- // {
- // framed.changeFrame(agent, info.frameName);
- // info.poseInFrame = framed.toEigen();
- // }
- // }
-
- // // Store attachment in new entity snapshot.
- // {
- // armem::Commit commit;
- // 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()};
- // }
- // iceMemory.commit(commit);
- // }
-
- // 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;
-
- // 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;
- // }
-
- // objpose::DetachObjectFromRobotNodeOutput
- // Segment::detachObjectFromRobotNode(const objpose::DetachObjectFromRobotNodeInput& input)
- // {
- // const armem::Time now = armem::Time::Now();
-
- // ObjectID objectID = armarx::fromIce(input.objectID);
- // std::string providerName = input.providerName;
-
- // std::optional<objpose::arondto::ObjectAttachmentInfo> attachment;
- // {
- // // Remove from latest pose (if it was cached).
- // // Find object pose (provider name can be empty).
- // wm::Entity* entity = this->findObjectEntity(objectID, input.providerName);
- // if (entity)
- // {
- // const arondto::ObjectInstance data = getLatestInstanceData(*entity);
- // if (data.pose.attachmentValid)
- // {
- // attachment = data.pose.attachment;
-
- // // Store non-attached pose in new snapshot.
- // storeDetachedSnapshot(*entity, data, now, input.commitAttachedPose);
- // }
- // if (providerName.empty())
- // {
- // providerName = data.pose.providerName;
- // }
- // }
- // }
-
- // 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;
- // }
-
- // objpose::DetachAllObjectsFromRobotNodesOutput
- // Segment::detachAllObjectsFromRobotNodes(
- // const objpose::DetachAllObjectsFromRobotNodesInput& input)
- // {
- // ARMARX_CHECK_NOT_NULL(segmentPtr);
-
- // const armem::Time now = armem::Time::Now();
-
- // 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)
- // {
- // ++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)
- // {
- // armem::Commit commit;
- // armem::EntityUpdate& update = commit.add();
- // update.entityID = entity.id();
- // update.referencedTime = now;
- // {
- // arondto::ObjectInstance updated;
- // if (commitAttachedPose and data.pose.attachmentValid)
- // {
- // ObjectPose objectPose;
- // fromAron(data, objectPose);
-
- // VirtualRobot::RobotPtr robot =
- // robots.get(objectPose.robotName, objectPose.providerName);
- // bool agentSynchronized = false;
- // updateAttachement(objectPose, robot, agentSynchronized);
-
- // objectPose.attachment = std::nullopt;
- // toAron(updated, objectPose);
- // }
- // else
- // {
- // updated = data;
- // updated.pose.attachmentValid = false;
- // toAron(updated.pose.attachment, objpose::ObjectAttachmentInfo{});
- // }
-
- // update.instancesData = {updated.toAron()};
- // }
- // iceMemory.commit(commit);
- // }
-
- // std::optional<wm::EntityInstance>
- // Segment::findClassInstance(const ObjectID& objectID) const
- // {
- // 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()))
- // {
- // 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";
- // }
- // return result;
- // }
- // catch (const armem::error::ArMemError& e)
- // {
- // // Some segment or entity did not exist.
- // ARMARX_WARNING << e.what();
- // return std::nullopt;
- // }
- // }
-
- // void
- // Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
- // {
- // if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
- // {
- // ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
- // try
- // {
- // simox::json::write(path.value(), scene, 2);
- // }
- // catch (const simox::json::error::JsonError& e)
- // {
- // ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
- // }
- // }
- // }
-
- // std::optional<armarx::objects::Scene>
- // Segment::loadScene(const std::string& filename)
- // {
- // if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
- // {
- // return loadScene(path.value());
- // }
- // else
- // {
- // return std::nullopt;
- // }
- // }
-
- // std::optional<armarx::objects::Scene>
- // Segment::loadScene(const std::filesystem::path& path)
- // {
- // try
- // {
- // ARMARX_CHECK(std::filesystem::exists(path)) << path;
- // return simox::json::read<armarx::objects::Scene>(path);
- // }
- // catch (const simox::json::error::JsonError& e)
- // {
- // ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
- // return std::nullopt;
- // }
- // }
-
- // const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
-
- // std::optional<std::filesystem::path>
- // Segment::resolveSceneFilepath(const std::string& _filename)
- // {
- // std::stringstream log;
- // std::filesystem::path filepath = _filename;
-
- // 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";
- // }
-
- // if (filepath.is_absolute())
- // {
- // return filepath;
- // }
-
- // armarx::PackagePath packagePath = [&filepath, this]()
- // {
- // if (filepath.has_parent_path())
- // {
- // // Interpret the first component as package name.
- // std::string packageName = *filepath.begin();
- // return PackagePath(packageName, std::filesystem::relative(filepath, packageName));
- // }
- // else
- // {
- // // Only a filename => Interprete it as relative to 'Package/scenes/'.
- // return PackagePath(p.sceneSnapshotsPackage, p.sceneSnapshotsDirectory / filepath);
- // }
- // }();
-
- // try
- // {
- // std::filesystem::path systemPath = packagePath.toSystemPath();
- // return systemPath;
- // }
- // catch (const armarx::PackageNotFoundException& e)
- // {
- // ARMARX_INFO << "Could not interpret '" << packagePath.serialize().package
- // << "' as ArmarX package name (" << e.what() << ").";
- // return std::nullopt;
- // }
- // }
-
- // armarx::objects::Scene
- // Segment::getSceneSnapshot() const
- // {
- // using armarx::objects::SceneObject;
-
- // // We only store the latest version of each objectID.
-
- // struct StampedSceneObject
- // {
- // SceneObject object;
- // Time timestamp;
- // };
-
- // std::map<ObjectID, StampedSceneObject> objects;
- // segmentPtr->forEachEntity(
- // [&objects](wm::Entity& entity)
- // {
- // const wm::EntityInstance* entityInstance = entity.findLatestInstance();
- // if (entityInstance)
- // {
- // std::optional<arondto::ObjectInstance> objectInstance =
- // tryCast<arondto::ObjectInstance>(*entityInstance);
- // if (objectInstance)
- // {
- // 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 = "";
-
- // 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;
- // }
- // }
- // }
- // });
-
- // armarx::objects::Scene scene;
- // for (const auto& [id, stamped] : objects)
- // {
- // scene.objects.emplace_back(std::move(stamped.object));
- // }
- // return scene;
- // }
-
- // void
- // Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
- // {
- // const Time now = Time::Now();
- // std::map<ObjectID, int> idCounters;
-
- // objpose::ObjectPoseSeq objectPoses;
-
- // for (const auto& object : scene.objects)
- // {
- // if (simox::alg::starts_with(object.className, "#"))
- // {
- // // marked to be ignored
- // continue;
- // }
-
- // const ObjectID classID = object.getClassID(objectFinder);
-
- // objpose::ObjectPose& pose = objectPoses.emplace_back();
-
- // pose.providerName = sceneName;
- // 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.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
- // pose.objectPoseRobot = pose.objectPoseGlobal;
- // pose.objectPoseOriginal = pose.objectPoseGlobal;
- // pose.objectPoseOriginalFrame = armarx::GlobalFrame;
-
- // pose.objectJointValues = object.jointValues;
-
- // pose.robotConfig = {};
- // pose.robotPose = Eigen::Matrix4f::Identity();
- // pose.robotName = "";
-
- // pose.confidence = 1.0;
- // pose.localOOBB = getObjectOOBB(classID);
- // pose.timestamp = now;
- // }
-
- // commitObjectPoses(objectPoses);
- // }
-
- // void
- // Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
- // {
- // std::stringstream ss;
- // ss << "Loading scene '" << filename << "' ...";
- // if (std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
- // {
- // ARMARX_INFO << ss.str() << "\nfrom " << path.value();
-
- // if (const auto snapshot = loadScene(path.value()))
- // {
- // auto makeSceneName = [](const std::filesystem::path& path)
- // {
- // std::filesystem::path filename = path.filename();
- // filename.replace_extension(); // Removes extension
- // return filename.string();
- // };
- // std::string sceneName = makeSceneName(path.value());
-
- // // The check seems useless?
- // if (lockMemory)
- // {
- // segmentPtr->doLocked([this, &snapshot, &sceneName]()
- // { commitSceneSnapshot(snapshot.value(), sceneName); });
- // }
- // else
- // {
- // commitSceneSnapshot(snapshot.value(), sceneName);
- // }
- // }
- // }
- // else
- // {
- // ARMARX_INFO << ss.str() << " failed: Could not resolve scene name or path.";
- // }
- // }
-
- // void
- // Segment::RemoteGui::setup(const Segment& data)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // maxHistorySize.setValue(std::max(1, int(data.properties.maxHistorySize)));
- // maxHistorySize.setRange(1, 1e6);
- // infiniteHistory.setValue(data.properties.maxHistorySize == -1);
- // discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
-
- // const std::string storeLoadPath = [&data]()
- // {
- // const std::vector<std::string> scenes = data.p.getSceneSnapshotsToLoad();
- // if (scenes.empty())
- // {
- // std::string storeLoadFilename = "Scene_" + timestampPlaceholder;
- // return data.p.sceneSnapshotsPackage + "/" + data.p.sceneSnapshotsToLoad;
- // }
- // else
- // {
- // return scenes.front();
- // }
- // }();
-
- // storeLoadLine.setValue(storeLoadPath);
- // loadButton.setLabel("Load Scene");
- // storeButton.setLabel("Store Scene");
-
- // HBoxLayout storeLoadLineLayout({storeLoadLine, Label(".json")});
- // HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
-
- // detachAllObjectsButton.setLabel("Detach All Objects");
- // detachAllObjectsCommitAttachedPoseCheckBox.setValue(true);
- // HBoxLayout detachAllObjectsCommitAttachedPoseLayout(
- // {Label("Commit Attached Pose"), detachAllObjectsCommitAttachedPoseCheckBox});
-
-
- // GridLayout grid;
- // int row = 0;
-
- // grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
- // row++;
- // grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
- // row++;
-
- // grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
- // 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});
- // row++;
-
- // grid.add(detachAllObjectsButton, {row, 0})
- // .add(detachAllObjectsCommitAttachedPoseLayout, {row, 1});
-
- // group.setLabel("Data");
- // group.addChild(grid);
- // }
-
- // void
- // Segment::RemoteGui::update(Segment& segment)
- // {
- // if (loadButton.wasClicked())
- // {
- // bool lockMemory = true;
- // segment.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
- // }
-
- // if (storeButton.wasClicked())
- // {
- // armarx::objects::Scene scene;
- // segment.doLocked([&scene, &segment]() { scene = segment.getSceneSnapshot(); });
- // segment.storeScene(storeLoadLine.getValue(), scene);
- // }
-
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged() ||
- // discardSnapshotsWhileAttached.hasValueChanged())
- // {
- // segment.doLocked(
- // [this, &segment]()
- // {
- // if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
- // {
- // segment.properties.maxHistorySize =
- // infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
- // if (segment.segmentPtr)
- // {
- // segment.segmentPtr->setMaxHistorySize(
- // long(segment.properties.maxHistorySize));
- // }
- // }
-
- // segment.p.discardSnapshotsWhileAttached =
- // discardSnapshotsWhileAttached.getValue();
- // });
- // }
-
- // if (detachAllObjectsButton.wasClicked())
- // {
- // objpose::DetachAllObjectsFromRobotNodesInput input;
- // input.commitAttachedPose = detachAllObjectsCommitAttachedPoseCheckBox.getValue();
-
- // objpose::DetachAllObjectsFromRobotNodesOutput output = segment.doLocked(
- // [&segment, &input]() { return segment.detachAllObjectsFromRobotNodes(input); });
- // (void)output;
- // }
- // }
VirtualRobot::RobotPtr
Segment::RobotsCache::get(const std::string& _robotName, const std::string& providerName)
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
index 9a113e8aecf42a562129d6cc1cef7db6645fdf73..f5fa90ad48d7b7a8d5529fc7b46e5d876f6b845f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Segment.h
@@ -70,102 +70,17 @@ namespace armarx::armem::server::obj::familiar_object_instance
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);
-
- // 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);
-
-
- /**
- * @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;
-
-
std::vector<armarx::armem::arondto::FamiliarObjectInstance>
getFamiliarObjects(const DateTime& now);
std::map<std::string, std::vector<armarx::armem::arondto::FamiliarObjectInstance>>
getFamiliarObjectsByProvider(const DateTime& now);
-
- // static ObjectPoseMap getLatestObjectPoses(const wm::ProviderSegment& provSeg);
- // static ObjectPose getLatestObjectPose(const wm::Entity& entity);
-
- // static void getLatestObjectPoses(const wm::CoreSegment& coreSeg, ObjectPoseMap& out);
- // static void getLatestObjectPoses(const wm::ProviderSegment& provSeg, ObjectPoseMap& out);
- // static void getLatestObjectPose(const wm::Entity& entity, ObjectPose& out);
-
- // static arondto::ObjectInstance getLatestInstanceData(const wm::Entity& entity);
-
- // static std::map<DateTime, ObjectPose>
- // 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;
-
-
- // ObjectPoseMap filterObjectPoses(const ObjectPoseMap& objectPoses) const;
-
-
- // void storeDetachedSnapshot(wm::Entity& entity,
- // const arondto::ObjectInstance& data,
- // Time now,
- // bool commitAttachedPose);
-
-
- // std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
-
+
friend struct DetachVisitor;
- private:
- // void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
-
- // armarx::objects::Scene getSceneSnapshot() const;
- // void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
- // void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
-
-
public:
/// Loaded robot models identified by the robot name.
struct RobotsCache : public armarx::Logging
@@ -194,54 +109,15 @@ namespace armarx::armem::server::obj::familiar_object_instance
private:
struct Properties
{
- // bool discardSnapshotsWhileAttached = true;
-
- // /// Package containing the scene snapshots
- // std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
- // std::string sceneSnapshotsDirectory = "scenes";
- // std::string sceneSnapshotsToLoad = "";
-
- // bool autoReloadSceneSnapshotsOnFileChange = false;
-
- // std::vector<std::string> getSceneSnapshotsToLoad() const;
+
};
Properties p;
-
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- // simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
- /// Class name -> dataset name.
- // simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
-
- // std::unique_ptr<CMakePackageFinder> finder;
-
+
static const std::string timestampPlaceholder;
- public:
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::LineEdit storeLoadLine;
- // armarx::RemoteGui::Client::Button storeButton;
- // armarx::RemoteGui::Client::Button loadButton;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // armarx::RemoteGui::Client::Button detachAllObjectsButton;
- // armarx::RemoteGui::Client::CheckBox detachAllObjectsCommitAttachedPoseCheckBox;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
-
- // private:
- // SimpleRunningTask<>::pointer_type fileWatcherTask;
+
};
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp
index 7a4397da146cb78a78f1caf7f96a82208ac36a64..dab922c5a7d4726ec5655e6450fe7ff864438461 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/SegmentAdapter.cpp
@@ -24,6 +24,7 @@
#include <iterator>
#include <memory>
+#include <optional>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <VirtualRobot/Robot.h>
@@ -33,6 +34,7 @@
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
@@ -112,8 +114,18 @@ namespace armarx::armem::server::obj::familiar_object_instance
const Ice::Current&)
{
+ if(data.empty())
+ {
+ return;
+ }
+
+ // Note, that in general, timestamps in `data` can differ. Therefore, we maintain the sync timestamp
+ // and check whether we have to update the robot. Otherwise, we can avoid these Ice calls.
+ std::optional<armarx::DateTime> syncTimestamp;
+
+
const auto convertAndFillFamiliarObjectInstances =
- [this, providerName](const ::armarx::aron::data::dto::DictPtr& data)
+ [this, providerName, &syncTimestamp](const ::armarx::aron::data::dto::DictPtr& data)
-> armarx::armem::arondto::FamiliarObjectInstance
{
auto famObjInstance = armarx::armem::arondto::FamiliarObjectInstance::FromAron(data);
@@ -130,9 +142,13 @@ namespace armarx::armem::server::obj::familiar_object_instance
segment.robots.get(famObjInstance.poseSensFrame.header.agent, providerName);
ARMARX_CHECK_NOT_NULL(robot) << famObjInstance.poseSensFrame.header.agent;
- // const Time timestamp = armarx::fromIce<Time>(famObjInstance);
- // FIXME proper timestmap
- ARMARX_CHECK(segment.robots.reader->synchronizeRobot(*robot, armarx::Clock::Now()));
+
+ if(not syncTimestamp.has_value() or syncTimestamp.value() != famObjInstance.timestamp)
+ {
+ ARMARX_CHECK(segment.robots.reader->synchronizeRobot(*robot, famObjInstance.timestamp));
+ syncTimestamp = famObjInstance.timestamp;
+ }
+
// fill global pose info
famObjInstance.poseGlobal.emplace();
@@ -154,40 +170,6 @@ namespace armarx::armem::server::obj::familiar_object_instance
segment.commit(familiarObjectInstances, providerName);
}
- // void SegmentAdapter::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
- // {
- // updateProviderInfo(providerName, info);
- // }
-
-
- // void SegmentAdapter::reportObjectPoses(
- // const std::string& providerName, const objpose::data::ProvidedObjectPoseSeq& providedPoses, const Ice::Current&)
- // {
- // ARMARX_VERBOSE << "Received " << providedPoses.size() << " object poses from provider '" << providerName << "'.";
- // updateObjectPoses(providerName, providedPoses);
- // }
-
-
- // void SegmentAdapter::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;
- // }
- // segment.doLocked([this, &providerName, &info]()
- // {
- // std::stringstream ss;
- // for (const auto& id : info.supportedObjects)
- // {
- // ss << "- " << id << "\n";
- // }
- // ARMARX_VERBOSE << "Provider '" << providerName << "' available.\n"
- // << "Supported objects: \n" << ss.str();
- // segment.providers[providerName] = info;
- // });
- // }
void
@@ -200,314 +182,16 @@ namespace armarx::armem::server::obj::familiar_object_instance
}
}
- // objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPoses(const Ice::Current&)
- // {
- // TIMING_START(tGetObjectPoses);
-
- // const Time now = Time::Now();
- // const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now]()
- // {
- // return simox::alg::get_values(segment.getObjectPoses(now));
- // });
- // const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
-
- // TIMING_END_STREAM(tGetObjectPoses, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t GetObjectPoses() [ms]", new Variant(tGetObjectPoses.toMilliSecondsDouble()) },
- // });
- // }
-
- // return result;
- // }
-
- // objpose::data::ObjectPoseSeq SegmentAdapter::getObjectPosesByProvider(const std::string& providerName, const Ice::Current&)
- // {
- // TIMING_START(GetObjectPoses);
-
- // const Time now = Time::Now();
- // const objpose::ObjectPoseSeq objectPoses = segment.doLocked([this, &now, &providerName]()
- // {
- // return simox::alg::get_values(segment.getObjectPosesByProvider(providerName, now));
- // });
- // const objpose::data::ObjectPoseSeq result = objpose::toIce(objectPoses);
-
- // TIMING_END_STREAM(GetObjectPoses, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t GetObjectPosesByProvider() [ms]", new Variant(GetObjectPoses.toMilliSecondsDouble()) },
- // });
- // }
-
- // return result;
- // }
-
-
- // objpose::observer::RequestObjectsOutput SegmentAdapter::requestObjects(
- // const objpose::observer::RequestObjectsInput& input, const Ice::Current&)
- // {
- // std::map<std::string, objpose::provider::RequestObjectsInput> providerRequests;
- // std::map<std::string, objpose::ObjectPoseProviderPrx> proxies;
-
- // objpose::observer::RequestObjectsOutput output;
-
- // auto updateProxy = [&](const std::string & providerName)
- // {
- // if (proxies.count(providerName) == 0)
- // {
- // if (auto it = segment.providers.find(providerName); it != segment.providers.end())
- // {
- // proxies[providerName] = it->second.proxy;
- // }
- // else
- // {
- // ARMARX_ERROR << "No proxy for provider ' " << providerName << "'.";
- // proxies[providerName] = nullptr;
- // }
- // }
- // };
-
- // if (input.provider.size() > 0)
- // {
- // providerRequests[input.provider] = input.request;
- // updateProxy(input.provider);
- // }
- // else
- // {
- // segment.doLocked([&]()
- // {
- // for (const auto& objectID : input.request.objectIDs)
- // {
- // bool found = true;
- // for (const auto& [providerName, info] : segment.providers)
- // {
- // // ToDo: optimize look up.
- // if (std::find(info.supportedObjects.begin(), info.supportedObjects.end(), objectID) != info.supportedObjects.end())
- // {
- // providerRequests[providerName].objectIDs.push_back(objectID);
- // updateProxy(providerName);
- // break;
- // }
- // }
- // if (!found)
- // {
- // ARMARX_ERROR << "Did not find a provider for " << objectID << ".";
- // output.results[objectID].providerName = "";
- // }
- // }
- // });
- // }
-
- // for (const auto& [providerName, request] : providerRequests)
- // {
- // if (objpose::ObjectPoseProviderPrx proxy = proxies.at(providerName); proxy)
- // {
- // ARMARX_INFO << "Requesting " << request.objectIDs.size() << " objects by provider '"
- // << providerName << "' for " << request.relativeTimeoutMS << " ms.";
- // objpose::provider::RequestObjectsOutput providerOutput = proxy->requestObjects(request);
-
- // int successful = 0;
- // for (const auto& [objectID, result] : providerOutput.results)
- // {
- // objpose::observer::ObjectRequestResult& res = output.results[objectID];
- // res.providerName = providerName;
- // res.result = result;
- // successful += int(result.success);
- // }
- // ARMARX_INFO << successful << " of " << request.objectIDs.size() << " object requests successful.";
- // }
- // }
- // return output;
- // }
-
-
- // objpose::ProviderInfoMap SegmentAdapter::getAvailableProvidersInfo(const Ice::Current&)
- // {
- // return segment.doLocked([this]()
- // {
- // return segment.providers;
- // });
- // }
-
-
- // Ice::StringSeq SegmentAdapter::getAvailableProviderNames(const Ice::Current&)
- // {
- // return segment.doLocked([this]()
- // {
- // return simox::alg::get_keys(segment.providers);
- // });
- // }
-
-
- // objpose::ProviderInfo SegmentAdapter::getProviderInfo(const std::string& providerName, const Ice::Current&)
- // {
- // return segment.doLocked([this, &providerName]()
- // {
- // return segment.getProviderInfo(providerName);
- // });
- // }
-
-
- // bool SegmentAdapter::hasProvider(const std::string& providerName, const Ice::Current&)
- // {
- // return segment.doLocked([this, &providerName]()
- // {
- // return segment.providers.count(providerName) > 0;
- // });
- // }
-
-
- // objpose::AttachObjectToRobotNodeOutput SegmentAdapter::attachObjectToRobotNode(
- // const objpose::AttachObjectToRobotNodeInput& input, const Ice::Current&)
- // {
- // return segment.doLocked([this, &input]()
- // {
- // return segment.attachObjectToRobotNode(input);
- // });
- // }
-
-
- // objpose::DetachObjectFromRobotNodeOutput SegmentAdapter::detachObjectFromRobotNode(
- // const objpose::DetachObjectFromRobotNodeInput& input, const Ice::Current&)
- // {
- // return segment.doLocked([this, &input]()
- // {
- // return segment.detachObjectFromRobotNode(input);
- // });
- // }
-
-
- // objpose::DetachAllObjectsFromRobotNodesOutput SegmentAdapter::detachAllObjectsFromRobotNodes(
- // const objpose::DetachAllObjectsFromRobotNodesInput& input, const Ice::Current&)
- // {
- // return segment.doLocked([this, &input]()
- // {
- // return segment.detachAllObjectsFromRobotNodes(input);
- // });
- // }
-
-
- // objpose::AgentFramesSeq SegmentAdapter::getAttachableFrames(const Ice::Current&)
- // {
- // return segment.doLocked([this]()
- // {
- // objpose::AgentFramesSeq output;
- // for (const auto& [name, agent] : segment.robots.loaded)
- // {
- // objpose::AgentFrames& frames = output.emplace_back();
- // frames.agent = agent->getName();
- // frames.frames = agent->getRobotNodeNames();
- // }
- // return output;
- // });
- // }
-
-
- // objpose::SignalHeadMovementOutput
- // SegmentAdapter::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
- // {
- // std::scoped_lock lock(robotHeadMutex);
- // return robotHead.signalHeadMovement(input);
- // }
-
- // objpose::ObjectPosePredictionResultSeq
- // SegmentAdapter::predictObjectPoses(const objpose::ObjectPosePredictionRequestSeq& requests,
- // const Ice::Current&)
- // {
- // objpose::ObjectPosePredictionResultSeq results;
- // std::vector<std::map<DateTime, objpose::ObjectPose>> poses;
- // std::vector<objpose::ObjectPose> latestPoses;
-
- // segment.doLocked(
- // [this, &requests, &results, &poses, &latestPoses]()
- // {
- // for (const auto& request : requests)
- // {
- // auto& result = results.emplace_back();
- // const ObjectID objID = armarx::fromIce(request.objectID);
-
- // const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
-
- // const wm::Entity* entity = segment.findObjectEntity(objID);
- // // Use result.success as a marker for whether to continue later
- // result.success = false;
- // poses.emplace_back();
- // latestPoses.emplace_back();
- // if (!entity)
- // {
- // std::stringstream sstream;
- // sstream << "Could not find object with ID " << objID << ".";
- // result.errorMessage = sstream.str();
- // continue;
- // }
-
- // const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
- // if (!dto)
- // {
- // std::stringstream sstream;
- // sstream << "No snapshots of the object " << objID << " were found."
- // << " Cannot make a prediction.";
- // result.errorMessage = sstream.str();
- // continue;
- // }
-
- // result.success = true;
- // poses.back() = segment.getObjectPosesInRange(
- // *entity, Time::Now() - timeWindow, Time::Invalid());
- // latestPoses.back() = aron::fromAron<objpose::ObjectPose>(dto.value().pose);
- // }
- // });
-
- // for (size_t i = 0; i < requests.size(); ++i)
- // {
- // const objpose::ObjectPosePredictionRequest& request = requests.at(i);
- // objpose::ObjectPosePredictionResult& result = results.at(i);
-
- // if (result.success)
- // {
- // armem::PredictionSettings settings =
- // armem::PredictionSettings::fromIce(request.settings);
-
- // if (settings.predictionEngineID.empty()
- // or settings.predictionEngineID == linearPredictionEngineID)
- // {
- // result = objpose::predictObjectPoseLinear(
- // poses.at(i),
- // armarx::fromIce<DateTime>(request.timestamp),
- // latestPoses.at(i));
- // }
- // else
- // {
- // result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
- // "' not available for object pose prediction.";
- // result.success = false;
- // }
- // }
- // }
- // return results;
- // }
-
- // armem::prediction::data::PredictionEngineSeq
- // SegmentAdapter::getAvailableObjectPoseEngines(const Ice::Current&)
- // {
- // return armarx::toIce<armem::prediction::data::PredictionEngineSeq>(predictionEngines);
- // }
-
- // FIXME implement
+
+
void
SegmentAdapter::visualizeRun()
{
- // if (not visu.enabled)
- // {
- // ARMARX_INFO << "Visu disabled";
- // return;
- // }
+ if (not visu.enabled)
+ {
+ ARMARX_INFO << "Visu disabled";
+ return;
+ }
armarx::Metronome metronome(armarx::Frequency::HertzDouble(visu.frequencyHz));
@@ -530,78 +214,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
}
}
- // ObjectFinder objectFinder;
-
- // CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
- // while (visu.updateTask && !visu.updateTask->isStopped())
- // {
- // {
- // std::scoped_lock lock(visuMutex);
-
- // if (visu.enabled)
- // {
- // TIMING_START(tVisu);
-
- // objpose::ObjectPoseMap objectPoses;
- // std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>> poseHistories;
- // visu.minConfidence = -1;
-
- // segment.doLocked([this, &objectPoses, &poseHistories, &objectFinder]()
- // {
- // const Time now = Time::Now();
-
- // // Also include decayed objects in result
- // // Store original setting.
- // const float minConf = segment.decay.removeObjectsBelowConfidence;
- // segment.decay.removeObjectsBelowConfidence = -1;
- // // Get result.
- // objectPoses = segment.getObjectPoses(now);
- // // Restore original setting.
- // segment.decay.removeObjectsBelowConfidence = minConf;
-
- // objectFinder = segment.objectFinder;
- // if (segment.decay.enabled)
- // {
- // visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
- // }
-
- // // Get histories.
- // for (const auto& [id, objectPose] : objectPoses)
- // {
- // const wm::Entity* entity = segment.findObjectEntity(id);
- // if (entity != nullptr)
- // {
- // poseHistories[id] = familiar_object_instance::Segment::getObjectPosesInRange(
- // *entity,
- // Time::Now() - Duration::SecondsDouble(
- // visu.linearPredictions.timeWindowSeconds),
- // Time::Invalid());
- // }
- // }
- // });
-
- // const std::vector<viz::Layer> layers =
- // visu.visualizeCommit(objectPoses, poseHistories, objectFinder);
- // arviz.commit(layers);
-
-
- // TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // armarx::StringVariantBaseMap debugValues;
- // debugValues["t Visualize [ms]"] = new Variant(tVisu.toMilliSecondsDouble());
- // for (const auto& [id, pose] : objectPoses)
- // {
- // debugValues["confidence(" + id.str() + ")"] = new Variant(pose.confidence);
- // }
- // debugObserver->setDebugChannel(getName(), debugValues);
- // }
- // }
- // }
- // cycle.waitForCycleDuration();
- // }
- // }
+
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
index 7dabba527d94f03ce7806832ccbf7a1f92e7a3c8..4fae54877da2538c247c102011e86d4a1ba2ceb0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.cpp
@@ -41,30 +41,8 @@ namespace armarx::armem::server::obj::familiar_object_instance
defs->optional(
objectFramesScale, prefix + "objectFramesScale", "Scaling of object frames.");
- gaussians.defineProperties(defs, prefix + "gaussians.");
- defs->optional(useArticulatedModels,
- prefix + "useArticulatedModels",
- "Prefer articulated object models if available.");
- }
-
- void
- Visu::Gaussians::defineProperties(PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(
- position, prefix + "position", "Enable showing pose gaussians (position part).");
- defs->optional(
- positionScale, prefix + "positionScale", "Scaling of pose gaussians (position part).");
-
- defs->optional(
- orientation, prefix + "position", "Enable showing pose gaussians (orientation part).");
- defs->optional(orientationScale,
- prefix + "positionScale",
- "Scaling of pose gaussians (orientation part).");
- defs->optional(
- orientationDisplaced,
- prefix + "positionDisplaced",
- "Displace center orientation (co)variance circle arrows along their rotation axis.");
+
}
void
@@ -125,366 +103,5 @@ namespace armarx::armem::server::obj::familiar_object_instance
arviz.commit(layers);
}
- std::vector<viz::Layer>
- Visu::visualizeCommit(
- const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
- const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
- poseHistories,
- const ObjectFinder& objectFinder) const
- {
- std::vector<viz::Layer> layers;
- for (const auto& [name, poses] : objectPoses)
- {
- if (name.empty())
- {
- continue;
- }
-
- auto poseHistoryMap = poseHistories.find(name);
- if (poseHistoryMap != poseHistories.end())
- {
- layers.push_back(
- visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
- }
- else
- {
- layers.push_back(visualizeProvider(name, poses, {}, objectFinder));
- }
- }
- return layers;
- }
-
- std::vector<viz::Layer>
- Visu::visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const
- {
- std::map<std::string, viz::Layer> stage;
- for (size_t i = 0; i < objectPoses.size(); ++i)
- {
- if (objectPoses.at(i).providerName.empty())
- {
- ARMARX_INFO << "Object pose provider not set!";
- continue;
- }
-
- visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
- objectPoses.at(i),
- poseHistories.at(i),
- objectFinder);
- }
- return simox::alg::get_values(stage);
- }
-
- std::vector<viz::Layer>
- Visu::visualizeCommit(
- const objpose::ObjectPoseMap& objectPoses,
- const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const
- {
- std::map<std::string, viz::Layer> stage;
- for (const auto& [id, pose] : objectPoses)
- {
-
- auto poseHistoryMap = poseHistories.find(id);
- if (poseHistoryMap != poseHistories.end())
- {
- visualizeObjectPose(
- getLayer(pose.providerName, stage), pose, poseHistoryMap->second, objectFinder);
- }
- else
- {
- visualizeObjectPose(getLayer(pose.providerName, stage), pose, {}, objectFinder);
- }
- }
- return simox::alg::get_values(stage);
- }
-
- viz::Layer&
- Visu::getLayer(const std::string& providerName, std::map<std::string, viz::Layer>& stage) const
- {
- auto it = stage.find(providerName);
- if (it == stage.end())
- {
- it = stage.emplace(providerName, arviz.layer(providerName)).first;
- }
- return it->second;
- }
-
- viz::Layer
- Visu::visualizeProvider(
- const std::string& providerName,
- const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const
- {
- viz::Layer layer = arviz.layer(providerName);
- for (size_t i = 0; i < poseHistories.size(); ++i)
- {
- visualizeObjectPose(layer, objectPoses.at(i), poseHistories.at(i), objectFinder);
- }
- return layer;
- }
-
- void
- Visu::visualizeObjectPose(viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- const std::map<DateTime, objpose::ObjectPose>& poseHistory,
- const ObjectFinder& objectFinder) const
- {
- const bool show = objectPose.confidence >= minConfidence;
- if (not show)
- {
- return;
- }
- const armarx::ObjectID id = objectPose.objectID;
- const std::string key = id.str();
-
- const Eigen::Matrix4f pose =
- inGlobalFrame ? objectPose.objectPoseGlobal : objectPose.objectPoseRobot;
- std::optional<ObjectInfo> objectInfo = objectFinder.findObject(id);
-
- auto makeObject = [&objectInfo, &id](const std::string& key)
- {
- viz::Object object(key);
- if (objectInfo)
- {
- object.file(objectInfo->package(), objectInfo->simoxXML().relativePath);
- }
- else
- {
- object.fileByObjectFinder(id);
- }
- return object;
- };
-
- bool hasObject = false;
- {
- bool done = false;
- if (useArticulatedModels and objectInfo)
- {
- if (std::optional<PackageFileLocation> model = objectInfo->getArticulatedModel())
- {
- viz::Robot robot(key);
- robot.pose(pose);
- robot.file(model->package, model->package + "/" + model->relativePath);
- robot.joints(objectPose.objectJointValues);
-
- layer.add(robot);
- done = true;
- }
- }
- if (not done)
- {
- viz::Object object = makeObject(key).pose(pose);
- 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);
- hasObject = true;
- }
- }
-
- if (oobbs and objectPose.localOOBB)
- {
- const simox::OrientedBoxf oobb =
- inGlobalFrame ? objectPose.oobbGlobal().value() : objectPose.oobbRobot().value();
- layer.add(viz::Box(key + " OOBB").set(oobb).color(simox::Color::lime(255, 64)));
- }
- if (objectFrames)
- {
- layer.add(viz::Pose(key + " Pose").pose(pose).scale(objectFramesScale));
- }
- if (gaussians.showAny() and
- (inGlobalFrame ? objectPose.objectPoseGlobalGaussian.has_value()
- : objectPose.objectPoseRobotGaussian.has_value()))
- {
- gaussians.draw(layer, objectPose, inGlobalFrame);
- }
- }
-
- bool
- Visu::Gaussians::showAny() const
- {
- return position or orientation;
- }
-
- void
- Visu::Gaussians::draw(viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- bool inGlobalFrame) const
- {
- const std::string key = objectPose.objectID.str();
-
- const objpose::PoseManifoldGaussian& gaussian =
- inGlobalFrame ? objectPose.objectPoseGlobalGaussian.value()
- : objectPose.objectPoseRobotGaussian.value();
- objpose::PoseManifoldGaussian::Ellipsoid ellipsoid = gaussian.getPositionEllipsoid();
-
- if (position)
- {
- layer.add(viz::Ellipsoid(key + " Gaussian (Translation)")
- .position(ellipsoid.center)
- .orientation(ellipsoid.orientation)
- .axisLengths(positionScale * ellipsoid.size)
- .color(viz::Color::azure(255, 32)));
-
- if (false) // Arrows can be visualized for debugging.
- {
- for (int i = 0; i < 3; ++i)
- {
- Eigen::Vector3i color = Eigen::Vector3i::Zero();
- color(i) = 255;
-
- layer.add(viz::Arrow(key + " Gaussian (Translation)" + std::to_string(i))
- .fromTo(ellipsoid.center,
- ellipsoid.center + positionScale * ellipsoid.size(i) *
- ellipsoid.orientation.col(i))
- .width(5)
- .color(simox::Color(color)));
- }
- }
- }
- if (orientation)
- {
- const float pi = static_cast<float>(M_PI);
- for (int i = 0; i < 3; ++i)
- {
- const bool global = true;
- Eigen::AngleAxisf rot = gaussian.getScaledRotationAxis(i, global);
-
- Eigen::Vector4i color = Eigen::Vector4i::Zero();
- color(i) = 255;
- color(3) = 64;
-
- layer.add(viz::ArrowCircle(key + " Gaussian (Orientation) " + std::to_string(i))
- .position(orientationDisplaced
- ? ellipsoid.center + orientationScale * rot.axis()
- : ellipsoid.center)
- .normal(rot.axis())
- .radius(orientationScale)
- .completion(simox::math::rescale(rot.angle(), 0.f, pi * 2, 0.f, 1.f))
- .width(simox::math::rescale(rot.angle(), 0.f, pi * 2, 2.f, 7.f))
- .color(simox::Color(color)));
- }
- }
- }
-
- 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);
-
- auto initScale = [](FloatSpinBox& scale, float value, float stepResolution)
- {
- float max = 10000;
- scale.setRange(0, max);
- scale.setDecimals(2);
- scale.setSteps(int(stepResolution * max));
- scale.setValue(value);
- };
- objectFrames.setValue(visu.objectFrames);
- initScale(objectFramesScale, visu.objectFramesScale, 10);
-
- gaussians.position.setValue(visu.gaussians.position);
- initScale(gaussians.positionScale, visu.gaussians.positionScale, 10);
-
- gaussians.orientation.setValue(visu.gaussians.orientation);
- initScale(gaussians.orientationScale, visu.gaussians.orientationScale, 0.5);
- gaussians.orientationDisplaced.setValue(visu.gaussians.orientationDisplaced);
-
- useArticulatedModels.setValue(visu.useArticulatedModels);
-
-
- 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++;
-
- gaussians.setup(visu);
- grid.add(gaussians.group, {row, 0}, {1, 4});
- row++;
-
- grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
- row++;
-
- group = GroupBox();
- group.setLabel("Visualization");
- group.addChild(grid);
- }
-
- void
- Visu::RemoteGui::Gaussians::setup(const Visu& data)
- {
- using namespace armarx::RemoteGui::Client;
-
- GridLayout grid;
- int row = 0;
-
- grid.add(Label("Position"), {row, 0}).add(position, {row, 1});
- grid.add(Label("Scale:"), {row, 2}).add(positionScale, {row, 3});
- row++;
-
- grid.add(Label("Orientation"), {row, 0}).add(orientation, {row, 1});
- grid.add(Label("Scale:"), {row, 2}).add(orientationScale, {row, 3});
- grid.add(Label("Displaced"), {row, 4}).add(orientationDisplaced, {row, 5});
- row++;
-
- group = GroupBox();
- group.setLabel("Pose Gaussians");
- group.addChild(grid);
- }
-
- void
- Visu::RemoteGui::Gaussians::update(Visu::Gaussians& data)
- {
- data.position = position.getValue();
- data.positionScale = positionScale.getValue();
-
- data.orientation = orientation.getValue();
- data.orientationScale = orientationScale.getValue();
- data.orientationDisplaced = orientationDisplaced.getValue();
- }
-
- 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();
-
- gaussians.update(visu.gaussians);
-
- visu.useArticulatedModels = useArticulatedModels.getValue();
- }
} // namespace armarx::armem::server::obj::familiar_object_instance
diff --git a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
index f5707a2366a32873c7df199f6d36e3711a7f7972..0cfc80fe42f775947e6adc1af61438f23378a859 100644
--- a/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/familiar_object_instance/Visu.h
@@ -36,41 +36,7 @@ namespace armarx::armem::server::obj::familiar_object_instance
std::vector<armarx::armem::arondto::FamiliarObjectInstance>>&
familiarObjectsByProvider);
- std::vector<viz::Layer> visualizeCommit(
- const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
- const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
- poseHistories,
- const ObjectFinder& objectFinder) const;
- /// Visualize the given object poses, with one layer per provider.
- std::vector<viz::Layer>
- visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const;
- std::vector<viz::Layer> visualizeCommit(
- const objpose::ObjectPoseMap& objectPoses,
- const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const;
-
- viz::Layer
- visualizeProvider(const std::string& providerName,
- const objpose::ObjectPoseSeq& objectPoses,
- const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
- const ObjectFinder& objectFinder) const;
-
-
- void visualizeObjectPose(viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- const std::map<DateTime, objpose::ObjectPose>& poseHistory,
- const ObjectFinder& objectFinder) const;
-
-
- private:
- viz::Layer& getLayer(const std::string& providerName,
- std::map<std::string, viz::Layer>& stage) const;
-
-
- public:
viz::Client arviz;
bool enabled = true;
@@ -85,66 +51,10 @@ namespace armarx::armem::server::obj::familiar_object_instance
bool objectFrames = false;
float objectFramesScale = 1.0;
- struct Gaussians
- {
- bool position = true;
- float positionScale = 1.0;
- bool orientation = false;
- float orientationScale = 100;
- bool orientationDisplaced = false;
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix);
-
- bool showAny() const;
- void draw(viz::Layer& layer,
- const objpose::ObjectPose& objectPose,
- bool inGlobalFrame) const;
- };
-
- Gaussians gaussians;
-
- /// Prefer articulated models if available.
- bool useArticulatedModels = true;
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;
-
- struct Gaussians
- {
- armarx::RemoteGui::Client::CheckBox position;
- armarx::RemoteGui::Client::FloatSpinBox positionScale;
-
- armarx::RemoteGui::Client::CheckBox orientation;
- armarx::RemoteGui::Client::FloatSpinBox orientationScale;
- armarx::RemoteGui::Client::CheckBox orientationDisplaced;
-
- armarx::RemoteGui::Client::GroupBox group;
-
- void setup(const Visu& data);
- void update(Visu::Gaussians& data);
- };
-
- Gaussians gaussians;
-
- armarx::RemoteGui::Client::CheckBox useArticulatedModels;
-
- void setup(const Visu& visu);
- void update(Visu& visu);
- };
};
} // namespace armarx::armem::server::obj::familiar_object_instance