Skip to content
Snippets Groups Projects

Draft: Make RobotStateMemory ready

Merged Draft: Make RobotStateMemory ready
armem/robot-state-memory into master
Merged Rainer Kartmann requested to merge armem/robot-state-memory into master
Viewing commit 42102108
Prev Next
Show latest version
2 files
+ 121
− 1
Compare changes
  • Side-by-side
  • Inline
Files
2
source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+ 59
− 140
#include "Segment.h"
#include <IceUtil/Time.h>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <sstream>
#include <thread>
#include <IceUtil/Time.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include "ArmarXCore/core/system/ArmarXDataPath.h"
#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
#include "RobotAPI/libraries/armem_robot/types.h"
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include <RobotAPI/libraries/armem/core/MemoryID.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>
@@ -24,51 +25,32 @@
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
#include <thread>
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
namespace armarx::armem::server::robot_state::description
{
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
std::mutex& memoryMutex) :
iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
SpecializedSegment(memoryToIceAdapter, arondto::RobotDescription::toAronType(),
"Description")
{
Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegment,
prefix + "seg.description.CoreSegment",
"Name of the robot description core segment.");
defs->optional(p.maxHistorySize,
prefix + "seg.description.MaxHistorySize",
"Maximal size of object poses history (-1 for infinite).");
}
void Segment::init()
void Segment::connect(const RobotUnitInterfacePrx& robotUnitPrx)
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
coreSegment = &iceMemory.workingMemory->addCoreSegment(
p.coreSegment, arondto::RobotDescription::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
{
// this->visu = std::make_unique<Visu>(arviz, *this);
robotUnit = robotUnitPrx;
// store the robot description linked to the robot unit in this segment
updateRobotDescription();
}
void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
void Segment::commitRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
@@ -88,139 +70,76 @@ namespace armarx::armem::server::robot_state::description
Commit commit;
commit.updates.push_back(update);
{
// std::lock_guard g{memoryMutex};
iceMemory.commit(commit);
}
iceMemory.commitLocking(commit);
}
void Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
auto kinematicUnit = robotUnit->getKinematicUnit();
ARMARX_CHECK_NOT_NULL(kinematicUnit);
KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
if (kinematicUnit)
{
const std::string robotName = kinematicUnit->getRobotName();
const std::string robotFilename = kinematicUnit->getRobotFilename();
const auto robotName = kinematicUnit->getRobotName();
const auto robotFilename = kinematicUnit->getRobotFilename();
const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
const robot::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
.xml = {package, kinematicUnit->getRobotFilename()}
}; // FIXME
const robot::RobotDescription robotDescription
commitRobotDescription(robotDescription);
}
else
{
.name = kinematicUnit->getRobotName(),
.xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
ARMARX_WARNING << "Robot unit '" << robotUnit->ice_getIdentity().name << "' does not have a kinematic unit."
<< "\n Cannot commit robot description.";
}
}
storeRobotDescription(robotDescription);
Segment::RobotDescriptionMap Segment::getRobotDescriptionsLocking(const armem::Time& timestamp) const
{
std::scoped_lock lock(mutex());
return getRobotDescriptions(timestamp);
}
Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
(void) timestamp; // Unused
RobotDescriptionMap robotDescriptions;
{
// std::lock_guard g{memoryMutex};
for (const auto &[_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
for (const auto& [_, provSeg] : *coreSegment)
{
for (const auto &[name, entity] : provSeg.entities())
for (const auto& [name, entity] : provSeg.entities())
{
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
const auto description = robot::convertRobotDescription(entityInstance);
if (not description)
if (description)
{
ARMARX_WARNING << "Could not convert entity instance to "
"'RobotDescription'";
continue;
ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
robotDescriptions.emplace(description->name, *description);
}
else
{
ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
}
ARMARX_DEBUG << "Key is " << armem::MemoryID(entity.id());
robotDescriptions.emplace(description->name, *description);
}
}
}
ARMARX_INFO << deactivateSpam(10) << "Number of known robot descriptions: " << robotDescriptions.size();
ARMARX_INFO << deactivateSpam(60) << "Number of known robot descriptions: " << robotDescriptions.size();
return robotDescriptions;
}
// std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
// {
// std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> objects;
// for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
// {
// for (const auto& [name, entity] : provSeg.entities())
// {
// const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
// const auto description = articulated_object::convertRobotDescription(entityInstance);
// if (not description)
// {
// ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
// continue;
// }
// ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
// objects.emplace(armem::MemoryID(entity.id()), *description);
// }
// }
// ARMARX_IMPORTANT << "Number of known articulated object classes: " << objects.size();
// return objects;
// }
// void Segment::RemoteGui::setup(const Segment& data)
// {
// using namespace armarx::RemoteGui::Client;
// maxHistorySize.setValue(std::max(1, int(data.p.maxHistorySize)));
// maxHistorySize.setRange(1, 1e6);
// infiniteHistory.setValue(data.p.maxHistorySize == -1);
// discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
// GridLayout grid;
// int row = 0;
// 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++;
// group.setLabel("Data");
// group.addChild(grid);
// }
// void Segment::RemoteGui::update(Segment& data)
// {
// if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
// || discardSnapshotsWhileAttached.hasValueChanged())
// {
// std::scoped_lock lock(data.memoryMutex);
// if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
// {
// data.p.maxHistorySize = infiniteHistory.getValue() ? -1 : maxHistorySize.getValue();
// if (data.coreSegment)
// {
// data.coreSegment->setMaxHistorySize(long(data.p.maxHistorySize));
// }
// }
// data.p.discardSnapshotsWhileAttached = discardSnapshotsWhileAttached.getValue();
// }
// }
} // namespace armarx::armem::server::robot_state::description