Newer
Older
#include "ObjectReader.h"
#include <mutex>
#include <optional>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/util/util.h>
#include <RobotAPI/libraries/armem_objects/aron/Attachment.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/aron/Robot.aron.generated.h>
// #include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
// #include <RobotAPI/libraries/armem_robot_state/robot_conversions.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::obj::instance
{
void
Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
const std::string prefix = propertyPrefix;
def->optional(properties.memoryName, prefix + "MemoryName");
Reader::connect(armem::client::MemoryNameSystem& memoryNameSystem)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "Waiting for memory '" << properties.memoryName << "' ...";
// simply wait until memory is ready. Do nothing with reader but get prx
auto r = memoryNameSystem.useReader(properties.memoryName);
// cast MemoryPrx to objPoseStoragePrx --> NOT WORKING DUE TO SOME ICE CACHING OR SO.
// Explanation:
// When the readingPrx disconnects and reconnects, the cast fails as it uses internally
// a ice_isA call which results in connection refused. The reason is, that for some
// reason the mns thinks that the reconnected prx is similar to the old info
// (check ice_identity) so it returnes the cached prx in its server map. Could be fixed
// by always querying the mns component for new proxies, but this may slow the system
// down.
//this->objPoseStorage =
// objpose::ObjectPoseStorageInterfacePrx::uncheckedCast(r.readingPrx);
// Current fix: Get prx from mns:
this->readingPrx = r.readingPrx;
ARMARX_INFO << "Connected to Memory '" << properties.memoryName << "'";
}
catch (const armem::error::CouldNotResolveMemoryServer& e)
{
ARMARX_ERROR << e.what();
return;
}
}
std::map<std::string, bool>
Reader::requestLocalization(const ObjectID& instanceId,
const armarx::core::time::Duration& until)
{
std::map<std::string, bool> ret;
auto providers = getObjectPoseStorage()->getAvailableProvidersInfo();
for (const auto& [k, p] : providers)
{
// TODO: check supported objects?
ret[k] = this->requestLocalization(instanceId, k, until);
}
return ret;
}
bool
Reader::requestLocalization(const ObjectID& instanceId,
const std::string& provider,
const armarx::core::time::Duration& until)
armarx::data::ObjectID requestObject;
armarx::toIce(requestObject, instanceId);
armarx::objpose::observer::RequestObjectsInput req;
req.provider = provider;
req.request.objectIDs = {requestObject};
req.request.relativeTimeoutMS = until.toMilliSeconds();
auto requestResult = getObjectPoseStorage()->requestObjects(req);
if (requestResult.results.count(requestObject))
{
return requestResult.results.at(requestObject).result.success;
}
return false;
std::optional<objpose::ObjectPose>
Reader::queryLatestObjectInstance(const ObjectID& instanceId)
// TODO: Shall we throw an exception if no instance index is set?
auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
fromIce(pose.objectID, oid);
if (oid == instanceId)
{
objpose::ObjectPose oi;
fromIce(pose, oi);
return oi;
}
}
return std::nullopt;
std::map<std::string, objpose::ObjectPose>
Reader::queryLatestObjectInstances()
{
std::map<std::string, objpose::ObjectPose> ret;
auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
fromIce(pose.objectID, oid);
fromIce(pose, ret[oid.str()]);
}
return ret;
}
std::map<std::string, objpose::ObjectPose>
Reader::queryLatestObjectInstances(const ObjectID& classId)
std::map<std::string, objpose::ObjectPose> ret;
auto objectPoses = getObjectPoseStorage()->getObjectPoses();
for (const auto& pose : objectPoses)
{
ObjectID oid;
fromIce(pose.objectID, oid);
{
fromIce(pose, ret[oid.str()]);
}
}
return ret;
} // namespace armarx::armem::obj::instance