diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 9d9342ab21b51d11fd188a70c5e4cf6066afff24..8b869fe31552807e5b47222e396eefa9d564092f 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -344,5 +344,13 @@
propertyName="MemoryNameSystemName"
propertyIsOptional="true"
propertyDefaultValue="MemoryNameSystem" />
+ <Proxy include="RobotAPI/interface/core/RobotLocalization.h"
+ humanName="GlobalRobotPoseProvider"
+ typeName="armarx::GlobalRobotPoseProviderPrx"
+ memberName="GlobalRobotPoseProvider"
+ getterName="getGlobalRobotPoseProvider"
+ propertyName="GlobalRobotPoseProviderName"
+ propertyIsOptional="true"
+ propertyDefaultValue="GlobalRobotPoseProvider" />
</Lib>
</VariantInfo>
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
index 985162f1726a66c88c4a310e6b287641294fa9f9..35287f4d55adc284fab6bf74b02f75ecb5307880 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.cpp
@@ -1,6 +1,6 @@
#include "Line.h"
-#include "ArmarXCore/interface/core/BasicVectorTypesHelpers.h"
+#include <ArmarXCore/interface/core/BasicVectorTypesHelpers.h>
namespace armarx::viz
{
@@ -17,4 +17,4 @@ namespace armarx::viz
return *this;
}
-} // namespace armarx::viz
\ No newline at end of file
+} // namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Line.h b/source/RobotAPI/components/ArViz/Client/elements/Line.h
index 6f6d9427e05b9d91606e8ecdfa8a2164f4847515..2d55b5d340f14cdd55252ec9f7f0007d625b5c7e 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Line.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Line.h
@@ -21,7 +21,7 @@
#pragma once
-#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/components/ArViz/Client/elements/ElementOps.h>
#include <RobotAPI/interface/ArViz/Elements.h>
namespace armarx::viz
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Path.h b/source/RobotAPI/components/ArViz/Client/elements/Path.h
index 7cfbecbd973a194cef0e0e350a5c7c3c1aedbc13..54615e9d44a0f12272ea35cecb3d62f63ccff08e 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Path.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Path.h
@@ -23,7 +23,7 @@
#pragma once
-#include "RobotAPI/components/ArViz/Client/elements/ElementOps.h"
+#include <RobotAPI/components/ArViz/Client/elements/ElementOps.h>
#include <RobotAPI/interface/ArViz/Elements.h>
namespace armarx::viz
@@ -42,8 +42,3 @@ namespace armarx::viz
Path& addPoint(Eigen::Vector3f p);
};
} // namespace armarx::viz
-
-
-
-
-
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 70b399165ad3c229d96289676e0da6f2280e91a4..d5c7d69917c8cbba5fafee284b18229f351ea9de 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -32,7 +32,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index e3747fb3f23fb4429ab7a9d028d85efd3f2befaa..d401eed5e741d3d797dfaa8a47f347f4616a53db 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -3,14 +3,14 @@
#include <VirtualRobot/VirtualRobot.h>
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/plugins.h>
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index 43f32295717ff031d2ac9b6f54184c21b8c60e73..6359111c24ef51d35360ff8def6853f9fdae55ae 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -70,7 +70,9 @@ namespace armarx
while (objectProcessingTask && !objectProcessingTask->isStopped())
{
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClient::getObjectPoses();
+ // This client can be copied to other classes to give them access to the object pose storage.
+ objpose::ObjectPoseClient client = getClient();
+ const objpose::ObjectPoseSeq objectPoses = client.fetchObjectPoses();
ARMARX_VERBOSE << "Received poses of " << objectPoses.size() << " objects.";
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 161acac707955748434292a6a477637137e8712d..412b526217bec76a64391cca1de105840032778a 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -55,8 +55,8 @@ namespace armarx::armem::server::grasp
void GraspMemory::onConnectComponent()
{
-// createRemoteGuiTab();
-// RemoteGui_startRunningTask();
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
}
@@ -402,6 +402,7 @@ namespace armarx::armem::server::grasp
RemoteGui_createTab(getName(), root, &gui.tab);
gui.tab.rebuild = false;
+
}
@@ -505,6 +506,7 @@ namespace armarx::armem::server::grasp
if (gui.tab.rebuild)
{
+ ARMARX_INFO << "Rebuilding remote gui tab";
createRemoteGuiTab();
}
}
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 0aef2635f6cec7714f654a3437fc95268e0f6963..c1bf3bc93f73ece3f5f85cf7a43a7a2d1ad270bb 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -21,7 +21,10 @@
*/
#include "RobotStateMemory.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
@@ -31,6 +34,10 @@
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
@@ -182,6 +189,22 @@ namespace armarx::armem::server::robot_state
}
+ armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp_us, const std::string& robotName, const ::Ice::Current& /*unused*/)
+ {
+ auto timestamp = armem::Time::microSeconds(timestamp_us);
+ ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << timestamp << ".";
+
+ auto poseMap = localizationSegment.getRobotGlobalPoses(timestamp);
+
+ bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
+ ARMARX_CHECK(robotNameFound)
+ << "Robot with name " << robotName << " does not exist at or before timestamp " << timestamp << ".\n"
+ << "Available robots are: " << simox::alg::get_keys(poseMap);
+
+ return new Pose(poseMap[robotName].matrix());
+ }
+
+
/*************************************************************/
// RobotUnit Streaming functions
/*************************************************************/
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 863c9b5711a1effc010488eb9badf3195a88c8c5..d402496819431eded99b506b69fbab48e6804086 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -39,6 +39,8 @@
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
+
namespace armarx::plugins
{
@@ -62,7 +64,8 @@ namespace armarx::armem::server::robot_state
class RobotStateMemory :
virtual public armarx::Component,
virtual public armem::server::ReadWritePluginUser,
- virtual public armarx::ArVizComponentPluginUser
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::GlobalRobotPoseProvider
{
public:
@@ -73,6 +76,10 @@ namespace armarx::armem::server::robot_state
std::string getDefaultName() const override;
+ // GlobalRobotPoseProvider interface
+ armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+
+
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
diff --git a/source/RobotAPI/components/units/ForceTorqueUnit.h b/source/RobotAPI/components/units/ForceTorqueUnit.h
index 30dcf8952b25685214accbd78eb108954a2378a8..d61360637f06d1d1d33ecc2d8c0fe283b7ed0b34 100644
--- a/source/RobotAPI/components/units/ForceTorqueUnit.h
+++ b/source/RobotAPI/components/units/ForceTorqueUnit.h
@@ -28,7 +28,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include "RobotAPI/components/units/SensorActorUnit.h"
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/ForceTorqueUnit.h>
@@ -94,4 +94,3 @@ namespace armarx
std::string listenerName;
};
}
-
diff --git a/source/RobotAPI/components/units/HapticUnit.h b/source/RobotAPI/components/units/HapticUnit.h
index df548dc31d42f481da979f56968d25ae89988fa9..26050b48062569e7d154ee2d82e6683e90a67589 100644
--- a/source/RobotAPI/components/units/HapticUnit.h
+++ b/source/RobotAPI/components/units/HapticUnit.h
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/Properties.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include "RobotAPI/components/units/SensorActorUnit.h"
+#include <RobotAPI/components/units/SensorActorUnit.h>
#include <RobotAPI/interface/units/HapticUnit.h>
@@ -83,4 +83,3 @@ namespace armarx
HapticUnitListenerPrx hapticTopicPrx;
};
}
-
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index 6f5c6b07454e2ad831d48ba327b33c7cd768ddde..b60e943849edd0720eb9b523ee6518e0fced18df 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -25,7 +25,7 @@
#include "PlatformUnit.h"
-#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 0f4d58b0fbc2be757117a70aac3a213bea3f8da9..3c9cde6b67a1106976beee9032795262755a4be3 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -24,7 +24,7 @@
#include "PlatformUnitSimulation.h"
-#include "RobotAPI/components/units/PlatformUnit.h"
+#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/interface/core/GeometryBase.h>
#include <cmath>
@@ -176,7 +176,7 @@ namespace armarx
Eigen::Rotation2Df rot(currentRotation);
targetVel = rot * targetVel;
velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
- currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
+ currentTranslationVelocity = timeDeltaInSeconds * velPID->getControlValue();
currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
index 23ae6d2299c3874e9f7501ef7e81c124bc4882d6..c67b41d1985b7bcfc2f9404a09878424302da9a3 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp
@@ -229,7 +229,3 @@ namespace armarx
}
}
} // namespace armarx
-
-
-
-
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
index dbaee0f7a61dd6b95244e55e06b08bf50e4febec..6e5b38a32d000f219f8a868dfa8d869945637249 100644
--- a/source/RobotAPI/interface/core/RobotLocalization.ice
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -24,10 +24,15 @@
#pragma once
+#include <RobotAPI/interface/armem/server/MemoryInterface.ice>
#include <RobotAPI/interface/core/GeometryBase.ice>
+#include <RobotAPI/interface/core/PoseBase.ice>
module armarx{
-
+ interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+ // timestamp in microseconds
+ PoseBase getGlobalRobotPose(long timestamp, string robotName);
+ }
interface GlobalRobotPoseLocalizationListener{
void reportGlobalRobotPose(TransformStamped currentPose);
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index de740d244c3e671016956df0b526065b21d3638c..8710af33c07522713bd833e53c7f1e982754e3ec 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -54,6 +54,17 @@ namespace armarx::objpose
}
return map;
}
+
+ ObjectPoseSeq ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
+ {
+ if (!objectPoseStorage)
+ {
+ ARMARX_WARNING << "No object pose observer.";
+ return {};
+ }
+ return fromIce(objectPoseStorage->getObjectPosesByProvider(providerName));
+ }
+
const ObjectPoseStorageInterfacePrx&
@@ -70,4 +81,3 @@ namespace armarx::objpose
}
}
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index 2e262cc155944c4d9d3cebd6ca57d2b8130da595..33f9a6dd7b40fd933a451d50d093afc937a08635 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -27,9 +27,13 @@ namespace armarx::objpose
ObjectPoseSeq
fetchObjectPoses();
+
ObjectPoseMap
fetchObjectPosesAsMap();
+ ObjectPoseSeq
+ fetchObjectPosesFromProvider(const std::string& providerName);
+
const ObjectPoseStorageInterfacePrx&
getObjectPoseStorage() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.h b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
index a4cc41d80b5fccc87b1e9d871808c685ce332401..3c2767c017ce2ac19faef9bde78b7ede5bba8b13 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/Scene.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
@@ -24,6 +24,7 @@
#include <map>
#include <string>
#include <vector>
+#include <optional>
#include <Eigen/Core>
#include <Eigen/Geometry>
@@ -43,6 +44,7 @@ namespace armarx::objects
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+ std::optional<bool> isStatic;
std::map<std::string, float> jointValues;
ObjectID getClassID() const;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 617baca941ebb86be7e3b05ecac1e79efc23490c..161983900123a6848d2e667258236f2bd95aa33a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -79,31 +79,51 @@ void armarx::objpose::from_json(const nlohmann::json& j, ObjectPose& op)
void armarx::objects::to_json(nlohmann::json& j, const SceneObject& rhs)
{
- // j["instanceID"] = rhs.instanceID;
j["class"] = rhs.className;
j["instanceName"] = rhs.instanceName;
j["collection"] = rhs.collection;
j["position"] = rhs.position;
j["orientation"] = rhs.orientation;
+ if (rhs.isStatic.has_value())
+ {
+ j["isStatic"] = rhs.isStatic.value();
+ }
j["jointValues"] = rhs.jointValues;
}
void armarx::objects::from_json(const nlohmann::json& j, SceneObject& rhs)
{
- // j.at("instanceID").get_to(rhs.instanceID);
j.at("class").get_to(rhs.className);
if (j.count("instanceName"))
{
j["instanceName"].get_to(rhs.instanceName);
}
+ else
+ {
+ rhs.instanceName.clear();
+ }
j.at("collection").get_to(rhs.collection);
j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
+
+ if (j.count("isStatic"))
+ {
+ rhs.isStatic = j.at("isStatic").get<bool>();
+ }
+ else
+ {
+ rhs.isStatic = std::nullopt;
+ }
+
if (j.count("jointValues"))
{
j.at("jointValues").get_to(rhs.jointValues);
}
+ else
+ {
+ rhs.jointValues.clear();
+ }
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
index a2b3ef47343443b4ac0e1fc85c9f33f52ccc857e..62161ff5cafce827a99191e97ce7bfa6bb2abee4 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.cpp
@@ -72,6 +72,12 @@ namespace armarx
return objpose::fromIce(objectPoseStorage->getObjectPoses());
}
+ objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPosesByProvider(const std::string& providerName)
+ {
+ return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder()).fetchObjectPosesFromProvider(providerName);
+ }
+
+
plugins::ObjectPoseClientPlugin& ObjectPoseClientPluginUser::getObjectPoseClientPlugin()
{
return *plugin;
@@ -86,4 +92,3 @@ namespace armarx
return plugin->getObjectFinder();
}
}
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
index 9f72ab1eb4b96542352eb34e64e3955382b2c0d2..00c53d4ead2c77fce8710312648d54ce59b29742 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h
@@ -79,6 +79,8 @@ namespace armarx
objpose::ObjectPoseSeq getObjectPoses();
+ objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName);
+
plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin();
const plugins::ObjectPoseClientPlugin& getObjectPoseClientPlugin() const;
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 8f875542bce9f7a5ed3fe91bc697eb3cd76e1edf..af59f5fa837a3d80e199350ee9da562bc5745703 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -246,7 +246,6 @@ namespace armarx::armem
void GraspCandidateReader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
ARMARX_DEBUG << "GraspCandidateReader: registerPropertyDefinitions";
- registerPropertyDefinitions(def);
const std::string prefix = propertyPrefix;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
index 743d8a9bc2904a707f42633567f9b59ef184c3af..d0c7371051effc8f00a007687723ece0e2fa5cb9 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -1,7 +1,7 @@
#include "HeartbeatComponentPlugin.h"
-#include "ArmarXCore/core/Component.h"
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/components/RobotHealthInterface.h>
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index ecd3f8c5f4a1b3ed0a7dde04c8dc1ac8a06b5265..0ea7000b229edce38d8a797d3b542d180c2de17a 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,5 +1,5 @@
#include "selectors.h"
-#include "RobotAPI/libraries/armem/core/ice_conversions.h"
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
index c4e703531a9508757a00e6c1dbf5429c7436b470..f26a62736999d28a55001dbaee56d515c6cdd16c 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -9,9 +9,9 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
-#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Time.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 64dfbc7f546bfc887fd408e444ad2332b8a8aaa0..9a4417080bb027e4ee8d4a9cf0c2dc080fca3890 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -5,11 +5,11 @@
#include <Eigen/Geometry>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
-#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
-#include "RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h"
-#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectInfo.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
index 9c5b48f39864b16637739b022c0d1908de69c9a5..5bb773c96354b108185811137602bdc443c0f9a7 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -3,7 +3,7 @@
#include <optional>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
@@ -13,4 +13,4 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotDescription>
convertRobotDescription(const armem::wm::EntityInstance& instance);
-} // namespace armarx::armem::articulated_object
\ No newline at end of file
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
index f1b90e5daec4edf42590ce0d8aad7a4aa9d83838..a8060557502fa895f8d632288e32a1c3e0abd5f0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/ArticulatedObjectVisu.cpp
@@ -4,7 +4,7 @@
#include <SimoxUtility/math/pose.h>
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 485715690f3fdbfe089f3557582bae86ef7210ad..72faed5fee3f85ff9e34d8e3b821c33598d52f37 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -32,6 +32,7 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/json.h>
#include <SimoxUtility/math/pose/pose.h>
@@ -100,8 +101,9 @@ namespace armarx::armem::server::obj::instance
defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
"Directory in Package/data/Package/ containing the scene snapshots.");
defs->optional(p.sceneSnapshotToLoad, prefix + "scene.12_SnapshotToLoad",
- "Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
- "You can also specify paths relative to 'Package/scenes/'.");
+ "Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
+ "You can also specify paths relative to 'Package/scenes/'. \n"
+ "You can also specify a ; separated list of scenes.");
decay.defineProperties(defs, prefix + "decay.");
}
@@ -113,8 +115,13 @@ namespace armarx::armem::server::obj::instance
if (not p.sceneSnapshotToLoad.empty())
{
- bool lockMemory = false;
- commitSceneSnapshotFromFilename(p.sceneSnapshotToLoad, lockMemory);
+ bool trim = true;
+ const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotToLoad, ";", trim);
+ for (const std::string& scene : scenes)
+ {
+ const bool lockMemory = false;
+ commitSceneSnapshotFromFilename(scene, lockMemory);
+ }
}
}
@@ -936,7 +943,8 @@ namespace armarx::armem::server::obj::instance
pose.providerName = sceneName;
pose.objectType = objpose::ObjectType::KnownObject;
- pose.isStatic = true; // Objects loaded from prior knowledge are considerd static to exclude them from decay.
+ // If not specified, assume loaded objects are static.
+ pose.isStatic = object.isStatic.has_value() ? object.isStatic.value() : true;
pose.objectID = classID.withInstanceName(
object.instanceName.empty()
? std::to_string(idCounters[classID]++)
diff --git a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
index 1bf28496f697da38e2172a41836ecaccceaa5561..0ab9ff1a46f9e756471ccf8d257e966dcfb7f2ef 100644
--- a/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_robot/aron_conversions.cpp
@@ -1,6 +1,6 @@
#include "aron_conversions.h"
-#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h
index 0a8473073f5b30dce3cb9f2d96682b4184a6a9b3..10ca7475f0d119aeb8f40e2f0d674bb3a6450691 100644
--- a/source/RobotAPI/libraries/armem_robot/types.h
+++ b/source/RobotAPI/libraries/armem_robot/types.h
@@ -8,8 +8,8 @@
#include <IceUtil/Time.h>
-#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
-#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <ArmarXCore/core/PackagePath.h>
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index 6610085a42af19314fdd5f3a062da01dcd1f8cb2..b3dee3440967cf79ae9feee73403116345a23cbf 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -1,7 +1,9 @@
#include "RobotReader.h"
+#include <chrono>
#include <mutex>
#include <optional>
+#include <thread>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
@@ -91,19 +93,48 @@ namespace armarx::armem::robot_state
return robot;
}
+ void
+ RobotReader::setSyncTimeout(const armem::Time& duration)
+ {
+ syncTimeout = duration;
+ }
+
+ void
+ RobotReader::setSleepAfterSyncFailure(const armem::Time& duration)
+ {
+ ARMARX_CHECK_NONNEGATIVE(duration.toMicroSeconds());
+ sleepAfterFailure = duration;
+ }
+
bool
RobotReader::synchronize(robot::Robot& obj, const armem::Time& timestamp)
{
- auto state = queryState(obj.description, timestamp);
+ const auto tsStartFunctionInvokation = armem::Time::now();
- if (not state) /* c++20 [[unlikely]] */
+ while (true)
{
- ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
- return false;
- }
+ auto state = queryState(obj.description, timestamp);
+
+ if (not state) /* c++20 [[unlikely]] */
+ {
+ ARMARX_VERBOSE << "Could not synchronize object " << obj.description.name;
+
+ // if the syncTime is non-positive there will be no retry
+ const auto elapsedTime = armem::Time::now() - tsStartFunctionInvokation;
+ if (elapsedTime > syncTimeout)
+ {
+ ARMARX_WARNING << "Could not synchronize object " << obj.description.name;
+ return false;
+ }
+
+ ARMARX_INFO << "Retrying to query robot state after failure";
+ std::this_thread::sleep_for(
+ std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ }
- obj.config = std::move(*state);
- return true;
+ obj.config = std::move(*state);
+ return true;
+ }
}
std::optional<robot::RobotDescription>
@@ -117,7 +148,7 @@ namespace armarx::armem::robot_state
.coreSegments().all() // withName(properties.descriptionCoreSegment)
.providerSegments().all() // .withName(name)
.entities().all()
- .snapshots().latest(); // TODO(fabian.reister): atTime(timestamp);
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
ARMARX_DEBUG << "Lookup query in reader";
@@ -188,7 +219,7 @@ namespace armarx::armem::robot_state
.coreSegments().withName(properties.proprioceptionCoreSegment)
.providerSegments().withName(description.name) // agent
.entities().all() // TODO
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -252,7 +283,7 @@ namespace armarx::armem::robot_state
.coreSegments().withName(properties.proprioceptionCoreSegment)
.providerSegments().withName(description.name) // agent
.entities().all() // TODO
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
@@ -322,8 +353,8 @@ namespace armarx::armem::robot_state
std::optional<AronClass>
tryCast(const wm::EntityInstance& item)
{
- static_assert(
- std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronClass>::value);
+ static_assert(std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass,
+ AronClass>::value);
try
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
index 20cafbd2225160c7b9abacc2d73b981f60260e34..46b0924fc2cb06ebee34ff8b61174746939dd7dc 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -24,6 +24,7 @@
#include <mutex>
#include <optional>
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem_robot/client/interfaces.h>
@@ -67,10 +68,10 @@ namespace armarx::armem::robot_state
const armem::Time& timestamp) const;
using JointTrajectory = std::map<armem::Time, robot::RobotState::JointMap>;
-
- JointTrajectory
- queryJointStates(const robot::RobotDescription& description,
- const armem::Time& begin, const armem::Time& end) const;
+
+ JointTrajectory queryJointStates(const robot::RobotDescription& description,
+ const armem::Time& begin,
+ const armem::Time& end) const;
std::optional<robot::RobotState::Pose>
queryGlobalPose(const robot::RobotDescription& description,
@@ -80,6 +81,8 @@ namespace armarx::armem::robot_state
queryPlatformState(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
+ void setSyncTimeout(const armem::Time& duration);
+ void setSleepAfterSyncFailure(const armem::Time& duration);
enum class Hand
{
@@ -96,6 +99,11 @@ namespace armarx::armem::robot_state
const armem::Time& start,
const armem::Time& end) const;
+ protected:
+ // by default, no timeout mechanism
+ armem::Time syncTimeout = armem::Time::microSeconds(0);
+ armem::Time sleepAfterFailure = armem::Time::microSeconds(0);
+
private:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory,
const std::string& name) const;
@@ -106,9 +114,8 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotState::JointMap>
getRobotJointState(const armarx::armem::wm::Memory& memory, const std::string& name) const;
- JointTrajectory
- getRobotJointStates(const armarx::armem::wm::Memory& memory,
- const std::string& name) const;
+ JointTrajectory getRobotJointStates(const armarx::armem::wm::Memory& memory,
+ const std::string& name) const;
std::optional<robot::PlatformState>
getRobotPlatformState(const armarx::armem::wm::Memory& memory,
@@ -117,7 +124,7 @@ namespace armarx::armem::robot_state
std::map<RobotReader::Hand, robot::ForceTorque>
getForceTorque(const armarx::armem::wm::Memory& memory, const std::string& name) const;
-
+
std::map<RobotReader::Hand, std::map<armem::Time, robot::ForceTorque>>
getForceTorques(const armarx::armem::wm::Memory& memory, const std::string& name) const;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 0907cb70b01220686d2b6b30a2d5577c1e6838c3..5ef7564f8a0e374060114d0391bfaeebf1df6198 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -1,14 +1,16 @@
#include "VirtualRobotReader.h"
#include <optional>
+#include <thread>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include "ArmarXCore/core/PackagePath.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/system/ArmarXDataPath.h"
-#include "ArmarXCore/core/system/cmake/CMakePackageFinder.h"
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
namespace armarx::armem::robot_state
@@ -19,30 +21,33 @@ namespace armarx::armem::robot_state
{
}
- void VirtualRobotReader::connect()
+ void
+ VirtualRobotReader::connect()
{
RobotReader::connect();
}
// TODO(fabian.reister): register property defs
- void VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
+ void
+ VirtualRobotReader::registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def)
{
RobotReader::registerPropertyDefinitions(def);
}
- bool VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot,
- const armem::Time& timestamp)
+ bool
+ VirtualRobotReader::synchronizeRobot(VirtualRobot::Robot& robot, const armem::Time& timestamp)
{
const auto packages = armarx::CMakePackageFinder::FindAllArmarXSourcePackages();
const auto package = armarx::ArmarXDataPath::getProject(packages, robot.getFilename());
- const robot::RobotDescription robotDescription{.name = robot.getName(),
- .xml = PackagePath{package, robot.getFilename()}};
+ const robot::RobotDescription robotDescription{
+ .name = robot.getName(), .xml = PackagePath{package, robot.getFilename()}};
const auto robotState = queryState(robotDescription, timestamp);
if (not robotState)
{
- ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / " << robot.getType() << "`!";
+ ARMARX_WARNING << "Querying robot state failed for robot `" << robot.getName() << " / "
+ << robot.getType() << "`!";
return false;
}
@@ -52,9 +57,10 @@ namespace armarx::armem::robot_state
return true;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getRobot(const std::string& name,
- const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getRobot(const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode)
{
ARMARX_INFO << "Querying robot description for robot '" << name << "'";
const auto description = queryDescription(name, timestamp);
@@ -64,24 +70,42 @@ namespace armarx::armem::robot_state
return nullptr;
}
- const std::string xmlFilename = ArmarXDataPath::resolvePath(description->xml.serialize().path);
- ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '" << xmlFilename << "'";
+ const std::string xmlFilename =
+ ArmarXDataPath::resolvePath(description->xml.serialize().path);
+ ARMARX_INFO << "Loading (virtual) robot '" << description->name << "' from XML file '"
+ << xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
robot->setName(name);
+
+ synchronizeRobot(*robot, timestamp);
+
return robot;
}
- VirtualRobot::RobotPtr VirtualRobotReader::getSynchronizedRobot(const std::string& name,
- const armem::Time& timestamp,
- const VirtualRobot::RobotIO::RobotDescription& loadMode)
+ VirtualRobot::RobotPtr
+ VirtualRobotReader::getSynchronizedRobot(
+ const std::string& name,
+ const armem::Time& timestamp,
+ const VirtualRobot::RobotIO::RobotDescription& loadMode,
+ const bool blocking)
{
- auto robot = getRobot(name, timestamp, loadMode);
-
- synchronizeRobot(*robot, timestamp);
+ while (blocking)
+ {
+ VirtualRobot::RobotPtr robot = getRobot(name, timestamp, loadMode);
+ if (robot and synchronizeRobot(*robot, timestamp))
+ {
+ return robot;
+ }
+
+ ARMARX_INFO << "Retrying to query robot after failure";
+ std::this_thread::sleep_for(
+ std::chrono::microseconds(sleepAfterFailure.toMicroSeconds()));
+ }
- return robot;
+ ARMARX_WARNING << "Failed to get synchronized robot `" << name << "`";
+ return nullptr;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index 68c0d5e7df911f23b06fcc582108bbf9f205e49c..c14e60aee78a490bd1ffb0937ca008dc56895499 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -21,12 +21,12 @@
#pragma once
-#include "RobotReader.h"
-
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
+#include "RobotReader.h"
+
namespace armarx::armem::robot_state
{
@@ -42,7 +42,7 @@ namespace armarx::armem::robot_state
{
public:
VirtualRobotReader(armem::client::MemoryNameSystem& memoryNameSystem);
- virtual ~VirtualRobotReader() = default;
+ ~VirtualRobotReader() override = default;
void connect();
void registerPropertyDefinitions(::armarx::PropertyDefinitionsPtr& def);
@@ -59,7 +59,8 @@ namespace armarx::armem::robot_state
getSynchronizedRobot(const std::string& name,
const armem::Time& timestamp,
const VirtualRobot::RobotIO::RobotDescription& loadMode =
- VirtualRobot::RobotIO::RobotDescription::eStructure);
+ VirtualRobot::RobotIO::RobotDescription::eStructure,
+ bool blocking = true);
};
} // namespace armarx::armem::robot_state
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 2b2c942490e2b7527552881cbf1be5e8a4c11cff..21b8e55a9fa9c74d63f61dc67aea86bce381c01f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -151,7 +151,7 @@ namespace armarx::armem::client::robot_state::localization
.coreSegments().withName(properties.localizationSegment)
.providerSegments().withName(query.header.agent) // agent
.entities().all() // parentFrame,frame
- .snapshots().latest();
+ .snapshots().beforeOrAtTime(query.header.timestamp);
// clang-format on
const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index dbf23a23772f1399aac9045593409e6c2d66c669..a595d5502c7139e51d9e940d71f24925ab39f199 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -5,6 +5,8 @@
#include <SimoxUtility/math/pose/interpolate.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
@@ -249,7 +251,10 @@ namespace armarx::armem::common::robot_state::localization
// }
std::vector<::armarx::armem::robot_state::Transform> transforms;
- transforms.push_back(_convertEntityToTransform(entity.getLatestSnapshot().getInstance(0)));
+
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ ARMARX_CHECK(snapshot) << "No snapshot found before or at time " << timestamp;
+ transforms.push_back(_convertEntityToTransform(snapshot->getInstance(0)));
// ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1)
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
index e16d599ba8fdd21f653a951ea16fdda6571bf243..26311961772c03b70519de0cde245b44e17bf0db 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/types.h
@@ -26,7 +26,7 @@
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
-#include <Eigen/src/Geometry/Transform.h>
+
#include <RobotAPI/libraries/armem_robot_state/types.h>
namespace armarx::armem::common::robot_state::localization
@@ -79,4 +79,4 @@ namespace armarx::armem::common::robot_state::localization
// bool exact;
};
-} // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file
+} // namespace armarx::armem::common::robot_state::localization
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 051ab03e793a0afceca524e007b834f4f510e6a0..67d537d19cf74af3da62dc72ae217f996977433f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -12,6 +12,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/interface/core/PackagePath.h>
#include <RobotAPI/libraries/armem/core/Time.h>
@@ -79,7 +80,7 @@ namespace armarx::armem::server::robot_state
{
for (const robot::Robot& robot : robots)
{
- const data::PackagePath xmlPath = robot.description.xml.serialize();
+ const armarx::data::PackagePath xmlPath = robot.description.xml.serialize();
// clang-format off
viz::Robot robotVisu = viz::Robot(robot.description.name)
diff --git a/source/RobotAPI/libraries/armem_robot_state/utils.h b/source/RobotAPI/libraries/armem_robot_state/utils.h
index 239e0ddb2d1b99a513ed97a3e1b8fb63122a4c70..9b495f91f30544fbdb2cfc2f68ee10bb111e1903 100644
--- a/source/RobotAPI/libraries/armem_robot_state/utils.h
+++ b/source/RobotAPI/libraries/armem_robot_state/utils.h
@@ -1,10 +1,9 @@
#pragma once
-#include "RobotAPI/libraries/armem/core/MemoryID.h"
-#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem_robot/types.h>
namespace armarx::armem::robot_state
{
armarx::armem::MemoryID makeMemoryID(const robot::RobotDescription& desc);
}
-
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 53527f2b5ad9357c8f9bd627b2c3d83793e7c9a8..681049dac2250e30ad6a3ccee5f25cc6412b074e 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -21,7 +21,7 @@
#pragma once
-#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
@@ -79,4 +79,4 @@ namespace armarx::armem
return aron::converter::AronEigenConverter::ConvertFromArray(grid);
}
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index 2cd6b7eb906d4b85cf6527979eb9573f4d9568ea..d408f45ef12fe9ac8624469e6a25326e96498295 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
@@ -1,6 +1,6 @@
#include "armarx.h"
-#include <Eigen/src/Core/Matrix.h>
+
#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 56f6690c41d8eef765a0788312d8dff220d92695..e1213b86b1f2f6386fb22d318f4ed798a72eb040 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -28,7 +28,7 @@
// Eigen
#include <Eigen/Geometry>
#include <Eigen/Core>
-#include <Eigen/src/Core/util/Constants.h>
+
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index f837f87117f5c8934ab5430af9491277010cb4e9..4849196ad868ebc11c5247c780e9dd6ea41814a6 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -432,3 +432,8 @@ armarx::PosePtr armarx::toIce(const Eigen::Matrix4f& pose)
{
return new Pose(pose);
}
+
+armarx::PosePtr armarx::toIce(const Eigen::Isometry3f& pose)
+{
+ return armarx::toIce(pose.matrix());
+}
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 268b6ba846022f0586fbdfafb5cf8d2999665c55..2748a0ded0a959633c3a44a1c8dd3f1c789c6ff6 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -289,6 +289,7 @@ namespace armarx
QuaternionPtr toIce(const Eigen::Matrix3f& rotation);
QuaternionPtr toIce(const Eigen::Quaternionf& quaternion);
PosePtr toIce(const Eigen::Matrix4f& pose);
+ PosePtr toIce(const Eigen::Isometry3f& pose);
}