From e127aba69b2a5ea0731f4cc73bb983e8c5725230 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 16 Jul 2021 12:16:44 +0200
Subject: [PATCH] Update robot state memory segments, lock segments in visu
thread
---
.../RobotStateMemory/RobotStateMemory.cpp | 9 +-
.../armem_robot_state/server/common/Visu.cpp | 39 ++---
.../armem_robot_state/server/common/Visu.h | 4 +-
.../server/description/Segment.cpp | 134 ++++++------------
.../server/description/Segment.h | 29 +---
.../server/localization/Segment.cpp | 47 ++++--
.../server/localization/Segment.h | 21 ++-
.../server/proprioception/Segment.cpp | 36 +++--
.../server/proprioception/Segment.h | 16 ++-
9 files changed, 163 insertions(+), 172 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 653aaaf6e..6d37d9203 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -45,12 +45,9 @@ namespace armarx::armem::server::robot_state
{
RobotStateMemory::RobotStateMemory()
- : descriptionSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- proprioceptionSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
- localizationSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex),
+ : descriptionSegment(server::ComponentPluginUser::iceMemory),
+ proprioceptionSegment(server::ComponentPluginUser::iceMemory),
+ localizationSegment(server::ComponentPluginUser::iceMemory),
commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
addPlugin(debugObserver);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 1af2dc371..97f47e93b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -35,6 +35,22 @@ namespace armarx::armem::server::robot_state
defs->optional(p.frequencyHz, prefix + "frequenzyHz", "Frequency of visualization.");
}
+
+ void Visu::init() {}
+
+
+ void Visu::connect(const viz::Client& arviz)
+ {
+ this->arviz = arviz;
+
+ updateTask = new SimpleRunningTask<>([this]()
+ {
+ this->visualizeRun();
+ });
+ updateTask->start();
+ }
+
+
void Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
{
@@ -58,6 +74,7 @@ namespace armarx::armem::server::robot_state
std::for_each(robots.begin(), robots.end(), visualizeRobot);
}
+
void Visu::visualizeFrames(
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
@@ -81,18 +98,8 @@ namespace armarx::armem::server::robot_state
}
}
- void Visu::init() {}
- void Visu::connect(const viz::Client& arviz)
- {
- this->arviz = arviz;
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- updateTask->start();
- }
// void Visu::RemoteGui::setup(const Visu& visu)
// {
@@ -144,6 +151,7 @@ namespace armarx::armem::server::robot_state
// visu.objectFramesScale = objectFramesScale.getValue();
// }
+
robot::Robots combine(
const std::unordered_map<std::string, robot::RobotDescription>& robotDescriptions,
const std::unordered_map<std::string, Eigen::Affine3f>& globalRobotPoseMap,
@@ -192,9 +200,9 @@ namespace armarx::armem::server::robot_state
return robots;
}
+
void Visu::visualizeRun()
{
-
CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
while (updateTask && not updateTask->isStopped())
{
@@ -207,24 +215,23 @@ namespace armarx::armem::server::robot_state
TIMING_START(Visu);
// TODO(fabian.reister): use timestamp
-
const auto timestamp = IceUtil::Time::now();
try
{
TIMING_START(tRobotDescriptions);
- const auto robotDescriptions =
- descriptionSegment.getRobotDescriptions(timestamp);
+ const description::Segment::RobotDescriptionMap robotDescriptions =
+ descriptionSegment.getRobotDescriptionsLocking(timestamp);
TIMING_END_STREAM(tRobotDescriptions, ARMARX_VERBOSE);
TIMING_START(tGlobalRobotPoseMap);
const auto globalRobotPoseMap =
- localizationSegment.getRobotGlobalPoses(timestamp);
+ localizationSegment.getRobotGlobalPosesLocking(timestamp);
TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_VERBOSE);
TIMING_START(tRobotJointPositionMap);
const auto robotJointPositionMap =
- proprioceptionSegment.getRobotJointPositions(timestamp);
+ proprioceptionSegment.getRobotJointPositionsLocking(timestamp);
TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_VERBOSE);
const auto frames = localizationSegment.getRobotFramePoses(timestamp);
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 7e4b3814c..e659981fb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -70,12 +70,12 @@ namespace armarx::armem::server::robot_state
localizationSegment(localizationSegment)
{}
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
void init();
-
void connect(const viz::Client& arviz);
+
protected:
static void visualizeRobots(
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 832dfb296..370f6f55b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -31,15 +31,15 @@
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) :
+ iceMemory(memoryToIceAdapter)
{
Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegment,
@@ -50,6 +50,7 @@ namespace armarx::armem::server::robot_state::description
"Maximal size of object poses history (-1 for infinite).");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
@@ -61,6 +62,7 @@ namespace armarx::armem::server::robot_state::description
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
+
void Segment::connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx)
{
// this->visu = std::make_unique<Visu>(arviz, *this);
@@ -70,6 +72,14 @@ namespace armarx::armem::server::robot_state::description
updateRobotDescription();
}
+
+ std::mutex& Segment::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return coreSegment->mutex();
+ }
+
+
void Segment::storeRobotDescription(const robot::RobotDescription& robotDescription)
{
const Time now = TimeUtil::GetTime();
@@ -95,134 +105,70 @@ namespace armarx::armem::server::robot_state::description
// std::lock_guard g{memoryMutex};
iceMemory.commit(commit);
}
-
}
+
void Segment::updateRobotDescription()
{
ARMARX_CHECK_NOT_NULL(robotUnit);
- auto kinematicUnit = robotUnit->getKinematicUnit();
-
+ KinematicUnitInterfacePrx kinematicUnit = robotUnit->getKinematicUnit();
ARMARX_CHECK_NOT_NULL(kinematicUnit);
- const auto robotName = kinematicUnit->getRobotName();
- const auto robotFilename = kinematicUnit->getRobotFilename();
+ const std::string robotName = kinematicUnit->getRobotName();
+ const std::string robotFilename = kinematicUnit->getRobotFilename();
- const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
- const auto package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
+ const std::vector<std::string> packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
+ const std::string package = armarx::ArmarXDataPath::getProject(packages, robotFilename);
ARMARX_INFO << "Robot description '" << robotFilename << "' found in package " << package;
const robot::RobotDescription robotDescription
{
.name = kinematicUnit->getRobotName(),
- .xml = {package, kinematicUnit->getRobotFilename()}}; // FIXME
+ .xml = {package, kinematicUnit->getRobotFilename()}
+ }; // FIXME
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_DEBUG << "Key is " << armem::MemoryID(entity.id());
+ robotDescriptions.emplace(description->name, *description);
+ }
+ else
{
ARMARX_WARNING << "Could not convert entity instance to "
"'RobotDescription'";
- continue;
}
-
- 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
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 347247a7d..a6e2d08ee 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -59,31 +59,30 @@ namespace armarx::armem::server::robot_state::description
class Segment : public armarx::Logging
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
- void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
-
void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
-
void init();
+ void connect(const viz::Client& arviz, const RobotUnitInterfacePrx& robotUnitPrx);
+
+ std::mutex& mutex() const;
/// mapping "robot name" -> "robot description"
using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
+ RobotDescriptionMap getRobotDescriptionsLocking(const armem::Time& timestamp) const;
+
private:
void storeRobotDescription(const robot::RobotDescription& robotDescription);
void updateRobotDescription();
-
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
RobotUnitInterfacePrx robotUnit;
@@ -94,22 +93,6 @@ namespace armarx::armem::server::robot_state::description
};
Properties p;
- // std::unique_ptr<Visu> visu;
-
- public:
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
- // armarx::RemoteGui::Client::CheckBox infiniteHistory;
- // armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
-
- // void setup(const Segment& data);
- // void update(Segment& data);
- // };
-
};
} // namespace armarx::armem::server::robot_state::description
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 143e4c30f..d2c02be7e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -39,15 +39,16 @@
namespace armarx::armem::server::robot_state::localization
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter,
- std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ iceMemory(memoryToIceAdapter)
{
Logging::setTag("LocalizationSegment");
}
+
Segment::~Segment() = default;
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegment,
@@ -58,6 +59,7 @@ namespace armarx::armem::server::robot_state::localization
"Maximal size of object poses history (-1 for infinite).");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
@@ -69,18 +71,35 @@ namespace armarx::armem::server::robot_state::localization
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
+
void Segment::connect(viz::Client arviz)
{
// this->visu = std::make_unique<Visu>(arviz, *this);
}
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>>
- Segment::getRobotFramePoses(const armem::Time& timestamp) const
+
+ std::mutex& Segment::mutex() const
+ {
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return coreSegment->mutex();
+ }
+
+
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePosesLocking(const armem::Time& timestamp) const
+ {
+ std::scoped_lock lock(mutex());
+ return getRobotFramePoses(timestamp);
+ }
+
+
+ Segment::RobotFramePoseMap
+ Segment::getRobotFramePoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> frames;
+ RobotFramePoseMap frames;
const auto& localizationCoreSegment =
iceMemory.workingMemory->getCoreSegment(p.coreSegment);
@@ -105,20 +124,29 @@ namespace armarx::armem::server::robot_state::localization
frames.emplace(robotName, result.transforms);
}
- ARMARX_INFO << deactivateSpam(10)
+ ARMARX_INFO << deactivateSpam(60)
<< "Number of known robot pose chains: " << frames.size();
return frames;
}
- std::unordered_map<std::string, Eigen::Affine3f>
+
+ Segment::RobotPoseMap
+ Segment::getRobotGlobalPosesLocking(const armem::Time& timestamp) const
+ {
+ std::scoped_lock lock(mutex());
+ return getRobotGlobalPoses(timestamp);
+ }
+
+
+ Segment::RobotPoseMap
Segment::getRobotGlobalPoses(const armem::Time& timestamp) const
{
using common::robot_state::localization::TransformHelper;
using common::robot_state::localization::TransformQuery;
- std::unordered_map<std::string, Eigen::Affine3f> robotGlobalPoses;
+ RobotPoseMap robotGlobalPoses;
const auto& localizationCoreSegment =
iceMemory.workingMemory->getCoreSegment(p.coreSegment);
@@ -149,6 +177,7 @@ namespace armarx::armem::server::robot_state::localization
return robotGlobalPoses;
}
+
bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
{
const armem::Time& timestamp = transform.header.timestamp;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 307eaae17..4bc4e607b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -59,19 +59,27 @@ namespace armarx::armem::server::robot_state::localization
class Segment : public armarx::Logging
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory,
- std::mutex& memoryMutex);
+ using RobotPoseMap = std::unordered_map<std::string, Eigen::Affine3f>;
+ using RobotFramePoseMap = std::unordered_map<std::string, std::vector<Eigen::Affine3f>>;
+
+
+ public:
+
+ Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void init();
void connect(viz::Client arviz);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ std::mutex& mutex() const;
- void init();
+ RobotPoseMap getRobotGlobalPoses(const armem::Time& timestamp) const;
+ RobotPoseMap getRobotGlobalPosesLocking(const armem::Time& timestamp) const;
- std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses(const armem::Time& timestamp) const;
- std::unordered_map<std::string, std::vector<Eigen::Affine3f>> getRobotFramePoses(const armem::Time& timestamp) const;
+ RobotFramePoseMap getRobotFramePoses(const armem::Time& timestamp) const;
+ RobotFramePoseMap getRobotFramePosesLocking(const armem::Time& timestamp) const;
bool storeTransform(const armarx::armem::robot_state::Transform& transform);
@@ -79,7 +87,6 @@ namespace armarx::armem::server::robot_state::localization
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
struct Properties
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 81d0e13cf..6885f5b0f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -29,21 +29,22 @@
namespace armarx::armem::server::robot_state::proprioception
{
- Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter, std::mutex& memoryMutex) :
- iceMemory(memoryToIceAdapter),
- memoryMutex(memoryMutex)
+ Segment::Segment(armem::server::MemoryToIceAdapter& memoryToIceAdapter) :
+ iceMemory(memoryToIceAdapter)
{
Logging::setTag("ProprioceptionSegment");
}
Segment::~Segment() = default;
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegment, prefix + "seg.proprioception.CoreSegment", "Name of the object instance core segment.");
defs->optional(p.maxHistorySize, prefix + "seg.proprioception.MaxHistorySize", "Maximal size of object poses history (-1 for infinite).");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
@@ -53,6 +54,7 @@ namespace armarx::armem::server::robot_state::proprioception
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
+
void Segment::connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx)
{
robotUnit = robotUnitPrx;
@@ -75,7 +77,7 @@ namespace armarx::armem::server::robot_state::proprioception
input.providerSegmentName = providerSegmentName;
{
- std::lock_guard g{memoryMutex};
+ std::lock_guard g{mutex()};
auto result = iceMemory.addSegments({input})[0];
if (!result.success)
@@ -89,18 +91,32 @@ namespace armarx::armem::server::robot_state::proprioception
robotUnitProviderID.providerSegmentName = providerSegmentName;
}
- std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
+
+ std::mutex& Segment::mutex() const
{
- namespace adn = aron::datanavigator;
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+ return coreSegment->mutex();
+ }
- std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ Segment::RobotJointPositionMap Segment::getRobotJointPositionsLocking(const armem::Time& timestamp) const
+ {
TIMING_START(tRobotJointPositionsLock);
- std::lock_guard g{memoryMutex};
+ std::scoped_lock lock(mutex());
TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_VERBOSE);
+ return getRobotJointPositions(timestamp);
+ }
+
+
+ Segment::RobotJointPositionMap Segment::getRobotJointPositions(const armem::Time& timestamp) const
+ {
+ namespace adn = aron::datanavigator;
+ ARMARX_CHECK_NOT_NULL(coreSegment);
+
+ RobotJointPositionMap jointMap;
TIMING_START(tProviders);
- for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ for (const auto& [robotName, provSeg] : *coreSegment)
{
TIMING_START(tEntities);
int i = 0;
@@ -151,7 +167,7 @@ namespace armarx::armem::server::robot_state::proprioception
}
TIMING_END_STREAM(tProviders, ARMARX_VERBOSE);
- ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
+ ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
return jointMap;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 9a14cc8c5..10f7297a7 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -58,25 +58,31 @@ namespace armarx::armem::server::robot_state::proprioception
class Segment : public armarx::Logging
{
public:
- Segment(server::MemoryToIceAdapter& iceMemory, std::mutex& memoryMutex);
+ using RobotJointPositionMap = std::unordered_map<std::string, std::map<std::string, float>>;
+
+ public:
+
+ Segment(server::MemoryToIceAdapter& iceMemory);
virtual ~Segment();
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ void init();
void connect(viz::Client arviz, RobotUnitInterfacePrx robotUnitPrx);
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+ std::mutex& mutex() const;
- void init();
- std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
+ RobotJointPositionMap getRobotJointPositions(const armem::Time& timestamp) const;
+ RobotJointPositionMap getRobotJointPositionsLocking(const armem::Time& timestamp) const;
const armem::MemoryID& getRobotUnitProviderID() const;
+
private:
server::MemoryToIceAdapter& iceMemory;
wm::CoreSegment* coreSegment = nullptr;
- std::mutex& memoryMutex;
RobotUnitInterfacePrx robotUnit;
--
GitLab