From e1cde2486126f18957830306ff04b0029e5d2350 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 8 Jun 2022 08:55:47 +0200
Subject: [PATCH] armem_objects/server: using virtual robot reader (new memory)
instead of robot state component
---
.../libraries/armem_objects/server/CMakeLists.txt | 4 +++-
.../armem_objects/server/instance/Segment.cpp | 15 +++++++++++++--
.../armem_objects/server/instance/Segment.h | 3 ++-
.../server/instance/SegmentAdapter.cpp | 11 +++++++----
.../server/instance/SegmentAdapter.h | 3 ++-
5 files changed, 27 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index cf9277330..9f977a7be 100644
--- a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
@@ -21,6 +21,9 @@ armarx_add_library(
RobotAPI::armem_robot
RobotAPI::armem_objects
+ armem_robot_state
+ armem_robot
+
HEADERS
class/FloorVis.h
@@ -51,4 +54,3 @@ armarx_add_library(
add_library(${PROJECT_NAME}::armem_objects_server ALIAS armem_objects_server)
-
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index aaf23c20e..493230e49 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -27,6 +27,7 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
@@ -190,6 +191,8 @@ namespace armarx::armem::server::obj::instance
// Update the entity.
stats.numUpdated++;
+ ARMARX_CHECK(virtualRobotReader->synchronizeRobot(*robot, timestamp));
+
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
if (provided.objectPoseFrame.empty())
{
@@ -394,7 +397,12 @@ namespace armarx::armem::server::obj::instance
if (!synchronized) // Synchronize only once.
{
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ const auto timestamp = armarx::Clock::Now();
+
+ ARMARX_CHECK_NOT_NULL(agent);
+ ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+
+ ARMARX_CHECK(virtualRobotReader->synchronizeRobot(*agent, timestamp));
synchronized = true;
}
objectPose.updateAttached(agent);
@@ -653,7 +661,10 @@ namespace armarx::armem::server::obj::instance
}
else
{
- RemoteRobot::synchronizeLocalClone(agent, robotStateComponent);
+ ARMARX_CHECK_NOT_NULL(virtualRobotReader);
+
+ const auto timestamp = armarx::Clock::Now();
+ ARMARX_CHECK(virtualRobotReader->synchronizeRobot(*agent, timestamp));
armarx::FramedPose framed(data.pose.objectPoseGlobal, armarx::GlobalFrame, agent->getName());
if (frameName == armarx::GlobalFrame)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index ba7c5539d..aab85a19f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -8,6 +8,7 @@
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
@@ -158,7 +159,7 @@ namespace armarx::armem::server::obj::instance
public:
- RobotStateComponentInterfacePrx robotStateComponent;
+ robot_state::VirtualRobotReader* virtualRobotReader;
VirtualRobot::RobotPtr robot;
objpose::ProviderInfoMap providers;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 7a36b0cbc..d72adb6fd 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -30,6 +30,7 @@
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -77,7 +78,7 @@ namespace armarx::armem::server::obj::instance
void SegmentAdapter::connect(
- RobotStateComponentInterfacePrx robotStateComponent,
+ robot_state::VirtualRobotReader* virtualRobotReader,
VirtualRobot::RobotPtr robot,
KinematicUnitObserverInterfacePrx kinematicUnitObserver,
viz::Client arviz,
@@ -88,7 +89,7 @@ namespace armarx::armem::server::obj::instance
this->arviz = arviz;
segment.robot = robot;
- segment.robotStateComponent = robotStateComponent;
+ segment.virtualRobotReader = virtualRobotReader;
robotHead.kinematicUnitObserver = kinematicUnitObserver;
robotHead.debugObserver = debugObserver;
@@ -175,7 +176,10 @@ namespace armarx::armem::server::obj::instance
segment.doLocked([&]()
{
- RemoteRobot::synchronizeLocalClone(segment.robot, segment.robotStateComponent);
+ const auto timestamp = armarx::Clock::Now();
+
+ ARMARX_CHECK_NOT_NULL(segment.robot);
+ ARMARX_CHECK(segment.virtualRobotReader->synchronizeRobot(*segment.robot, timestamp));
if (segment.robot->hasRobotNode(calibration.robotNode))
{
@@ -653,4 +657,3 @@ namespace armarx::armem::server::obj::instance
}
}
-
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
index aafe61f5c..bdfa58f68 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h
@@ -28,6 +28,7 @@
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/interface/armem/server/ObjectMemoryInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
@@ -62,7 +63,7 @@ namespace armarx::armem::server::obj::instance
void init();
void connect(
- RobotStateComponentInterfacePrx robotStateComponent,
+ robot_state::VirtualRobotReader* virtualRobotReader,
VirtualRobot::RobotPtr robot,
KinematicUnitObserverInterfacePrx kinematicUnitObserver,
viz::Client arviz,
--
GitLab