From 539e3860a5dc6db13a2e1e1e4eb83447796b892f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 12 Jul 2021 09:31:57 +0200
Subject: [PATCH] Add profiling, improve runtime
---
.../server/proprioception/Segment.cpp | 46 ++++++++++++++++---
1 file changed, 39 insertions(+), 7 deletions(-)
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 ac5e5e1c3..5781964af 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -91,33 +91,65 @@ namespace armarx::armem::server::robot_state::proprioception
std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
{
+ namespace adn = aron::datanavigator;
+
std::unordered_map<std::string, std::map<std::string, float>> jointMap;
+ TIMING_START(tRobotJointPositionsLock);
std::lock_guard g{memoryMutex};
+ TIMING_END_STREAM(tRobotJointPositionsLock, ARMARX_VERBOSE);
+ TIMING_START(tProviders);
for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
{
+ TIMING_START(tEntities);
+ int i = 0;
for (const auto& [name, entity] : provSeg.entities())
{
+ TIMING_START(tTryCatch);
try
{
+ TIMING_START(tGetLatestInstance);
const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
-
- if (not jointState)
+ TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_VERBOSE);
+
+ TIMING_START(tRobotJointPositionsTryCast);
+ // tryCast is too slow.
+ // const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
+ aron::datanavigator::DictNavigatorPtr data = entityInstance.data();
+ // Avoid throwing an exception.
+ if (!(data->hasElement("name") && data->hasElement("position")))
{
- // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
continue;
}
- jointMap[robotName].emplace(jointState->name, jointState->position);
+ armarx::armem::arondto::JointState jointState;
+ jointState.name = adn::StringNavigator::DynamicCastAndCheck(data->getElement("name"))->getValue();
+ jointState.position = adn::FloatNavigator::DynamicCastAndCheck(data->getElement("position"))->getValue();
+ TIMING_END_COMMENT_STREAM(tRobotJointPositionsTryCast, "tRobotJointPositionsTryCast " + std::to_string(i), ARMARX_VERBOSE);
+ TIMING_START(tEmplace);
+ if (jointState.name.size() > 0)
+ {
+ jointMap[robotName].emplace(jointState.name, jointState.position);
+ }
+ else
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ }
+ TIMING_END_COMMENT_STREAM(tEmplace, "tEmplace " + std::to_string(i), ARMARX_VERBOSE);
}
- catch (...) // empty history etc
+ catch (const armem::error::EntityHistoryEmpty&)
+ {
+ }
+ catch (const armem::error::MissingEntry&)
{
-
}
+ TIMING_END_STREAM(tTryCatch, ARMARX_VERBOSE);
+ ++i;
}
+ TIMING_END_STREAM(tEntities, ARMARX_VERBOSE);
}
+ TIMING_END_STREAM(tProviders, ARMARX_VERBOSE);
ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
--
GitLab