diff --git a/source/RobotAPI/components/ArViz/ArVizStorage.cpp b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
index c442efa5337fd4febba4afa6eaba25ac0e520575..5430091b2b917f819e2acb0e8717e904e8d4e45f 100644
--- a/source/RobotAPI/components/ArViz/ArVizStorage.cpp
+++ b/source/RobotAPI/components/ArViz/ArVizStorage.cpp
@@ -605,7 +605,7 @@ armarx::viz::data::RecordingSeq armarx::ArVizStorage::getAllRecordings(const Ice
for (std::filesystem::directory_entry const& entry : std::filesystem::directory_iterator(historyPath))
{
- ARMARX_INFO << "Checking: " << entry.path();
+ ARMARX_DEBUG << "Checking: " << entry.path();
if (!entry.is_directory())
{
diff --git a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
index a75475aee537d035edc6f34dd0ea3dbe0370a01e..b2530de115a276e2080ac05ab98c587a3cab4855 100644
--- a/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
+++ b/source/RobotAPI/components/ArViz/Client/elements/Mesh.h
@@ -1,17 +1,15 @@
#pragma once
-#include <functional>
-#include <numeric> // for std::accumulate
-#include <vector>
+#include "ElementOps.h"
-#include <Eigen/Core>
+#include <RobotAPI/interface/ArViz/Elements.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <Eigen/Core>
-#include <RobotAPI/interface/ArViz/Elements.h>
+#include <functional>
+#include <numeric> // for std::accumulate
+#include <vector>
-#include "ElementOps.h"
namespace CGAL
{
diff --git a/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
index 9f5144595cad0732ba17cfb6ec3cc71ad3597545..629941eccbd35840f92df938bfc5b0f60732b55f 100644
--- a/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
+++ b/source/RobotAPI/components/ArViz/Example/ArVizInteractExample.cpp
@@ -31,6 +31,7 @@
namespace armarx
{
+
struct SingleSlider
{
SingleSlider(std::string const& name, viz::Color color)
@@ -202,6 +203,216 @@ namespace armarx
};
+ // ---------------
+
+ // What abstractions are needed?
+ // MovableElement
+ // SpawnerElement
+
+ //template <typename ElementT>
+ template <typename ElementT>
+ struct Transformable
+ {
+ Transformable(std::string const& name)
+ : element(name)
+ {
+ }
+
+ Transformable& pose(Eigen::Matrix4f const& pose)
+ {
+ element.pose(pose);
+ initialPose = pose;
+ return *this;
+ }
+
+ Transformable& position(Eigen::Vector3f const& position)
+ {
+ element.position(position);
+ initialPose.block<3, 1>(0, 3) = position;
+ return *this;
+ }
+
+ Transformable& orientation(Eigen::Matrix3f const& rotationMatrix)
+ {
+ element.orientation(rotationMatrix);
+ initialPose.block<3, 3>(0, 0) = rotationMatrix;
+ return *this;
+ }
+
+ Transformable& orientation(Eigen::Quaternionf const& quaternion)
+ {
+ element.orientation(quaternion);
+ initialPose.block<3, 3>(0, 0) = quaternion.toRotationMatrix();
+ return *this;
+ }
+
+ Transformable& enable(viz::InteractionDescription const& interaction)
+ {
+ element.enable(interaction);
+ // A movable element is always hidden during the interaction
+ element.data_->interaction.enableFlags |= viz::data::InteractionEnableFlags::TRANSFORM_HIDE;
+ return *this;
+ }
+
+ // The pose after the current transformation has been applied
+ Eigen::Matrix4f getCurrentPose() const
+ {
+ return transformation * initialPose;
+ }
+
+ // Returns true, if the element has been changed and the layer needs to be comitted again
+ bool handle(viz::InteractionFeedback const& interaction)
+ {
+ if (interaction.element() == element.data_->id)
+ {
+ switch (interaction.type())
+ {
+ case viz::InteractionFeedbackType::Transform:
+ {
+ // Keep track of the transformation
+ transformation = interaction.transformation();
+ } break;
+ case viz::InteractionFeedbackType::Deselect:
+ {
+ // If an object is deselected, we apply the transformation
+ initialPose = transformation * initialPose;
+ transformation = Eigen::Matrix4f::Identity();
+ element.pose(initialPose);
+ return true;
+ } break;
+ default:
+ {
+ // Ignore the other events
+ }
+ }
+ }
+ return false;
+ }
+
+
+ Eigen::Matrix4f initialPose = Eigen::Matrix4f::Identity();
+ Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
+
+ ElementT element;
+ };
+
+
+ struct SlidersState2
+ {
+ SlidersState2(Eigen::Vector3f origin)
+ : origin(origin)
+ , x("BoxX")
+ , y("BoxY")
+ , z("BoxZ")
+ , sphere("Sphere")
+ {
+ float boxSize = 50.0f;
+
+ // We use the Transformable<T>::position to set the position
+ // This keeps track of the internal state
+ x.position(origin + Eigen::Vector3f(0.5f * ARROW_LENGTH, 0.0f, 0.0f));
+ // A movable object is always hidden during the transformation
+ // The hideDuringTransform() flag is automatically set here
+ x.enable(viz::interaction().translation(viz::AXES_X));
+
+ // Other attributes of the element can be set directly on the member
+ x.element.color(viz::Color::red())
+ .size(boxSize);
+
+ y.position(origin + Eigen::Vector3f(0.0f, 0.5f * ARROW_LENGTH, 0.0f));
+ y.enable(viz::interaction().translation(viz::AXES_Y));
+
+ y.element.color(viz::Color::green())
+ .size(boxSize);
+
+ z.position(origin + Eigen::Vector3f(0.0f, 0.0f, 0.5f * ARROW_LENGTH));
+ z.enable(viz::interaction().translation(viz::AXES_Z));
+ z.element.color(viz::Color::blue())
+ .size(boxSize);
+
+ sphere.position(origin + 0.5f * ARROW_LENGTH * Eigen::Vector3f(1.0f, 1.0f, 1.0f))
+ .color(viz::Color::orange())
+ .radius(30.0f);
+ }
+
+ static constexpr const float ARROW_LENGTH = 1000.0f;
+
+ void visualize(viz::Client& arviz)
+ {
+ layerInteract = arviz.layer("Sliders2");
+
+ float arrowWidth = 10.0f;
+
+ viz::Arrow arrowX = viz::Arrow("ArrowX")
+ .color(viz::Color::red())
+ .fromTo(origin, origin + Eigen::Vector3f(ARROW_LENGTH, 0.0f, 0.0f))
+ .width(arrowWidth);
+ layerInteract.add(arrowX);
+
+ viz::Arrow arrowY = viz::Arrow("ArrowY")
+ .color(viz::Color::green())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, ARROW_LENGTH, 0.0f))
+ .width(arrowWidth);
+ layerInteract.add(arrowY);
+
+ viz::Arrow arrowZ = viz::Arrow("ArrowZ")
+ .color(viz::Color::blue())
+ .fromTo(origin, origin + Eigen::Vector3f(0.0f, 0.0f, ARROW_LENGTH))
+ .width(arrowWidth);
+ layerInteract.add(arrowZ);
+
+
+ layerInteract.add(x.element);
+ layerInteract.add(y.element);
+ layerInteract.add(z.element);
+
+ layerResult = arviz.layer("SlidersResult2");
+ layerResult.add(sphere);
+ }
+
+ void handle(viz::InteractionFeedback const& interaction,
+ viz::StagedCommit* stage)
+ {
+ // Let the Transformable<T> handle all internal events
+ bool needsLayerUpdate = false;
+ needsLayerUpdate |= x.handle(interaction);
+ needsLayerUpdate |= y.handle(interaction);
+ needsLayerUpdate |= z.handle(interaction);
+ if (needsLayerUpdate)
+ {
+ // The handle() functions indicated that the layer needs to be updated
+ stage->add(layerInteract);
+ }
+
+ // We handle the transform event ourselves to add custom behavior
+ // Here, we move the sphere based on the position of the sliders
+ if (interaction.type() == viz::InteractionFeedbackType::Transform)
+ {
+ // We can query getCurrentPose() to determine the poses of the sliders
+ // with the transformation applied to them
+ Eigen::Vector3f spherePosition(
+ x.getCurrentPose().col(3).x(),
+ y.getCurrentPose().col(3).y(),
+ z.getCurrentPose().col(3).z());
+ sphere.position(spherePosition);
+
+ stage->add(layerResult);
+ }
+ }
+
+ Eigen::Vector3f origin;
+ Transformable<viz::Box> x;
+ Transformable<viz::Box> y;
+ Transformable<viz::Box> z;
+
+ viz::Sphere sphere;
+
+ viz::Layer layerInteract;
+ viz::Layer layerResult;
+ };
+ // ---------------
+
+
enum class SpawnerType
{
Box,
@@ -587,12 +798,17 @@ namespace armarx
SlidersState sliders(origin1 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
+ SlidersState2 sliders2(origin3 + Eigen::Vector3f(500.0f, 500.0f, 0.0f));
SpawnersState spawners(origin2);
sliders.visualize(arviz);
stage.add(sliders.layerInteract);
stage.add(sliders.layerResult);
+ sliders2.visualize(arviz);
+ stage.add(sliders2.layerInteract);
+ stage.add(sliders2.layerResult);
+
spawners.visualize(arviz);
stage.add(spawners.layerSpawners);
stage.add(spawners.layerObjects);
@@ -609,6 +825,7 @@ namespace armarx
stage.reset();
stage.requestInteraction(sliders.layerInteract);
+ stage.requestInteraction(sliders2.layerInteract);
stage.requestInteraction(spawners.layerSpawners);
stage.requestInteraction(spawners.layerObjects);
@@ -619,6 +836,10 @@ namespace armarx
{
sliders.handle(interaction, &stage);
}
+ if (interaction.layer() == "Sliders2")
+ {
+ sliders2.handle(interaction, &stage);
+ }
if (interaction.layer() == "Spawners" || interaction.layer() == "SpawnedObjects")
{
spawners.handle(interaction, &stage);
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
index 2edd13823da1470efe6e9db79d5e252589e38532..e65fa6a4f4d0242b686656493ef93dba95cc5b36 100644
--- a/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerHelper.cpp
@@ -25,7 +25,6 @@
#include <VirtualRobot/math/Helpers.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -188,16 +187,18 @@ namespace armarx::detail::DebugDrawerHelper
void FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses)
{
ARMARX_TRACE;
- for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+ for (std::size_t idx = 0; idx < poses.size(); ++idx)
{
+ Eigen::Matrix4f const& pose = poses[idx];
drawPose(prefix + std::to_string(idx), pose);
}
}
void FrameView::drawPoses(const std::string& prefix, const std::vector<Eigen::Matrix4f>& poses, float scale)
{
ARMARX_TRACE;
- for (const auto& [idx, pose] : MakeIndexedContainer(poses))
+ for (std::size_t idx = 0; idx < poses.size(); ++idx)
{
+ Eigen::Matrix4f const& pose = poses[idx];
drawPose(prefix + std::to_string(idx), pose, scale);
}
}
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index b4f8abe9bb17d7b6e339c33da91673a2e884bd69..10a8b7ae1b665966787ea6a056ab3f1056f8abeb 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -93,6 +93,7 @@ namespace armarx
sendDebugObserverBatch();
}
+ viz::StagedCommit stage = arviz.stage();
{
// Visualize the objects.
viz::Layer layer = arviz.layer("Objects");
@@ -103,20 +104,22 @@ namespace armarx
.fileByObjectFinder(objectPose.objectID)
.alpha(objectPose.confidence));
}
- arviz.commit({layer});
+ stage.add(layer);
}
- visualizePredictions(client, objectPoses);
+ stage.add(visualizePredictions(client, objectPoses));
+ arviz.commit(stage);
cycle.waitForCycleDuration();
}
}
- void
+
+ viz::Layer
ObjectPoseClientExample::visualizePredictions(const objpose::ObjectPoseClient& client,
const objpose::ObjectPoseSeq& objectPoses)
{
-
viz::Layer layer = arviz.layer("PredictionArray");
+
objpose::ObjectPosePredictionRequestSeq requests;
for (const objpose::ObjectPose& objectPose : objectPoses)
{
@@ -128,7 +131,7 @@ namespace armarx
toIce(request.timestamp,
DateTime::Now() +
Duration::MilliSeconds((i + 1) * p.millisecondPredictionIncrement));
- toIce(request.timeWindow, Duration::Seconds(2));
+ toIce(request.timeWindow, Duration::Seconds(10));
requests.push_back(request);
}
}
@@ -168,5 +171,7 @@ namespace armarx
<< "' failed: " << result.errorMessage;
}
}
+
+ return layer;
}
} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
index 6bfffbf35b87f7323eafc028a12b68278c7c727e..8cc667b26bad490c8fb314a6ef759e52f850430d 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -75,8 +75,8 @@ namespace armarx
void objectProcessingTaskRun();
- void visualizePredictions(const objpose::ObjectPoseClient& client,
- const objpose::ObjectPoseSeq& objectPoses);
+ viz::Layer visualizePredictions(const objpose::ObjectPoseClient& client,
+ const objpose::ObjectPoseSeq& objectPoses);
private:
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 142f277e97ac974a3113516efdae0e019029a752..b13fdcc0f60f34aa2bc28a982730c553d639f6c3 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -240,9 +240,7 @@ namespace armarx::armem::server::obj
{
objpose::ObjectPosePredictionRequest objPoseRequest;
toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
- toIce(objPoseRequest.objectID,
- ObjectID(boRequest.snapshotID.entityName + "/" +
- boRequest.snapshotID.instanceIndexStr()));
+ objPoseRequest.objectID = toIce(ObjectID(request.snapshotID.entityName));
objPoseRequest.settings = request.settings;
toIce(objPoseRequest.timestamp, boRequest.snapshotID.timestamp);
objpose::ObjectPosePredictionResult objPoseResult =
@@ -252,7 +250,6 @@ namespace armarx::armem::server::obj
if (objPoseResult.success)
{
-
armem::client::QueryBuilder builder;
builder.singleEntitySnapshot(boRequest.snapshotID);
auto queryResult = armarx::fromIce<armem::client::QueryResult>(
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
index 16f222d827a91c7eba560695487f756b3e7de2f7..c544ea8dd4b54eb15cebd9fb10e36a00e0623234 100644
--- a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/util/algorithm.h>
#include <RobotAPI/libraries/core/math/MathUtils.h>
#include <RobotAPI/libraries/core/PIDController.h>
-#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
// #define DEBUG_POS_CTRL
#ifdef DEBUG_POS_CTRL
#include <boost/circular_buffer.hpp>
@@ -203,20 +202,6 @@ namespace armarx
{
public:
RampedAccelerationVelocityControllerConfiguration() = default;
- static RampedAccelerationVelocityControllerConfigurationPtr CreateFromXml(DefaultRapidXmlReaderNode node)
- {
- RampedAccelerationVelocityControllerConfigurationPtr config(new RampedAccelerationVelocityControllerConfiguration());
- FillFromXml(*config, node);
- return config;
- }
- static void FillFromXml(RampedAccelerationVelocityControllerConfiguration& config, DefaultRapidXmlReaderNode node)
- {
- config.maxVelocityRad = node.first_node("maxVelocityRad").value_as_float();
- config.maxDecelerationRad = node.first_node("maxDecelerationRad").value_as_float();
- config.jerk = node.first_node("jerk").value_as_float();
- config.maxDt = node.first_node("maxDt").value_as_float();
- config.directSetVLimit = node.first_node("directSetVLimit").value_as_float();
- }
float maxVelocityRad;
float maxDecelerationRad;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
index 3cbe59bdfd36963431399159f48886f6ed24942a..ff1afaf689c23ebd13e118965ff0848ebf1300c7 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianNaturalPositionController.cpp
@@ -1,4 +1,3 @@
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
@@ -356,8 +355,9 @@ namespace armarx
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
- for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
+ for (std::size_t idx = 0; idx < _rtVelTargets.size(); ++idx)
{
+ float* ptr = _rtVelTargets[idx];
*ptr = goal(idx);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
index 0ae54afc683c28b67a68856f86cdc8a2a3eb9e91..2b53ca18ef577c4ea609134e9d30351ca85100bc 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp
@@ -1,4 +1,3 @@
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
@@ -150,8 +149,9 @@ namespace armarx
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
- for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelSensors))
+ for (std::size_t idx = 0; idx < _rtVelSensors.size(); ++idx)
{
+ const float* ptr = _rtVelSensors[idx];
_rtJointVelocityFeedbackBuffer(idx) = *ptr;
}
_rtWpController->setCurrentJointVelocity(_rtJointVelocityFeedbackBuffer);
@@ -272,8 +272,9 @@ namespace armarx
const Eigen::VectorXf& goal = _rtWpController->calculate(timeSinceLastIteration.toSecondsDouble());
//write targets
ARMARX_CHECK_EQUAL(static_cast<std::size_t>(goal.size()), _rtVelTargets.size());
- for (const auto& [idx, ptr] : MakeIndexedContainer(_rtVelTargets))
+ for (std::size_t idx = 0; idx < _rtVelTargets.size(); ++idx)
{
+ float* ptr = _rtVelTargets[idx];
*ptr = goal(idx);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
index b18689724a120d17a3eff61158d9e57cfd2ee7a9..d393f02ea5f90ff5ed41eab507fb6bf94cc7415a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h
@@ -23,6 +23,7 @@
#pragma once
#include <RobotAPI/interface/units/RobotUnit/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h> // for ARMARX_CHECK
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 5cf5fed082abdcb79f4f1de97ec9982c90f10d92..c1b9e88ca02c778cb61328ead80e9922e4161cf5 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -27,7 +27,6 @@
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include "../util/ControlThreadOutputBuffer.h"
@@ -893,8 +892,10 @@ Logging::_postFinishDeviceInitialization()
ValueMetaData data;
const auto names = val->getDataFieldNames();
data.fields.resize(names.size());
- for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
+
+ for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
{
+ std::string const& fieldName = names[fieldIdx];
data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index b9649773942d2c539cad4f1b38b03dc4a78d3f40..ccb1d28518ea3e30c3cb6cde8347cd1c3fdf8978 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -169,13 +169,18 @@ namespace armarx::introspection
std::map<std::string, VariantBasePtr> result;
for (const auto& e : GetEntries().values())
{
- for (auto& elem : e.toVariants(timestamp, ptr))
+ auto newMap = e.toVariants(timestamp, ptr);
+ for (auto& elem : newMap)
{
if (result.count(elem.first))
{
throw std::invalid_argument {"mergeMaps: newMap would override values from oldMap"};
}
}
+ for (auto& elem : newMap)
+ {
+ result[elem.first] = elem.second;
+ }
//mergeMaps(result, , MergeMapsMode::OverrideNoValues);
}
return result;
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index 144dacf08d6f3f2fdbb33aab1e73d1d67052b1b1..1973976129415be09d640896b3bd2ec68736e60e 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -147,7 +147,7 @@ void KinematicUnitWidgetController::onInitComponent()
void KinematicUnitWidgetController::onConnectComponent()
{
- ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
+ // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
reportedJointAngles.clear();
reportedJointVelocities.clear();
reportedJointControlModes.clear();
@@ -156,10 +156,11 @@ void KinematicUnitWidgetController::onConnectComponent()
reportedJointStatuses.clear();
reportedJointTemperatures.clear();
-
jointAnglesUpdateFrequency = new filters::MedianFilter(100);
kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
topicName = kinematicUnitInterfacePrx->getReportTopicName();
+ usingTopic(topicName);
+
lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
robotVisu->removeAllChildren();
@@ -168,10 +169,9 @@ void KinematicUnitWidgetController::onConnectComponent()
std::string rfile;
Ice::StringSeq includePaths;
- // get robot filename
+ // Get robot filename
try
{
-
Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
packages.push_back(Application::GetProjectName());
ARMARX_VERBOSE << "ArmarX packages " << packages;
@@ -200,7 +200,7 @@ void KinematicUnitWidgetController::onConnectComponent()
}
catch (...)
{
- ARMARX_ERROR << "Unable to retrieve robot filename";
+ ARMARX_ERROR << "Unable to retrieve robot filename.";
}
try
@@ -208,6 +208,10 @@ void KinematicUnitWidgetController::onConnectComponent()
ARMARX_INFO << "Loading robot from file " << rfile;
robot = loadRobotFile(rfile);
}
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init robot: " << e.what();
+ }
catch (...)
{
ARMARX_ERROR << "Failed to init robot";
@@ -216,35 +220,34 @@ void KinematicUnitWidgetController::onConnectComponent()
if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
{
getObjectScheduler()->terminate();
-
if (getWidget()->parentWidget())
{
getWidget()->parentWidget()->close();
}
-
- std::cout << "returning" << std::endl;
return;
}
- // check robot name and disable setZero Button if necessary
+ // Check robot name and disable setZero Button if necessary
if (not simox::alg::starts_with(robot->getName(), "Armar3"))
{
- ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+ ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
ui.pushButtonKinematicUnitPos1->setDisabled(true);
}
kinematicUnitFile = rfile;
robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
-
- initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
- initGUIJointListTable(robotNodeSet);
-
kinematicUnitVisualization = getCoinVisualization(robot);
kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
robotVisu->addChild(kinematicUnitNode);
- // init control mode map
+ // Fetch the current joint angles.
+ synchronizeRobotJointAngles();
+
+ initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
+ initGUIJointListTable(robotNodeSet);
+
+ // Init control mode map
try
{
reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
@@ -255,7 +258,6 @@ void KinematicUnitWidgetController::onConnectComponent()
initializeUi();
- usingTopic(topicName);
QMetaObject::invokeMethod(this, "resetSlider");
enableMainWidgetAsync(true);
updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
@@ -355,13 +357,11 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
settings->setValue("enableValueValidator", enableValueValidator);
settings->setValue("viewerEnabled", viewerEnabled);
assert(historyTime % 1000 == 0);
- settings->setValue("historyTime", (int)(historyTime / 1000));
+ settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
settings->setValue("currentValueMax", currentValueMax);
}
-
-
void KinematicUnitWidgetController::showVisuLayers(bool show)
{
if (debugDrawer)
@@ -410,7 +410,9 @@ void KinematicUnitWidgetController::updateGuiElements()
// modelUpdateCB();
}
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
+void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
+{
+}
void KinematicUnitWidgetController::modelUpdateCB()
{
@@ -418,7 +420,6 @@ void KinematicUnitWidgetController::modelUpdateCB()
SoNode* KinematicUnitWidgetController::getScene()
{
-
if (viewerEnabled)
{
ARMARX_INFO << "Returning scene ";
@@ -500,7 +501,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
{
}
- //set slider to 0 if position control mode is used
+ // Set slider to 0 if position control mode is used.
if (selectedControlMode == ePositionControl)
{
ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -615,9 +616,9 @@ void KinematicUnitWidgetController::setControlModePosition()
if (currentNode)
{
- QString unit = currentNode->isRotationalJoint() ?
- (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad") :
- "mm";
+ QString unit = currentNode->isRotationalJoint()
+ ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+ : "mm";
ui.labelUnit->setText(unit);
const bool isDeg = ui.checkBoxUseDegree->isChecked();
const bool isRot = currentNode->isRotationalJoint();
@@ -638,8 +639,18 @@ void KinematicUnitWidgetController::setControlModePosition()
return;
}
+ {
+ // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+ // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+ // Therefore, we first have to fetch the actual joint values and use that one.
+ // However, this should actually not be necessary, as the robot model should be updated
+ // via the topics.
+ synchronizeRobotJointAngles();
+ }
+
float pos = currentNode->getJointValue() * conversionFactor;
- ARMARX_INFO << "setting position control for current Node Name: " << currentNode->getName() << " with current value: " << pos;
+ ARMARX_INFO << "Setting position control for current node "
+ << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
// Setting the slider position to pos will set the position to the slider tick closest to pos
// This will initially send a position target with a small delta to the joint.
@@ -695,10 +706,7 @@ void KinematicUnitWidgetController::setControlModeVelocity()
ui.horizontalSliderKinematicUnitPos->setMinimum(lo);
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
-
}
-
}
void KinematicUnitWidgetController::setControlModeTorque()
@@ -736,10 +744,7 @@ void KinematicUnitWidgetController::setControlModeTorque()
ui.horizontalSliderKinematicUnitPos->blockSignals(false);
resetSlider();
-
}
-
-
}
VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
@@ -1720,3 +1725,9 @@ void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
ARMARX_ERROR << "failed to switch mode or set angles";
}
}
+
+void KinematicUnitWidgetController::synchronizeRobotJointAngles()
+{
+ const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+ robot->setJointValues(currentJointAngles);
+}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index 8287176d247aebc0fbebac9b5a6b91d20df22c25..d0a98a0a1a57efb6958b103b81732e022afbedf4 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -231,7 +231,6 @@ namespace armarx
// ice proxies
KinematicUnitInterfacePrx kinematicUnitInterfacePrx;// send commands to kinematic unit
- KinematicUnitListenerPrx kinematicUnitListenerPrx; // receive reports from kinematic unit
bool verbose;
@@ -269,9 +268,13 @@ namespace armarx
void highlightCriticalValues();
protected slots:
+
void showVisuLayers(bool show);
void copyToClipboard();
void on_pushButtonFromJson_clicked();
+
+ void synchronizeRobotJointAngles();
+
private:
std::recursive_mutex mutexNodeSet;
diff --git a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
index 273a2d631d091f03652c326f7b55b5312996153d..5e117d1e62dfaa3c0dd00b996d0aaff662e6b5fd 100644
--- a/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/RobotUnitPlugin/RobotUnitPlugin/RobotUnitPluginWidgetController.cpp
@@ -31,7 +31,6 @@
#include <QSortFilterProxyModel>
#include <QAction>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
@@ -523,8 +522,10 @@ void armarx::RobotUnitPluginWidgetController::on_lineEditLoggingFilter_textChang
}
}
widget.treeWidgetLoggingNames->blockSignals(true);
- for (auto item : MakeReversedRange(loggingData.allItems))
+ // Iterate in reverse order
+ for (auto iter = loggingData.allItems.rbegin(); iter != loggingData.allItems.rend(); ++iter)
{
+ QTreeWidgetItem* item = *iter;
loggingData.updateCheckStateUpward(item, false);
}
widget.treeWidgetLoggingNames->blockSignals(false);
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
index 431ce92cade5f9d8fc92d8ac7bd80fd402e84c51..e9942f7ec8875466f32951983775783f8f659679 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/SkillManagerMonitorWidgetController.cpp
@@ -22,8 +22,6 @@
#include <string>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
#include "SkillManagerMonitorWidgetController.h"
#include "aronTreeWidget/visitors/AronTreeWidgetCreator.h"
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
index e9effb321210d3c3201adaa40368a359785a1524..0da02a754f257e348be003a839f3dadae6d9d2a8 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetConverter.cpp
@@ -22,8 +22,6 @@
#include <string>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
#include "AronTreeWidgetConverter.h"
// debug
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
index b3a8682081312bc1f3dbc7f036db10dbe40fb5f9..37a59c62ab72eef562f46dbf4f04d0d24b0b3977 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetCreator.cpp
@@ -22,8 +22,6 @@
#include <string>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
#include "AronTreeWidgetCreator.h"
// debug
diff --git a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
index a2a2240d354758a257a022fa07e211b1d993495b..20108945952003a98bc2949c5cf151f0244bb9c8 100644
--- a/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
+++ b/source/RobotAPI/gui-plugins/SkillManagerPlugin/aronTreeWidget/visitors/AronTreeWidgetModalCreator.cpp
@@ -22,8 +22,6 @@
#include <string>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-
#include "AronTreeWidgetModalCreator.h"
#include <SimoxUtility/algorithm/string.h>
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 2b2fb5f9d4ea899930351b1234873b2354cb50bf..ec6658fa85afba280fef0613fbc60596da57ee8a 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -31,6 +31,7 @@ set(LIB_FILES
plugins/ObjectPoseClientPlugin.cpp
plugins/RequestedObjects.cpp
+ predictions.cpp
util.cpp
)
set(LIB_HEADERS
@@ -57,6 +58,7 @@ set(LIB_HEADERS
plugins/ObjectPoseClientPlugin.h
plugins/RequestedObjects.h
+ predictions.h
util.h
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f3550f8746ec4a2b8470d11f907acbb0d7845f9e
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -0,0 +1,96 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "predictions.h"
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+namespace armarx::objpose
+{
+
+ objpose::ObjectPosePredictionResult
+ predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose,
+ const armem::client::PredictionSettings& settings)
+ {
+ objpose::ObjectPosePredictionResult result;
+ result.success = false;
+
+ if (!settings.predictionEngineID.empty() &&
+ settings.predictionEngineID != "Linear Position Regression")
+ {
+ result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
+ "' not available for object pose prediction.";
+ return result;
+ }
+
+ const DateTime timeOrigin = DateTime::Now();
+
+ std::vector<double> timestampsSec;
+ std::vector<Eigen::Vector3d> positions;
+
+ // ToDo: How to handle attached objects?
+ for (const auto& [timestamp, pose] : poses)
+ {
+ timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
+ positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+ }
+
+ ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+
+ Eigen::Vector3d prediction;
+ // Static objects don't move. Objects that haven't moved for a while probably won't either.
+ if (timestampsSec.size() <= 1 || latestPose.isStatic)
+ {
+ prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+ }
+ else
+ {
+ using simox::math::LinearRegression3d;
+ const bool inputOffset = false;
+
+ const LinearRegression3d model =
+ LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+ const auto predictionTime = armarx::fromIce<DateTime>(time);
+ prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+ }
+
+ // Used as a template for the returned pose prediction.
+ ObjectPose latestCopy = latestPose;
+
+ // Reuse the rotation from the latest pose.
+ // This is a pretty generous assumption, but the linear model doesn't cover rotations,
+ // so it's our best guess.
+ Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
+ simox::math::position(latest) = prediction.cast<float>();
+ latestCopy.setObjectPoseGlobal(latest);
+
+ result.success = true;
+ result.prediction = latestCopy.toIce();
+
+ return result;
+ }
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.h b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
new file mode 100644
index 0000000000000000000000000000000000000000..90075859090689255ab28d5269b4e208719ee3a9
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
@@ -0,0 +1,46 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects
+ * @author phesch ( ulila at student dot kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+
+namespace armarx::objpose
+{
+ /**
+ * @brief Predict the pose of an object given a history of poses.
+ *
+ * @param poses the history of poses to base the prediction on
+ * @param time the timestamp to make the prediction for
+ * @param latestPose used for metadata so the result is valid even if poses is empty
+ * @param settings the settings to use for the prediction
+ * @return the result of the prediction
+ */
+ objpose::ObjectPosePredictionResult
+ predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose,
+ const armem::client::PredictionSettings& settings);
+} // namespace armarx::objpose
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index f5a88c0ae45c0960938f1ef819002a563bc198c1..4f2d0c35fa4c95375cf67d09e332c1ecec5b77fa 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -130,18 +130,18 @@ namespace armarx
return new RobotInfoNode(name, profile, value, children);
}
- RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side) const
+ Arm RobotNameHelper::getArm(const std::string& side) const
{
return Arm(shared_from_this(), side);
}
- RobotNameHelper::RobotArm
+ RobotArm
RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
{
return RobotArm(Arm(shared_from_this(), side), robot);
}
- std::shared_ptr<const RobotNameHelper> RobotNameHelper::Arm::getRobotNameHelper() const
+ std::shared_ptr<const RobotNameHelper> Arm::getRobotNameHelper() const
{
return rnh;
}
@@ -197,150 +197,150 @@ namespace armarx
}
}
- std::string RobotNameHelper::Arm::getSide() const
+ std::string Arm::getSide() const
{
return side;
}
- std::string RobotNameHelper::Arm::getKinematicChain() const
+ std::string Arm::getKinematicChain() const
{
ARMARX_TRACE;
return select("KinematicChain");
}
- std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
+ std::string Arm::getTorsoKinematicChain() const
{
ARMARX_TRACE;
return select("TorsoKinematicChain");
}
- std::string RobotNameHelper::Arm::getTCP() const
+ std::string Arm::getTCP() const
{
ARMARX_TRACE;
return select("TCP");
}
- std::string RobotNameHelper::Arm::getForceTorqueSensor() const
+ std::string Arm::getForceTorqueSensor() const
{
ARMARX_TRACE;
return select("ForceTorqueSensor");
}
- std::string RobotNameHelper::Arm::getForceTorqueSensorFrame() const
+ std::string Arm::getForceTorqueSensorFrame() const
{
ARMARX_TRACE;
return select("ForceTorqueSensorFrame");
}
- std::string RobotNameHelper::Arm::getEndEffector() const
+ std::string Arm::getEndEffector() const
{
ARMARX_TRACE;
return select("EndEffector");
}
- std::string RobotNameHelper::Arm::getMemoryHandName() const
+ std::string Arm::getMemoryHandName() const
{
ARMARX_TRACE;
return select("MemoryHandName");
}
- std::string RobotNameHelper::Arm::getHandControllerName() const
+ std::string Arm::getHandControllerName() const
{
ARMARX_TRACE;
return select("HandControllerName");
}
- std::string RobotNameHelper::Arm::getHandRootNode() const
+ std::string Arm::getHandRootNode() const
{
ARMARX_TRACE;
return select("HandRootNode");
}
- std::string RobotNameHelper::Arm::getHandModelPath() const
+ std::string Arm::getHandModelPath() const
{
ARMARX_TRACE;
return select("HandModelPath");
}
- std::string RobotNameHelper::Arm::getAbsoluteHandModelPath() const
+ std::string Arm::getAbsoluteHandModelPath() const
{
ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
auto path = getHandModelPath();
return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
}
- std::string RobotNameHelper::Arm::getHandModelPackage() const
+ std::string Arm::getHandModelPackage() const
{
ARMARX_TRACE;
return select("HandModelPackage");
}
- std::string RobotNameHelper::Arm::getPalmCollisionModel() const
+ std::string Arm::getPalmCollisionModel() const
{
ARMARX_TRACE;
return select("PalmCollisionModel");
}
- std::string RobotNameHelper::Arm::getFullHandCollisionModel() const
+ std::string Arm::getFullHandCollisionModel() const
{
ARMARX_TRACE;
return select("FullHandCollisionModel");
}
- RobotNameHelper::RobotArm
- RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+ RobotArm
+ Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
ARMARX_TRACE;
return RobotArm(*this, robot);
}
- RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
+ Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
const std::string& side) :
rnh(rnh), side(side)
{
}
- std::string RobotNameHelper::Arm::select(const std::string& path) const
+ std::string Arm::select(const std::string& path) const
{
ARMARX_TRACE;
return rnh->select(side + "Arm/" + path);
}
- std::string RobotNameHelper::RobotArm::getSide() const
+ std::string RobotArm::getSide() const
{
ARMARX_TRACE;
return arm.getSide();
}
- VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
+ VirtualRobot::RobotNodeSetPtr RobotArm::getKinematicChain() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getKinematicChain());
}
- VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
+ VirtualRobot::RobotNodeSetPtr RobotArm::getTorsoKinematicChain() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
}
- VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
+ VirtualRobot::RobotNodePtr RobotArm::getTCP() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getTCP());
}
- VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getPalmCollisionModel() const
+ VirtualRobot::RobotNodePtr RobotArm::getPalmCollisionModel() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getPalmCollisionModel());
}
- VirtualRobot::TriMeshModelPtr RobotNameHelper::RobotArm::getFullHandCollisionModel() const
+ VirtualRobot::TriMeshModelPtr RobotArm::getFullHandCollisionModel() const
{
ARMARX_TRACE;
std::string abs;
@@ -349,14 +349,14 @@ namespace armarx
return VirtualRobot::TriMeshModel::FromFile(abs);
}
- VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
+ VirtualRobot::RobotNodePtr RobotArm::getHandRootNode() const
{
ARMARX_TRACE;
ARMARX_CHECK_NOT_NULL(robot);
return robot->getRobotNode(arm.getHandRootNode());
}
- Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
+ Eigen::Matrix4f RobotArm::getTcp2HandRootTransform() const
{
ARMARX_TRACE;
const auto tcp = getTCP();
@@ -366,17 +366,17 @@ namespace armarx
return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
}
- const VirtualRobot::RobotPtr& RobotNameHelper::RobotArm::getRobot() const
+ const VirtualRobot::RobotPtr& RobotArm::getRobot() const
{
return robot;
}
- const RobotNameHelper::Arm& RobotNameHelper::RobotArm::getArm() const
+ const Arm& RobotArm::getArm() const
{
return arm;
}
- RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
+ RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
arm(arm), robot(robot)
{
ARMARX_CHECK_NOT_NULL(robot);
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 92330d05d3e18b1447e68a9d36bb4c8b7472ad64..76c9b283f5d915e54d1f240c94d01aa9774d8e08 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -36,23 +36,130 @@ namespace armarx
class RapidXmlReaderNode;
- class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
+ struct RobotArm;
+
+ struct Arm
{
+ friend class RobotNameHelper;
+
public:
- static void writeRobotInfoNode(
- const RobotInfoNodePtr& n,
- std::ostream& str,
- std::size_t indent = 0,
- char indentChar = ' ');
+ std::string getSide() const;
+
+ std::string getKinematicChain() const;
+
+ std::string getTorsoKinematicChain() const;
+
+ std::string getTCP() const;
+
+ std::string getForceTorqueSensor() const;
+
+ std::string getForceTorqueSensorFrame() const;
+
+ std::string getEndEffector() const;
+
+ std::string getMemoryHandName() const;
+
+ std::string getHandControllerName() const;
+
+ std::string getHandRootNode() const;
+
+ std::string getHandModelPath() const;
+
+ std::string getAbsoluteHandModelPath() const;
+
+ std::string getHandModelPackage() const;
+
+ std::string getPalmCollisionModel() const;
+
+ std::string getFullHandCollisionModel() const;
+
+ RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
+
+ Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side);
+
+ Arm() = default;
+
+ Arm(Arm&&) = default;
+
+ Arm(const Arm&) = default;
+
+ Arm& operator=(Arm&&) = default;
+
+ Arm& operator=(const Arm&) = default;
+
+ std::shared_ptr<const RobotNameHelper> getRobotNameHelper() const;
+
+ std::string select(const std::string& path) const;
+
+ private:
+
+ std::shared_ptr<const RobotNameHelper> rnh;
+ std::string side;
+ };
+
+ struct RobotArm
+ {
+ friend class RobotNameHelper;
+
+ friend struct Arm;
+ public:
+ std::string getSide() const;
+
+ VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
+
+ VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
+
+ VirtualRobot::RobotNodePtr getTCP() const;
+
+ VirtualRobot::RobotNodePtr getHandRootNode() const;
+
+ VirtualRobot::RobotNodePtr getPalmCollisionModel() const;
+
+ VirtualRobot::TriMeshModelPtr getFullHandCollisionModel() const;
+
+ Eigen::Matrix4f getTcp2HandRootTransform() const;
+
+ const Arm& getArm() const;
+
+ const VirtualRobot::RobotPtr& getRobot() const;
+
+ RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
+
+ RobotArm() = default;
+
+ RobotArm(RobotArm&&) = default;
+
+ RobotArm(const RobotArm&) = default;
+
+ RobotArm& operator=(RobotArm&&) = default;
+
+ RobotArm& operator=(const RobotArm&) = default;
+
+ private:
+
+ Arm arm;
+ VirtualRobot::RobotPtr robot;
+ };
+
+ class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
+ {
+ public:
+ static void
+ writeRobotInfoNode(const RobotInfoNodePtr& n, std::ostream& str, std::size_t indent = 0, char indentChar = ' ');
+ using Arm = armarx::Arm;
+ using RobotArm = armarx::RobotArm;
class Node
{
public:
Node(const RobotInfoNodePtr& robotInfo);
+
std::string value();
Node select(const std::string& name, const std::vector<std::string>& profiles);
+
bool isValid();
+
void checkValid();
private:
@@ -62,89 +169,31 @@ namespace armarx
static const std::string LocationLeft;
static const std::string LocationRight;
- struct RobotArm;
- struct Arm
- {
- friend class RobotNameHelper;
- public:
-
- std::string getSide() const;
- std::string getKinematicChain() const;
- std::string getTorsoKinematicChain() const;
- std::string getTCP() const;
- std::string getForceTorqueSensor() const;
- std::string getForceTorqueSensorFrame() const;
- std::string getEndEffector() const;
- std::string getMemoryHandName() const;
- std::string getHandControllerName() const;
- std::string getHandRootNode() const;
- std::string getHandModelPath() const;
- std::string getAbsoluteHandModelPath() const;
- std::string getHandModelPackage() const;
- std::string getPalmCollisionModel() const;
- std::string getFullHandCollisionModel() const;
- RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
-
- Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side);
- Arm() = default;
- Arm(Arm&&) = default;
- Arm(const Arm&) = default;
- Arm& operator=(Arm&&) = default;
- Arm& operator=(const Arm&) = default;
-
- std::shared_ptr<const RobotNameHelper> getRobotNameHelper() const;
-
- std::string select(const std::string& path) const;
- private:
-
- std::shared_ptr<const RobotNameHelper> rnh;
- std::string side;
- };
-
- struct RobotArm
- {
- friend class RobotNameHelper;
- friend struct Arm;
- public:
- std::string getSide() const;
- VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
- VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
- VirtualRobot::RobotNodePtr getTCP() const;
- VirtualRobot::RobotNodePtr getHandRootNode() const;
- VirtualRobot::RobotNodePtr getPalmCollisionModel() const;
- VirtualRobot::TriMeshModelPtr getFullHandCollisionModel() const;
- Eigen::Matrix4f getTcp2HandRootTransform() const;
- const Arm& getArm() const;
- const VirtualRobot::RobotPtr& getRobot() const;
-
- RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
- RobotArm() = default;
- RobotArm(RobotArm&&) = default;
- RobotArm(const RobotArm&) = default;
- RobotArm& operator=(RobotArm&&) = default;
- RobotArm& operator=(const RobotArm&) = default;
- private:
-
- Arm arm;
- VirtualRobot::RobotPtr robot;
- };
-
std::string select(const std::string& path) const;
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
- static RobotNameHelperPtr Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
+
+ static RobotNameHelperPtr
+ Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(RobotNameHelper&&) = default;
+
RobotNameHelper(const RobotNameHelper&) = default;
+
RobotNameHelper& operator=(RobotNameHelper&&) = default;
+
RobotNameHelper& operator=(const RobotNameHelper&) = default;
Arm getArm(const std::string& side) const;
+
RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const;
+
const std::vector<std::string>& getProfiles() const;
+
const RobotInfoNodePtr& getRobotInfoNodePtr() const;
+
private:
static RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
index ce25109502b1da09cb01399396a5c7bca92ed50f..b0bbf2869134f92cdf15e45bde4dd264dda2d1b0 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -20,6 +20,8 @@ namespace armarx::armem::client::util
const std::string prefix = propertyPrefix();
+ props = defaultProperties();
+
def->optional(props.memoryName, prefix + "Memory");
def->optional(props.coreSegmentName, prefix + "CoreSegment");
}
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
index 2e315e6f06f8e141c3911479685424b3f5585580..9fe9db837e7cfdd3ecff3c26143fcc74093d94a1 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.cpp
@@ -8,7 +8,6 @@
#include <cmath>
-
namespace armarx::armem::gui
{
@@ -51,6 +50,11 @@ namespace armarx::armem::gui
connect(this, &This::updateSingle, this, &This::update);
connect(this, &This::updatePeriodic, this, &This::update);
+
+ // See `startTimerIfEnabled` for the signal reasoning.
+ // qOverload is required because `QTimer::start()` is overloaded.
+ connect(this, &This::startTimerSignal, _timer, qOverload<>(&QTimer::start));
+ connect(this, &This::stopTimerSignal, _timer, &QTimer::stop);
}
QPushButton* PeriodicUpdateWidget::updateButton()
@@ -66,22 +70,25 @@ namespace armarx::armem::gui
void PeriodicUpdateWidget::startTimerIfEnabled()
{
+ /* A QTimer can only be started and stopped within its own thread (the thread for which
+ * it has the greatest affinity). Since this method can be called from any thread, we
+ * need to take a detour through these signals, which can be emitted from any thread and
+ * will always be caught in this object's (and thus the timer's) native thread.
+ */
if (_autoCheckBox->isChecked())
{
- _timer->start();
+ emit startTimerSignal();
}
else
{
- _timer->stop();
+ emit stopTimerSignal();
}
}
void PeriodicUpdateWidget::stopTimer()
{
- if (_timer)
- {
- _timer->stop();
- }
+ // See `startTimerIfEnabled` for the signal reasoning.
+ emit stopTimerSignal();
}
void PeriodicUpdateWidget::_updateTimerFrequency()
@@ -91,6 +98,7 @@ namespace armarx::armem::gui
void PeriodicUpdateWidget::_toggleAutoUpdates(bool enabled)
{
+ // This method is already a slot, so it doesn't need to use the timer signals.
_frequencySpinBox->setEnabled(enabled);
if (enabled)
{
diff --git a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
index db8d9d5a30264180d93243c3a6f631f5666ed784..d9438993cc765f5d7b5350f28ac85dac53ed5202 100644
--- a/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/PeriodicUpdateWidget.h
@@ -50,10 +50,10 @@ namespace armarx::armem::gui
void _updateTimerFrequency();
void _toggleAutoUpdates(bool enabled);
-
signals:
-
+ void startTimerSignal();
+ void stopTimerSignal();
private:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index e871a70d41b3f7932b13cdebe6b7fb0dbef52f9c..2c8ca4e7d00a27665104e6002e09c39ff52284f2 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -547,6 +547,35 @@ namespace armarx::armem::server::obj::instance
}
+ std::map<DateTime, objpose::ObjectPose>
+ Segment::getObjectPosesInRange(const wm::Entity& entity,
+ const DateTime& start,
+ const DateTime& end)
+ {
+ std::map<DateTime, objpose::ObjectPose> result;
+
+ entity.forEachSnapshotInTimeRange(
+ start,
+ end,
+ [&result](const wm::EntitySnapshot& snapshot)
+ {
+ snapshot.forEachInstance(
+ [&result, &snapshot](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ ObjectPose pose;
+ fromAron(dto, pose);
+
+ result.emplace(snapshot.time(), pose);
+ });
+ });
+
+ return result;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
@@ -573,109 +602,6 @@ namespace armarx::armem::server::obj::instance
}
- objpose::ObjectPosePredictionResult
- Segment::predictObjectPose(const objpose::ObjectPosePredictionRequest& request)
- {
- objpose::ObjectPosePredictionResult result;
- result.success = false;
-
- if (!request.settings.predictionEngineID.empty() &&
- request.settings.predictionEngineID != "Linear Position Regression")
- {
- result.errorMessage = "Prediction engine '" + request.settings.predictionEngineID +
- "' not available for object pose prediction.";
- return result;
- }
-
- const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
-
- const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
-
- const wm::Entity* entity = findObjectEntity(objID);
- if (!entity)
- {
- std::stringstream sstream;
- sstream << "Could not find object with ID " << objID << ".";
- result.errorMessage = sstream.str();
- return result;
- }
-
- const Time timeOrigin = Time::Now();
-
- std::vector<double> timestampsSec;
- std::vector<Eigen::Vector3d> positions;
-
- entity->forEachSnapshotInTimeRange(
- Time::Now() - timeWindow,
- Time::Invalid(),
- [&timeOrigin, ×tampsSec, &positions](const wm::EntitySnapshot& snapshot)
- {
- snapshot.forEachInstance(
- [&timeOrigin, &snapshot, &positions, ×tampsSec](
- const wm::EntityInstance& instance)
- {
- // ToDo: How to handle attached objects?
-
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
-
- ObjectPose pose;
- fromAron(dto, pose);
-
- timestampsSec.push_back(
- (snapshot.time() - timeOrigin).toSecondsDouble());
- positions.push_back(
- simox::math::position(pose.objectPoseGlobal).cast<double>());
- });
- });
- ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
-
- // We always retrieve the latest instance in addition to the positions to check
- // the isStatic attribute.
- const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
- if (!dto)
- {
- std::stringstream sstream;
- sstream << "No snapshots of the object " << objID << " were found."
- << " Cannot make a prediction.";
- result.errorMessage = sstream.str();
- return result;
- }
-
- Eigen::Vector3d prediction;
- if (timestampsSec.size() <= 1 || dto.value().pose.isStatic)
- {
- prediction = simox::math::position(dto.value().pose.objectPoseGlobal).cast<double>();
- }
- else
- {
- using simox::math::LinearRegression3d;
- const bool inputOffset = false;
-
- const LinearRegression3d model =
- LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
- const Time predictionTime = armarx::fromIce<Time>(request.timestamp);
- prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
-
- }
-
- // Used as a template for the returned pose prediction.
- ObjectPose objectPoseTemplate = getLatestObjectPose(*entity);
-
- // Reuse the rotation from the latest pose.
- // This is a pretty generous assumption, but the linear model doesn't cover rotations,
- // so it's our best guess.
- Eigen::Matrix4f latestPose = objectPoseTemplate.objectPoseGlobal;
- simox::math::position(latestPose) = prediction.cast<float>();
- objectPoseTemplate.setObjectPoseGlobal(latestPose);
-
- result.success = true;
- result.prediction = objectPoseTemplate.toIce();
-
- return result;
- }
-
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index e735eab3316aa11139c390f8615b3b4a73344ae3..0aa9b930411f0a6f6336ea439fbb98ff6dc74a50 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -18,6 +18,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
@@ -104,8 +105,8 @@ namespace armarx::armem::server::obj::instance
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
- objpose::ObjectPosePredictionResult
- predictObjectPose(const objpose::ObjectPosePredictionRequest& request);
+ static std::map<DateTime, ObjectPose>
+ getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
private:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 433b2d248d130847af8f2873e85183178707f813..010263cebeae200367f317e642a0ca73beb6eb06 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -25,8 +25,14 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <VirtualRobot/Robot.h>
@@ -98,7 +104,7 @@ namespace armarx::armem::server::obj::instance
}
visu.predictor = [this](const objpose::ObjectPosePredictionRequest& request)
- { return segment.predictObjectPose(request); };
+ { return predictObjectPoses({request}).at(0); };
segment.connect(arviz);
}
@@ -439,9 +445,59 @@ namespace armarx::armem::server::obj::instance
const Ice::Current&)
{
objpose::ObjectPosePredictionResultSeq results;
- for (const auto& request : requests)
+ std::vector<std::map<DateTime, objpose::ObjectPose>> poses;
+ std::vector<objpose::ObjectPose> latestPoses;
+
+ segment.doLocked(
+ [this, &requests, &results, &poses, &latestPoses]()
+ {
+ for (const auto& request : requests)
+ {
+ auto& result = results.emplace_back();
+ const ObjectID objID = armarx::fromIce(request.objectID);
+
+ const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
+
+ const wm::Entity* entity = segment.findObjectEntity(objID);
+ // Use result.success as a marker for whether to continue later
+ result.success = false;
+ poses.emplace_back();
+ latestPoses.emplace_back();
+ if (!entity)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find object with ID " << objID << ".";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
+ if (!dto)
+ {
+ std::stringstream sstream;
+ sstream << "No snapshots of the object " << objID << " were found."
+ << " Cannot make a prediction.";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ result.success = true;
+ poses.back() = segment.getObjectPosesInRange(
+ *entity, Time::Now() - timeWindow, Time::Invalid());
+ latestPoses.back() = aron::fromAron<objpose::ObjectPose>(dto.value().pose);
+ }
+ });
+
+ for (size_t i = 0; i < requests.size(); ++i)
{
- results.push_back(segment.predictObjectPose(request));
+ if (results.at(i).success)
+ {
+ results.at(i) = objpose::predictObjectPose(
+ poses.at(i),
+ armarx::fromIce<DateTime>(requests.at(i).timestamp),
+ latestPoses.at(i),
+ armem::client::PredictionSettings::fromIce(requests.at(i).settings));
+ }
}
return results;
}
@@ -493,9 +549,14 @@ namespace armarx::armem::server::obj::instance
}
});
+ /* TODO(phesch): Fetch data required for predictions from the memory
+ * in the locked part (above), run visualization code below.
+ */
+
const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
+
TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
if (debugObserver)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 07ded4465a2b8e81f4ea0b75a02fabed80ed9a36..5f1a8c0aee43eed0230c806b0099e90819e403a8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -305,10 +305,6 @@ namespace armarx::armem::server::obj::instance
useArticulatedModels.setValue(visu.useArticulatedModels);
- showLinearPredictions.setValue(visu.showLinearPredictions);
- linearPredictionTimeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
- linearPredictionTimeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
- linearPredictionTimeWindowSeconds.setRange(0, std::numeric_limits<float>::max());
GridLayout grid;
int row = 0;
@@ -331,17 +327,11 @@ namespace armarx::armem::server::obj::instance
grid.add(gaussians.group, {row, 0}, {1, 4});
row++;
-
grid.add(Label("Use Articulated Models"), {row, 0}).add(useArticulatedModels, {row, 1});
row++;
- grid.add(Label("Show Linear Predictions"), {row, 0}).add(showLinearPredictions, {row, 1});
- row++;
- grid.add(Label("Time (seconds from now):"), {row, 0})
- .add(linearPredictionTimeOffsetSeconds, {row, 1});
- row++;
- grid.add(Label("Time Window (seconds):"), {row, 0})
- .add(linearPredictionTimeWindowSeconds, {row, 1});
+ linearPredictions.setup(visu);
+ grid.add(linearPredictions.group, {row, 0}, {1, 4});
row++;
group = GroupBox();
@@ -392,9 +382,47 @@ namespace armarx::armem::server::obj::instance
visu.useArticulatedModels = useArticulatedModels.getValue();
- visu.showLinearPredictions = showLinearPredictions.getValue();
- visu.linearPredictionTimeOffsetSeconds = linearPredictionTimeOffsetSeconds.getValue();
- visu.linearPredictionTimeWindowSeconds = linearPredictionTimeWindowSeconds.getValue();
+ linearPredictions.update(visu);
+ }
+
+
+ void Visu::RemoteGui::LinearPredictions::setup(const Visu& visu)
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ show.setValue(visu.showLinearPredictions);
+ timeOffsetSeconds.setValue(visu.linearPredictionTimeOffsetSeconds);
+ timeOffsetSeconds.setRange(-1e6, 1e6);
+ timeOffsetSeconds.setSteps(2 * 2 * 1000 * 1000);
+
+ timeWindowSeconds.setValue(visu.linearPredictionTimeWindowSeconds);
+ timeWindowSeconds.setRange(0, 1e6);
+ timeWindowSeconds.setSteps(2 * 1000 * 1000);
+
+
+ GridLayout grid;
+ int row = 0;
+
+ grid.add(Label("Show"), {row, 0}).add(show, {row, 1});
+ row++;
+ grid.add(Label("Time (seconds from now):"), {row, 0})
+ .add(timeOffsetSeconds, {row, 1});
+ row++;
+ grid.add(Label("Time Window (seconds):"), {row, 0})
+ .add(timeWindowSeconds, {row, 1});
+ row++;
+
+ group = GroupBox();
+ group.setLabel("Linear Predictions");
+ group.addChild(grid);
+ }
+
+
+ void Visu::RemoteGui::LinearPredictions::update(Visu& visu)
+ {
+ visu.showLinearPredictions = show.getValue();
+ visu.linearPredictionTimeOffsetSeconds = timeOffsetSeconds.getValue();
+ visu.linearPredictionTimeWindowSeconds = timeWindowSeconds.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index a88e3d2c85877c3ca0a3e430351715c310127825..56380159b45c3ca3274f6e88cfad0cfa439c9c2b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -128,11 +128,20 @@ namespace armarx::armem::server::obj::instance
armarx::RemoteGui::Client::CheckBox useArticulatedModels;
- armarx::RemoteGui::Client::CheckBox showLinearPredictions;
- armarx::RemoteGui::Client::FloatSpinBox linearPredictionTimeOffsetSeconds;
- armarx::RemoteGui::Client::FloatSpinBox linearPredictionTimeWindowSeconds;
+ struct LinearPredictions
+ {
+ armarx::RemoteGui::Client::CheckBox show;
+
+ armarx::RemoteGui::Client::FloatSpinBox timeOffsetSeconds;
+ armarx::RemoteGui::Client::FloatSpinBox timeWindowSeconds;
+
+ armarx::RemoteGui::Client::GroupBox group;
+ void setup(const Visu& data);
+ void update(Visu& data);
+ };
+ LinearPredictions linearPredictions;
void setup(const Visu& visu);
void update(Visu& visu);
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 beebabd2977e8f7e46e976892ee4da3dc18f2bad..f8eaaa6430967056012d9f9bcc7333ddabb7853a 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -205,7 +205,7 @@ namespace armarx::armem::robot_state
.timestamp = timestamp, .globalPose = *globalPose, .jointMap = *jointMap};
}
- std::optional<robot::RobotState::Pose>
+ std::optional<::armarx::armem::robot_state::Transform>
RobotReader::queryOdometryPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const
{
@@ -220,13 +220,18 @@ namespace armarx::armem::robot_state
}
};
- const auto result = transformReader.lookupTransform(query);
- if (not result)
- {
+ try {
+ const auto result = transformReader.lookupTransform(query);
+ if (not result)
+ {
+ return std::nullopt;
+ }
+ return result.transform;
+
+ } catch (...) {
+ ARMARX_WARNING << GetHandledExceptionString();
return std::nullopt;
}
-
- return result.transform.transform;
}
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 e72b5fc64744a7ad3481990705568e92a8c56208..247deb3b20c2613ab2d3c4d14ce076b76f076790 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h
@@ -108,7 +108,7 @@ namespace armarx::armem::robot_state
* @param timestamp
* @return std::optional<robot::RobotState::Pose>
*/
- std::optional<robot::RobotState::Pose>
+ std::optional<::armarx::armem::robot_state::Transform>
queryOdometryPose(const robot::RobotDescription& description,
const armem::Time& timestamp) const;
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 a595d5502c7139e51d9e940d71f24925ab39f199..8e2246c3dd4dc4175c23e0bb01455b380d94e86c 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -6,6 +6,9 @@
#include <ArmarXCore/core/exceptions/LocalException.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/time/DateTime.h"
+#include "ArmarXCore/core/time/Duration.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include <RobotAPI/libraries/core/FramedPose.h>
@@ -41,6 +44,14 @@ namespace armarx::armem::common::robot_state::localization
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+
+ const armem::Time sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ auto header = query.header;
+
+ // ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
+
+ header.timestamp = sanitizedTimestamp;
+
if (transforms.empty())
{
ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
@@ -58,7 +69,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Found valid transform";
- return {.transform = {.header = query.header, .transform = transform},
+ return {.transform = {.header = header, .transform = transform},
.status = TransformResult::Status::Success};
}
@@ -187,6 +198,30 @@ namespace armarx::armem::common::robot_state::localization
return chain;
}
+ template <class ...Args>
+ armarx::core::time::DateTime
+ TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
+ {
+
+ // first we check which the newest timestamp is
+ int64_t timeSinceEpochUs = 0;
+
+ localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
+
+ const auto& dataTs = tf.header.timestamp;
+
+ timeSinceEpochUs = std::max(timeSinceEpochUs, dataTs.toMicroSecondsSinceEpoch());
+ });
+
+ // then we ensure that the timestamp is not more recent than the query timestamp
+ timeSinceEpochUs = std::min(timeSinceEpochUs, timestamp.toMicroSecondsSinceEpoch());
+
+ return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs));
+ }
+
template <class ...Args>
std::vector<Eigen::Affine3f>
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 9de1fbd994f5a17c00987ce70d31525db7d9d1dd..6e2611f7a178ba64ab3e25082b5e74a6b968dc23 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
@@ -111,6 +111,10 @@ namespace armarx::armem::common::robot_state::localization
const armem::base::ProviderSegmentBase<Args...>& agentProviderSegment,
const armem::Time& timestamp);
+ template <class ...Args>
+ static
+ armarx::core::time::DateTime
+ _obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp);
static
Eigen::Affine3f
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
index 9b7c4fea29c8d2bd2471ca13feca3c384e194433..02325df8c3333ffa5671bcdc109e04ff6edc4ceb 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.cpp
@@ -48,9 +48,9 @@ namespace armarx::armem::server::robot_state::proprioception
{
const std::vector<std::string> split = simox::alg::split(entryName, ".", false, false);
ARMARX_CHECK_GREATER_EQUAL(split.size(), 3);
- const std::set<size_t> acceptedSizes{3, 4};
+ const std::set<size_t> acceptedSizes{3, 4, 5};
ARMARX_CHECK_GREATER(acceptedSizes.count(split.size()), 0)
- << "Data entry name could not be parsed (exected 3 or 4 components between '.'): "
+ << "Data entry name could not be parsed (exected 3 or 4 or 5 components between '.'): "
<< "\n- split: '" << split << "'";
const std::string& category = split.at(0);
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index cc81788baa40cf3089de08f3699ac8e9cf026c36..f8bbe5ec1db7641c9f634ff51ba6416e1d2dbb81 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -19,13 +19,18 @@ armarx_add_library(
./client/laser_scans/Writer.h
./client/occupancy_grid/Reader.h
./client/occupancy_grid/Writer.h
+ ./client/laser_scanner_features/Reader.h
+ ./client/laser_scanner_features/Writer.h
./OccupancyGridHelper.h
+ constants.h
SOURCES
./aron_conversions.cpp
./client/laser_scans/Reader.cpp
./client/laser_scans/Writer.cpp
./client/occupancy_grid/Reader.cpp
./client/occupancy_grid/Writer.cpp
+ ./client/laser_scanner_features/Reader.cpp
+ ./client/laser_scanner_features/Writer.cpp
./OccupancyGridHelper.cpp
)
@@ -34,6 +39,7 @@ armarx_enable_aron_file_generation_for_target(
"${LIB_NAME}"
ARON_FILES
aron/LaserScan.xml
+ aron/LaserScannerFeatures.xml
aron/OccupancyGrid.xml
)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
index 4b936b6e5bbc7de8141ea380efdac63f3e8d14ad..89ee1de1132faa3298765bd1c28fda208ca164a7 100644
--- a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -2,14 +2,14 @@
#include <Eigen/Core>
-namespace armarx::armem
+namespace armarx::armem::vision
{
struct OccupancyGrid;
}
namespace armarx
{
- using armarx::armem::OccupancyGrid;
+ using armarx::armem::vision::OccupancyGrid;
namespace detail
{
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index f2d11c2e4111dfd0c24896ba807aef6435b7014b..9ad92092e4f2fde9a14450e0bf36bacb8cc84326 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::arondto::LaserScannerInfo'>
+ <Object name='armarx::armem::vision::arondto::LaserScannerInfo'>
<ObjectChild key='device'>
<string />
</ObjectChild>
@@ -26,7 +26,7 @@
</ObjectChild>
</Object>
- <Object name="armarx::armem::arondto::SensorHeader">
+ <Object name="armarx::armem::vision::arondto::SensorHeader">
<ObjectChild key="agent">
<string/>
</ObjectChild>
@@ -39,9 +39,9 @@
</Object>
- <Object name='armarx::armem::arondto::LaserScanStamped'>
+ <Object name='armarx::armem::vision::arondto::LaserScanStamped'>
<ObjectChild key="header">
- <armarx::armem::arondto::SensorHeader />
+ <armarx::armem::vision::arondto::SensorHeader />
</ObjectChild>
<!--
@@ -51,4 +51,4 @@
</Object>
</GenerateTypes>
-</AronTypeDefinition>
\ No newline at end of file
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
new file mode 100644
index 0000000000000000000000000000000000000000..32600d7d19eef8f3969bde99c4a66d60635cf4c6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.xml
@@ -0,0 +1,67 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::armem::vision::arondto::Ellipsoid'>
+ <ObjectChild key='globalPose'>
+ <Pose />
+ </ObjectChild>
+ <ObjectChild key='radii'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ </Object>
+
+ <Object name='armarx::armem::vision::arondto::Circle'>
+ <ObjectChild key='center'>
+ <Matrix rows="2" cols="1" type="float32" />
+ </ObjectChild>
+ <ObjectChild key='radius'>
+ <float />
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeature">
+ <ObjectChild key="convexHull">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ <ObjectChild key="circle">
+ <armarx::armem::vision::arondto::Circle/>
+ </ObjectChild>
+ <ObjectChild key="ellipsoid">
+ <armarx::armem::vision::arondto::Ellipsoid/>
+ </ObjectChild>
+ <!-- <ObjectChild key="chain">
+ <armarx::armem::vision::arondto::Chain/>
+ </ObjectChild> -->
+ <ObjectChild key="points">
+ <List>
+ <Matrix rows="2" cols="1" type="float32" />
+ </List>
+ </ObjectChild>
+ </Object>
+
+ <Object name="armarx::armem::vision::arondto::LaserScannerFeatures">
+ <ObjectChild key="frame">
+ <String/>
+ </ObjectChild>
+ <ObjectChild key="frameGlobalPose">
+ <Pose/>
+ </ObjectChild>
+ <ObjectChild key="features">
+ <List>
+ <armarx::armem::vision::arondto::LaserScannerFeature />
+ </List>
+ </ObjectChild>
+ </Object>
+
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
index 0c508a4e2138b4b04c126287bf46d8826fb3da6f..3f273cded23f6942d6be82f76521f929146fb21e 100644
--- a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -8,7 +8,7 @@
<GenerateTypes>
- <Object name='armarx::armem::arondto::OccupancyGrid'>
+ <Object name='armarx::armem::vision::arondto::OccupancyGrid'>
<ObjectChild key='resolution'>
<float />
</ObjectChild>
@@ -27,4 +27,4 @@
</GenerateTypes>
-</AronTypeDefinition>
\ No newline at end of file
+</AronTypeDefinition>
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index 4819a27abc53f7b10696da6d452bb2d133a82fd4..cadb4fdcda9f18695833a462e0bef2acfe91451d 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,8 +5,9 @@
#include <iterator>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/aron/converter/common/Converter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
@@ -14,36 +15,38 @@
#include "types.h"
-namespace armarx::armem
+namespace armarx::armem::vision
{
/************ fromAron ************/
- SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
+ SensorHeader
+ fromAron(const arondto::SensorHeader& aronSensorHeader)
{
- return {.agent = aronSensorHeader.agent,
- .frame = aronSensorHeader.frame,
+ return {.agent = aronSensorHeader.agent,
+ .frame = aronSensorHeader.frame,
.timestamp = aron::fromAron<Time>(aronSensorHeader.timestamp)};
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScanStamped& laserScan)
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan)
{
laserScan.header = fromAron(aronLaserScan.header);
// laserScan.data = fromAron(aronLaserScan.data);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScan& laserScan,
- std::int64_t& timestamp,
- std::string& frame,
- std::string& agentName)
+ void
+ fromAron(const arondto::LaserScanStamped& aronLaserScan,
+ LaserScan& laserScan,
+ std::int64_t& timestamp,
+ std::string& frame,
+ std::string& agentName)
{
const auto header = fromAron(aronLaserScan.header);
// laserScan = fromAron(aronLaserScan.data);
timestamp = header.timestamp.toMicroSecondsSinceEpoch();
- frame = header.frame;
+ frame = header.frame;
agentName = header.agent;
}
@@ -54,46 +57,48 @@ namespace armarx::armem
// aronLaserScan.scan = toAron(laserScan);
// }
- int64_t toAron(const armem::Time& timestamp)
+ int64_t
+ toAron(const armem::Time& timestamp)
{
return timestamp.toMicroSecondsSinceEpoch();
}
- arondto::SensorHeader toAron(const SensorHeader& sensorHeader)
+ arondto::SensorHeader
+ toAron(const SensorHeader& sensorHeader)
{
arondto::SensorHeader aronSensorHeader;
- aronSensorHeader.agent = sensorHeader.agent;
- aronSensorHeader.frame = sensorHeader.frame;
+ aronSensorHeader.agent = sensorHeader.agent;
+ aronSensorHeader.frame = sensorHeader.frame;
toAron(aronSensorHeader.timestamp, sensorHeader.timestamp);
return aronSensorHeader;
}
- void toAron(const LaserScanStamped& laserScanStamped,
- arondto::LaserScanStamped& aronLaserScanStamped)
+ void
+ toAron(const LaserScanStamped& laserScanStamped,
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
aronLaserScanStamped.header = toAron(laserScanStamped.header);
// toAron(laserScanStamped.data, aronLaserScanStamped.data);
}
- void toAron(const LaserScan& laserScan,
- const armem::Time& timestamp,
- const std::string& frame,
- const std::string& agentName,
- arondto::LaserScanStamped& aronLaserScanStamped)
+ void
+ toAron(const LaserScan& laserScan,
+ const armem::Time& timestamp,
+ const std::string& frame,
+ const std::string& agentName,
+ arondto::LaserScanStamped& aronLaserScanStamped)
{
- const SensorHeader header
- {
- .agent = agentName, .frame = frame, .timestamp = timestamp};
+ const SensorHeader header{.agent = agentName, .frame = frame, .timestamp = timestamp};
- const LaserScanStamped laserScanStamped{.header = header,
- .data = laserScan};
+ const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
toAron(laserScanStamped, aronLaserScanStamped);
}
- void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+ void
+ toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
{
aron::toAron(dto.frame, bo.frame);
aron::toAron(dto.pose, bo.pose);
@@ -101,7 +106,8 @@ namespace armarx::armem
// bo.grid is NdArray -> need special handling.
}
- void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+ void
+ fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
{
aron::fromAron(dto.frame, bo.frame);
aron::fromAron(dto.pose, bo.pose);
@@ -109,4 +115,70 @@ namespace armarx::armem
// bo.grid is NdArray -> need special handling.
}
-} // namespace armarx::armem
+ // LaserScannerFeatures
+
+
+ void
+ toAron(arondto::Circle& dto, const Circle& bo)
+ {
+ dto.center = bo.center;
+ dto.radius = bo.radius;
+ }
+
+ void
+ toAron(arondto::Ellipsoid& dto, const Ellipsoid& bo)
+ {
+ dto.globalPose = bo.pose.matrix();
+ dto.radii = bo.radii;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeature& dto, const LaserScannerFeature& bo)
+ {
+ toAron(dto.circle, bo.circle);
+ dto.convexHull = bo.convexHull;
+ toAron(dto.ellipsoid, bo.ellipsoid);
+ dto.points = bo.points;
+ }
+
+ void
+ toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo)
+ {
+ aron::toAron(dto.frame, bo.frame);
+ aron::toAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::toAron(dto.features, bo.features);
+ }
+
+ void
+ fromAron(const arondto::Circle& dto, Circle& bo)
+ {
+ bo.center = dto.center;
+ bo.radius = dto.radius;
+ }
+ void
+ fromAron(const arondto::Ellipsoid& dto, Ellipsoid& bo)
+ {
+ bo.pose = dto.globalPose;
+ bo.radii = dto.radii;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeature& dto, LaserScannerFeature& bo)
+ {
+ bo.convexHull = dto.convexHull;
+ fromAron(dto.circle, bo.circle);
+ fromAron(dto.ellipsoid, bo.ellipsoid);
+
+ // bo.chain = dto.chain;
+ bo.points = dto.points;
+ }
+
+ void
+ fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo)
+ {
+ aron::fromAron(dto.frame, bo.frame);
+ aron::fromAron(dto.frameGlobalPose, bo.frameGlobalPose);
+ aron::fromAron(dto.features, bo.features);
+ }
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
index 681049dac2250e30ad6a3ccee5f25cc6412b074e..bb1e525344c71ac33fabc7d8eda2102f69777107 100644
--- a/source/RobotAPI/libraries/armem_vision/aron_conversions.h
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -21,15 +21,16 @@
#pragma once
-#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/LaserScannerFeatures.aron.generated.h>
#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
-namespace armarx::armem
+namespace armarx::armem::vision
{
namespace arondto
@@ -47,14 +48,13 @@ namespace armarx::armem
std::string& agentName);
template <typename T>
- auto fromAron(const aron::data::NDArrayPtr& navigator)
+ auto
+ fromAron(const aron::data::NDArrayPtr& navigator)
{
- return aron::converter::AronVectorConverter::ConvertToVector<T>(
- navigator);
+ return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
}
- void fromAron(const arondto::LaserScanStamped& aronLaserScan,
- LaserScanStamped& laserScan);
+ void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
void toAron(const LaserScan& laserScan,
const armem::Time& timestamp,
@@ -79,4 +79,8 @@ namespace armarx::armem
return aron::converter::AronEigenConverter::ConvertFromArray(grid);
}
-} // namespace armarx::armem
+ // LaserScannerFeatures
+ void toAron(arondto::LaserScannerFeatures& dto, const LaserScannerFeatures& bo);
+ void fromAron(const arondto::LaserScannerFeatures& dto, LaserScannerFeatures& bo);
+
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9cf78c698b31345a9fcc17262bb7a23a25cd7fc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.cpp
@@ -0,0 +1,218 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Reader::~Reader() = default;
+
+
+ void
+ Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ // ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
+ // registerPropertyDefinitions(def);
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.coreSegmentName,
+ prefix + "CoreSegment",
+ "Name of the mapping memory core segment to use.");
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "MappingDataReader: Waiting for memory '" << properties.memoryName
+ << "' ...";
+ try
+ {
+ memoryReader =
+ memoryNameSystem.useReader(MemoryID().withMemoryName(properties.memoryName));
+ ARMARX_IMPORTANT << "MappingDataReader: Connected to memory '" << properties.memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+ }
+
+ armarx::armem::client::query::Builder
+ Reader::buildQuery(const Query& query) const
+ {
+ armarx::armem::client::query::Builder qb;
+
+ qb.coreSegments()
+ .withName(properties.coreSegmentName)
+ .providerSegments()
+ .withName(query.providerName)
+ .entities()
+ .all()
+ .snapshots()
+ .beforeOrAtTime(query.timestamp);
+
+ // auto entitySel = [&]()
+ // {
+ // if (query.name.empty())
+ // {
+ // ARMARX_INFO << "querying all entities";
+ // return sel.entities().all();
+ // }
+ // return sel.entities().withName(query.name);
+ // }();
+
+ // entitySel.snapshots().beforeOrAtTime(query.timestamp);
+
+ return qb;
+ }
+
+ std::vector<LaserScannerFeatures>
+ asFeaturesList(const wm::ProviderSegment& providerSegment)
+ {
+ if (providerSegment.empty())
+ {
+ ARMARX_WARNING << "No entities!";
+ return {};
+ }
+
+ // const auto convert = [](const auto& aronLaserScanStamped,
+ // const wm::EntityInstance& ei) -> LaserScanStamped
+ // {
+ // LaserScanStamped laserScanStamped;
+ // fromAron(aronLaserScanStamped, laserScanStamped);
+
+ // const auto ndArrayNavigator =
+ // aron::data::NDArray::DynamicCast(ei.data()->getElement("scan"));
+
+ // ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+ // laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
+ // ARMARX_IMPORTANT << "4";
+
+ // return laserScanStamped;
+ // };
+
+ std::vector<LaserScannerFeatures> laserScannerFeatures;
+
+ // loop over all entities and their snapshots
+ providerSegment.forEachEntity(
+ [&](const wm::Entity& entity)
+ {
+ // If we don't need this warning, we could directly iterate over the snapshots.
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "Empty history for " << entity.id();
+ }
+ ARMARX_DEBUG << "History size: " << entity.size();
+
+ entity.forEachInstance(
+ [&](const wm::EntityInstance& entityInstance)
+ {
+ if (const auto o = tryCast<arondto::LaserScannerFeatures>(entityInstance))
+ {
+ LaserScannerFeatures& f = laserScannerFeatures.emplace_back();
+ fromAron(o.value(), f);
+ }
+ return true;
+ });
+ return true;
+ });
+
+ return laserScannerFeatures;
+ }
+
+ Reader::Result
+ Reader::queryData(const Query& query) const
+ {
+ const auto qb = buildQuery(query);
+
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+ if (not qResult.success)
+ {
+ ARMARX_WARNING << "Failed to query data from memory: " << qResult.errorMessage;
+ return {.features = {},
+ .status = Result::Status::Error,
+ .errorMessage = qResult.errorMessage};
+ }
+
+ // now create result from memory
+ const wm::ProviderSegment& providerSegment =
+ qResult.memory.getCoreSegment(properties.coreSegmentName)
+ .getProviderSegment(query.providerName);
+
+ const auto features = asFeaturesList(providerSegment);
+
+ // const auto laserScans = asLaserScans(providerSegment);
+ // std::vector<std::string> sensors;
+ // providerSegment.forEachEntity(
+ // [&sensors](const wm::Entity& entity)
+ // {
+ // sensors.push_back(entity.name());
+ // return true;
+ // });
+
+ return {.features = features,
+ // .sensors = sensors,
+ .status = Result::Status::Success,
+ .errorMessage = ""};
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..4752ca85cf3df286b3d1378c2a0526f5a89f1a31
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h
@@ -0,0 +1,113 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+
+namespace armarx
+{
+ class ManagedIceObject;
+}
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Reader
+ {
+ public:
+ Reader(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Reader();
+
+ void connect();
+
+ struct Query
+ {
+ std::string providerName;
+
+ std::string name; // sensor name
+
+ armarx::core::time::DateTime timestamp;
+
+ // std::vector<std::string> sensorList;
+ };
+
+ struct Result
+ {
+ std::vector<LaserScannerFeatures> features;
+
+ // std::vector<std::string> sensors;
+
+ enum Status
+ {
+ Error,
+ Success
+ } status;
+
+ std::string errorMessage;
+ };
+
+ Result queryData(const Query& query) const;
+
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+
+
+ protected:
+ armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Reader memoryReader;
+
+ // Properties
+ struct Properties
+ {
+ std::string memoryName = "Vision";
+ std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5746bffa1880c3031e183acf5cecd74f2d6d515a
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.cpp
@@ -0,0 +1,111 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/constants.h"
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScannerFeatures.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
+ memoryNameSystem(memoryNameSystem)
+ {
+ }
+ Writer::~Writer() = default;
+
+
+ void
+ Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ // def->optional(properties.coreSegmentName,
+ // prefix + "CoreSegment",
+ // "Name of the mapping memory core segment to use.");
+
+ // def->optional(properties.memoryName, prefix + "MemoryName");
+ }
+
+ void
+ Writer::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '" << constants::memoryName
+ << "' ...";
+ try
+ {
+ memoryWriter =
+ memoryNameSystem.useWriter(MemoryID().withMemoryName(constants::memoryName));
+ ARMARX_IMPORTANT << "MappingDataWriter: Connected to memory '" << constants::memoryName
+ << "'";
+ }
+ catch (const armem::error::CouldNotResolveMemoryServer& e)
+ {
+ ARMARX_ERROR << e.what();
+ return;
+ }
+
+ ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '" << constants::memoryName;
+ }
+
+ bool
+ Writer::store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp)
+ {
+ std::lock_guard g{memoryWriterMutex};
+
+ // const auto result = memoryWriter.addSegment(constants::memoryName,
+ // constants::laserScannerFeaturesCoreSegment);
+
+ // if (not result.success)
+ // {
+ // ARMARX_ERROR << result.errorMessage;
+
+ // // TODO(fabian.reister): message
+ // return false;
+ // }
+
+ const auto entityID = armem::MemoryID()
+ .withMemoryName(constants::memoryName)
+ .withCoreSegmentName(constants::laserScannerFeaturesCoreSegment)
+ .withProviderSegmentName(providerName)
+ .withEntityName(features.frame)
+ .withTimestamp(timestamp);
+
+ ARMARX_VERBOSE << "Memory id is " << entityID.str();
+
+ armem::EntityUpdate update;
+ update.entityID = entityID;
+
+ ARMARX_TRACE;
+
+ arondto::LaserScannerFeatures dto;
+ toAron(dto, features);
+
+ ARMARX_TRACE;
+
+ update.instancesData = {dto.toAron()};
+ update.timeCreated = timestamp;
+
+ ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
+
+ ARMARX_TRACE;
+ armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.success)
+ {
+ ARMARX_ERROR << updateResult.errorMessage;
+ }
+
+ return updateResult.success;
+ }
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca58005772268d70f7eedf06189c02250c75fef6
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scanner_features/Writer.h
@@ -0,0 +1,85 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
+
+
+namespace armarx::armem::vision::laser_scanner_features::client
+{
+
+ /**
+ * @defgroup Component-ExampleClient ExampleClient
+ * @ingroup RobotAPI-Components
+ * A description of the component ExampleClient.
+ *
+ * @class ExampleClient
+ * @ingroup Component-ExampleClient
+ * @brief Brief description of class ExampleClient.
+ *
+ * Detailed description of class ExampleClient.
+ */
+ class Writer
+ {
+ public:
+ Writer(armem::client::MemoryNameSystem& memoryNameSystem);
+ virtual ~Writer();
+
+
+ void connect();
+
+ // MappingDataWriterInterface
+ /// to be called in Component::onConnectComponent
+ // void connect() override;
+
+ /// to be called in Component::addPropertyDefinitions
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+
+ bool store(const LaserScannerFeatures& features,
+ const std::string& providerName,
+ const Time& timestamp);
+
+ private:
+ armem::client::MemoryNameSystem& memoryNameSystem;
+ armem::client::Writer memoryWriter;
+
+ // Properties
+ struct Properties
+ {
+ // std::string memoryName = "Vision";
+ // std::string coreSegmentName = "LaserScannerFeatures";
+ } properties;
+
+ std::mutex memoryWriterMutex;
+
+ const std::string propertyPrefix = "mem.vision.laser_scanner_features.";
+ };
+
+} // namespace armarx::armem::vision::laser_scanner_features::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index 5faf563cfab3c4c380258268671f5c7268fe9dd1..396dd1bd8b4f40cfef062fa963e87c97f52f45b4 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -131,7 +131,7 @@ namespace armarx::armem::vision::laser_scans::client
}
const auto convert =
- [](const arondto::LaserScanStamped & aronLaserScanStamped,
+ [](const auto& aronLaserScanStamped,
const wm::EntityInstance & ei) -> LaserScanStamped
{
LaserScanStamped laserScanStamped;
@@ -177,7 +177,7 @@ namespace armarx::armem::vision::laser_scans::client
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index 138717c42257e02275a15785940783330126ee4d..349b65a5af3bea2fc6acb17324a42b87fbb3bf8d 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -113,7 +113,7 @@ namespace armarx::armem::vision::laser_scans::client
std::string coreSegmentName = "LaserScans";
} properties;
- const std::string propertyPrefix = "mem.vision.";
+ const std::string propertyPrefix = "mem.vision.laser_scans.";
};
diff --git a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index afc383634b87a31fc84a841a5658f83ec845ee27..b4f7502ef9493692fc82bf79740d2367fe165d4a 100644
--- a/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -81,7 +81,7 @@ namespace armarx::armem::vision::laser_scans::client
std::mutex memoryWriterMutex;
- const std::string propertyPrefix = "mem.vision.";
+ const std::string propertyPrefix = "mem.vision.laser_scans.";
};
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
index bb3bdf5546650f2bb8c45db39b092c0f601c2c9a..91505d40e5ea0046fc2419ac532db85010062011 100644
--- a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -115,7 +115,7 @@ namespace armarx::armem::vision::occupancy_grid::client
{
const auto qb = buildQuery(query);
- ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+ ARMARX_DEBUG << "[MappingDataReader] query ... ";
const armem::client::QueryResult qResult =
memoryReader().query(qb.buildQueryInput());
diff --git a/source/RobotAPI/libraries/armem_vision/constants.h b/source/RobotAPI/libraries/armem_vision/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..653d6fda6fc9e787d00f570af5440a6c81c6108b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/constants.h
@@ -0,0 +1,33 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2022
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <string>
+
+namespace armarx::armem::vision::constants
+{
+ const inline std::string memoryName = "Vision";
+
+ // core segments
+ const inline std::string laserScannerFeaturesCoreSegment = "LaserScannerFeatures";
+
+} // namespace armarx::armem::vision::constants
diff --git a/source/RobotAPI/libraries/armem_vision/types.h b/source/RobotAPI/libraries/armem_vision/types.h
index 00fb545a104afb19c38c9d7b01fd864ed6ba1fe6..85322d188ce8fdbd9b065ce476e281408d38f254 100644
--- a/source/RobotAPI/libraries/armem_vision/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -23,10 +23,12 @@
#include <vector>
+#include <VirtualRobot/MathTools.h>
+
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/Time.h>
-namespace armarx::armem
+namespace armarx::armem::vision
{
struct SensorHeader
@@ -52,10 +54,50 @@ namespace armarx::armem
// using ValueType = _ValueT;
using CellType = float;
- using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+ using Grid = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
Grid grid;
};
+ struct Ellipsoid
+ {
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+
+ Eigen::Vector2f radii = Eigen::Vector2f::Zero();
+ };
+
+ struct Circle
+ {
+ Eigen::Vector2f center = Eigen::Vector2f::Zero();
+ float radius = 0.F;
+ };
+
+ struct LaserScannerFeature
+ {
+ using Points = std::vector<Eigen::Vector2f>;
+ using Chain = Points;
+
+ Points convexHull;
+
+ Circle circle;
+ Ellipsoid ellipsoid;
+
+ Chain chain;
+
+ Points points;
+ };
+
+ struct LaserScannerFeatures
+ {
+ // TODO(fabian.reister): framed pose
+ std::string frame;
+ Eigen::Isometry3f frameGlobalPose;
+
+ std::vector<LaserScannerFeature> features;
+
+
+ // std::vector<Ellipsoid> linesAsEllipsoids(float axisLength) const;
+ };
+
-} // namespace armarx::armem
+} // namespace armarx::armem::vision
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
index 0bf14ec90439e50a4869234b81449adaf068d173..deed74c9ada113f05d10286a370b703d6a712642 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.cpp
@@ -14,4 +14,14 @@ namespace armarx::aron
dto = bo.matrix();
}
+ void fromAron(const AronPose& dto, Eigen::Isometry3f& bo)
+ {
+ bo.matrix() = dto;
+ }
+
+ void toAron(AronPose& dto, const Eigen::Isometry3f& bo)
+ {
+ dto = bo.matrix();
+ }
+
} // namespace armarx::aron
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
index 3f091dbfc40ee8e91b8d2922687273269ce790a8..3520b2dedd7c10f2d358cb7997269871979b52ed 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
+++ b/source/RobotAPI/libraries/aron/common/aron_conversions/eigen.h
@@ -9,4 +9,7 @@ namespace armarx::aron
void fromAron(const AronPose& dto, Eigen::Affine3f& bo);
void toAron(AronPose& dto, const Eigen::Affine3f& bo);
+ void fromAron(const AronPose& dto, Eigen::Isometry3f& bo);
+ void toAron(AronPose& dto, const Eigen::Isometry3f& bo);
+
} // namespace armarx
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index e1213b86b1f2f6386fb22d318f4ed798a72eb040..8d6ccf594f8d0adcd168a066111b1ad2fecdcf39 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -111,8 +111,9 @@ namespace armarx::aron::converter
using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
- MatrixT ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+ ARMARX_CHECK_EQUAL(dims.size(), 2); // for now ...
+ MatrixT ret(dims.at(0), dims.at(1));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<>()));
return ret;
}
@@ -128,7 +129,7 @@ namespace armarx::aron::converter
auto dims = nav.getShape();
Eigen::Matrix<T, Rows, Cols> ret;
- memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+ memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), sizeof(T), std::multiplies<int>()));
return ret;
}