From af35d029ad69c9ebdcd3ace71b49268e531ff2ea Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 May 2021 08:22:21 +0200
Subject: [PATCH] robotstatememory: changed field name of streamed data:
removing e.g 'sens.Platform' as it prevents direct casting (e.g. JointState)
---
.../RobotStateMemory/RobotStateMemory.cpp | 38 ++++++++++++++-----
.../RobotStateMemory/RobotStateMemory.h | 4 +-
2 files changed, 30 insertions(+), 12 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 12efa6541..0fb608487 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -31,6 +31,7 @@
// Header
#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h"
#include "RobotAPI/libraries/core/Pose.h"
// Simox
@@ -50,6 +51,7 @@
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <memory>
namespace armarx::armem::server::robot_state
{
@@ -59,8 +61,10 @@ namespace armarx::armem::server::robot_state
proprioceptionSegment(server::ComponentPluginUser::iceMemory,
server::ComponentPluginUser::workingMemoryMutex),
localizationSegment(server::ComponentPluginUser::iceMemory,
- server::ComponentPluginUser::workingMemoryMutex)
+ server::ComponentPluginUser::workingMemoryMutex),
+ commonVisu(descriptionSegment, proprioceptionSegment, localizationSegment)
{
+
}
RobotStateMemory::~RobotStateMemory() = default;
@@ -77,6 +81,7 @@ namespace armarx::armem::server::robot_state
descriptionSegment.defineProperties(defs);
proprioceptionSegment.defineProperties(defs);
localizationSegment.defineProperties(defs);
+ commonVisu.defineProperties(defs);
return defs;
}
@@ -94,6 +99,7 @@ namespace armarx::armem::server::robot_state
descriptionSegment.init();
proprioceptionSegment.init();
localizationSegment.init();
+ commonVisu.init();
robotUnitMemoryBatchSize = std::max((unsigned int) 1, robotUnitMemoryBatchSize);
robotUnitPollFrequency = std::clamp(robotUnitPollFrequency, 1, ROBOT_UNIT_MAXIMUM_FREQUENCY);
@@ -130,8 +136,9 @@ namespace armarx::armem::server::robot_state
localizationSegment.connect(getArvizClient());
- commonVisu = std::make_unique<Visu>(getArvizClient(), descriptionSegment, proprioceptionSegment, localizationSegment);
- commonVisu->init();
+ commonVisu.connect(
+ getArvizClient()
+ );
cfg.loggingNames.emplace_back(robotUnitSensorPrefix);
handler = make_shared<RobotUnitDataStreamingReceiver>(this, getRobotUnit(), cfg);
@@ -171,8 +178,11 @@ namespace armarx::armem::server::robot_state
auto start = std::chrono::high_resolution_clock::now();
RobotUnitData convertedAndGroupedData;
- for (const auto& [name, dataEntry] : keys.entries)
+ for (const auto& [nameEntry, dataEntry] : keys.entries)
{
+ std::string name = nameEntry;
+ std::string jointOrWhateverName;
+
std::string groupName = "";
if (configSensorMapping.find(name) != configSensorMapping.end())
{
@@ -180,13 +190,18 @@ namespace armarx::armem::server::robot_state
}
else
{
- size_t dot_pos = name.find(".", name.find(".") + 1); // find second occurence of "."
- if (dot_pos == std::string::npos)
+ const size_t first_dot_pos = name.find(".");
+ const size_t second_dot_pos = name.find(".", first_dot_pos + 1); // find second occurence of "."
+ if (second_dot_pos == std::string::npos)
{
ARMARX_WARNING << "Could not find a groupname for the sensor with name " << name << ". All sensors must be called sens.X.Y where X is the name of the group and Y is the actual sensor. Ignoring this sensor.";
continue;
}
- groupName = name.substr(0, dot_pos);
+ groupName = name.substr(0, second_dot_pos);
+
+ jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
+
+ name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
}
RobotUnitData::RobotUnitDataGroup e;
@@ -200,6 +215,9 @@ namespace armarx::armem::server::robot_state
auto iterId = std::make_shared<aron::datanavigator::LongNavigator>(currentTimestep.iterationId);
dict->addElement("IterationId", iterId);
+ const auto nameId = std::make_shared<aron::datanavigator::StringNavigator>(jointOrWhateverName);
+ dict->addElement("name", nameId);
+
e.timestamp = currentTimestep.timestampUSec;
e.name = groupName;
e.data = dict;
@@ -320,9 +338,9 @@ namespace armarx::armem::server::robot_state
ARMARX_INFO << " found odometry data.";
const RobotUnitData::RobotUnitDataGroup& timestep = it->second;
- const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionX"))->getValue();
- const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionY"))->getValue();
- const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("sens.Platform.relativePositionRotation"))->getValue();
+ const float relPosX = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionX"))->getValue();
+ const float relPosY = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionY"))->getValue();
+ const float relOrientation = aron::datanavigator::FloatNavigator::DynamicCast(timestep.data->getElement("relativePositionRotation"))->getValue();
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index c48e44ac8..45530f17a 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -44,10 +44,10 @@
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
+#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
namespace armarx::armem::server::robot_state
{
- class Visu;
/**
* @defgroup Component-RobotSensorMemory RobotSensorMemory
@@ -116,7 +116,7 @@ namespace armarx::armem::server::robot_state
localization::Segment localizationSegment;
// Joint visu for all segments => robot pose and configuration
- std::unique_ptr<Visu> commonVisu;
+ Visu commonVisu;
// RobotUnit stuff
RobotUnitDataStreaming::DataStreamingDescription keys;
--
GitLab