From 3490d38516e45485aa4724502d8cd418830e9c47 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 17 Aug 2021 09:19:25 +0200
Subject: [PATCH] Update RobotState proprioception segment
---
.../server/proprioception/RobotUnitReader.cpp | 4 +-
.../server/proprioception/Segment.cpp | 123 +++++++-----------
.../server/proprioception/Segment.h | 9 +-
3 files changed, 60 insertions(+), 76 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index f4b79b65f..4fb42da5e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -89,11 +89,11 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitData result;
result.proprioception = converter->convert(data.value(), description);
- result.timestamp = Time::microSeconds(data->timestampUSec);;
+ result.timestamp = Time::microSeconds(data->timestampUSec);
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit s: " << duration;
+ ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: " << duration;
if (debugObserver)
{
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 8a88e6fae..5290d004c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -1,26 +1,14 @@
#include "Segment.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
+#include <ArmarXCore/observers/ObserverObjectFactories.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_state/types.h>
-
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.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>
-#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
-#include <RobotAPI/libraries/armem/util/util.h>
-
-#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-
-#include <RobotAPI/libraries/armem_objects/types.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
@@ -78,7 +66,8 @@ namespace armarx::armem::server::robot_state::proprioception
}
- Segment::RobotJointPositionMap Segment::getRobotJointPositions(
+ Segment::RobotJointPositionMap
+ Segment::getRobotJointPositions(
const armem::Time& timestamp,
DebugObserverHelper* debugObserver) const
{
@@ -86,74 +75,40 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_CHECK_NOT_NULL(segment);
RobotJointPositionMap jointMap;
+ int i = 0;
- TIMING_START(tProcessProviders);
- segment->forEachProviderSegment([&](const wm::ProviderSegment & provSeg)
+ Duration tFindData = Duration::milliSeconds(0), tReadJointPositions = Duration::milliSeconds(0);
+ TIMING_START(tProcessEntities)
+ segment->forEachEntity([&](const wm::Entity & entity)
{
- TIMING_START(tProcessEntities);
- int i = 0;
-
- std::map<std::string, float>& robotJointMap = jointMap[provSeg.name()];
- provSeg.forEachEntity([&](const wm::Entity & entity)
+ adn::DictNavigatorPtr data;
+ TIMING_START(_tFindData)
+ if (const wm::EntitySnapshot* snapshot = entity.findFirstSnapshotAfterOrAt(timestamp))
{
- TIMING_START(tProcessEntity);
-
- TIMING_START(tGetLatestInstance);
- if (entity.empty())
- {
- return true;
- }
- const wm::EntitySnapshot& snapshot = entity.getLatestSnapshot();
- if (snapshot.empty())
- {
- return true;
- }
- const wm::EntityInstance& entityInstance = snapshot.getInstance(0);
- TIMING_END_COMMENT_STREAM(tGetLatestInstance, "tGetLatestInstance " + std::to_string(i), ARMARX_DEBUG);
-
- TIMING_START(tCastAndEmplaceJointState);
- // Only cast the joint angles while avoiding throwing an exception.
- adn::DictNavigatorPtr joints = getDictElement(*entityInstance.data(), "joints");
- if (joints)
- {
- adn::DictNavigatorPtr jointPositions = getDictElement(*joints, "position");
- if (jointPositions)
- {
- for (const auto& [name, value] : jointPositions->getElements())
- {
- const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(value)->getValue();
- robotJointMap.emplace(name, jointPosition);
- }
- }
- }
- TIMING_END_COMMENT_STREAM(tCastAndEmplaceJointState, "tCastAndEmplaceJointState " + std::to_string(i), ARMARX_DEBUG);
-
- TIMING_END_COMMENT_STREAM(tProcessEntity, "tProcessEntity " + std::to_string(i), ARMARX_DEBUG);
+ data = snapshot->findInstanceData();
+ }
+ TIMING_END_COMMENT_STREAM(_tFindData, "tFindData " + std::to_string(i), ARMARX_DEBUG)
+ tFindData += _tFindData;
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.1 GetLatestInstance (ms)", tGetLatestInstance.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.2 CastAndEmplaceJointState (ms)", tCastAndEmplaceJointState.toMilliSecondsDouble());
- debugObserver->setDebugObserverDatafield(dp + "t 1.1.1.T Process Entity (ms)", tProcessEntity.toMilliSecondsDouble());
- }
- ++i;
+ if (data)
+ {
+ TIMING_START(_tReadJointPositions)
- return true;
- });
+ jointMap.emplace(entity.id().providerSegmentName, readJointPositions(*data));
- TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG);
- if (debugObserver)
- {
- debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
+ TIMING_END_COMMENT_STREAM(_tReadJointPositions, "tReadJointPositions " + std::to_string(i), ARMARX_DEBUG)
+ tReadJointPositions += _tReadJointPositions;
}
- });
- TIMING_END_STREAM(tProcessProviders, ARMARX_DEBUG);
- ARMARX_INFO << deactivateSpam(60) << "Number of known robot joint maps: " << jointMap.size();
+ ++i;
+ });
+ TIMING_END_STREAM(tProcessEntities, ARMARX_DEBUG)
if (debugObserver)
{
- debugObserver->setDebugObserverDatafield(dp + "t 1 Process Providers (ms)", tProcessProviders.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1 Process Entities (ms)", tProcessEntities.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.1 FindData (ms)", tFindData.toMilliSecondsDouble());
+ debugObserver->setDebugObserverDatafield(dp + "t 1.1.2 ReadJointPositions (ms)", tReadJointPositions.toMilliSecondsDouble());
}
return jointMap;
@@ -165,4 +120,26 @@ namespace armarx::armem::server::robot_state::proprioception
return robotUnitProviderID;
}
+
+ std::map<std::string, float>
+ Segment::readJointPositions(const wm::EntityInstanceData& data)
+ {
+ namespace adn = aron::datanavigator;
+
+ // Just get what we need without casting the whole data.
+ std::map<std::string, float> jointPositions;
+ if (adn::DictNavigatorPtr joints = getDictElement(data, "joints"))
+ {
+ if (adn::DictNavigatorPtr jointsPosition = getDictElement(*joints, "position"))
+ {
+ for (const auto& [name, value] : jointsPosition->getElements())
+ {
+ const float jointPosition = adn::FloatNavigator::DynamicCastAndCheck(value)->getValue();
+ jointPositions[name] = jointPosition;
+ }
+ }
+ }
+ return jointPositions;
+ }
+
} // namespace armarx::armem::server::robot_state::proprioception
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 c1c3e44a7..3beeeacc2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -14,6 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
* @date 2021
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
@@ -31,7 +32,6 @@
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
// RobotAPI
-#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/server/segment/Segment.h>
@@ -66,6 +66,13 @@ namespace armarx::armem::server::robot_state::proprioception
const armem::MemoryID& getRobotUnitProviderID() const;
+ private:
+
+ static
+ std::map<std::string, float>
+ readJointPositions(const wm::EntityInstanceData& data);
+
+
private:
RobotUnitInterfacePrx robotUnit;
--
GitLab