From 58ddcc332430ae7e9a9207dfe05ab61b3099f392 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 10 May 2021 15:08:13 +0200
Subject: [PATCH] odometry streaming now up and running
---
.../RobotStateMemory/RobotStateMemory.cpp | 46 +++++++++++++++++++
.../server/localization/Segment.cpp | 31 ++++++++++++-
.../server/localization/Segment.h | 4 ++
3 files changed, 80 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 45855e314..12efa6541 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -23,6 +23,8 @@
#include "RobotStateMemory.h"
// STD
+#include <Eigen/src/Geometry/Transform.h>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <algorithm>
#include <iostream>
#include <fstream>
@@ -45,6 +47,10 @@
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
+#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
namespace armarx::armem::server::robot_state
{
RobotStateMemory::RobotStateMemory()
@@ -284,6 +290,9 @@ namespace armarx::armem::server::robot_state
// send batch to memory
armem::data::Commit c;
+
+ armem::data::Commit odometryCommit;
+
for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
{
std::lock_guard g{robotUnitDataMutex};
@@ -303,6 +312,43 @@ namespace armarx::armem::server::robot_state
update.timeCreatedMicroSeconds = timeUSec;
update.instancesData = {aron->toAronDictPtr()};
}
+
+ // odometry pose -> localization segment
+ auto it = convertedAndGroupedData.groups.find("sens.Platform");
+ if (it != convertedAndGroupedData.groups.end())
+ {
+ 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();
+
+ Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
+ odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
+ odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
+
+ const ::armarx::armem::robot_state::Transform transform
+ {
+ .header =
+ {
+ .parentFrame = OdometryFrame,
+ .frame = "root", // TODO: robot root node
+ .agent = robotUnitProviderID.providerSegmentName,
+ .timestamp = it->second.timestamp
+ },
+ .transform = odometryPose
+ };
+
+ localizationSegment.storeTransform(transform);
+
+ }
+ else
+ {
+ ARMARX_WARNING << "no odometry data!";
+ }
+
+
robotUnitDataQueue.pop();
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 37f543b57..d245d2c87 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -1,6 +1,8 @@
#include "Segment.h"
+#include <IceUtil/Time.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
+#include <iterator>
#include <sstream>
#include <ArmarXCore/core/time/TimeUtil.h>
@@ -17,8 +19,10 @@
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
namespace armarx::armem::server::robot_state::localization
{
@@ -46,6 +50,8 @@ namespace armarx::armem::server::robot_state::localization
coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegment, arondto::Transform::toInitialAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+
}
void Segment::connect(viz::Client arviz)
@@ -62,7 +68,30 @@ namespace armarx::armem::server::robot_state::localization
return robotGlobalPoses;
}
+ bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
+ {
+
+ const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
+ const MemoryID providerID = coreSegment->id().withProviderSegmentName(transform.header.agent);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
+
+ Commit commit;
+
+ EntityUpdate& update = commit.updates.emplace_back();
+ update.entityID = providerID.withEntityName(transform.header.parentFrame + "," + transform.header.frame);
+ update.timeArrived = update.timeCreated = update.timeSent = timestamp;
+
+ arondto::Transform aronTransform;
+ toAron(aronTransform, transform);
+ update.instancesData = {aronTransform.toAron()};
+
+ const armem::CommitResult result = iceMemory.commit(commit);
+ return result.allSuccess();
+ }
// std::unordered_map<armem::MemoryID, ::armarx::armem::articulated_object::ArticulatedObjectDescription> Segment::getKnownObjectClasses() const
// {
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
index 465ba2d8b..28730c956 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.h
@@ -36,6 +36,8 @@
#include "RobotAPI/libraries/armem/core/MemoryID.h"
#include "RobotAPI/libraries/armem_objects/types.h"
+#include <RobotAPI/libraries/armem_robot_state/types.h>
+
namespace armarx::armem
{
namespace server
@@ -70,6 +72,8 @@ namespace armarx::armem::server::robot_state::localization
std::unordered_map<std::string, Eigen::Affine3f> getRobotGlobalPoses() const;
+ bool storeTransform(const armarx::armem::robot_state::Transform& transform);
+
private:
server::MemoryToIceAdapter& iceMemory;
--
GitLab