From 2cbeb27e8335a1ebf76ed20fcec8b717e73c003e Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 30 Jul 2021 08:17:03 +0200
Subject: [PATCH] Pass Aron dict in RobotUnitData
---
.../server/proprioception/RobotStateWriter.cpp | 10 ++++++----
.../server/proprioception/RobotUnitData.h | 11 ++++++++---
.../converters/Armar6RobotUnitDataConverter.cpp | 6 ++++--
.../converters/Armar6RobotUnitDataConverter.h | 2 +-
.../converters/RobotUnitDataConverterInterface.cpp | 1 -
.../converters/RobotUnitDataConverterInterface.h | 8 +++++++-
6 files changed, 26 insertions(+), 12 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
index 99592121e..fd8ace277 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
@@ -39,6 +39,7 @@
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
@@ -143,19 +144,20 @@ namespace armarx::armem::server::robot_state::proprioception
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
up.timeCreated = data.timestamp;
- up.instancesData = { data.proprioception.toAron() };
+ up.instancesData = { data.proprioception };
// odometry pose -> localization segment
+ if (data.proprioception->hasElement("platform"))
{
ARMARX_DEBUG << "Found odometry data.";
- const Eigen::Vector3f& relPose = data.proprioception.platform.relativePosition;
+ prop::arondto::Platform platform;
+ platform.fromAron(aron::datanavigator::DictNavigator::DynamicCastAndCheck(data.proprioception->getElement("platform")));
+ const Eigen::Vector3f& relPose = platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relPose.z());
- // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
-
armem::robot_state::Transform& transform = update.localization;
transform.header.parentFrame = armarx::OdometryFrame;
transform.header.frame = "root"; // TODO: robot root node
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
index a066754e8..4045742af 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
@@ -1,16 +1,21 @@
#pragma once
+#include <memory>
+
#include <RobotAPI/libraries/armem/core/Time.h>
-#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
+namespace armarx::aron::datanavigator
+{
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+}
namespace armarx::armem::server::robot_state::proprioception
{
struct RobotUnitData
{
- armem::Time timestamp;
- arondto::Proprioception proprioception;
+ Time timestamp;
+ aron::datanavigator::DictNavigatorPtr proprioception;
};
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.cpp
index 292bd120f..96f550c01 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.cpp
@@ -4,6 +4,7 @@
#include <SimoxUtility/algorithm/advanced.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
@@ -164,7 +165,8 @@ namespace armarx::armem::server::robot_state::proprioception
}
- arondto::Proprioception Armar6RobotUnitDataConverter::convert(
+ aron::datanavigator::DictNavigatorPtr
+ Armar6RobotUnitDataConverter::convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description)
{
@@ -175,7 +177,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
process(dto, dataEntryName, {data, dataEntry});
}
- return dto;
+ return dto.toAron();
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.h
index 71d06cfb2..96a7b15aa 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6RobotUnitDataConverter.h
@@ -23,7 +23,7 @@ namespace armarx::armem::server::robot_state::proprioception
virtual ~Armar6RobotUnitDataConverter() override;
- arondto::Proprioception convert(
+ aron::datanavigator::DictNavigatorPtr convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description) override;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.cpp
index 740f37b3b..718094e1f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.cpp
@@ -6,7 +6,6 @@ namespace armarx::armem::server::robot_state
RobotUnitDataConverterInterface::~RobotUnitDataConverterInterface()
{
-
}
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.h
index 7ff34a1dc..349e9c91a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/RobotUnitDataConverterInterface.h
@@ -1,5 +1,7 @@
#pragma once
+#include <memory>
+
namespace armarx::RobotUnitDataStreaming
{
@@ -11,6 +13,10 @@ namespace armarx::armem::arondto
{
class Proprioception;
}
+namespace armarx::aron::datanavigator
+{
+ using DictNavigatorPtr = std::shared_ptr<class DictNavigator>;
+}
namespace armarx::armem::server::robot_state
{
@@ -20,7 +26,7 @@ namespace armarx::armem::server::robot_state
virtual ~RobotUnitDataConverterInterface();
- virtual arondto::Proprioception convert(
+ virtual aron::datanavigator::DictNavigatorPtr convert(
const RobotUnitDataStreaming::TimeStep& data,
const RobotUnitDataStreaming::DataStreamingDescription& description) = 0;
--
GitLab