odometry streaming now up and running

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();
             }
 

source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp

 #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
     // {

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;