From a776b29e3dbea2fd51f350f350a7ec0481a5bb41 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 May 2021 08:20:24 +0200
Subject: [PATCH] odometry streaming
---
.../server/proprioception/Segment.cpp | 39 ++++++++++---------
1 file changed, 21 insertions(+), 18 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 cc8d03a87..38fe69a91 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -6,6 +6,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem_robot_state/types.h"
#include "RobotAPI/libraries/aron/common/aron_conversions.h"
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -18,9 +19,12 @@
#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/JointState.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
namespace armarx::armem::server::robot_state::proprioception
{
@@ -45,7 +49,7 @@ namespace armarx::armem::server::robot_state::proprioception
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
ARMARX_INFO << "Adding core segment '" << p.coreSegment << "'";
- coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment); // TODO , arondto::Robot::toInitialAronType());
+ coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment);
coreSegment->setMaxHistorySize(p.maxHistorySize);
}
@@ -83,32 +87,31 @@ namespace armarx::armem::server::robot_state::proprioception
robotUnitProviderID.memoryName = iceMemory.workingMemory->id().memoryName;
robotUnitProviderID.coreSegmentName = p.coreSegment;
robotUnitProviderID.providerSegmentName = providerSegmentName;
-
- // this->visu = std::make_unique<Visu>(arviz, *this);
}
std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions() const
{
std::unordered_map<std::string, std::map<std::string, float>> jointMap;
- // for (const auto& [provName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreClassSegmentName))
- // {
- // for (const auto& [entityName, entity] : provSeg.entities())
- // {
- // const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // const auto description = articulated_object::convertRobotDescription(entityInstance);
+ for (const auto& [robotName, provSeg] : iceMemory.workingMemory->getCoreSegment(p.coreSegment))
+ {
+ for (const auto& [name, entity] : provSeg.entities())
+ {
+ const auto& entityInstance = entity.getLatestSnapshot().getInstance(0);
- // if (not description)
- // {
- // ARMARX_WARNING << "Could not convert entity instance to 'RobotDescription'";
- // continue;
- // }
+ const auto jointState = tryCast<armarx::armem::arondto::JointState>(entityInstance);
- // ARMARX_INFO << "Key is " << armem::MemoryID(entity.id());
+ if (not jointState)
+ {
+ // ARMARX_WARNING << "Could not convert entity instance to 'JointState'";
+ continue;
+ }
+
+ jointMap[robotName].emplace(jointState->name, jointState->position);
+ }
+ }
- // objects.emplace(armem::MemoryID(entity.id()), *description);
- // }
- // }
+ ARMARX_INFO << deactivateSpam(10) << "Number of known robot joint maps: " << jointMap.size();
return jointMap;
}
--
GitLab