diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h b/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
index 87fd71f938e32f0324c8dd3f3c9fadfd60892235..179864cc951672758ad36351829e97105d9452e9 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationArrowCircle.h
@@ -3,13 +3,15 @@
#include "ElementVisualizer.h"
#include <RobotAPI/interface/ArViz/Elements.h>
+
#include <Inventor/nodes/SoCone.h>
+#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoCylinder.h>
+#include <Inventor/nodes/SoIndexedFaceSet.h>
+#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoTranslation.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
namespace armarx::viz::coin
{
@@ -17,50 +19,172 @@ namespace armarx::viz::coin
{
using ElementType = data::ElementArrowCircle;
- bool update(ElementType const& element)
+ SoCoordinate3* coords;
+ SoIndexedFaceSet* torusFaceSet;
+ SoSeparator* torus;
+ SoSeparator* coneSep;
+ SoTransform* coneTransform;
+ SoTransform* coneSignRotation;
+ SoCone* cone;
+
+ std::vector<SbVec3f> vertexPositions;
+ std::vector<int32_t> faces;
+ std::vector<int32_t> matInx;
+
+ static const int RINGS = 32;
+ static const int SIDES = 8;
+
+ VisualizationArrowCircle()
{
- int rings = 32;
+ SoMaterialBinding* torusBinding = new SoMaterialBinding;
+ torusBinding->value = SoMaterialBinding::PER_VERTEX_INDEXED;
+
+ coords = new SoCoordinate3;
+ SoShapeHints* torusHints = new SoShapeHints;
+ // Disable back culling and enable two-sided lighting
+ torusHints->vertexOrdering = SoShapeHints::VertexOrdering::COUNTERCLOCKWISE;
+ torusHints->shapeType = SoShapeHints::ShapeType::UNKNOWN_SHAPE_TYPE;
+
+ torusFaceSet = new SoIndexedFaceSet();
+ torus = new SoSeparator;
+
+ torus->addChild(torusBinding);
+ torus->addChild(coords);
+ torus->addChild(torusHints);
+ torus->addChild(torusFaceSet);
+
+ coneSep = new SoSeparator();
+ coneTransform = new SoTransform;
+
+ node->addChild(torus);
+ node->addChild(coneSep);
+
+ coneSignRotation = new SoTransform;
+ cone = new SoCone;
+
+ coneSep->addChild(coneTransform);
+ coneSep->addChild(coneSignRotation);
+ coneSep->addChild(cone);
+
+ int numVerticesPerRow = SIDES + 1;
+ int numVerticesPerColumn = RINGS + 1;
+ vertexPositions.resize(numVerticesPerRow * numVerticesPerColumn);
+
+ int numFaces = RINGS * SIDES * 2;
+ faces.resize(numFaces * 4);
+ matInx.resize(numFaces * 4);
+ }
+
+ bool update(ElementType const& element)
+ {
float completion = std::min<float>(1.0f, std::max(-1.0f, element.completion));
int sign = completion >= 0 ? 1 : -1;
- float torusCompletion = completion - 1.0f / rings * sign;
+ float torusCompletion = completion - 1.0f / RINGS * sign;
if (torusCompletion * sign < 0)
{
torusCompletion = 0;
}
- auto color = element.color;
- const float conv = 1.0f / 255.0f;
- float r = color.r * conv;
- float g = color.g * conv;
- float b = color.b * conv;
- float a = color.a * conv;
- auto torusNode = VirtualRobot::CoinVisualizationFactory().createTorus(
- element.radius, element.width, torusCompletion,
- r, g, b, 1.0f - a,
- 8, rings);
- SoNode* torus = dynamic_cast<VirtualRobot::CoinVisualizationNode&>(*torusNode).getCoinVisualization();
+ const float TWO_PI = 2.0f * (float)M_PI;
+ {
+ // Create a torus mesh (for the completion circle
+ float radius = element.radius;
+ float tubeRadius = element.width;
+ float completion = torusCompletion;
- float angle0 = (float)(rings - 2) / rings * 2 * M_PI * completion;
- float x0 = element.radius * cos(angle0);
- float y0 = element.radius * sin(angle0);
- float angle1 = (float)(rings - 1) / rings * 2 * M_PI * completion;
+ int numVerticesPerRow = SIDES + 1;
+ int numVerticesPerColumn = RINGS + 1;
- SoSeparator* subSep = new SoSeparator();
- SoTransform* tr = new SoTransform;
- tr->translation.setValue(x0, y0, 0);
- tr->rotation.setValue(SbVec3f(0, 0, 1), angle1);
- subSep->addChild(tr);
+ float theta = 0.0f;
+ float phi = 0.0f;
+ float verticalAngularStride = TWO_PI / RINGS;
+ float horizontalAngularStride = TWO_PI / SIDES;
- subSep->addChild(VirtualRobot::CoinVisualizationFactory::CreateArrow(
- Eigen::Vector3f::UnitY()*sign, 0, element.width,
- VirtualRobot::VisualizationFactory::Color(r, g, b, 1.0f - a)));
+ int numVertices = numVerticesPerColumn * numVerticesPerRow;
+ for (int verticalIt = 0; verticalIt < numVerticesPerColumn; verticalIt++)
+ {
+ theta = verticalAngularStride * verticalIt * completion;
- node->removeAllChildren();
- node->addChild(torus);
- node->addChild(subSep);
+ for (int horizontalIt = 0; horizontalIt < numVerticesPerRow; horizontalIt++)
+ {
+ phi = horizontalAngularStride * horizontalIt;
+
+ // position
+ float x = std::cos(theta) * (radius + tubeRadius * std::cos(phi));
+ float y = std::sin(theta) * (radius + tubeRadius * std::cos(phi));
+ float z = tubeRadius * std::sin(phi);
+
+ int vertexIndex = verticalIt * numVerticesPerRow + horizontalIt;
+ vertexPositions[vertexIndex].setValue(x, y, z);
+ }
+ }
+ coords->point.setValuesPointer(numVertices, vertexPositions.data());
+
+ for (int verticalIt = 0; verticalIt < RINGS; verticalIt++)
+ {
+ for (int horizontalIt = 0; horizontalIt < SIDES; horizontalIt++)
+ {
+ int faceIndex = (verticalIt * SIDES + horizontalIt) * 2;
+
+ short lt = (short)(horizontalIt + verticalIt * (numVerticesPerRow));
+ short rt = (short)((horizontalIt + 1) + verticalIt * (numVerticesPerRow));
+
+ short lb = (short)(horizontalIt + (verticalIt + 1) * (numVerticesPerRow));
+ short rb = (short)((horizontalIt + 1) + (verticalIt + 1) * (numVerticesPerRow));
+
+
+ faces[faceIndex * 4 + 0] = lt;
+ faces[faceIndex * 4 + 1] = rt;
+ faces[faceIndex * 4 + 2] = lb;
+ faces[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
+
+ matInx[faceIndex * 4 + 0] = 0;
+ matInx[faceIndex * 4 + 1] = 0;
+ matInx[faceIndex * 4 + 2] = 0;
+ matInx[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
+
+ faceIndex += 1;
+
+ faces[faceIndex * 4 + 0] = rt;
+ faces[faceIndex * 4 + 1] = rb;
+ faces[faceIndex * 4 + 2] = lb;
+ faces[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
+
+ matInx[faceIndex * 4 + 0] = 0;
+ matInx[faceIndex * 4 + 1] = 0;
+ matInx[faceIndex * 4 + 2] = 0;
+ matInx[faceIndex * 4 + 3] = SO_END_FACE_INDEX;
+ }
+ }
+
+ torusFaceSet->coordIndex.setValuesPointer(faces.size(), faces.data());
+ torusFaceSet->materialIndex.setValuesPointer(matInx.size(), matInx.data());
+ }
+
+ {
+ // Create a cone to make the arrow for the completion circle
+ float angle0 = (RINGS - 1.0f) / RINGS * TWO_PI * completion;
+ float x0 = element.radius * std::cos(angle0);
+ float y0 = element.radius * std::sin(angle0);
+ float angle1 = (RINGS - 0.5f) / RINGS * TWO_PI * completion;
+
+ coneTransform->translation.setValue(x0, y0, 0);
+
+ coneTransform->rotation.setValue(SbVec3f(0, 0, 1), angle1);
+
+ float coneHeight = element.width * 6.0f;
+ float coneBottomRadius = element.width * 2.5f;
+
+ SbVec3f axis(0.0f, 0.0f, 1.0f);
+ float angle = sign > 0.0f ? 0.0f : (float)M_PI;
+ coneSignRotation->rotation.setValue(axis, angle);
+
+ cone->bottomRadius.setValue(coneBottomRadius);
+ cone->height.setValue(coneHeight);
+ }
return true;
}
diff --git a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
index 785b27019532891f0f53ec97b10c3835c97c3cdf..120bb629a6ddb6008ebeb7780e31d4954441be4e 100644
--- a/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
+++ b/source/RobotAPI/components/ArViz/Coin/VisualizationMesh.h
@@ -46,8 +46,7 @@ namespace armarx::viz::coin
{
colorSize = 1;
}
- matAmb.resize(colorSize);
- matDif.resize(colorSize);
+ matColor.resize(colorSize);
transp.resize(colorSize);
const float conv = 1.0f / 255.0f;
@@ -58,8 +57,7 @@ namespace armarx::viz::coin
float g = color.g * conv;
float b = color.b * conv;
float a = color.a * conv;
- matAmb[0].setValue(r, g, b);
- matDif[0].setValue(r, g, b);
+ matColor[0].setValue(r, g, b);
transp[0] = 1.0f - a;
}
else
@@ -71,15 +69,14 @@ namespace armarx::viz::coin
float g = color.g * conv;
float b = color.b * conv;
float a = color.a * conv;
- matAmb[i].setValue(r, g, b);
- matDif[i].setValue(r, g, b);
+ matColor[i].setValue(r, g, b);
transp[i] = 1.0f - a;
}
}
// Define colors for the faces
- materials->diffuseColor.setValuesPointer(colorSize, matDif.data());
- materials->ambientColor.setValuesPointer(colorSize, matAmb.data());
+ materials->diffuseColor.setValuesPointer(colorSize, matColor.data());
+ materials->ambientColor.setValuesPointer(colorSize, matColor.data());
materials->transparency.setValuesPointer(colorSize, transp.data());
// define vertex array
@@ -123,8 +120,7 @@ namespace armarx::viz::coin
SoCoordinate3* coords;
SoIndexedFaceSet* faceSet;
- std::vector<SbColor> matAmb;
- std::vector<SbColor> matDif;
+ std::vector<SbColor> matColor;
std::vector<float> transp;
std::vector<SbVec3f> vertexPositions;
std::vector<int32_t> faces;
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index ff525ff98eda9eb834dfabb2f6bdfcd42ddb019b..89c8340b7fd524b9d3df06bb4332e7e591701011 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -426,14 +426,14 @@ namespace armarx::viz
}
}
- void CoinVisualizer::exportToVRML(const std::string& s)
+ void CoinVisualizer::exportToVRML(const std::string& exportFilePath)
{
SoOutput* so = new SoOutput();
- if (!so->openFile(s.c_str()))
+ if (!so->openFile(exportFilePath.c_str()))
{
- ARMARX_ERROR << "Could not open file " << s << " for writing." << std::endl;
+ ARMARX_ERROR << "Could not open file " << exportFilePath << " for writing." << std::endl;
return;
}
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index e2d9a4634d5f1747174c125292a33233699dceda..01d7072ca3a59dc6b69fb6b5c0adf3e6f25b4ead 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -129,7 +129,7 @@ namespace armarx::viz
void update();
- void exportToVRML(const std::string& s);
+ void exportToVRML(std::string const& exportFilePath);
template <typename ElementVisuT>
void registerVisualizerFor()
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 735aae1176215b9940b73afe01d5dcece4cfb363..6016f68207af9112ab5835774e786593a404e336 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -24,8 +24,6 @@
#include "RobotStateComponent.h"
-#include <thread>
-
#include <Ice/ObjectAdapter.h>
#include <VirtualRobot/RobotNodeSet.h>
@@ -33,7 +31,6 @@
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
@@ -48,553 +45,23 @@ using namespace Eigen;
using namespace Ice;
using namespace VirtualRobot;
-namespace armarx::detail
+
+namespace armarx
{
- class RobotStateComponentWorker
+ RobotStateComponent::~RobotStateComponent()
{
- public:
- struct Job
- {
- enum class JobType
- {
- none,
- getSynchronizedRobot,
- getRobotSnapshot,
- getRobotSnapshotAtTimestamp,
- getJointConfigAtTimestamp,
- getRobotStateAtTimestamp,
-
- reportGlobalRobotPose,
- reportPlatformPose,
- reportJointAngles,
- reportJointVelocities,
- simulatorWasReset
- };
- AMD_RobotStateComponentInterface_getSynchronizedRobotPtr getSynchronizedRobot;
- AMD_RobotStateComponentInterface_getRobotSnapshotPtr getRobotSnapshot;
- AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr getRobotSnapshotAtTimestamp;
- AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr getJointConfigAtTimestamp;
- AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr getRobotStateAtTimestamp;
-
- JobType type = JobType::none;
- PoseStamped currentPose;
- std::string robotName;
- Eigen::Matrix4f pose;
- long timestamp;
- double time;
- NameValueMap perJointValues;
- bool aValueChanged;
- };
- template <class ValueT>
- struct Timestamped
- {
- IceUtil::Time timestamp;
- ValueT value;
- };
-
- VirtualRobot::RobotPtr synchronized;
-
- std::thread worker;
- std::atomic_bool stopWrk{false};
-
- std::map<IceUtil::Time, NameValueMap> jointHistory;
- std::map<IceUtil::Time, FramedPosePtr> poseHistory;
-
- size_t jointHistoryLength;
- size_t poseHistoryLength;
- RobotStateObserverPtr robotStateObs;
- SharedRobotInterfacePrx synchronizedPrx;
- SharedRobotServantPtr sharedRobotServant;
- RobotStateListenerInterfacePrx robotStateListenerPrx;
- RobotStateComponentInterfacePrx parent;
- Ice::ObjectAdapterPtr objectAdapter;
-
- std::mutex jobsMutex;
- std::deque<Job> jobs;
-
- public:
- RobotStateComponentWorker()
- {
- try
- {
- if (synchronizedPrx)
- {
- synchronizedPrx->unref();
- }
- }
- catch (...)
- {}
- }
- ~RobotStateComponentWorker()
- {
- stopWorker();
- }
-
- void startWorker()
- {
- stopWrk = false;
- ARMARX_CHECK_EXPRESSION(!worker.joinable());
- ARMARX_INFO << "starting worker";
- worker = std::thread{[&]{work();}};
- }
- void stopWorker()
- {
- stopWrk = true;
- if (worker.joinable())
- {
- ARMARX_INFO << "stopping worker";
- worker.join();
- }
- }
-
- void addJob(Job j)
- {
- std::lock_guard g{jobsMutex};
- jobs.emplace_back(std::move(j));
- ARMARX_DEBUG <<::deactivateSpam(1) << "adding job -> queue size: " << jobs.size();
- }
- private:
- void work()
- {
- ARMARX_INFO << "started worker";
- ARMARX_ON_SCOPE_EXIT
- {
- ARMARX_INFO << "stopped worker";
- };
- while (!stopWrk)
- {
- std::deque<Job> jobs;
- {
- std::lock_guard g{jobsMutex};
- std::swap(jobs, this->jobs);
- }
- if (jobs.empty())
- {
- std::this_thread::sleep_for(std::chrono::milliseconds{2});
- ARMARX_DEBUG_S << ::deactivateSpam(1) << "no jobs";
- continue;
- }
- using clock_t = std::chrono::high_resolution_clock;
- const auto start = clock_t::now();
- for (auto& j : jobs)
- {
- run(j);
- }
- const auto msecs = std::chrono::duration_cast<std::chrono::nanoseconds>(clock_t::now() - start).count() * 1e-6;
- ARMARX_DEBUG_S << ::deactivateSpam(1) << jobs.size() << " jobs took " << msecs << " ms";
- }
- }
-
- void run(Job& j)
- {
- switch (j.type)
- {
- case Job::JobType::getSynchronizedRobot:
- getSynchronizedRobot(j);
- break;
- case Job::JobType::getRobotSnapshot:
- getRobotSnapshot(j);
- break;
- case Job::JobType::getRobotSnapshotAtTimestamp:
- getRobotSnapshotAtTimestamp(j);
- break;
- case Job::JobType::getJointConfigAtTimestamp:
- getJointConfigAtTimestamp(j);
- break;
- case Job::JobType::getRobotStateAtTimestamp:
- getRobotStateAtTimestamp(j);
- break;
- case Job::JobType::reportGlobalRobotPose:
- reportGlobalRobotPose(j);
- break;
- case Job::JobType::reportPlatformPose:
- reportPlatformPose(j);
- break;
- case Job::JobType::reportJointAngles:
- reportJointAngles(j);
- break;
- case Job::JobType::reportJointVelocities:
- reportJointVelocities(j);
- break;
- case Job::JobType::simulatorWasReset:
- simulatorWasReset(j);
- break;
- default:
- ARMARX_FATAL << "job type not set!";
- std::terminate();
- }
- }
-
- void getSynchronizedRobot(Job& j)
- {
- if (!synchronizedPrx)
- {
- j.getSynchronizedRobot->ice_exception(
- NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot"));
- return;
- }
- j.getSynchronizedRobot->ice_response(synchronizedPrx);
- }
- void getRobotSnapshot(Job& j)
- {
- if (!synchronized)
- {
- j.getRobotSnapshot->ice_exception(NotInitializedException("Shared Robot is NULL", "getRobotSnapshot"));
- return;
- }
-
- auto clone = synchronized->clone(synchronized->getName());
-
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(parent), nullptr);
- p->setTimestamp(IceUtil::Time::microSecondsDouble(sharedRobotServant->getTimestamp()->timestamp));
- auto result = objectAdapter->addWithUUID(p);
- // virtal robot clone is buggy -> set global pose here
- p->setGlobalPose(new Pose(synchronized->getGlobalPose()));
- j.getRobotSnapshot->ice_response(SharedRobotInterfacePrx::uncheckedCast(result));
- }
- void getRobotSnapshotAtTimestamp(Job& j)
- {
- const IceUtil::Time time = IceUtil::Time::microSecondsDouble(j.time);
-
- if (!synchronized)
- {
- j.getRobotSnapshotAtTimestamp->ice_exception(
- NotInitializedException("Shared Robot is NULL", "getRobotSnapshot"));
- return;
- }
-
- VirtualRobot::RobotPtr clone = synchronized->clone(synchronized->getName());
- auto config = interpolate(time);
- if (!config)
- {
- ARMARX_WARNING << "Could not find or interpolate a robot state for time " << time.toDateTime();
- j.getRobotSnapshotAtTimestamp->ice_response(nullptr);
- return;
- }
-
- clone->setJointValues(config->jointMap);
- SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(parent), nullptr);
- p->setTimestamp(time);
- auto result = objectAdapter->addWithUUID(p);
- // Virtal robot clone is buggy -> set global pose here.
- p->setGlobalPose(config->globalPose);
-
- j.getRobotSnapshotAtTimestamp->ice_response(SharedRobotInterfacePrx::uncheckedCast(result));
- }
- void getJointConfigAtTimestamp(Job& j)
- {
- auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(j.time));
- j.getJointConfigAtTimestamp->ice_response(jointAngles ? jointAngles->value : NameValueMap());
- }
- void getRobotStateAtTimestamp(Job& j)
- {
- auto config = interpolate(IceUtil::Time::microSecondsDouble(j.time));
- j.getRobotStateAtTimestamp->ice_response(config ? *config : RobotStateConfig());
- }
- void reportPlatformPose(Job& j)
- {
- const Eigen::Matrix4f globalPose = j.currentPose.pose;
-
- IceUtil::Time time = IceUtil::Time::microSeconds(j.currentPose.header.timestampInMicroSeconds);
- insertPose(time, globalPose);
-
- if (sharedRobotServant)
- {
- sharedRobotServant->setTimestamp(time);
- }
- }
- void reportGlobalRobotPose(Job& j)
- {
- if (synchronized)
- {
- std::string localRobotName = synchronized->getName();
- ARMARX_DEBUG << "Comparing " << localRobotName
- << " and " << j.robotName << ".";
- if (localRobotName == j.robotName)
- {
- const IceUtil::Time time = IceUtil::Time::microSeconds(j.timestamp);
-
- insertPose(time, j.pose);
- synchronized->setGlobalPose(j.pose);
-
- if (sharedRobotServant)
- {
- sharedRobotServant->setTimestamp(time);
- }
- }
- }
- else
- {
- throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
- }
- }
- void reportJointAngles(Job& j)
- {
- if (j.timestamp <= 0)
- {
- j.timestamp = IceUtil::Time::now().toMicroSeconds();
- }
-
- IceUtil::Time time = IceUtil::Time::microSeconds(j.timestamp);
-
- ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << j.perJointValues
- << " from timestamp " << time.toDateTime()
- << " a value changed: " << j.aValueChanged;
-
- if (j.aValueChanged)
- {
- {
- WriteLockPtr lock = synchronized->getWriteLock();
-
- synchronized->setJointValues(j.perJointValues);
- }
-
- if (robotStateObs)
- {
- robotStateObs->updatePoses();
- }
- }
- if (sharedRobotServant)
- {
- sharedRobotServant->setTimestamp(time);
- }
-
- {
- jointHistory.emplace_hint(jointHistory.end(), time, j.perJointValues);
-
- if (jointHistory.size() > jointHistoryLength)
- {
- jointHistory.erase(jointHistory.begin());
- }
- }
-
- robotStateListenerPrx->reportJointValues(j.perJointValues, j.timestamp, j.aValueChanged);
- }
- void reportJointVelocities(Job& j)
- {
- if (robotStateObs)
- {
- robotStateObs->updateNodeVelocities(j.perJointValues, j.timestamp);
- }
- }
- void simulatorWasReset(Job& j)
- {
- poseHistory.clear();
- jointHistory.clear();
- }
-
- private:
- void
- insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
- {
- IceUtil::Time duration;
- {
- IceUtil::Time start = IceUtil::Time::now();
- duration = IceUtil::Time::now() - start;
-
- poseHistory.emplace_hint(poseHistory.end(),
- timestamp, new FramedPose(globalPose, GlobalFrame, ""));
-
- if (poseHistory.size() > poseHistoryLength)
- {
- poseHistory.erase(poseHistory.begin());
- }
- }
-
- if (robotStateObs)
- {
- robotStateObs->updatePoses();
- }
- }
- std::optional<RobotStateConfig>
- interpolate(IceUtil::Time time) const
- {
- auto joints = interpolateJoints(time);
- auto globalPose = interpolatePose(time);
-
- if (joints && globalPose)
- {
- RobotStateConfig config;
- // Use time stamp from interpolation.
- // config.timestamp = time.toMicroSeconds();
- config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
- config.jointMap = joints->value;
- config.globalPose = globalPose->value;
- return config;
- }
- else
- {
- return std::nullopt;
- }
- }
- auto
- interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
- {
- if (jointHistory.empty())
- {
- ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
- return std::nullopt;
- }
-
- const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
- if (time > newestTimeInHistory)
- {
- IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
- if (time <= newestTimeInHistory + maxOffset)
- {
- ARMARX_INFO << deactivateSpam(5)
- << "Requested joint timestamp is newer than newest available timestamp!"
- << "\n- requested timestamp: \t" << time.toDateTime()
- << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
- << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
- }
- else
- {
- ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
- << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
- << "\n- requested timestamp: \t" << time.toDateTime()
- << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
- return std::nullopt;
- }
-
- return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
- }
-
- // Get the oldest entry whose time >= time.
- auto nextIt = jointHistory.lower_bound(time);
- if (nextIt == jointHistory.end())
- {
- ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
- << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
- << "\n- newest available value: " << newestTimeInHistory.toDateTime();
- return std::nullopt;
- }
-
- if (nextIt == jointHistory.begin())
- {
- // Next was oldest entry.
- return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
- }
-
- const NameValueMap& next = nextIt->second;
- auto prevIt = std::prev(nextIt);
-
-
- // Interpolate.
-
- IceUtil::Time prevTime = prevIt->first;
- IceUtil::Time nextTime = nextIt->first;
-
- float t = static_cast<float>((time - prevTime).toSecondsDouble()
- / (nextTime - prevTime).toSecondsDouble());
-
- NameValueMap jointAngles;
- auto prevJointIt = prevIt->second.begin();
- for (const auto& [name, nextAngle] : next)
- {
- float value = (1 - t) * prevJointIt->second + t * nextAngle;
- prevJointIt++;
-
- jointAngles.emplace(name, value);
- }
-
- return Timestamped<NameValueMap> {time, std::move(jointAngles)};
- }
- auto
- interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
+ try
{
- if (poseHistory.empty())
- {
- ARMARX_INFO << deactivateSpam(10)
- << "Pose history is empty. This can happen if there is no platform unit.";
-
- ReadLockPtr readLock = synchronized->getReadLock();
-
- FramedPosePtr pose = new FramedPose(synchronized->getGlobalPose(), armarx::GlobalFrame, "");
- return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
- }
-
-
- const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
- if (time > newestTimeInHistory)
+ if (_synchronizedPrx)
{
- IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
- if (time <= newestTimeInHistory + maxOffset)
- {
- ARMARX_INFO << deactivateSpam(5)
- << "Requested pose timestamp is newer than newest available timestamp!"
- << "\n- requested timestamp: \t" << time.toDateTime()
- << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
- << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
- }
- else
- {
- ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
- << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
- << "\n- requested timestamp: \t" << time.toDateTime()
- << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
- return std::nullopt;
- }
- return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
- }
-
-
- auto nextIt = poseHistory.lower_bound(time);
- if (nextIt == poseHistory.end())
- {
- ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate platform pose for time " << time.toDateTime()
- << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
- << "\n- newest available value: " << newestTimeInHistory.toDateTime();
- return std::nullopt;
+ _synchronizedPrx->unref();
}
-
-
- if (nextIt == poseHistory.begin())
- {
- // Next was oldest entry.
- return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
- }
-
- auto prevIt = std::prev(nextIt);
- ARMARX_CHECK_EXPRESSION(prevIt->second);
-
- const FramedPosePtr& next = nextIt->second;
- const FramedPosePtr& prev = prevIt->second;
-
-
- // Interpolate.
-
- IceUtil::Time prevTime = prevIt->first;
- IceUtil::Time nextTime = nextIt->first;
-
- float t = static_cast<float>((time - prevTime).toSecondsDouble()
- / (nextTime - prevTime).toSecondsDouble());
-
- Eigen::Matrix4f globalPoseNext = next->toEigen();
- Eigen::Matrix4f globalPosePrev = prev->toEigen();
-
-
- Eigen::Matrix4f globalPose;
-
- math::Helpers::Position(globalPose) =
- (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
-
- Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
- Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
- Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
-
- math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
-
- return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
}
- };
-}
+ catch (...)
+ {}
+ }
-namespace armarx
-{
RobotStatePropertyDefinitions::RobotStatePropertyDefinitions(std::string prefix) :
ComponentPropertyDefinitions(prefix)
{
@@ -610,11 +77,6 @@ namespace armarx
}
- RobotStateComponent::RobotStateComponent()
- {
- _worker = std::make_unique<detail::RobotStateComponentWorker>();
- }
-
std::string RobotStateComponent::getDefaultName() const
{
return "RobotStateComponent";
@@ -623,9 +85,15 @@ namespace armarx
void RobotStateComponent::onInitComponent()
{
- ARMARX_CHECK_NOT_NULL(_worker);
robotStateTopicName = getProperty<std::string>("RobotStateReportingTopic").getValue();
offeringTopic(getProperty<std::string>("RobotStateReportingTopic"));
+ const std::size_t historyLength = static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
+
+ jointHistory.clear();
+ jointHistoryLength = historyLength;
+
+ poseHistory.clear();
+ poseHistoryLength = historyLength;
relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
@@ -634,34 +102,21 @@ namespace armarx
throw UserException("Could not find robot file " + robotFile);
}
- //setup worker
- {
- const std::size_t historyLength = static_cast<std::size_t>(getProperty<int>("HistoryLength").getValue());
- _worker->jointHistoryLength = historyLength;
- _worker->poseHistoryLength = historyLength;
- _worker->objectAdapter = getObjectAdapter();
- _worker->synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
- _worker->synchronized->setName(getProperty<std::string>("AgentName").getValue());
- }
-
- robotName = _worker->synchronized->getName();
+ this->_synchronized = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
+ _synchronized->setName(getProperty<std::string>("AgentName").getValue());
robotModelScaling = getProperty<float>("RobotModelScaling").getValue();
ARMARX_INFO << "scale factor: " << robotModelScaling;
if (robotModelScaling != 1.0f)
{
ARMARX_INFO << "Scaling robot model with scale factor " << robotModelScaling;
- _worker->synchronized =
- _worker->synchronized->clone(_worker->synchronized->getName(),
- _worker->synchronized->getCollisionChecker(),
- robotModelScaling);
+ _synchronized = _synchronized->clone(_synchronized->getName(), _synchronized->getCollisionChecker(), robotModelScaling);
}
- if (_worker->synchronized)
+ if (this->_synchronized)
{
- ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: "
- << _worker->synchronized->getName();
- _worker->synchronized->setPropagatingJointValuesEnabled(false);
+ ARMARX_VERBOSE << "Loaded robot from file " << robotFile << ". Robot name: " << this->_synchronized->getName();
+ this->_synchronized->setPropagatingJointValuesEnabled(false);
}
else
{
@@ -676,7 +131,7 @@ namespace armarx
throw UserException("RobotNodeSet not defined");
}
- VirtualRobot::RobotNodeSetPtr rns = _worker->synchronized->getRobotNodeSet(robotNodeSetName);
+ VirtualRobot::RobotNodeSetPtr rns = this->_synchronized->getRobotNodeSet(robotNodeSetName);
if (!rns)
{
@@ -735,89 +190,96 @@ namespace armarx
void RobotStateComponent::onConnectComponent()
{
- {
- getProxy(_worker->parent, -1);
- _worker->robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
-
- _worker->sharedRobotServant = new SharedRobotServant(_worker->synchronized,
- RobotStateComponentInterfacePrx::uncheckedCast(getProxy()),
- _worker->robotStateListenerPrx);
- _worker->sharedRobotServant->ref();
- _worker->sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
- _worker->synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_worker->sharedRobotServant));
+ robotStateListenerPrx = getTopic<RobotStateListenerInterfacePrx>(getProperty<std::string>("RobotStateReportingTopic"));
+ _sharedRobotServant = new SharedRobotServant(this->_synchronized, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), robotStateListenerPrx);
+ _sharedRobotServant->ref();
- if (_worker->robotStateObs)
- {
- _worker->robotStateObs->setRobot(_worker->synchronized);
- }
-
- _worker->startWorker();
- }
+ _sharedRobotServant->setRobotStateComponent(RobotStateComponentInterfacePrx::uncheckedCast(getProxy()));
+ this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
ARMARX_INFO << "Started RobotStateComponent" << flush;
+ if (robotStateObs)
+ {
+ robotStateObs->setRobot(_synchronized);
+ }
}
+
void RobotStateComponent::onDisconnectComponent()
{
- _worker->stopWorker();
}
- void RobotStateComponent::getSynchronizedRobot_async(
- const AMD_RobotStateComponentInterface_getSynchronizedRobotPtr& amd,
- const Ice::Current&) const
+
+ SharedRobotInterfacePrx RobotStateComponent::getSynchronizedRobot(const Current&) const
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::getSynchronizedRobot;
- j.getSynchronizedRobot = amd;
- _worker->addJob(std::move(j));
+ if (!this->_synchronizedPrx)
+ {
+ throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
+ }
+
+ return this->_synchronizedPrx;
}
- void RobotStateComponent::getRobotSnapshot_async(
- const AMD_RobotStateComponentInterface_getRobotSnapshotPtr& amd,
- const std::string& deprecated,
- const Ice::Current&)
+
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshot(const std::string& deprecated, const Current&)
{
(void) deprecated;
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotSnapshot;
- j.getRobotSnapshot = amd;
- _worker->addJob(std::move(j));
+
+ if (!_synchronized)
+ {
+ throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
+ }
+
+ auto clone = this->_synchronized->clone(_synchronized->getName());
+
+ SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
+ p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
+ auto result = getObjectAdapter()->addWithUUID(p);
+ // virtal robot clone is buggy -> set global pose here
+ p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
+ return SharedRobotInterfacePrx::uncheckedCast(result);
}
- void RobotStateComponent::getRobotSnapshotAtTimestamp_async(
- const AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr& amd,
- double time,
- const Ice::Current&)
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotSnapshotAtTimestamp;
- j.time = time;
- j.getRobotSnapshotAtTimestamp = amd;
- _worker->addJob(std::move(j));
+ const IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
+
+ if (!_synchronized)
+ {
+ throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
+ }
+
+ VirtualRobot::RobotPtr clone = this->_synchronized->clone(_synchronized->getName());
+ auto config = interpolate(time);
+ if (!config)
+ {
+ ARMARX_WARNING << "Could not find or interpolate a robot state for time " << time.toDateTime();
+ return nullptr;
+ }
+
+ clone->setJointValues(config->jointMap);
+ SharedRobotServantPtr p = new SharedRobotServant(clone, RobotStateComponentInterfacePrx::uncheckedCast(getProxy()), nullptr);
+ p->setTimestamp(time);
+ auto result = getObjectAdapter()->addWithUUID(p);
+ // Virtal robot clone is buggy -> set global pose here.
+ p->setGlobalPose(config->globalPose);
+
+ return SharedRobotInterfacePrx::uncheckedCast(result);
}
- void RobotStateComponent::getJointConfigAtTimestamp_async(
- const AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr& amd,
- double time,
- const Ice::Current&) const
+
+ NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::getJointConfigAtTimestamp;
- j.time = time;
- j.getJointConfigAtTimestamp = amd;
- _worker->addJob(std::move(j));
+ auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
+ return jointAngles ? jointAngles->value : NameValueMap();
}
- void RobotStateComponent::getRobotStateAtTimestamp_async(
- const AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr& amd,
- double time,
- const Ice::Current&) const
+
+ RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::getRobotStateAtTimestamp;
- j.time = time;
- j.getRobotStateAtTimestamp = amd;
- _worker->addJob(std::move(j));
+ auto config = interpolate(IceUtil::Time::microSecondsDouble(timestamp));
+ return config ? *config : RobotStateConfig();
}
+
std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
{
return relativeRobotFile;
@@ -833,17 +295,53 @@ namespace armarx
return robotInfo;
}
+
void RobotStateComponent::reportJointAngles(
const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current&)
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::reportJointAngles;
- j.perJointValues = jointAngles;
- j.timestamp = timestamp;
- j.aValueChanged = aValueChanged;
- _worker->addJob(std::move(j));
+ if (timestamp <= 0)
+ {
+ timestamp = IceUtil::Time::now().toMicroSeconds();
+ }
+
+ IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+
+ ARMARX_DEBUG << deactivateSpam(1) << "Got new jointangles: " << jointAngles
+ << " from timestamp " << time.toDateTime()
+ << " a value changed: " << aValueChanged;
+
+ if (aValueChanged)
+ {
+ {
+ WriteLockPtr lock = _synchronized->getWriteLock();
+
+ _synchronized->setJointValues(jointAngles);
+ }
+
+ if (robotStateObs)
+ {
+ robotStateObs->updatePoses();
+ }
+ }
+ if (_sharedRobotServant)
+ {
+ _sharedRobotServant->setTimestamp(time);
+ }
+
+ {
+ std::unique_lock lock(jointHistoryMutex);
+ jointHistory.emplace_hint(jointHistory.end(), time, jointAngles);
+
+ if (jointHistory.size() > jointHistoryLength)
+ {
+ jointHistory.erase(jointHistory.begin());
+ }
+ }
+
+ robotStateListenerPrx->reportJointValues(jointAngles, timestamp, aValueChanged);
}
+
void
RobotStateComponent::reportGlobalRobotPose(
const std::string& robotName,
@@ -851,21 +349,47 @@ namespace armarx
const long timestamp,
const Ice::Current&)
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::reportGlobalRobotPose;
- j.robotName = robotName;
- j.pose = pose;
- j.timestamp = timestamp;
- _worker->addJob(std::move(j));
+ if (_synchronized)
+ {
+ std::string localRobotName = _synchronized->getName();
+ ARMARX_DEBUG << "Comparing " << localRobotName << " and " << robotName << ".";
+ if (localRobotName == robotName)
+ {
+ const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+
+ insertPose(time, pose);
+ _synchronized->setGlobalPose(pose);
+
+ if (_sharedRobotServant)
+ {
+ _sharedRobotServant->setTimestamp(time);
+ }
+ }
+ }
+ else
+ {
+ throw NotInitializedException("Robot Ptr is NULL", "reportGlobalRobotPose");
+ }
}
void RobotStateComponent::reportPlatformPose(const PoseStamped& platformPose, const Current&)
+
+ void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::reportPlatformPose;
- j.currentPose = platformPose;
- _worker->addJob(std::move(j));
+ const float z = 0;
+ const Eigen::Vector3f position(currentPose.x, currentPose.y, z);
+ const Eigen::Matrix3f orientation =
+ Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+ const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
+
+ IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+ insertPose(time, globalPose);
+
+ if (_sharedRobotServant)
+ {
+ _sharedRobotServant->setTimestamp(time);
+ }
}
@@ -876,8 +400,8 @@ namespace armarx
void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
{
- ARMARX_CHECK_EXPRESSION(_worker);
- _worker->robotStateObs = observer;
+ // Unused.
+ (void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
}
@@ -912,22 +436,57 @@ namespace armarx
return result;
}
+ void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointModes, (void) timestamp, (void) aValueChanged;
+ }
void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Current&)
{
// Unused.
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::reportJointVelocities;
- j.perJointValues = jointVelocities;
- j.timestamp = timestamp;
- j.aValueChanged = aValueChanged;
- _worker->addJob(std::move(j));
+ (void) aValueChanged;
+ if (robotStateObs)
+ {
+ robotStateObs->updateNodeVelocities(jointVelocities, timestamp);
+ }
+ }
+ void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointTorques, (void) timestamp, (void) aValueChanged;
+ }
+
+ void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointAccelerations, (void) timestamp, (void) aValueChanged;
+ }
+ void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointCurrents, (void) timestamp, (void) aValueChanged;
+ }
+ void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointMotorTemperatures, (void) timestamp, (void) aValueChanged;
+ }
+ void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Current&)
+ {
+ // Unused.
+ (void) jointStatuses, (void) timestamp, (void) aValueChanged;
}
void RobotStateComponent::simulatorWasReset(const Current&)
{
- detail::RobotStateComponentWorker::Job j;
- j.type = detail::RobotStateComponentWorker::Job::JobType::simulatorWasReset;
- _worker->addJob(std::move(j));
+ {
+ std::lock_guard lock(poseHistoryMutex);
+ poseHistory.clear();
+ }
+ {
+ std::lock_guard lock(jointHistoryMutex);
+ jointHistory.clear();
+ }
}
PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
@@ -936,16 +495,22 @@ namespace armarx
getConfigIdentifier()));
}
+ void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
+ {
+ robotStateObs = observer;
+ }
+
std::string RobotStateComponent::getRobotName(const Current&) const
{
- if (!robotName.empty())
+ if (_synchronized)
{
- return robotName;
+ return _synchronized->getName(); // _synchronizedPrx->getName();
}
else
{
throw NotInitializedException("Robot Ptr is NULL", "getName");
}
+
}
std::string RobotStateComponent::getRobotNodeSetName(const Current&) const
@@ -961,4 +526,217 @@ namespace armarx
{
return robotStateTopicName;
}
+
+
+ void RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
+ {
+ IceUtil::Time duration;
+ {
+ IceUtil::Time start = IceUtil::Time::now();
+ std::unique_lock lock(poseHistoryMutex);
+ duration = IceUtil::Time::now() - start;
+
+ poseHistory.emplace_hint(poseHistory.end(),
+ timestamp, new FramedPose(globalPose, GlobalFrame, ""));
+
+ if (poseHistory.size() > poseHistoryLength)
+ {
+ poseHistory.erase(poseHistory.begin());
+ }
+ }
+
+ if (robotStateObs)
+ {
+ robotStateObs->updatePoses();
+ }
+ }
+
+ std::optional<RobotStateConfig> RobotStateComponent::interpolate(IceUtil::Time time) const
+ {
+ auto joints = interpolateJoints(time);
+ auto globalPose = interpolatePose(time);
+
+ if (joints && globalPose)
+ {
+ RobotStateConfig config;
+ // Use time stamp from interpolation.
+ // config.timestamp = time.toMicroSeconds();
+ config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
+ config.jointMap = joints->value;
+ config.globalPose = globalPose->value;
+ return config;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+ auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
+ {
+ std::shared_lock lock(jointHistoryMutex);
+ if (jointHistory.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Joint history of robot state component is empty!";
+ return std::nullopt;
+ }
+
+ const IceUtil::Time newestTimeInHistory = jointHistory.rbegin()->first;
+ if (time > newestTimeInHistory)
+ {
+ IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+ if (time <= newestTimeInHistory + maxOffset)
+ {
+ ARMARX_INFO << deactivateSpam(5)
+ << "Requested joint timestamp is newer than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
+ << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
+ << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
+ return std::nullopt;
+ }
+
+ return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
+ }
+
+ // Get the oldest entry whose time >= time.
+ auto nextIt = jointHistory.lower_bound(time);
+ if (nextIt == jointHistory.end())
+ {
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
+ << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "\n- newest available value: " << newestTimeInHistory.toDateTime();
+ return std::nullopt;
+ }
+
+ if (nextIt == jointHistory.begin())
+ {
+ // Next was oldest entry.
+ return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
+ }
+
+ const NameValueMap& next = nextIt->second;
+ auto prevIt = std::prev(nextIt);
+
+
+ // Interpolate.
+
+ IceUtil::Time prevTime = prevIt->first;
+ IceUtil::Time nextTime = nextIt->first;
+
+ float t = static_cast<float>((time - prevTime).toSecondsDouble()
+ / (nextTime - prevTime).toSecondsDouble());
+
+ NameValueMap jointAngles;
+ auto prevJointIt = prevIt->second.begin();
+ for (const auto& [name, nextAngle] : next)
+ {
+ float value = (1 - t) * prevJointIt->second + t * nextAngle;
+ prevJointIt++;
+
+ jointAngles.emplace(name, value);
+ }
+
+ return Timestamped<NameValueMap> {time, std::move(jointAngles)};
+ }
+
+ auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
+ {
+ std::shared_lock lock(poseHistoryMutex);
+
+ if (poseHistory.empty())
+ {
+ ARMARX_INFO << deactivateSpam(10)
+ << "Pose history is empty. This can happen if there is no platform unit.";
+
+ ReadLockPtr readLock = _synchronized->getReadLock();
+
+ FramedPosePtr pose = new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+ return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
+ }
+
+
+ const IceUtil::Time newestTimeInHistory = poseHistory.rbegin()->first;
+ if (time > newestTimeInHistory)
+ {
+ IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
+ if (time <= newestTimeInHistory + maxOffset)
+ {
+ ARMARX_INFO << deactivateSpam(5)
+ << "Requested pose timestamp is newer than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
+ << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
+ << maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
+ return std::nullopt;
+ }
+ return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
+ }
+
+
+ auto nextIt = poseHistory.lower_bound(time);
+ if (nextIt == poseHistory.end())
+ {
+ ARMARX_WARNING << deactivateSpam(1)
+ << "Could not find or interpolate platform pose for time " << time.toDateTime()
+ << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "\n- newest available value: " << newestTimeInHistory.toDateTime();
+ return std::nullopt;
+ }
+
+
+ if (nextIt == poseHistory.begin())
+ {
+ // Next was oldest entry.
+ return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
+ }
+
+ auto prevIt = std::prev(nextIt);
+ ARMARX_CHECK_EXPRESSION(prevIt->second);
+
+ const FramedPosePtr& next = nextIt->second;
+ const FramedPosePtr& prev = prevIt->second;
+
+
+ // Interpolate.
+
+ IceUtil::Time prevTime = prevIt->first;
+ IceUtil::Time nextTime = nextIt->first;
+
+ float t = static_cast<float>((time - prevTime).toSecondsDouble()
+ / (nextTime - prevTime).toSecondsDouble());
+
+ Eigen::Matrix4f globalPoseNext = next->toEigen();
+ Eigen::Matrix4f globalPosePrev = prev->toEigen();
+
+
+ Eigen::Matrix4f globalPose;
+
+ math::Helpers::Position(globalPose) =
+ (1 - t) * math::Helpers::Position(globalPosePrev) + t * math::Helpers::Position(globalPoseNext);
+
+ Eigen::Quaternionf rotNext(math::Helpers::Orientation(globalPoseNext));
+ Eigen::Quaternionf rotPrev(math::Helpers::Orientation(globalPosePrev));
+ Eigen::Quaternionf rotNew = rotPrev.slerp(t, rotNext);
+
+ math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
+
+ return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
+ }
+
+
+
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index f36f95e2fd954e40bf53cb6abf7dfbffe5cae896..aba4d1e828a5b55b9165fb6681ca291f4d2bb5fb 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -41,10 +41,6 @@
#include <shared_mutex>
#include <mutex>
-namespace armarx::detail
-{
- using RobotStateComponentWorkerPtr = std::unique_ptr<class RobotStateComponentWorker>;
-}
namespace armarx
{
@@ -86,7 +82,6 @@ namespace armarx
virtual public RobotStateComponentInterface
{
public:
- RobotStateComponent();
std::string getDefaultName() const override;
@@ -94,8 +89,7 @@ namespace armarx
// RobotStateComponentInterface interface
/// \return SharedRobotInterface proxy to the internal synchronized robot.
- void getSynchronizedRobot_async(const AMD_RobotStateComponentInterface_getSynchronizedRobotPtr& amd,
- const Ice::Current& = Ice::emptyCurrent) const override;
+ SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const override;
/**
* Creates a snapshot of the robot state at this moment in time.
@@ -103,18 +97,11 @@ namespace armarx
* \return Clone of the internal synchronized robot fixed to the configuration from the time of calling this function.
*/
// [[deprecated]]
- void getRobotSnapshot_async(const AMD_RobotStateComponentInterface_getRobotSnapshotPtr& amd,
- const std::string& deprecated,
- const Ice::Current& = Ice::emptyCurrent) override;
- void getRobotSnapshotAtTimestamp_async(const AMD_RobotStateComponentInterface_getRobotSnapshotAtTimestampPtr& amd,
- double time,
- const Ice::Current& = Ice::emptyCurrent) override;
- void getJointConfigAtTimestamp_async(const AMD_RobotStateComponentInterface_getJointConfigAtTimestampPtr& amd,
- double time,
- const Ice::Current& = Ice::emptyCurrent) const override;
- void getRobotStateAtTimestamp_async(const AMD_RobotStateComponentInterface_getRobotStateAtTimestampPtr& amd,
- double time,
- const Ice::Current& = Ice::emptyCurrent) const override;
+ SharedRobotInterfacePrx getRobotSnapshot(const std::string& deprecated, const Ice::Current&) override;
+
+ SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current) override;
+ NameValueMap getJointConfigAtTimestamp(double timestamp, const Ice::Current&) const override;
+ RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const override;
/// \return the robot xml filename as specified in the configuration
std::string getRobotFilename(const Ice::Current&) const override;
@@ -148,8 +135,11 @@ namespace armarx
/// Does nothing.
void reportPlatformOdometryPose(const PoseStamped& odometryPose, const Ice::Current&) override;
- //called from application
+
+ // Own interface.
void setRobotStateObserver(RobotStateObserverPtr observer);
+
+
protected:
// Component interface.
@@ -164,6 +154,9 @@ namespace armarx
void onConnectComponent() override;
void onDisconnectComponent() override;
+ /// Calls unref() on RobotStateComponent::_synchronizedPrx.
+ ~RobotStateComponent() override;
+
/// Create an instance of RobotStatePropertyDefinitions.
PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -171,40 +164,77 @@ namespace armarx
// Inherited from KinematicUnitInterface
/// Does nothing.
- void reportControlModeChanged(const NameControlModeMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Stores the reported joint angles in the joint history and publishes the new joint angles.
void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Sends the joint velocities to the robot state observer.
void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointTorques(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointAccelerations(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c) override;
/// Does nothing.
- void reportJointCurrents(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointMotorTemperatures(const NameValueMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
/// Does nothing.
- void reportJointStatuses(const NameStatusMap&, Ice::Long, bool, const Ice::Current&) override {}
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = Ice::emptyCurrent) override;
void simulatorWasReset(const Ice::Current& = Ice::emptyCurrent) override;
private:
void readRobotInfo(const std::string& robotFile);
RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+
+ void insertPose(IceUtil::Time timestamp, const Eigen::Matrix4f& globalPose);
+
+
+ template <class ValueT>
+ struct Timestamped
+ {
+ IceUtil::Time timestamp;
+ ValueT value;
+ };
+
+ /// Interpolate the robot state from histories and store it in `config`.
+ std::optional<RobotStateConfig> interpolate(IceUtil::Time time) const;
+ /// Interpolate the joint angles from history and store it in `jointAngles`.
+ std::optional<Timestamped<NameValueMap>> interpolateJoints(IceUtil::Time time) const;
+ /// Interpolate the robot pose from history and store it in `pose`.
+ std::optional<Timestamped<FramedPosePtr>> interpolatePose(IceUtil::Time time) const;
+
+
private:
+ /// Local robot model.
+ VirtualRobot::RobotPtr _synchronized;
+ /// Local shared robot.
+ SharedRobotServantPtr _sharedRobotServant;
+ /// Ice proxy to `_sharedRobotServant`.
+ SharedRobotInterfacePrx _synchronizedPrx;
+
+ RobotStateListenerInterfacePrx robotStateListenerPrx;
+ RobotStateObserverPtr robotStateObs;
+
std::string robotStateTopicName;
std::string robotFile;
std::string relativeRobotFile;
- std::string robotName;
+
+ mutable std::shared_mutex jointHistoryMutex;
+ std::map<IceUtil::Time, NameValueMap> jointHistory;
+ size_t jointHistoryLength;
+
+ mutable std::shared_mutex poseHistoryMutex;
+ std::map<IceUtil::Time, FramedPosePtr> poseHistory;
+ size_t poseHistoryLength;
+
std::string robotNodeSetName;
float robotModelScaling;
RobotInfoNodePtr robotInfo;
- detail::RobotStateComponentWorkerPtr _worker;
};
+
}
diff --git a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
index 6dd944d27b72db26da41223bdd35b05766331748..2636ee8ade97c82c97a40a93d4741bddfa466569 100644
--- a/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArMemMemoryViewer/ArMemMemoryViewerWidgetController.cpp
@@ -271,7 +271,12 @@ namespace armarx
const armem::EntitySnapshot* snapshot = nullptr;
if (!id.hasTimestamp())
{
- snapshot = &memoryData->getEntity(id).getLatestSnapshot();
+ const armem::Entity& entity = memoryData->getEntity(id);
+ if (entity.empty())
+ {
+ return;
+ }
+ snapshot = &entity.getLatestSnapshot();
id.timestamp = snapshot->time();
}
if (!id.hasInstanceIndex())
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
index a90f9ebec033fe01b2ea927b00283106bc10c872..890f754e5a2e26d8c22226868c5bcbbc98ee5a98 100644
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidget.ui
@@ -54,7 +54,7 @@
<string>Left</string>
</property>
<property name="checked">
- <bool>true</bool>
+ <bool>false</bool>
</property>
</widget>
</item>
@@ -63,6 +63,9 @@
<property name="text">
<string>Right</string>
</property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
</widget>
</item>
<item>
@@ -211,64 +214,111 @@
<widget class="QWidget" name="widgetTarget" native="true">
<layout class="QGridLayout" name="gridLayout_2">
<item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRX">
+ <property name="minimum">
+ <double>-100.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
</item>
<item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRZ">
+ <property name="minimum">
+ <double>-100.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
</item>
<item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTargRY">
+ <property name="minimum">
+ <double>-100.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.010000000000000</double>
+ </property>
+ </widget>
</item>
<item row="0" column="3">
<widget class="QDoubleSpinBox" name="doubleSpinBoxTargTZ"/>
</item>
<item row="2" column="0" colspan="4">
<layout class="QGridLayout" name="gridLayout_3">
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButtonTargSet">
+ <item row="1" column="0">
+ <widget class="QPushButton" name="pushButtonTargGet">
<property name="text">
- <string>Set Pose</string>
+ <string>Set to current</string>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QPushButton" name="pushButtonTargAdd">
+ <item row="3" column="2">
+ <widget class="QRadioButton" name="radioButtonVisuAdd">
<property name="text">
<string>Add Pose</string>
</property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
</widget>
</item>
- <item row="1" column="0">
+ <item row="3" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Visu</string>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButtonTargGet">
+ <item row="3" column="1">
+ <widget class="QRadioButton" name="radioButtonVisuSet">
<property name="text">
- <string>Set to current</string>
+ <string>Set Pose</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QRadioButton" name="radioButtonVisuAdd">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="pushButtonTargSet">
<property name="text">
- <string>Add Pose</string>
+ <string>Set Pose</string>
</property>
- <property name="checked">
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QPushButton" name="pushButtonTargAdd">
+ <property name="enabled">
<bool>false</bool>
</property>
+ <property name="text">
+ <string>Add Pose</string>
+ </property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QRadioButton" name="radioButtonVisuSet">
+ <item row="2" column="2">
+ <widget class="QCheckBox" name="checkBoxEnableAddPose">
<property name="text">
- <string>Set Pose</string>
+ <string>enable add pose</string>
</property>
- <property name="checked">
- <bool>true</bool>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QCheckBox" name="checkBoxAutoUpdatePose">
+ <property name="text">
+ <string>Auto set pose</string>
</property>
</widget>
</item>
@@ -394,8 +444,8 @@
<y>121</y>
</hint>
<hint type="destinationlabel">
- <x>195</x>
- <y>149</y>
+ <x>207</x>
+ <y>139</y>
</hint>
</hints>
</connection>
@@ -439,7 +489,7 @@
<hints>
<hint type="sourcelabel">
<x>590</x>
- <y>43</y>
+ <y>41</y>
</hint>
<hint type="destinationlabel">
<x>281</x>
@@ -479,5 +529,21 @@
</hint>
</hints>
</connection>
+ <connection>
+ <sender>checkBoxEnableAddPose</sender>
+ <signal>clicked(bool)</signal>
+ <receiver>pushButtonTargAdd</receiver>
+ <slot>setEnabled(bool)</slot>
+ <hints>
+ <hint type="sourcelabel">
+ <x>449</x>
+ <y>281</y>
+ </hint>
+ <hint type="destinationlabel">
+ <x>461</x>
+ <y>259</y>
+ </hint>
+ </hints>
+ </connection>
</connections>
</ui>
diff --git a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
index 502caf22f0a3dfa746df260967fd459f681de7a7..6b34ac81d1d2bd48e8f6bdaae2cbf2bd2b711b26 100644
--- a/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianImpedanceController/CartesianImpedanceControllerWidgetController.cpp
@@ -114,12 +114,18 @@ namespace armarx
{
synchronizeLocalClone(_robot);
}
+ if (_ui.checkBoxAutoUpdatePose->isChecked())
+ {
+ ARMARX_TRACE;
+ on_pushButtonTargSet_clicked();
+ }
const bool lockY = _ui.checkBoxLockY->isChecked();
const bool lockZ = _ui.checkBoxLockZ->isChecked();
bool anyTracking = lockY || lockZ;
_ui.widgetTarget->setEnabled(!anyTracking);
if (_rns && (lockY || lockZ))
{
+ ARMARX_TRACE;
Eigen::Matrix4f pose = _rns->getTCP()->getPoseInRootFrame();
if (lockY)
{
@@ -154,9 +160,12 @@ namespace armarx
{
ARMARX_TRACE;
std::lock_guard g{_allMutex};
- if (_visuHandRobotFile.empty() || !_robot)
+ if (_visuHandRobotFile.empty() || !_robot || !arviz.topic())
{
- arviz.commitLayerContaining("hand");
+ if (arviz.topic())
+ {
+ arviz.commitLayerContaining("hand");
+ }
return;
}
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 1beb3e37d11fd1fe1f4478e8c657766048928b28..332959df89b7c0c4fe55ce2442f240d29e45213e 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -198,28 +198,28 @@ module armarx
/**
* @return proxy to the shared robot which constantly updates all joint values
*/
- ["cpp:const", "amd"]
+ ["cpp:const"]
idempotent
SharedRobotInterface* getSynchronizedRobot() throws NotInitializedException;
/**
* @return proxy to a copy of the shared robot with non updating joint values
*/
- ["amd"] SharedRobotInterface* getRobotSnapshot(string deprecated) throws NotInitializedException;
+ SharedRobotInterface* getRobotSnapshot(string deprecated) throws NotInitializedException;
/**
* Create robot snapshot proxy from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
* @return Snapshot
*
* */
- ["amd"] SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
+ SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
/**
* Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
* @return Jointname-jointvalue map
*
* */
- ["cpp:const", "amd"]
+ ["cpp:const"]
idempotent
NameValueMap getJointConfigAtTimestamp(double time) throws NotInitializedException;
@@ -228,7 +228,7 @@ module armarx
* @return Robot configuration containing jointvalue map and globalpose
*
* */
- ["cpp:const", "amd"]
+ ["cpp:const"]
idempotent
RobotStateConfig getRobotStateAtTimestamp(double time) throws NotInitializedException;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index f9ef01e542e65c421fb5a80e82a8f4d23815c81d..2f039d60c1343421a5a22bc576eec0ea7b3fc456 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -516,6 +516,7 @@ module armarx
bool isManipulability = false;
Ice::DoubleSeq kmani;
Ice::DoubleSeq positionManipulability;
+ Ice::DoubleSeq maniWeight;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
index 91318db432c554018bced9906a407c1e7738d9f5..800eccc800ea2ad6b8e6e2bb796c95eac73f4d91 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.cpp
@@ -28,26 +28,26 @@ namespace armarx
kxyz.addWidget(ui.doubleSpinBoxKTX);
kxyz.addWidget(ui.doubleSpinBoxKTY);
kxyz.addWidget(ui.doubleSpinBoxKTZ);
- kxyz.setMinMax(0, 1000);
- kxyz.set(500);
+ kxyz.setMinMax(0, 5000);
+ kxyz.set(1000);
krpy.addWidget(ui.doubleSpinBoxKRX);
krpy.addWidget(ui.doubleSpinBoxKRY);
krpy.addWidget(ui.doubleSpinBoxKRZ);
- krpy.setMinMax(0, 1000);
- krpy.set(1);
+ krpy.setMinMax(0, 5000);
+ krpy.set(500);
dxyz.addWidget(ui.doubleSpinBoxDTX);
dxyz.addWidget(ui.doubleSpinBoxDTY);
dxyz.addWidget(ui.doubleSpinBoxDTZ);
- dxyz.setMinMax(0, 1000);
- dxyz.set(0);
+ dxyz.setMinMax(0, 5000);
+ dxyz.set(250);
drpy.addWidget(ui.doubleSpinBoxDRX);
drpy.addWidget(ui.doubleSpinBoxDRY);
drpy.addWidget(ui.doubleSpinBoxDRZ);
- drpy.setMinMax(0, 1000);
- drpy.set(0);
+ drpy.setMinMax(0, 5000);
+ drpy.set(100);
using T = CartesianImpedanceControllerConfigWidget;
connect(ui.pushButtonNullspaceUpdateJoints, &QPushButton::clicked, this, &T::on_pushButtonNullspaceUpdateJoints_clicked);
@@ -218,7 +218,7 @@ namespace armarx
lay->addWidget(b, i, 2);
b->setMinimum(0);
b->setMaximum(1000);
- b->setValue(1);
+ b->setValue(2);
}
{
auto b = new QDoubleSpinBox;
@@ -226,12 +226,12 @@ namespace armarx
lay->addWidget(b, i, 3);
b->setMinimum(0);
b->setMaximum(1000);
- b->setValue(0);
+ b->setValue(1);
}
++i;
}
- jointKnull.set(5);
- jointDnull.set(0);
+ jointKnull.set(2);
+ jointDnull.set(1);
on_pushButtonNullspaceUpdateJoints_clicked();
}
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
index 3d3e53610438552d803aaf65c999c698c313c2fe..669892bd956030629ddc31abc71804c4c8e31140 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
+++ b/source/RobotAPI/libraries/RobotAPINJointControllerWidgets/CartesianImpedanceControllerConfigWidget.ui
@@ -30,23 +30,45 @@
<widget class="QWidget" name="widget" native="true">
<layout class="QGridLayout" name="gridLayout_3">
<item row="2" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRX"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKRX">
+ <property name="toolTip">
+ <string>200 - 800 (use 500)</string>
+ </property>
+ </widget>
</item>
<item row="4" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRZ"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDRZ">
+ <property name="toolTip">
+ <string>50 - 500</string>
+ </property>
+ </widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_5">
+ <property name="toolTip">
+ <string>200 - 800 (use 500)</string>
+ </property>
<property name="text">
<string>K RPY</string>
</property>
</widget>
</item>
<item row="5" column="1" colspan="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxTorqueLim"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxTorqueLim">
+ <property name="maximum">
+ <double>1000.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>20.000000000000000</double>
+ </property>
+ </widget>
</item>
<item row="1" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTX"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKTX">
+ <property name="toolTip">
+ <string>500 - 2000 (use 1000)</string>
+ </property>
+ </widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_3">
@@ -56,13 +78,20 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="toolTip">
+ <string>500 - 2000 (use 1000)</string>
+ </property>
<property name="text">
<string>K XYZ</string>
</property>
</widget>
</item>
<item row="2" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRY"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKRY">
+ <property name="toolTip">
+ <string>200 - 800 (use 500)</string>
+ </property>
+ </widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_6">
@@ -72,16 +101,32 @@
</widget>
</item>
<item row="1" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTY"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKTY">
+ <property name="toolTip">
+ <string>500 - 2000 (use 1000)</string>
+ </property>
+ </widget>
</item>
<item row="4" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRY"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDRY">
+ <property name="toolTip">
+ <string>50 - 500</string>
+ </property>
+ </widget>
</item>
<item row="4" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDRX"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDRX">
+ <property name="toolTip">
+ <string>50 - 500</string>
+ </property>
+ </widget>
</item>
<item row="3" column="1">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTX"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDTX">
+ <property name="toolTip">
+ <string>100 - 500</string>
+ </property>
+ </widget>
</item>
<item row="0" column="0" colspan="4">
<widget class="QGroupBox" name="groupBox_3">
@@ -122,6 +167,9 @@
</item>
<item row="0" column="3">
<widget class="QLabel" name="label_10">
+ <property name="toolTip">
+ <string>0.5 - 1</string>
+ </property>
<property name="text">
<string>Dnull</string>
</property>
@@ -129,6 +177,9 @@
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_12">
+ <property name="toolTip">
+ <string>1 - 10</string>
+ </property>
<property name="text">
<string>Knull</string>
</property>
@@ -157,30 +208,52 @@
</widget>
</item>
<item row="3" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTZ"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDTZ">
+ <property name="toolTip">
+ <string>100 - 500</string>
+ </property>
+ </widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_2">
+ <property name="toolTip">
+ <string>50 - 500</string>
+ </property>
<property name="text">
<string>D RPY</string>
</property>
</widget>
</item>
<item row="3" column="2">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxDTY"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxDTY">
+ <property name="toolTip">
+ <string>100 - 500</string>
+ </property>
+ </widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_4">
+ <property name="toolTip">
+ <string>100 - 500</string>
+ </property>
<property name="text">
<string>D XYZ</string>
</property>
</widget>
</item>
<item row="2" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKRZ"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKRZ">
+ <property name="toolTip">
+ <string>200 - 800 (use 500)</string>
+ </property>
+ </widget>
</item>
<item row="1" column="3">
- <widget class="QDoubleSpinBox" name="doubleSpinBoxKTZ"/>
+ <widget class="QDoubleSpinBox" name="doubleSpinBoxKTZ">
+ <property name="toolTip">
+ <string>500 - 2000 (use 1000)</string>
+ </property>
+ </widget>
</item>
<item row="6" column="0" colspan="4">
<widget class="QPushButton" name="pushButtonSettingsSend">
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index ccd553f342786bc813a913b80e9b95b0feb4cf46..79a090adbae49e70b400ca3cd358b0599a11c9f1 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -130,7 +130,22 @@ namespace armarx
isManipulability = cfg->isManipulability;
- auto manipulability = std::make_shared<VirtualRobot::SingleRobotNodeSetManipulability>(rns, rns->getTCP(), VirtualRobot::AbstractManipulability::Mode::Position, VirtualRobot::AbstractManipulability::Type::Velocity, rns->getRobot()->getRootNode());
+
+
+ ARMARX_CHECK_EQUAL(cfg->maniWeight.size(), rns->getNodeNames().size());
+ Eigen::VectorXd maniWeightVec;
+ maniWeightVec.setOnes(rns->getNodeNames().size());
+ for (size_t i = 0; i < cfg->maniWeight.size(); ++i)
+ {
+ maniWeightVec(i) = cfg->maniWeight[i];
+ }
+
+ Eigen::MatrixXd maniWeightMat = maniWeightVec.asDiagonal();
+ VirtualRobot::SingleRobotNodeSetManipulabilityPtr manipulability(
+ new VirtualRobot::SingleRobotNodeSetManipulability(rns, rns->getTCP(),
+ VirtualRobot::AbstractManipulability::Mode::Position,
+ VirtualRobot::AbstractManipulability::Type::Velocity,
+ rns->getRobot()->getRootNode(), maniWeightMat));
manipulabilityTracker.reset(new VirtualRobot::SingleChainManipulabilityTracking(manipulability));
ARMARX_CHECK_EQUAL(cfg->positionManipulability.size(), 9);
@@ -469,7 +484,12 @@ namespace armarx
// targetPose.block<3, 3>(0, 0) = rotVel * targetPose.block<3, 3>(0, 0);
// inverse dynamic controller
- jacobi.block<3, 8>(0, 0) = 0.001 * jacobi.block<3, 8>(0, 0); // convert mm to m
+
+ for (size_t ki = 0; ki < 3; ++ki)
+ {
+ jacobi.row(ki) = 0.001 * jacobi.row(ki); // convert mm to m
+
+ }
@@ -516,7 +536,7 @@ namespace armarx
float manidist = 0;
if (isManipulability)
{
- nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani);
+ nullspaceTorque = manipulabilityTracker->calculateVelocity(targetManipulability, kmani, true);
manidist = manipulabilityTracker->computeDistance(targetManipulability);
}
else
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.cpp b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
index 664319410949b11c7563512a6ccc22dc0c2dd3e1..61dd4836ca9f16ffd6b36342facbd2f610c75ff3 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.cpp
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.cpp
@@ -1,20 +1,45 @@
#include "MemoryID.h"
-#include <SimoxUtility/algorithm/string.h>
-
#include "error/ArMemError.h"
+#include <SimoxUtility/algorithm/advanced.h>
+#include <SimoxUtility/algorithm/string/string_tools.h>
+
+#include <boost/algorithm/string.hpp>
+
+#include <forward_list>
+
namespace armarx::armem
{
+ const std::string MemoryID::delimiter = "/";
+
+
MemoryID::MemoryID()
{
}
MemoryID::MemoryID(const std::string& string)
{
- std::vector<std::string> items = simox::alg::split(string, "/", false, false);
+ std::forward_list<std::string> items;
+ boost::split(items, string, boost::is_any_of(delimiter));
+
+ // Handle escaped /'s
+ for (auto it = items.begin(); it != items.end(); ++it)
+ {
+ while (it->size() > 0 && it->back() == '\\')
+ {
+ // The / causing the split was escaped. Merge the items together.
+ auto next = simox::alg::advanced(it, 1);
+ if (next != items.end())
+ {
+ // "it\" + "next" => "it/next"
+ *it = it->substr(0, it->size() - 1) + delimiter + *next;
+ items.erase_after(it); // Invalidates `next`, but not `it`.
+ }
+ }
+ }
auto it = items.begin();
if (it == items.end())
@@ -55,19 +80,19 @@ namespace armarx::armem
}
- std::string MemoryID::str() const
+ std::string MemoryID::str(bool escapeDelimiters) const
{
- return str("/");
+ return str(delimiter, escapeDelimiters);
}
- std::string MemoryID::str(const std::string& delimeter) const
+ std::string MemoryID::str(const std::string& delimiter, bool escapeDelimiter) const
{
- std::vector<std::string> items = getAllItems();
+ std::vector<std::string> items = getAllItems(escapeDelimiter);
while (items.size() > 0 && items.back().empty())
{
items.pop_back();
}
- return simox::alg::join(items, delimeter, false, false);
+ return simox::alg::join(items, delimiter, false, false);
}
std::string MemoryID::getLeafItem() const
@@ -86,52 +111,55 @@ namespace armarx::armem
bool MemoryID::hasGap() const
{
bool emptyFound = false;
- for (const std::string& name : getAllItems())
+ for (const std::string& item : getAllItems())
{
- if (name.empty())
+ if (item.empty())
{
emptyFound = true;
}
- else
+ else if (emptyFound)
{
- if (emptyFound)
- {
- return true;
- }
+ // Found a non-empty item after an empty item.
+ return true;
}
}
return false;
}
+ bool MemoryID::isWellDefined() const
+ {
+ return !hasGap();
+ }
+
MemoryID MemoryID::fromString(const std::string& string)
{
return MemoryID(string);
}
- std::vector<std::string> MemoryID::getItems() const
+ std::vector<std::string> MemoryID::getItems(bool escapeDelimiters) const
{
std::vector<std::string> items;
- items.push_back(memoryName);
+ items.push_back(escape(memoryName, escapeDelimiters));
if (!hasCoreSegmentName())
{
return items;
}
- items.push_back(coreSegmentName);
+ items.push_back(escape(coreSegmentName, escapeDelimiters));
if (!hasProviderSegmentName())
{
return items;
}
- items.push_back(providerSegmentName);
+ items.push_back(escape(providerSegmentName, escapeDelimiters));
if (!hasEntityName())
{
return items;
}
- items.push_back(entityName);
+ items.push_back(escape(entityName, escapeDelimiters));
if (!hasTimestamp())
{
@@ -148,11 +176,12 @@ namespace armarx::armem
return items;
}
- std::vector<std::string> MemoryID::getAllItems() const
+ std::vector<std::string> MemoryID::getAllItems(bool escapeDelimiters) const
{
return
{
- memoryName, coreSegmentName, providerSegmentName, entityName,
+ escape(memoryName, escapeDelimiters), escape(coreSegmentName, escapeDelimiters),
+ escape(providerSegmentName, escapeDelimiters), escape(entityName, escapeDelimiters),
timestampStr(), instanceIndexStr()
};
}
@@ -276,6 +305,7 @@ namespace armarx::armem
return id;
}
+
std::string MemoryID::timestampStr() const
{
return hasTimestamp() ? std::to_string(timestamp.toMicroSeconds()) : "";
@@ -306,8 +336,46 @@ namespace armarx::armem
and instanceIndex == other.instanceIndex;
}
+ bool MemoryID::operator<(const MemoryID& rhs) const
+ {
+ int c = memoryName.compare(rhs.memoryName);
+ if (c != 0)
+ {
+ return c < 0;
+ }
+ // Equal memory name
+ c = coreSegmentName.compare(rhs.coreSegmentName);
+ if (c != 0)
+ {
+ return c < 0;
+ }
+ // Equal core segment ID
+ c = providerSegmentName.compare(rhs.providerSegmentName);
+ if (c != 0)
+ {
+ return c < 0;
+ }
+ // Equal provider segment ID
+ c = entityName.compare(rhs.entityName);
+ if (c != 0)
+ {
+ return c < 0;
+ }
+ // Equal entity ID
+ if (timestamp != rhs.timestamp)
+ {
+ return timestamp < rhs.timestamp;
+ }
+ // Equal entity snapshot ID
+ return instanceIndex < rhs.instanceIndex;
+ }
+
long long MemoryID::parseInteger(const std::string& string, const std::string& semanticName)
{
+ if (string.empty())
+ {
+ return -1;
+ }
try
{
return std::stoll(string);
@@ -322,15 +390,44 @@ namespace armarx::armem
}
}
+ std::string MemoryID::escapeDelimiter(const std::string& name)
+ {
+ return simox::alg::replace_all(name, delimiter, "\\" + delimiter);
+ }
+
+ std::string MemoryID::escape(const std::string& name, bool escapeDelimiters)
+ {
+ if (escapeDelimiters)
+ {
+ return escapeDelimiter(name);
+ }
+ else
+ {
+ return name;
+ }
+ }
+
std::ostream& operator<<(std::ostream& os, const MemoryID id)
{
return os << "'" << id.str() << "'";
}
- bool
- contains(const MemoryID& general, const MemoryID& specific)
+ bool contains(const MemoryID& general, const MemoryID& specific)
{
+ if (!general.isWellDefined())
+ {
+ std::stringstream ss;
+ ss << "ID `general` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ throw error::InvalidMemoryID(general, ss.str());
+ }
+ if (!specific.isWellDefined())
+ {
+ std::stringstream ss;
+ ss << "ID `specific` is not well-defined, which is required for `" << __FUNCTION__ << "()`.";
+ throw error::InvalidMemoryID(specific, ss.str());
+ }
+
if (general.memoryName.empty())
{
return true;
diff --git a/source/RobotAPI/libraries/armem/core/MemoryID.h b/source/RobotAPI/libraries/armem/core/MemoryID.h
index 1fc22f7288b51970f298a9309cbe33b087d48f5f..09415ec4abd73185f77050c89683b326f3c3e9d0 100644
--- a/source/RobotAPI/libraries/armem/core/MemoryID.h
+++ b/source/RobotAPI/libraries/armem/core/MemoryID.h
@@ -12,11 +12,37 @@ namespace armarx::armem
/**
* @brief A memory ID.
*
- * Structure:
- * `MemoryName/CoreSegmentName/ProviderSegmentName/EntityName/Timestamp/InstanceIndex`
+ * A memory ID is an index into the hierarchical memory structure.
+ * It specifies the keys for the different levels, starting from the
+ * memory name and ending at the instance index.
*
- * Example:
- * `VisionMemory/RGBImages/PrimesenseRGB/image/1245321323/0`
+ * A memory ID need not be complete, e.g. it may specify only the memory
+ * and core segment names (thus representing a core segment ID).
+ * A memory ID that fully identifies a level starting from the memory is
+ * called well-defined.
+ * @see `isWellDefined()`
+ *
+ * Memory IDs can be encoded in strings using a delimiter:
+ * - Structure: "MemoryName/CoreSegmentName/ProviderSegmentName/EntityName/Timestamp/InstanceIndex"
+ * - Example: "Vision/RGBImages/Primesense/image/1245321323/0"
+ * @see `str()`
+ *
+ * If an ID does not specify the lower levels, these parts can be omitted.
+ * Thus, an entity ID could look like:
+ * - Structure: "MemoryName/CoreSegmentName/ProviderSegmentName/EntityName"
+ * - Example: "Vision/RGBImages/Primesense/image"
+ *
+ * If a name contains a "/", it will be escaped:
+ * - Example: "Vision/RGBImages/Primesense/my\/entity\/with\/slashes"
+ *
+ * Memory IDs may be not well-defined. This can occur e.g. when preparing
+ * an entity instance ID which is still pending the timestamp.
+ * It could look like (note the missing timestamp):
+ * - Example: "Vision/RGBImages/Primesense/image//0"
+ *
+ * These IDs are still valid and can be handled (encoded as string etc.).
+ * However, some operations may not be well-defined for non-well-defined IDs
+ * (such as `contains()`).
*/
class MemoryID
{
@@ -30,26 +56,37 @@ namespace armarx::armem
int instanceIndex = -1;
-
public:
+ /// Construct a default (empty) memory ID.
MemoryID();
+ /// (Re-)Construct a memory ID from a string representation as returned by `str()`.
explicit MemoryID(const std::string& string);
- std::string str() const;
- std::string str(const std::string& delimeter) const;
- static MemoryID fromString(const std::string& string);
-
- std::string timestampStr() const;
- std::string instanceIndexStr() const;
-
-
- /// Get all levels as string.
- std::vector<std::string> getAllItems() const;
- /// Get the levels from root to first not defined level (excluding).
- std::vector<std::string> getItems() const;
-
+ /**
+ * @brief Indicate whether this ID is well-defined.
+ *
+ * A well-defined ID has no specified level after a non-specified level (i.e., no gaps).
+ *
+ * Well-defined examples:
+ * - "" (empty, but well-defined)
+ * - "Memory" (a memory ID)
+ * - "Memory/Core" (a core segment ID)
+ * - "Memory/Core/Provider" (a provider segment ID)
+ *
+ * Non-well-defined examples:
+ * - "Memory//Provider" (no core segment name)
+ * - "/Core" (no memory name)
+ * - "Mem/Core/Prov/entity//0" (no timestamp)
+ * - "///entity//0" (no memory, core segment and provider segment names)
+ *
+ * @return True if `*this` is a well-defined memory ID.
+ */
+ bool isWellDefined() const;
+
+
+ // Checks whether a specific level is specified.
bool hasMemoryName() const
{
@@ -113,15 +150,49 @@ namespace armarx::armem
MemoryID withInstanceIndex(int index) const;
+ // String conversion
+
+ /**
+ * @brief Get a string representation of this memory ID.
+ *
+ * Items are separated by a delimiter. If `escapeDelimiter` is true,
+ * delimiters occuring inside names are escaped with backward slashes.
+ * This allows to reconstruct the memory ID from the result of `str()`
+ * in these cases.
+ *
+ * @param escapeDelimiter If true, escape delimiters inside names
+ * @return A string representation of this MemoryID.
+ */
+ std::string str(bool escapeDelimiters = true) const;
+
+ /// Get the timestamp as string.
+ std::string timestampStr() const;
+ /// Get the instance index as string.
+ std::string instanceIndexStr() const;
+
+ /// Alias for constructor from string.
+ static MemoryID fromString(const std::string& string);
+ /// Reconstruct a timestamp from a string as returned by `timestampStr()`.
+ static Time timestampFromStr(const std::string& timestamp);
+ /// Reconstruct an instance index from a string as returned by `instanceIndexStr()`.
+ static int instanceIndexFromStr(const std::string& index);
+
+
+ /// Get all levels as strings.
+ std::vector<std::string> getAllItems(bool escapeDelimiters = false) const;
+ /// Get the levels from root to first not defined level (excluding).
+ std::vector<std::string> getItems(bool escapeDelimiters = false) const;
+
+
+ // Other utility.
+
/// Indicate whether this ID has a gap such as in 'Memory//MyProvider' (no core segment name).
bool hasGap() const;
/// Get the lowest defined level (or empty string if there is none).
std::string getLeafItem() const;
- static Time timestampFromStr(const std::string& timestamp);
- static int instanceIndexFromStr(const std::string& index);
-
+ // Operators
bool operator ==(const MemoryID& other) const;
inline bool operator !=(const MemoryID& other) const
@@ -129,18 +200,59 @@ namespace armarx::armem
return !(*this == other);
}
+ bool operator< (const MemoryID& rhs) const;
+ inline bool operator> (const MemoryID& rhs) const
+ {
+ return rhs < (*this);
+ }
+ inline bool operator<=(const MemoryID& rhs) const
+ {
+ return !operator> (rhs);
+ }
+ inline bool operator>=(const MemoryID& rhs) const
+ {
+ return !operator< (rhs);
+ }
+
friend std::ostream& operator<<(std::ostream& os, const MemoryID id);
private:
static long long parseInteger(const std::string& string, const std::string& semanticName);
+ static std::string escapeDelimiter(const std::string& name);
+ static std::string escape(const std::string& name, bool escapeDelimiters);
+
+ static const std::string delimiter;
+
+
+ // Do not allow specifying the delimiter from outside.
+ std::string str(const std::string& delimiter, bool escapeDelimiter) const;
};
- bool
- contains(const MemoryID& general, const MemoryID& specific);
+ /**
+ * @brief Indicates whether `general` is "less specific" than, or equal to, `specific`,
+ * i.e. `general` "contains" `specific`.
+ *
+ * A memory ID A is said to be less specific than B, if B the same values as A
+ * for all levels specified in A, but potentially also specifies the lower levels.
+ *
+ * Examples:
+ * - "" contains ""
+ * - "m" contains "m" and "m/c", but not "n" and "n/c"
+ * - "m/c" contains "m/c" and "m/c/p", but not "m/d" and "m/c/q"
+ *
+ * If a memory ID has a gap (`@see MemoryID::hasGap()`), such as "m//p",
+ * the levels after the gap are ignored.
+ * - "m//p" contains "m", "m/c" and "m/c/p".
+ *
+ * @param general The less specific memory ID
+ * @param specific The more specific memory ID
+ * @return True if `general` is less specific than `specific`.
+ */
+ bool contains(const MemoryID& general, const MemoryID& specific);
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp
index d325f4b9e522ae376bcd598ce8c67c64a2dfdcff..c062c089e31f8cf5c1314e4e4fcd7608ee5f2a48 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/EntityQueryProcessor.cpp
@@ -74,6 +74,11 @@ namespace armarx::armem::query_proc
void EntityQueryProcessor::process(Entity& result, const armem::query::data::entity::IndexRange& query, const Entity& entity) const
{
+ if (entity.empty())
+ {
+ return;
+ }
+
size_t first = negativeIndexSemantics(query.first, entity.history.size());
size_t last = negativeIndexSemantics(query.last, entity.history.size());
@@ -103,6 +108,8 @@ namespace armarx::armem::query_proc
void EntityQueryProcessor::process(Entity& result, const Time& min, const Time& max, const Entity& entity,
const armem::query::data::EntityQuery& query) const
{
+ (void) query;
+
// Returns an iterator pointing to the first element that is not less than (i.e. greater or equal to) key.
auto begin = min.toMicroSeconds() > 0 ? entity.history.lower_bound(min) : entity.history.begin();
// Returns an iterator pointing to the first element that is *greater than* key.
@@ -121,14 +128,14 @@ namespace armarx::armem::query_proc
size_t EntityQueryProcessor::negativeIndexSemantics(long index, size_t size)
{
- size_t max = size > 0 ? size - 1 : 0;
+ const size_t max = size > 0 ? size - 1 : 0;
if (index >= 0)
{
return std::clamp<size_t>(static_cast<size_t>(index), 0, max);
}
else
{
- return std::clamp<size_t>(size - static_cast<size_t>(- index), 0, max);
+ return static_cast<size_t>(std::clamp<long>(static_cast<long>(size) + index, 0, static_cast<long>(max)));
}
}
diff --git a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
index d811c053ba4c57864030eaa8b0d5a7485a6df58c..1f94d9adb9a2d023a3813c0e09edf8b41367ace1 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemMemoryIDTest.cpp
@@ -26,6 +26,7 @@
#include <RobotAPI/Test.h>
#include "../core/MemoryID.h"
+#include "../core/error.h"
#include <iostream>
@@ -33,22 +34,106 @@
namespace armem = armarx::armem;
-BOOST_AUTO_TEST_CASE(test_memoryid_contains)
+BOOST_AUTO_TEST_CASE(test_MemoryID_contains)
{
armem::MemoryID general, specific;
- BOOST_CHECK(armem::contains(general, specific));
+ // Both empty.
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK(armem::contains(general, specific));
+ }
+ // Diverging.
general.memoryName = "a";
specific.memoryName = "b";
- BOOST_CHECK(not armem::contains(general, specific));
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK(not armem::contains(general, specific));
+ BOOST_CHECK(not armem::contains(specific, general));
+ }
+ // Identical.
specific.memoryName = "a";
- BOOST_CHECK(armem::contains(general, specific));
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK(armem::contains(general, specific));
+ BOOST_CHECK(armem::contains(specific, general));
+ }
- specific.providerSegmentName = "aa";
+ // general contains specific
+ specific.coreSegmentName = "c";
+
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK(armem::contains(general, specific));
+ BOOST_CHECK(not armem::contains(specific, general));
+ }
+
+ // general contains specific
+ specific.providerSegmentName = "d";
+
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK(armem::contains(general, specific));
+ BOOST_CHECK(not armem::contains(specific, general));
+ }
+
+ // Not well-defined ID - throw an exception.
+ specific.coreSegmentName.clear();
+
+ BOOST_TEST_CONTEXT(VAROUT(general) << " | " << VAROUT(specific))
+ {
+ BOOST_CHECK_THROW(armem::contains(general, specific), armem::error::InvalidMemoryID);
+ BOOST_CHECK_THROW(armem::contains(specific, general), armem::error::InvalidMemoryID);
+ }
+}
+
+
+
+BOOST_AUTO_TEST_CASE(test_MemoryID_from_to_string)
+{
+ armem::MemoryID idIn {"Memory/Core/Prov/entity/2810381/2"};
+
+ BOOST_CHECK_EQUAL(idIn.memoryName, "Memory");
+ BOOST_CHECK_EQUAL(idIn.coreSegmentName, "Core");
+ BOOST_CHECK_EQUAL(idIn.providerSegmentName, "Prov");
+ BOOST_CHECK_EQUAL(idIn.entityName, "entity");
+ BOOST_CHECK_EQUAL(idIn.timestamp, IceUtil::Time::microSeconds(2810381));
+ BOOST_CHECK_EQUAL(idIn.instanceIndex, 2);
+
+
+ BOOST_TEST_CONTEXT(VAROUT(idIn.str()))
+ {
+ armem::MemoryID idOut(idIn.str());
+ BOOST_CHECK_EQUAL(idOut, idIn);
+ }
+
+ idIn.entityName = "KIT/Amicelli/0"; // Like an ObjectID
+ BOOST_CHECK_EQUAL(idIn.entityName, "KIT/Amicelli/0");
+
+ BOOST_TEST_CONTEXT(VAROUT(idIn.str()))
+ {
+ armem::MemoryID idOut(idIn.str());
+ BOOST_CHECK_EQUAL(idOut.entityName, "KIT/Amicelli/0");
+ BOOST_CHECK_EQUAL(idOut, idIn);
+ }
+
+ idIn = armem::MemoryID {"InThe\\/Mid/AtTheEnd\\//\\/AtTheStart/YCB\\/sugar\\/-1//2"};
+ BOOST_CHECK_EQUAL(idIn.memoryName, "InThe/Mid");
+ BOOST_CHECK_EQUAL(idIn.coreSegmentName, "AtTheEnd/");
+ BOOST_CHECK_EQUAL(idIn.providerSegmentName, "/AtTheStart");
+ BOOST_CHECK_EQUAL(idIn.entityName, "YCB/sugar/-1");
+ BOOST_CHECK_EQUAL(idIn.timestamp, IceUtil::Time::microSeconds(-1));
+ BOOST_CHECK_EQUAL(idIn.instanceIndex, 2);
+
+ BOOST_TEST_CONTEXT(VAROUT(idIn.str()))
+ {
+ armem::MemoryID idOut(idIn.str());
+ BOOST_CHECK_EQUAL(idOut, idIn);
+ }
- BOOST_CHECK(armem::contains(general, specific));
}
+
diff --git a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
index f36411237759df97bf1ff0beb81d60cfec95f59a..b35fb39452fd014e7d9a7bec6353ca181b5a14e2 100644
--- a/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
+++ b/source/RobotAPI/libraries/armem/test/ArMemQueryTest.cpp
@@ -119,10 +119,10 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_latest)
addResults(query::entity::Single());
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 1);
+ BOOST_CHECK_EQUAL(result.size(), 1);
BOOST_CHECK_EQUAL(result.history.begin()->second.time(), entity.getLatestSnapshot().time());
BOOST_CHECK_NE(&result.history.begin()->second, &entity.history.rbegin()->second);
}
@@ -134,10 +134,10 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_existing)
addResults(query::entity::Single { 3000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 1);
+ BOOST_CHECK_EQUAL(result.size(), 1);
BOOST_CHECK_EQUAL(result.history.begin()->second.time(), armem::Time::microSeconds(3000));
}
}
@@ -148,10 +148,10 @@ BOOST_AUTO_TEST_CASE(test_entity_Single_non_existing)
addResults(query::entity::Single { 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 0);
+ BOOST_CHECK_EQUAL(result.size(), 0);
}
}
@@ -161,10 +161,10 @@ BOOST_AUTO_TEST_CASE(test_entity_All)
addResults(query::entity::All());
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), entity.history.size());
+ BOOST_CHECK_EQUAL(result.size(), entity.size());
auto jt = entity.history.begin();
for (auto it = result.history.begin(); it != result.history.end(); ++it, ++jt)
{
@@ -182,11 +182,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_slice)
addResults(query::entity::TimeRange{ 1500, 3500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 2);
+ BOOST_CHECK_EQUAL(result.size(), 2);
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected
@@ -203,11 +203,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_exact)
addResults(query::entity::TimeRange{ 2000, 4000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 3);
+ BOOST_CHECK_EQUAL(result.size(), 3);
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected
@@ -224,11 +224,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_all)
addResults(query::entity::TimeRange{ -1, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), entity.history.size());
+ BOOST_CHECK_EQUAL(result.size(), entity.size());
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected = simox::alg::get_keys(entity.history);
@@ -243,11 +243,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_empty)
addResults(query::entity::TimeRange{ 6000, 1000 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 0);
+ BOOST_CHECK_EQUAL(result.size(), 0);
}
}
@@ -257,11 +257,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_from_start)
addResults(query::entity::TimeRange{ -1, 2500 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 2);
+ BOOST_CHECK_EQUAL(result.size(), 2);
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected
@@ -278,11 +278,11 @@ BOOST_AUTO_TEST_CASE(test_entity_TimeRange_to_end)
addResults(query::entity::TimeRange{ 2500, -1 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 3);
+ BOOST_CHECK_EQUAL(result.size(), 3);
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected
@@ -314,10 +314,15 @@ BOOST_AUTO_TEST_CASE(test_negative_index_semantics)
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 1), 0);
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 1), 0);
+ BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 2), 1);
+ BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 2), 0);
+
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-1, 5), 4);
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 5), 0);
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 6), 1);
BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-5, 10), 5);
+ BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-10, 10), 0);
+ BOOST_CHECK_EQUAL(EntityQueryProcessor::negativeIndexSemantics(-20, 10), 0);
}
@@ -327,11 +332,11 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_all_default)
addResults(query::entity::IndexRange(0, -1));
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), entity.history.size());
+ BOOST_CHECK_EQUAL(result.size(), entity.size());
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected = simox::alg::get_keys(entity.history);
@@ -350,11 +355,11 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
addResults(query::entity::IndexRange{ -4, -2 });
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 3);
+ BOOST_CHECK_EQUAL(result.size(), 3);
std::vector<armem::Time> times = simox::alg::get_keys(result.history);
std::vector<armem::Time> expected
@@ -366,7 +371,7 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_slice)
}
-BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty)
+BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_range)
{
BOOST_REQUIRE_EQUAL(entity.size(), 5);
addResults(query::entity::IndexRange{ 1, 0 });
@@ -378,16 +383,34 @@ BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty)
BOOST_REQUIRE_GT(results.size(), 0);
- for (const auto& result : results)
+ for (const armem::Entity& result : results)
{
BOOST_CHECK_EQUAL(result.name(), entity.name());
- BOOST_CHECK_EQUAL(result.history.size(), 0);
+ BOOST_CHECK_EQUAL(result.size(), 0);
}
}
+BOOST_AUTO_TEST_CASE(test_entity_IndexRange_empty_entity)
+{
+ entity.clear();
+ BOOST_REQUIRE_EQUAL(entity.size(), 0);
+ addResults(query::entity::IndexRange{ 0, 0 });
+ addResults(query::entity::IndexRange{ 0, 10 });
+ addResults(query::entity::IndexRange{-10, -1 });
+ addResults(query::entity::IndexRange{ 2, 5 });
+ addResults(query::entity::IndexRange{ 3, -3 });
+ addResults(query::entity::IndexRange{ -1, 10 });
+
+ BOOST_REQUIRE_GT(results.size(), 0);
+ for (const armem::Entity& result : results)
+ {
+ BOOST_CHECK_EQUAL(result.name(), entity.name());
+ BOOST_CHECK_EQUAL(result.size(), 0);
+ }
+}
BOOST_AUTO_TEST_SUITE_END()