diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index fe2a7471594c303e21ed0638bb1fa6f5e1635fa5..c2a37675fe7c14de21f949b0d2ad01e07e0a3703 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -21,7 +21,9 @@
*/
#include "ObjectMemory.h"
+#include <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
@@ -65,6 +67,8 @@ namespace armarx::armem::server::obj
"Duration of time window into the past to use for predictions"
" when requested via the PredictingMemoryInterface (in seconds).");
+ defs->optional(robotName, "robotName", "");
+
return defs;
}
@@ -74,6 +78,7 @@ namespace armarx::armem::server::obj
classSegment(iceAdapter()),
attachmentSegment(iceAdapter())
{
+ addPlugin(virtualRobotReaderPlugin);
}
@@ -130,20 +135,26 @@ namespace armarx::armem::server::obj
{
// onConnect can be called multiple times, but addRobot will fail if called more than once with the same ID
// So we need to always make sure to guard a call to addRobot
- const std::string robotKey = "robot";
- VirtualRobot::RobotPtr robot = RobotState::hasRobot(robotKey)
- ? RobotState::getRobot(robotKey)
- : RobotState::addRobot(robotKey, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ // const std::string robotKey = "robot";
+ // VirtualRobot::RobotPtr robot = RobotState::hasRobot(robotKey)
+ // ? RobotState::getRobot(robotKey)
+ // : RobotState::addRobot(robotKey, VirtualRobot::RobotIO::RobotDescription::eStructure);
- robotStateComponent = RobotState::getRobotStateComponent();
+ // TODO: the instance segment should check the provided object poses and instantiate the needed robots by itself.
+ ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
+
+ VirtualRobot::RobotPtr robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(robotName, armarx::Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure, true);
+ ARMARX_CHECK_NOT_NULL(robot);
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
// Create first to use the original values.
createRemoteGuiTab();
+ // ARMARX_CHECK(virtualRobotReaderPlugin->isAvailable());
+
instance::SegmentAdapter::connect(
- robotStateComponent,
+ &virtualRobotReaderPlugin->get(),
robot,
kinematicUnitObserver,
ArVizComponentPluginUser::getArvizClient(),
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 1a939eebff7c94f887b4d68f5d363c5f986e5194..5cf03b6e31f15526cf6f99e0be43c0aacedabb11 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -22,6 +22,8 @@
#pragma once
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
#include <RobotAPI/libraries/armem_objects/server/class/Segment.h>
#include <RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.h>
#include <RobotAPI/libraries/armem_objects/server/attachments/Segment.h>
@@ -61,15 +63,11 @@ namespace armarx::armem::server::obj
, virtual public armarx::armem::server::ObjectMemoryInterface
, virtual public armarx::armem::server::ReadWritePluginUser
, virtual public armarx::armem::server::obj::instance::SegmentAdapter
-
- , virtual public armarx::RobotStateComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
{
public:
- using RobotState = armarx::RobotStateComponentPluginUser;
-
static const std::string defaultMemoryName;
@@ -119,7 +117,6 @@ namespace armarx::armem::server::obj
private:
DebugObserverInterfacePrx debugObserver;
- RobotStateComponentInterfacePrx robotStateComponent;
KinematicUnitObserverInterfacePrx kinematicUnitObserver;
double predictionTimeWindow = 2;
@@ -137,6 +134,8 @@ namespace armarx::armem::server::obj
};
std::unique_ptr<RemoteGuiTab> tab;
+ std::string robotName = "Armar6";
+ armem::client::plugins::ReaderWriterPlugin<robot_state::VirtualRobotReader>* virtualRobotReaderPlugin;
};
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/server/CMakeLists.txt
index cf92773305aac9c8d4c1ce69abbf86bd3b6789ea..9f977a7beaa67789b181cbabb18752cbf4242fef 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 aaf23c20e21eff50cf135664b6c8c951d9980abd..6ba35fd2042b04540721c95c1b48d566ad9d3d5a 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>
@@ -150,6 +151,10 @@ namespace armarx::armem::server::obj::instance
// Build new poses.
objpose::ObjectPoseSeq newObjectPoses;
stats.numUpdated = 0;
+
+ // timestamp used to reduce the rpc calls for robot sync
+ Time robotSyncTimestamp = -1;
+
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
const Time timestamp = armarx::fromIce<Time>(provided.timestamp);
@@ -190,6 +195,13 @@ namespace armarx::armem::server::obj::instance
// Update the entity.
stats.numUpdated++;
+ // update the robot to obtain correct local -> global transformation
+ if(robotSyncTimestamp != timestamp)
+ {
+ ARMARX_CHECK(virtualRobotReader->synchronizeRobot(*robot, timestamp));
+ robotSyncTimestamp = timestamp;
+ }
+
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
if (provided.objectPoseFrame.empty())
{
@@ -394,7 +406,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 +670,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 ba7c5539df7299be11ceead0c4253938bfe9ed73..aab85a19f618a7c47c93de465987349d015db3fd 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 7a36b0cbce48d258eded0545492bd4974019069f..d72adb6fd350f5ca40c432b519c2509255879f16 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 aafe61f5c87e5a1363ef837d56cd7cbeec08407c..bdfa58f683e4705a6a6623621a1cdca0027be98f 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,