diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 76d042a9ecd96b1d6f03814efe6cfb8d609eb0d5..da5440d7da1443c59b3414d48988505f6a298d8f 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -143,8 +143,8 @@ namespace armarx::armem::server::obj
// TODO: the instance segment should check the provided object poses and instantiate the needed robots by itself.
ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
- VirtualRobot::RobotPtr robot = virtualRobotReaderPlugin->get().getSynchronizedRobot(
- robotName, VirtualRobot::RobotIO::RobotDescription::eStructure, true);
+ VirtualRobot::RobotPtr robot = virtualRobotReaderPlugin->get().getRobot(
+ robotName, Clock::Now(), VirtualRobot::RobotIO::RobotDescription::eStructure);
ARMARX_CHECK_NOT_NULL(robot);
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
diff --git a/source/RobotAPI/components/units/PlatformUnit.cpp b/source/RobotAPI/components/units/PlatformUnit.cpp
index b60e943849edd0720eb9b523ee6518e0fced18df..4cbf370ff4803d16e103684877a9b0353d6d0526 100644
--- a/source/RobotAPI/components/units/PlatformUnit.cpp
+++ b/source/RobotAPI/components/units/PlatformUnit.cpp
@@ -38,6 +38,7 @@ namespace armarx
def->topic(odometryPrx);
def->topic(globalPosePrx);
+ def->topic(listenerPrx, listenerChannelName, "");
def->component(robotStateComponent);
@@ -58,8 +59,6 @@ namespace armarx
void PlatformUnit::onConnectComponent()
{
- listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
-
this->onStartPlatformUnit();
}
diff --git a/source/RobotAPI/components/units/PlatformUnit.h b/source/RobotAPI/components/units/PlatformUnit.h
index 097de6689522498722a1ec499b83cbeb12c5fa25..1b0b9499a4f50e6ec0b64631aaf9f85c9fa5bca5 100644
--- a/source/RobotAPI/components/units/PlatformUnit.h
+++ b/source/RobotAPI/components/units/PlatformUnit.h
@@ -133,4 +133,3 @@ namespace armarx
PlatformPose toPlatformPose(const TransformStamped& transformStamped);
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index bcbf667a05a4bf246cc074c2f253ed2f08f56810..afb8f6a0c352085ffc08e72e7f0c74f1f419680f 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -34,6 +34,7 @@ set(LIB_FILES
Units/InertialMeasurementSubUnit.cpp
Units/KinematicSubUnit.cpp
Units/PlatformSubUnit.cpp
+ Units/LocalizationSubUnit.cpp
Units/TrajectoryControllerSubUnit.cpp
Units/TCPControllerSubUnit.cpp
@@ -57,6 +58,9 @@ set(LIB_FILES
Devices/SensorDevice.cpp
Devices/ControlDevice.cpp
+
+ Devices/GlobalRobotPoseSensorDevice.cpp
+
)
set(LIB_HEADERS
util.h
@@ -105,6 +109,7 @@ set(LIB_HEADERS
Units/InertialMeasurementSubUnit.h
Units/KinematicSubUnit.h
Units/PlatformSubUnit.h
+ Units/LocalizationSubUnit.h
Units/TrajectoryControllerSubUnit.h
Units/TCPControllerSubUnit.h
@@ -133,6 +138,7 @@ set(LIB_HEADERS
Devices/ControlDevice.h
Devices/RTThreadTimingsSensorDevice.h
+ Devices/GlobalRobotPoseSensorDevice.h
)
###########################################################################
#since the robot unit is a complex class, it is split into several modules.
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..57f7cf102806a02d1bf502eaf5920e6b43dfff3f
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.cpp
@@ -0,0 +1,158 @@
+#include "GlobalRobotPoseSensorDevice.h"
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
+
+namespace armarx
+{
+ SensorValueBase::SensorValueInfo<SensorValueGlobalPoseCorrection>
+ SensorValueGlobalPoseCorrection::GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValueGlobalPoseCorrection> svi;
+ svi.addMemberVariable(&SensorValueGlobalPoseCorrection::global_T_odom, "global_T_odom")
+ .setVariantReportFrame("", GlobalFrame);
+ return svi;
+ }
+
+
+ bool
+ SensorValueGlobalPoseCorrection::isAvailable() const
+ {
+ return not global_T_odom.isIdentity();
+ }
+
+ SensorValueBase::SensorValueInfo<SensorValueGlobalRobotPose>
+ SensorValueGlobalRobotPose::GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValueGlobalRobotPose> svi;
+ svi.addMemberVariable(&SensorValueGlobalRobotPose::global_T_root, "global_T_root")
+ .setVariantReportFrame("", GlobalFrame);
+ return svi;
+ }
+
+
+ bool
+ SensorValueGlobalRobotPose::isAvailable() const
+ {
+ return not global_T_root.isIdentity();
+ }
+
+
+ std::string
+ GlobalRobotPoseCorrectionSensorDevice::DeviceName()
+ {
+ return "GlobalRobotPoseCorrectionSensorDevice";
+ }
+
+
+ GlobalRobotPoseCorrectionSensorDevice::GlobalRobotPoseCorrectionSensorDevice() :
+ DeviceBase(DeviceName()), SensorDevice(DeviceName())
+ {
+ transformationBuffer.reinitAllBuffers(SensorValueType::Transformation::Identity());
+ }
+
+
+ const SensorValueBase*
+ GlobalRobotPoseCorrectionSensorDevice::getSensorValue() const
+ {
+ return &sensor;
+ }
+
+
+ std::string
+ GlobalRobotPoseCorrectionSensorDevice::getReportingFrame() const
+ {
+ return GlobalFrame;
+ }
+
+
+ void
+ GlobalRobotPoseCorrectionSensorDevice::rtReadSensorValues(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ sensor.global_T_odom = transformationBuffer.getUpToDateReadBuffer();
+ }
+
+
+ void
+ GlobalRobotPoseCorrectionSensorDevice::updateGlobalPositionCorrection(
+ const SensorValueType::Transformation& global_T_odom)
+ {
+ transformationBuffer.getWriteBuffer() = global_T_odom;
+ transformationBuffer.commitWrite();
+ }
+
+
+ std::string
+ GlobalRobotPoseSensorDevice::DeviceName()
+ {
+ return "GlobalRobotPoseSensorDevice";
+ }
+
+
+ GlobalRobotPoseSensorDevice::GlobalRobotPoseSensorDevice() :
+ DeviceBase(DeviceName()), SensorDevice(DeviceName())
+ {
+ }
+
+
+ const SensorValueBase*
+ GlobalRobotPoseSensorDevice::getSensorValue() const
+ {
+ return &sensor;
+ }
+
+
+ std::string
+ GlobalRobotPoseSensorDevice::getReportingFrame() const
+ {
+ return GlobalFrame;
+ }
+
+
+ void
+ GlobalRobotPoseSensorDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ }
+
+
+ std::string
+ GlobalRobotLocalizationSensorDevice::DeviceName()
+ {
+ return "GlobalRobotLocalizationSensorDevice";
+ }
+
+
+ GlobalRobotLocalizationSensorDevice::GlobalRobotLocalizationSensorDevice() :
+ DeviceBase(DeviceName()), SensorDevice(DeviceName())
+ {
+ }
+
+
+ void
+ GlobalRobotLocalizationSensorDevice::rtReadSensorValues(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+
+ if (sensorGlobalPositionCorrection == nullptr or sensorRelativePosition == nullptr)
+ {
+ ARMARX_ERROR << "one of the sensors is not available";
+ return;
+ }
+
+ const auto global_T_relative = sensorGlobalPositionCorrection->global_T_odom;
+ const auto relative_T_robot =
+ simox::math::pos_rpy_to_mat4f(sensorRelativePosition->relativePositionX,
+ sensorRelativePosition->relativePositionY,
+ 0,
+ 0,
+ 0,
+ sensorRelativePosition->relativePositionRotation);
+ const auto global_T_robot = global_T_relative * relative_T_robot;
+
+ sensor.global_T_root = global_T_robot;
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
new file mode 100644
index 0000000000000000000000000000000000000000..c3cd19f4a3777a6903ceca8b4a1e8660f25b617b
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h
@@ -0,0 +1,120 @@
+
+#pragma once
+
+#include <Eigen/Core>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
+
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
+#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+namespace armarx
+{
+
+ /**
+ * @brief The pose correction to obtain the robot's global pose.
+ *
+ * In conjunction with the SensorValueHolonomicPlatformRelativePosition
+ * information, the global robot pose can be obtained which
+ * - provides the robot's initial localization within the map
+ * - compensates odometry drift over time.
+ *
+ */
+ class SensorValueGlobalPoseCorrection : virtual public SensorValueBase
+ {
+ public:
+ using Transformation = Eigen::Matrix4f;
+ Transformation global_T_odom = Transformation::Identity();
+
+ // TODO add timestamp
+
+ static SensorValueInfo<SensorValueGlobalPoseCorrection> GetClassMemberInfo();
+
+ bool isAvailable() const;
+
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ };
+
+
+ class SensorValueGlobalRobotPose : virtual public SensorValueBase
+ {
+ public:
+ using Transformation = Eigen::Matrix4f;
+ Transformation global_T_root = Transformation::Identity();
+
+ static SensorValueInfo<SensorValueGlobalRobotPose> GetClassMemberInfo();
+
+ bool isAvailable() const;
+
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ };
+
+ TYPEDEF_PTRS_SHARED(GlobalRobotPoseCorrectionSensorDevice);
+ class GlobalRobotPoseCorrectionSensorDevice : virtual public SensorDevice
+ {
+ public:
+ using SensorValueType = SensorValueGlobalPoseCorrection;
+
+ static std::string DeviceName();
+
+ GlobalRobotPoseCorrectionSensorDevice();
+
+ const SensorValueBase* getSensorValue() const override;
+
+ std::string getReportingFrame() const override;
+
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ void updateGlobalPositionCorrection(const SensorValueType::Transformation& global_T_odom);
+
+ private:
+ SensorValueType sensor;
+
+ TripleBuffer<SensorValueType::Transformation> transformationBuffer;
+ };
+
+
+ TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+ class GlobalRobotPoseSensorDevice : virtual public SensorDevice
+ {
+ public:
+ using SensorValueType = SensorValueGlobalRobotPose;
+
+ static std::string DeviceName();
+
+ GlobalRobotPoseSensorDevice();
+
+ const SensorValueBase* getSensorValue() const override;
+
+ std::string getReportingFrame() const override;
+
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ protected:
+ SensorValueType sensor;
+ };
+
+
+ TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
+ class GlobalRobotLocalizationSensorDevice : virtual public GlobalRobotPoseSensorDevice
+ {
+ public:
+ static std::string DeviceName();
+
+ GlobalRobotLocalizationSensorDevice();
+
+ void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ const SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
+ const SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
+
+ };
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 013af71634177b541922df2733ad495098b435eb..074efbc511936171b36d34acd0adbac87a1dde8a 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -22,9 +22,13 @@
* GNU General Public License
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h"
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <cmath>
@@ -42,8 +46,8 @@ namespace armarx
opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{
- const SensorValueBase* sv = useSensorValue(cfg->platformName);
- this->sv = sv->asA<SensorValueHolonomicPlatform>();
+ const SensorValueBase* sv = useSensorValue(GlobalRobotLocalizationSensorDevice::DeviceName());
+ this->sv = sv->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -55,45 +59,64 @@ namespace armarx
void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
{
- currentPosition << sv->relativePositionX, sv->relativePositionY;
- currentOrientation = sv->relativePositionRotation;
+ const auto global_T_robot = sv->global_T_root;
+ const Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(global_T_robot);
+ const float global_orientation = rpy.z();
+ const Eigen::Vector2f global_P_robot = global_T_robot.block<2,1>(0,3);
if (rtGetControlStruct().newTargetSet)
{
pid.reset();
opid.reset();
- getWriterControlStruct().newTargetSet = false;
- writeControlStruct();
+ getWriterControlStruct().newTargetSet = false;
+ writeControlStruct();
- isAborted = false;
+ isTargetSet = true;
}
- if (isAborted)
+
+
+ // if ((sv->lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+ // {
+ // ARMARX_RT_LOGF_WARNING << deactivateSpam(0.5) << "Waiting for global pos";
+
+ // target->velocityX = 0;
+ // target->velocityY = 0;
+
+ // target->velocityRotation = 0;
+ // isAborted = true;
+
+ // return;
+ // }
+
+ if (not isTargetSet)
{
+ target->velocityX = 0;
+ target->velocityY = 0;
+ target->velocityRotation = 0;
+
return;
}
- else if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
- {
- // ARMARX_RT_LOGF_WARNING << deactivateSpam(0.5) << "Waiting for global pos";
+ if(not sv->isAvailable())
+ {
+ // ARMARX_RT_LOGF_INFO << deactivateSpam(1) << "global pose not available";
target->velocityX = 0;
target->velocityY = 0;
-
target->velocityRotation = 0;
- isAborted = true;
return;
}
- const float measuredOrientation = rtGetControlStruct().globalOrientation;
+ const float measuredOrientation = global_orientation;
- pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
+ pid.update(timeSinceLastIteration.toSecondsDouble(), global_P_robot, rtGetControlStruct().target);
opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
const Eigen::Rotation2Df global_R_local(-measuredOrientation);
+ const Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
- Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
target->velocityX = velocities.x();
target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue());
@@ -101,6 +124,9 @@ namespace armarx
void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
{
+ target->velocityX = 0;
+ target->velocityY = 0;
+ target->velocityRotation = 0;
}
void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
@@ -111,28 +137,12 @@ namespace armarx
getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
+
getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+
getWriterControlStruct().newTargetSet = true;
writeControlStruct();
}
-
- void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
- {
- // ..todo: check if norm is too large
-
- getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
- getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
-
- getWriterControlStruct().startPosition = currentPosition;
- getWriterControlStruct().startOrientation = currentOrientation;
-
- getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
- writeControlStruct();
-
-
- }
-
} // namespace armarx
-
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index a343e404f97f759d232669b2a1ed72c5b49bd861..37a0de6feb1720dc702b1b5e5dd356b36e974399 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -34,6 +34,7 @@
#include "../SensorValues/SensorValueHolonomicPlatform.h"
#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
#include <RobotAPI/libraries/core/PIDController.h>
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
@@ -71,11 +72,11 @@ namespace armarx
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
- Eigen::Vector2f startPosition;
- float startOrientation;
+ // Eigen::Vector2f startPosition;
+ // float startOrientation;
- Eigen::Vector2f globalPosition;
- float globalOrientation;
+ // Eigen::Vector2f globalPosition;
+ // float globalOrientation;
IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
};
@@ -108,21 +109,14 @@ namespace armarx
void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
- void setGlobalPos(const PlatformPose& currentPose);
-
-
protected:
- const SensorValueHolonomicPlatform* sv;
+ const GlobalRobotLocalizationSensorDevice::SensorValueType* sv;
MultiDimPIDController pid;
PIDController opid;
ControlTargetHolonomicPlatformVelocity* target;
- Eigen::Vector2f currentPosition;
- float currentOrientation;
- bool isAborted = false;
+ bool isTargetSet = false;
};
} // namespace armarx
-
-
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index a3269b4d0e11cb792f639e4c477218dcb58183e8..5d7d7d01ece91cb274e2f4e6a6f0619c06f4a65e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -22,12 +22,15 @@
#include "RobotUnitModuleControlThread.h"
+#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
@@ -1083,12 +1086,16 @@ namespace armarx::RobotUnitModule
{
if (njctrl->rtGetRobot())
{
+ // update joints / nodes
auto& from = rtRobotNodes;
auto& to = njctrl->rtGetRobotNodes();
for (std::size_t i = 0; i < from.size(); ++i)
{
to.at(i)->copyPoseFrom(from.at(i));
}
+
+ // update global root pose
+ njctrl->rtGetRobot()->setGlobalPose(rtRobot->getGlobalPose(), false);
}
}
@@ -1169,6 +1176,25 @@ namespace armarx::RobotUnitModule
return str.str();
}
+ void
+ ControlThread::rtUpdateRobotGlobalPose()
+ {
+ const ConstSensorDevicePtr globalPoseSensorDevice = _module<Devices>().getSensorDevice(GlobalRobotPoseSensorDevice::DeviceName());
+ if(not globalPoseSensorDevice)
+ {
+ return;
+ }
+
+ const auto *const sensorValue = globalPoseSensorDevice->getSensorValue()->asA<SensorValueGlobalRobotPose>();
+ if(sensorValue == nullptr)
+ {
+ return;
+ }
+
+ const auto global_T_robot = sensorValue->global_T_root;
+ rtSetRobotGlobalPose(global_T_robot, false);
+ }
+
void
ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
{
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index 0dcf52aa47c01ddcd9cacd970373cf5e3423edf1..9b14a13ba370166793e188c8c8577fcc5ea19516 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -274,6 +274,7 @@ namespace armarx::RobotUnitModule
*/
void rtSetEmergencyStopState(EmergencyStopState state);
+ void rtUpdateRobotGlobalPose();
void rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot = true);
/// Activate a joint controller from the rt loop (only works in switch mode RTThread)
@@ -390,4 +391,3 @@ namespace armarx::RobotUnitModule
friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
};
}
-
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
index fb8b38fc28ad83a8ba07ac395049796ac797d956..cbc9e50f06bc09a60635f6b6dd37e225f798377f 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp
@@ -26,6 +26,7 @@
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/util/algorithm.h>
+#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
namespace armarx::RobotUnitModule
{
@@ -448,6 +449,11 @@ namespace armarx::RobotUnitModule
{
addSensorDevice(createRTThreadTimingSensorDevice());
}
+
+ // this device will be used by the PlatformUnit to make the robot's global pose
+ // available to e.g. the NJointControllers.
+ addSensorDevice(std::make_shared<GlobalRobotPoseCorrectionSensorDevice>());
+ addSensorDevice(std::make_shared<GlobalRobotLocalizationSensorDevice>());
}
void Devices::_postFinishDeviceInitialization()
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index c1b9e88ca02c778cb61328ead80e9922e4161cf5..92f65d1941cd1c88dde3ace948d0c37a85d4ab60 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -77,6 +77,7 @@ namespace armarx::RobotUnitModule::details
DurationsEntry msg;
};
} // namespace armarx::RobotUnitModule::details
+
namespace armarx::RobotUnitModule
{
void
@@ -672,7 +673,8 @@ namespace armarx::RobotUnitModule
{
ARMARX_TRACE;
- //header
+
+ // Header.
{
durations.header.start();
ARMARX_TRACE;
@@ -702,428 +704,434 @@ namespace armarx::RobotUnitModule
}
durations.header.stop();
}
- //process devices
- {//sens
- {ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
+
+ // Process devices.
{
- const SensorValueBase* val = data.sensors.at(idxDev);
- //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ // Sensors.
{
- if (!rtLoggingEntries.empty())
+ ARMARX_TRACE;
+ durations.sens.start();
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
- durations.sens_csv.start();
- const auto str = val->getDataFieldAs<std::string>(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ const SensorValueBase* val = data.sensors.at(idxDev);
+ //dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ if (!rtLoggingEntries.empty())
{
- entry->stream << ";" << str;
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.sens_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.sens_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ durations.sens_stream_elem.start();
+ data.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
+ }
+ durations.sens_stream.stop();
}
}
- durations.sens_csv.stop();
- }
- if (!rtDataStreamingEntry.empty())
- {
- durations.sens_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.sens_stream_elem.start();
- data.processSens(*val, idxDev, idxField);
- durations.sens_stream_elem.stop();
- }
- durations.sens_stream.stop();
}
+ durations.sens.stop();
}
- }
- durations.sens.stop();
- }
- //ctrl
- {
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
- {
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+
+ // Controller.
{
- const ControlTargetBase* val = vals.at(idxVal);
- //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
- if (!rtLoggingEntries.empty())
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.ctrl_csv.start();
- std::string str;
- val->getDataFieldAs(idxField, str); // expensive function
- for (auto& [_, entry] : rtLoggingEntries)
+ const ControlTargetBase* val = vals.at(idxVal);
+ //dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
{
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ if (!rtLoggingEntries.empty())
{
- entry->stream << ";" << str;
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
+ {
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
+ {
+ entry->stream << ";" << str;
+ }
+ }
+ durations.ctrl_csv.stop();
+ }
+ if (!rtDataStreamingEntry.empty())
+ {
+ durations.ctrl_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ durations.ctrl_stream_elem.start();
+ data.processCtrl(*val, idxDev, idxVal, idxField);
+ durations.ctrl_stream_elem.stop();
+ }
+ durations.ctrl_stream.stop();
}
}
- durations.ctrl_csv.stop();
}
- if (!rtDataStreamingEntry.empty())
+ }
+
+ durations.ctrl.stop();
+ }
+ }
+
+ //finish processing
+ {
+ //store data to backlog
+ {
+ durations.backlog.start();
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
+ {
+ backlog.emplace_back(data, true); //true for minimal copy
+ }
+ durations.backlog.stop();
+ }
+ //print + reset messages
+ {
+ durations.msg.start();
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ {
+ if (!ptr)
{
- durations.ctrl_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.ctrl_stream_elem.start();
- data.processCtrl(*val, idxDev, idxVal, idxField);
- durations.ctrl_stream_elem.stop();
- }
- durations.ctrl_stream.stop();
+ break;
}
+ ptr->print(controlThreadId);
}
+ durations.msg.stop();
}
}
- durations.ctrl.stop();
}
-} // namespace armarx::RobotUnitModule
-//finish processing
-{
- //store data to backlog
+
+ bool
+ Logging::MatchName(const std::string& pattern, const std::string& name)
{
- durations.backlog.start();
ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
+ if (pattern.empty())
{
- backlog.emplace_back(data, true); //true for minimal copy
+ return false;
}
- durations.backlog.stop();
- }
- //print + reset messages
- {
- durations.msg.start();
- ARMARX_TRACE;
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
}
- durations.msg.stop();
+ static const std::regex reg_dot{"[.]"};
+ static const std::regex reg_star{"[*]"};
+ const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
+ const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
+ const std::regex key_regex{rpl2};
+ return std::regex_search(name, key_regex);
}
-}
-}
-bool
-Logging::MatchName(const std::string& pattern, const std::string& name)
-{
- ARMARX_TRACE;
- if (pattern.empty())
- {
- return false;
- }
- static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
- if (!std::regex_match(pattern, pattern_regex))
+ void
+ Logging::_postOnInitRobotUnit()
{
- throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
- }
- static const std::regex reg_dot{"[.]"};
- static const std::regex reg_star{"[*]"};
- const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
- const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
- const std::regex key_regex{rpl2};
- return std::regex_search(name, key_regex);
-}
-
-void
-Logging::_postOnInitRobotUnit()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
- ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
+ ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
- rtLoggingBacklogRetentionTime =
- IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
-}
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+ }
-void
-Logging::_postFinishDeviceInitialization()
-{
- ARMARX_TRACE;
- throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- //init buffer
+ void
+ Logging::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
- std::size_t ctrlThreadPeriodUs =
- static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
- std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
- std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
+ {
+ ARMARX_TRACE;
+ std::size_t ctrlThreadPeriodUs =
+ static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
+ std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
+ std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
- const auto bufferSize =
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
- nBuffers,
- _module<Devices>().getControlDevices(),
- _module<Devices>().getSensorDevices(),
- messageBufferSize,
- messageBufferNumberEntries,
- messageBufferMaxSize,
- messageBufferMaxNumberEntries);
- ARMARX_INFO << "RTLogging activated! using " << nBuffers << "buffers "
- << "(buffersize = " << bufferSize << " bytes)";
- }
- //init logging names + field types
- {
- ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ const auto bufferSize =
+ _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
+ nBuffers,
+ _module<Devices>().getControlDevices(),
+ _module<Devices>().getSensorDevices(),
+ messageBufferSize,
+ messageBufferNumberEntries,
+ messageBufferMaxSize,
+ messageBufferMaxNumberEntries);
+ ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
+ << "(buffersize = " << bufferSize << " bytes)";
+ }
+ //init logging names + field types
{
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ {
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
- for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
+ {
+ std::string const& fieldName = names[fieldIdx];
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ }
+ return data;
+ };
+
+ //sensorDevices
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
+ for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- std::string const& fieldName = names[fieldIdx];
- data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
- data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
+ for (auto jointC : cd->getJointControllers())
+ {
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
+ }
}
- return data;
- };
-
- //sensorDevices
- controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
- {
- ARMARX_TRACE;
- controlDeviceValueMetaData.emplace_back();
- auto& dataForDev = controlDeviceValueMetaData.back();
- dataForDev.reserve(cd->getJointControllers().size());
- for (auto jointC : cd->getJointControllers())
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
- "ctrl." + cd->getDeviceName() + "." +
- jointC->getControlMode()));
+ ARMARX_TRACE;
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
+ {
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ }
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+
+ void
+ Logging::DataStreamingEntry::clearResult()
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
+ for (auto& e : result)
{
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
+ entryBuffer.emplace_back(std::move(e));
}
+ result.clear();
}
-}
-void
-Logging::DataStreamingEntry::clearResult()
-{
- ARMARX_TRACE;
- for (auto& e : result)
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::allocateResultElement() const
{
- entryBuffer.emplace_back(std::move(e));
+ ARMARX_TRACE;
+ RobotUnitDataStreaming::TimeStep data;
+ data.bools.resize(numBools);
+ data.bytes.resize(numBytes);
+ data.shorts.resize(numShorts);
+ data.ints.resize(numInts);
+ data.longs.resize(numLongs);
+ data.floats.resize(numFloats);
+ data.doubles.resize(numDoubles);
+ return data;
}
- result.clear();
-}
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::allocateResultElement() const
-{
- ARMARX_TRACE;
- RobotUnitDataStreaming::TimeStep data;
- data.bools.resize(numBools);
- data.bytes.resize(numBytes);
- data.shorts.resize(numShorts);
- data.ints.resize(numInts);
- data.longs.resize(numLongs);
- data.floats.resize(numFloats);
- data.doubles.resize(numDoubles);
- return data;
-}
-
-RobotUnitDataStreaming::TimeStep
-Logging::DataStreamingEntry::getResultElement()
-{
- ARMARX_TRACE;
- if (entryBuffer.empty())
+ RobotUnitDataStreaming::TimeStep
+ Logging::DataStreamingEntry::getResultElement()
{
- return allocateResultElement();
+ ARMARX_TRACE;
+ if (entryBuffer.empty())
+ {
+ return allocateResultElement();
+ }
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
}
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
-}
-void
-Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
{
- return;
- }
- result.emplace_back(getResultElement());
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ result.emplace_back(getResultElement());
- auto& data = result.back();
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
-}
+ auto& data = result.back();
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ }
-void
-WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
-{
- ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
+ void
+ WriteTo(const auto& dentr,
+ const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
{
ARMARX_TRACE;
- switch (out.value)
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ try
{
- case enum_t::Bool:
- bool b;
- val.getDataFieldAs(fidx, b);
- data.bools.at(out.idx) = b;
- return;
- case enum_t::Byte:
- val.getDataFieldAs(fidx, data.bytes.at(out.idx));
- return;
- case enum_t::Short:
- val.getDataFieldAs(fidx, data.shorts.at(out.idx));
- return;
- case enum_t::Int:
- val.getDataFieldAs(fidx, data.ints.at(out.idx));
- return;
- case enum_t::Long:
- val.getDataFieldAs(fidx, data.longs.at(out.idx));
- return;
- case enum_t::Float:
- val.getDataFieldAs(fidx, data.floats.at(out.idx));
- return;
- case enum_t::Double:
- val.getDataFieldAs(fidx, data.doubles.at(out.idx));
- return;
- case enum_t::Skipped:
- return;
+ ARMARX_TRACE;
+ switch (out.value)
+ {
+ case enum_t::Bool:
+ bool b;
+ val.getDataFieldAs(fidx, b);
+ data.bools.at(out.idx) = b;
+ return;
+ case enum_t::Byte:
+ val.getDataFieldAs(fidx, data.bytes.at(out.idx));
+ return;
+ case enum_t::Short:
+ val.getDataFieldAs(fidx, data.shorts.at(out.idx));
+ return;
+ case enum_t::Int:
+ val.getDataFieldAs(fidx, data.ints.at(out.idx));
+ return;
+ case enum_t::Long:
+ val.getDataFieldAs(fidx, data.longs.at(out.idx));
+ return;
+ case enum_t::Float:
+ val.getDataFieldAs(fidx, data.floats.at(out.idx));
+ return;
+ case enum_t::Double:
+ val.getDataFieldAs(fidx, data.doubles.at(out.idx));
+ return;
+ case enum_t::Skipped:
+ return;
+ }
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
+ << "\n"
+ << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
+ << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
+ << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
+ << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
+ << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
+ << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
+ << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
+ throw;
}
}
- catch (...)
- {
- ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
- << "\n"
- << VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
- << VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
- << VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
- << VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
- << VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
- << VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
- << VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
- throw;
- }
-}
-void
-Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-void
-Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx)
-{
- ARMARX_TRACE;
- if (stopStreaming)
+ void
+ Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
{
- return;
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
}
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
-}
-
-void
-Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
-{
- ARMARX_TRACE;
- const auto start_send = IceUtil::Time::now();
- updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
- const auto start_clear = IceUtil::Time::now();
- clearResult();
- const auto end = IceUtil::Time::now();
- ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
- << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
- << result.size() << " timesteps)"
- << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
- //now execute all ready callbacks
- std::size_t i = 0;
- for (; i < updateCalls.size(); ++i)
+ void
+ Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
{
- try
+ ARMARX_TRACE;
+ const auto start_send = IceUtil::Time::now();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = IceUtil::Time::now();
+ clearResult();
+ const auto end = IceUtil::Time::now();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
+ << result.size() << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
{
- if (!updateCalls.at(i)->isCompleted())
+ try
{
- break;
+ if (!updateCalls.at(i)->isCompleted())
+ {
+ break;
+ }
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
- {
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
+ catch (...)
{
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > rtStreamMaxClientErrors)
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ updateCalls.clear();
+ break;
+ }
}
}
+ if (!updateCalls.empty())
+ {
+ updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ }
}
- if (!updateCalls.empty())
- {
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
- }
-}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index ce63e07571da1dfd17d42b99e0c446a72aa83532..732b3a193d6da5831634a1c6caa0206e9c391140 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -73,8 +73,8 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::size_t>(
"RTLogging_KeepIterationsForMs", 60 * 1000,
- "All logging data (SensorValues, ControlTargets, Messages) is keept for this duration "
- "and can be dunped in case of an error.");
+ "All logging data (SensorValues, ControlTargets, Messages) is kept for this duration "
+ "and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
"RTLogging_StreamingDataMaxClientConnectionFailures", 10,
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index d562fa2dc3bb421eb09640c119db2f6f73c45018..80efce08aeaefed56bf599ceb928c7e1615e36a0 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -23,6 +23,10 @@
#include <ArmarXCore/core/IceManager.h>
#include <ArmarXCore/core/IceGridAdmin.h>
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
+#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
+
#include "RobotUnitModuleUnits.h"
#include "../Units/ForceTorqueSubUnit.h"
@@ -152,6 +156,11 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "initializing default units";
initializeKinematicUnit();
ARMARX_DEBUG << "KinematicUnit initialized";
+
+ ARMARX_DEBUG << "initializing LocalizationUnit";
+ initializeLocalizationUnit();
+ ARMARX_DEBUG << "LocalizationUnit initialized";
+
initializePlatformUnit();
ARMARX_DEBUG << "PlatformUnit initialized";
initializeForceTorqueUnit();
@@ -162,6 +171,7 @@ namespace armarx::RobotUnitModule
ARMARX_DEBUG << "TrajectoryControllerUnit initialized";
initializeTcpControllerUnit();
ARMARX_DEBUG << "TcpControllerUnit initialized";
+
}
ARMARX_INFO << "initializing default units...done! " << (TimeUtil::GetTime(true) - beg).toMicroSeconds() << " us";
}
@@ -394,8 +404,7 @@ namespace armarx::RobotUnitModule
unit->pt = ctrl;
unit->relativePosCtrl = ctrlRelativePosition;
- unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
-
+ // unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
@@ -415,6 +424,56 @@ namespace armarx::RobotUnitModule
addUnit(std::move(unit));
}
+ void Units::initializeLocalizationUnit()
+ {
+ ARMARX_DEBUG << "initializeLocalizationUnit";
+
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ using UnitT = LocalizationSubUnit;
+ using IfaceT = LocalizationUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+
+ ARMARX_DEBUG << "Getting device SensorValueHolonomicPlatformRelativePosition";
+ const SensorDevicePtr& sensorDeviceRelativePosition = _module<Devices>().getSensorDevices().at(_module<RobotData>().getRobotPlatformName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>());
+
+ // const SensorDevicePtr& sensorDevicePoseCorrection = _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName());
+ // ARMARX_CHECK_EXPRESSION(sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueGlobalPoseCorrection>());
+
+
+ //add it
+ const std::string configName = "LocalizationUnit";
+ const std::string confPre = getConfigDomain() + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ // properties->setProperty(confPre + "PlatformName", _module<RobotData>().getRobotPlatformInstanceName());
+ ARMARX_DEBUG << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties, configName, getConfigDomain());
+ //config
+
+ ARMARX_DEBUG << "Setting device GlobalRobotPoseCorrectionSensorDevice";
+ unit->globalPositionCorrectionSensorDevice = dynamic_cast<decltype(unit->globalPositionCorrectionSensorDevice)>(
+ _module<Devices>().getSensorDevices().at(GlobalRobotPoseCorrectionSensorDevice::DeviceName()).get());
+
+
+ const SensorDevicePtr& sensorDeviceGlobalRobotLocalization = _module<Devices>().getSensorDevices().at(GlobalRobotLocalizationSensorDevice::DeviceName());
+ ARMARX_CHECK_EXPRESSION(sensorDeviceGlobalRobotLocalization->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>());
+
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorRelativePosition = sensorDeviceRelativePosition->getSensorValue()->asA<SensorValueHolonomicPlatformRelativePosition>();
+ dynamic_cast<GlobalRobotLocalizationSensorDevice&>(*sensorDeviceGlobalRobotLocalization).sensorGlobalPositionCorrection = unit->globalPositionCorrectionSensorDevice->getSensorValue()->asA<SensorValueGlobalPoseCorrection>();
+
+ //add
+ addUnit(unit);
+ }
+
void Units::initializeForceTorqueUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index d2f4ee59c173c06007479a14539f2025bc3ea793..ecb6e49b5e3ebd44fb358e976b7ffdfe68d56a82 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -251,6 +251,8 @@ namespace armarx::RobotUnitModule
virtual void initializeTrajectoryControllerUnit();
/// @brief Initializes the TcpControllerUnit
virtual void initializeTcpControllerUnit();
+ /// @brief Initializes the TcpControllerUnit
+ virtual void initializeLocalizationUnit();
/// @brief Create the \ref KinematicUnit (this function should be called in \ref initializeKinematicUnit)
ManagedIceObjectPtr createKinematicSubUnit(const Ice::PropertiesPtr& properties,
const std::string& positionControlMode = ControlModes::Position1DoF,
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
index 51b775a8ff44717716e6c3a83e47aaeb938199de..b7318e72fe1ae0cc7811389a0eccaa95f80a5ef5 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
@@ -67,13 +67,20 @@ namespace armarx
}
};
+ /**
+ * @brief The robot's position relative to its initial pose when starting the robot unit based on odometry information.
+ *
+ * Note: The relative position undergoes a significant drift over time.
+ */
class SensorValueHolonomicPlatformRelativePosition : virtual public SensorValueBase
{
public:
DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
float relativePositionX = 0;
float relativePositionY = 0;
float relativePositionRotation = 0;
+
static SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> GetClassMemberInfo()
{
SensorValueInfo<SensorValueHolonomicPlatformRelativePosition> svi;
@@ -84,22 +91,28 @@ namespace armarx
}
};
- class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
- {
- public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- float absolutePositionX;
- float absolutePositionY;
- float absolutePositionRotation;
- static SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> GetClassMemberInfo()
- {
- SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> svi;
- svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionX, "absolutePositionX");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionY, "absolutePositionY");
- svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionRotation, "absolutePositionRotation");
- return svi;
- }
- };
+
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
+
+ // class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
+ // {
+ // public:
+ // DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ // float absolutePositionX;
+ // float absolutePositionY;
+ // float absolutePositionRotation;
+ // static SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> GetClassMemberInfo()
+ // {
+ // SensorValueInfo<SensorValueHolonomicPlatformAbsolutePosition> svi;
+ // svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionX, "absolutePositionX");
+ // svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionY, "absolutePositionY");
+ // svi.addMemberVariable(&SensorValueHolonomicPlatformAbsolutePosition::absolutePositionRotation, "absolutePositionRotation");
+ // return svi;
+ // }
+ // };
+
+
class SensorValueHolonomicPlatform :
virtual public SensorValueHolonomicPlatformVelocity,
@@ -126,16 +139,19 @@ namespace armarx
}
};
- class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
- {
- public:
- DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
- static SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> GetClassMemberInfo()
- {
- SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> svi;
- svi.addBaseClass<SensorValueHolonomicPlatform>();
- svi.addBaseClass<SensorValueHolonomicPlatformAbsolutePosition>();
- return svi;
- }
- };
+ // The following class is deprecated: if the absolute position is provided, it will become inconsistent with the relative position.
+ // Therefore, SensorValueHolonomicPlatformRelativePosition + SensorValueHolonomicPlatformGlobalPositionCorrection should be used instead.
+
+ // class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
+ // {
+ // public:
+ // DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ // static SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> GetClassMemberInfo()
+ // {
+ // SensorValueInfo<SensorValueHolonomicPlatformWithAbsolutePosition> svi;
+ // svi.addBaseClass<SensorValueHolonomicPlatform>();
+ // svi.addBaseClass<SensorValueHolonomicPlatformAbsolutePosition>();
+ // return svi;
+ // }
+ // };
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..12c3faf13ffb311a36994002f8f05ef7122dc206
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.cpp
@@ -0,0 +1,56 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::LocalizationSubUnit
+ * @author Raphael ( raphael dot grimm at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "LocalizationSubUnit.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
+#include <RobotAPI/interface/core/GeometryBase.h>
+#include <RobotAPI/interface/core/PoseBase.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+
+namespace armarx
+{
+ LocalizationSubUnit::~LocalizationSubUnit() = default;
+
+
+ void LocalizationSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
+ {
+ if (!getProxy())
+ {
+ //this unit is not initialized yet
+ ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet - skipping this update";
+ return;
+ }
+ }
+
+
+ void LocalizationSubUnit::reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current&)
+ {
+ ARMARX_CHECK_EXPRESSION(globalPositionCorrectionSensorDevice);
+ globalPositionCorrectionSensorDevice->updateGlobalPositionCorrection(global_T_odom.transform);
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..f628ecc3164f02134d1a4c8c2e9b5915295e5762
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h
@@ -0,0 +1,89 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::GlobalPoseSubUnit
+ * @author Raphael ( raphael dot grimm at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <Eigen/Core>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/components/units/SensorActorUnit.h>
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
+
+#include "RobotUnitSubUnit.h"
+
+namespace armarx
+{
+
+ class LocalizationUnit :
+ virtual public LocalizationUnitInterface,
+ virtual public SensorActorUnit
+ {
+ public:
+ // inherited from Component
+ std::string getDefaultName() const override
+ {
+ return "LocalizationUnit";
+ }
+
+ void onInitComponent() override {}
+ void onConnectComponent() override {}
+ };
+
+
+
+ class GlobalRobotPoseCorrectionSensorDevice;
+
+ TYPEDEF_PTRS_HANDLE(LocalizationSubUnit);
+ class LocalizationSubUnit:
+ virtual public RobotUnitSubUnit,
+ virtual public LocalizationUnit,
+ virtual public LocalizationSubUnitInterface
+ {
+ public:
+ LocalizationSubUnit() = default;
+ ~LocalizationSubUnit() override;
+
+ void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
+
+ void reportGlobalRobotPoseCorrection(const TransformStamped& global_T_odom, const Ice::Current& = Ice::emptyCurrent) override;
+
+ /**
+ * This device partially holds the information about the robot's global pose.
+ * It is the transformation from the `global` to the `odometry` frame which can be
+ * obtained by SLAM. In conjunction with the relative pose (pose within the `odometry` frame),
+ * the global pose can be obtained.
+ */
+ GlobalRobotPoseCorrectionSensorDevice* globalPositionCorrectionSensorDevice;
+
+ private:
+
+ std::string agentName;
+ std::string robotRootFrame;
+
+
+ };
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 49e49a65f436d35425e2babce87e0940f0857ce4..83635aac61c22444388dfb4646b4b32f1029f918 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -24,10 +24,13 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
-#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
+
namespace armarx
{
@@ -72,47 +75,51 @@ namespace armarx
globalPoseHeader.agent = agentName;
globalPoseHeader.timestampInMicroSeconds = timestamp;
+
//pos
{
ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
- relX = s->relativePositionX;
- relY = s->relativePositionY;
- relR = s->relativePositionRotation;
+ const auto odom_T_robot = VirtualRobot::MathTools::posrpy2eigen4f(s->relativePositionX, s->relativePositionY, 0, 0, 0, s->relativePositionRotation);
// @@@ CHECK BELOW:
- if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
- {
- const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
- abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
+ // if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
+ // {
+ // const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
+ // abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
- TransformStamped currentPose;
- currentPose.header = globalPoseHeader;
- currentPose.transform = abs;
+ // TransformStamped currentPose;
+ // currentPose.header = globalPoseHeader;
+ // currentPose.transform = abs;
- globalPosePrx->reportGlobalRobotPose(currentPose);
+ // globalPosePrx->reportGlobalRobotPose(currentPose);
- // legacy interfaces
- PlatformPose platformPose = toPlatformPose(currentPose);
- listenerPrx->reportPlatformPose(platformPose);
- globalPosCtrl->setGlobalPos(platformPose);
- }
- else
- {
- abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
- }
+ // // legacy interfaces
+ // PlatformPose platformPose = toPlatformPose(currentPose);
+ // listenerPrx->reportPlatformPose(platformPose);
+ // globalPosCtrl->setGlobalPos(platformPose);
+ // }
+ // else
+ // {
+ // global_T_robot = global_T_odom * odom_T_robot;
+ // }
+
+ // const auto global_T_robot = global_T_odom * odom_T_robot;
- TransformStamped odomPose;
- odomPose.header = odomPoseHeader;
- odomPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
+ const TransformStamped odomPose
+ {
+ .header = odomPoseHeader,
+ .transform = odom_T_robot
+ };
odometryPrx->reportOdometryPose(odomPose);
// legacy interface
const auto odomLegacyPose = toPlatformPose(odomPose);
listenerPrx->reportPlatformOdometryPose(odomLegacyPose.x, odomLegacyPose.y, odomLegacyPose.rotationAroundZ);
}
+
//vel
{
ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
@@ -127,7 +134,7 @@ namespace armarx
// legacy interface
const auto& vel = odomVelocity.twist;
- listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
+ // listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
}
}
@@ -175,59 +182,23 @@ namespace armarx
maxVAng = std::abs(mxVAng);
}
- void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
- {
- std::lock_guard<std::mutex> guard {dataMutex};
- PosePtr p = PosePtr::dynamicCast(globalPose);
- positionCorrection = p->toEigen() * abs.inverse();
- }
+ // void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
+ // {
+ // std::lock_guard<std::mutex> guard {dataMutex};
+ // PosePtr p = PosePtr::dynamicCast(globalPose);
+ // global_T_odom = p->toEigen() * global_T_robot.inverse();
+ // }
- armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
- {
- std::lock_guard<std::mutex> guard {dataMutex};
- return new Pose {abs};
- }
+ // armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
+ // {
+ // std::lock_guard<std::mutex> guard {dataMutex};
+ // return new Pose {global_T_robot};
+ // }
void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
{
move(0, 0, 0);
}
- Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
- {
- Eigen::Matrix4f rp = VirtualRobot::MathTools::rpy2eigen4f(0, 0, relR);
- rp(0, 3) = relX;
- rp(1, 3) = relY;
- return rp;
- }
-
- void armarx::PlatformSubUnit::reportGlobalRobotPose(const TransformStamped& currentPose, const Ice::Current&)
- {
- globalPosCtrl->setGlobalPos(toPlatformPose(currentPose));
- }
-
-
-
- // legacy stuff
-
-
- void armarx::PlatformSubUnit::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
- {
- globalPosCtrl->setGlobalPos(currentPose);
- }
-
- void armarx::PlatformSubUnit::reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c)
- {
-
- }
-
- void armarx::PlatformSubUnit::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
- {
-
- }
-
- void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
- {
- }
-}
\ No newline at end of file
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 21ab6338ee505a526222b52b3dfea085404a91af..24f8ac9db2bd10eb6c9e8e7e2f36ac75931a69e0 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -40,6 +40,8 @@
namespace armarx
{
+ class GlobalRobotPoseCorrectionSensorDevice;
+
TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
class PlatformSubUnit:
virtual public RobotUnitSubUnit,
@@ -53,20 +55,8 @@ namespace armarx
void move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current& = Ice::emptyCurrent) override;
void moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
void moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = Ice::emptyCurrent) override;
-
void setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current& = Ice::emptyCurrent) override;
- virtual void setGlobalPose(PoseBasePtr globalPose, const Ice::Current& = Ice::emptyCurrent) /*override*/;
- virtual PoseBasePtr getGlobalPose(const Ice::Current& = Ice::emptyCurrent) /*override*/;
-
- void reportGlobalRobotPose(const TransformStamped& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
-
-
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = ::Ice::Current()) override ;
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformOdometryPose(::Ice::Float, ::Ice::Float, ::Ice::Float, const ::Ice::Current& = ::Ice::Current()) override;
-
// PlatformUnit interface
void onInitPlatformUnit() override
@@ -84,24 +74,16 @@ namespace armarx
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
+
std::size_t platformSensorIndex;
+
protected:
- Eigen::Matrix4f getRelativePose() const;
mutable std::mutex dataMutex;
Ice::Float maxVLin = std::numeric_limits<Ice::Float>::infinity();
Ice::Float maxVAng = std::numeric_limits<Ice::Float>::infinity();
- float relX = 0;
- float relY = 0;
- float relR = 0;
-
- Eigen::Matrix4f abs;
-
- // TODO(fabian.reister): likely remove or adapt
- Eigen::Matrix4f positionCorrection = Eigen::Matrix4f::Identity();
-
private:
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 2dd62115bcf3ff0895cb3ecaa04fb2d2614d2294..8e5aa6c3238a3149180217c31bfff947adc27bff 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -100,7 +100,7 @@ namespace armarx
)
{
ARMARX_TRACE;
- auto& entry = entries.at(toBounds(onePastLoggingReadPosition));
+ detail::ControlThreadOutputBufferEntry& entry = entries.at(toBounds(onePastLoggingReadPosition));
consumer(entry, offset, num);
entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 41a52f9f8a0a2d981583ae1a5c4e85a07f0ff750..bc9e81e3ed47da9b47fdf570caf549ee1fce7e65 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -51,6 +51,7 @@ set(SLICE_FILES
units/HeadIKUnit.ice
units/KinematicUnitInterface.ice
units/PlatformUnitInterface.ice
+ units/LocalizationUnitInterface.ice
units/RobotPoseUnitInterface.ice
units/TCPControlUnit.ice
units/TCPMoverUnitInterface.ice
@@ -162,4 +163,3 @@ set(SLICE_FILES_ADDITIONAL_SOURCES
armarx_interfaces_generate_library(RobotAPI "${ROBOTAPI_INTERFACE_DEPEND}")
target_link_libraries(RobotAPIInterfaces PUBLIC ArmarXCore)
-
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
index 6e5b38a32d000f219f8a868dfa8d869945637249..1d65d3a1102b1197713cf0d0d40834cfbd64b0e8 100644
--- a/source/RobotAPI/interface/core/RobotLocalization.ice
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -29,18 +29,26 @@
#include <RobotAPI/interface/core/PoseBase.ice>
module armarx{
- interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+ interface GlobalRobotPoseProvider extends armem::server::MemoryInterface
+ {
// timestamp in microseconds
PoseBase getGlobalRobotPose(long timestamp, string robotName);
}
- interface GlobalRobotPoseLocalizationListener{
+ interface GlobalRobotPoseLocalizationListener
+ {
void reportGlobalRobotPose(TransformStamped currentPose);
}
+
+ interface GlobalRobotPoseLocalizationCorrectionListener
+ {
+ void reportGlobalRobotPoseCorrection(TransformStamped global_T_odom);
+ }
- interface OdometryListener{
+ interface OdometryListener
+ {
void reportOdometryVelocity(TwistStamped platformVelocity);
void reportOdometryPose(TransformStamped odometryPose);
}
-} // module armarx
\ No newline at end of file
+} // module armarx
diff --git a/source/RobotAPI/interface/units/LocalizationUnitInterface.ice b/source/RobotAPI/interface/units/LocalizationUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..8799ee4f0e75822c2c6d59dfb77212213ec1f7bf
--- /dev/null
+++ b/source/RobotAPI/interface/units/LocalizationUnitInterface.ice
@@ -0,0 +1,53 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package ArmarX::Core
+ * @author Manfred Kroehnert (manfred dot kroehnert at kit dot edu)
+ * @copyright 2013 Manfred Kroehnert
+ * @license http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/UnitInterface.ice>
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/core/RobotLocalization.ice>
+
+#include <RobotAPI/interface/core/GeometryBase.ice>
+
+module armarx
+{
+
+ interface LocalizationUnitInterface extends SensorActorUnitInterface
+ {
+ void reportGlobalRobotPoseCorrection(TransformStamped global_T_odom);
+ };
+
+ interface LocalizationUnitListener
+ {
+
+ };
+
+ interface LocalizationSubUnitInterface extends LocalizationUnitInterface
+ {
+
+ };
+
+};
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index 148e807f9514e1ea69db397c337a7030235f5702..f7f62d13d0b490fd50284582096e0308fff90223 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
@@ -117,10 +117,9 @@ module armarx
void reportPlatformOdometryPose(float x, float y, float angle);
};
- interface PlatformSubUnitInterface extends PlatformUnitInterface, GlobalRobotPoseLocalizationListener, PlatformUnitListener
+ interface PlatformSubUnitInterface extends PlatformUnitInterface
{
};
};
-
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
index 13de0bb80cccb9d9e7a63819907d3b7d81688fbd..a85ae2c2c5531450d3f9c0440e31d8c1695884ba 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.cpp
@@ -36,7 +36,7 @@ namespace armarx::objpose
ObjectPoseSeq
- ObjectPoseClient::fetchObjectPoses()
+ ObjectPoseClient::fetchObjectPoses() const
{
if (not objectPoseStorage)
{
@@ -48,7 +48,7 @@ namespace armarx::objpose
ObjectPoseMap
- ObjectPoseClient::fetchObjectPosesAsMap()
+ ObjectPoseClient::fetchObjectPosesAsMap() const
{
ObjectPoseMap map;
for (auto& pose : fetchObjectPoses())
@@ -60,7 +60,7 @@ namespace armarx::objpose
std::optional<ObjectPose>
- ObjectPoseClient::fetchObjectPose(const ObjectID& objectID)
+ ObjectPoseClient::fetchObjectPose(const ObjectID& objectID) const
{
const auto *object = findObjectPoseByID(fetchObjectPoses(), objectID);
@@ -74,7 +74,7 @@ namespace armarx::objpose
ObjectPoseSeq
- ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName)
+ ObjectPoseClient::fetchObjectPosesFromProvider(const std::string& providerName) const
{
if (!objectPoseStorage)
{
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
index a5b5fd18d7686e0cc46cd7e0154588ca3b13b40f..8a1cb31f23f30b37724177aaff7ba30e77ec5604 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h
@@ -57,14 +57,14 @@ namespace armarx::objpose
* @return The known object poses.
*/
ObjectPoseSeq
- fetchObjectPoses();
+ fetchObjectPoses() const;
/**
* @brief Fetch all known object poses.
* @return The known object poses, with object ID as key.
*/
ObjectPoseMap
- fetchObjectPosesAsMap();
+ fetchObjectPosesAsMap() const;
/**
* @brief Fetch the pose of a single object.
@@ -76,7 +76,7 @@ namespace armarx::objpose
* @return The object's pose, if known.
*/
std::optional<ObjectPose>
- fetchObjectPose(const ObjectID& objectID);
+ fetchObjectPose(const ObjectID& objectID) const;
/**
* @brief Fetch object poses from a specific provider.
@@ -84,7 +84,7 @@ namespace armarx::objpose
* @return The object poses from that provider.
*/
ObjectPoseSeq
- fetchObjectPosesFromProvider(const std::string& providerName);
+ fetchObjectPosesFromProvider(const std::string& providerName) const;
/**
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 6ba35fd2042b04540721c95c1b48d566ad9d3d5a..46fe311e31ce6a9f80650d2178a2a425f08e17c5 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -153,7 +153,7 @@ namespace armarx::armem::server::obj::instance
stats.numUpdated = 0;
// timestamp used to reduce the rpc calls for robot sync
- Time robotSyncTimestamp = -1;
+ Time robotSyncTimestamp = Time::Invalid();
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
index f32527a0a5b541d3ecc33015e3a3a7a482109439..6e3cd9af2259531d336dda34e206013b46334206 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/RobotReader.cpp
@@ -137,15 +137,18 @@ namespace armarx::armem::robot_state
std::optional<robot::RobotDescription>
RobotReader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
+
+ const auto sanitizedTimestamp = timestamp.isValid() ? timestamp : Clock::Now();
+
// Query all entities from provider.
armem::client::query::Builder qb;
// clang-format off
qb
- .coreSegments().all() // withName(properties.descriptionCoreSegment)
- .providerSegments().all() // .withName(name)
+ .coreSegments().withName(constants::descriptionCoreSegment)
+ .providerSegments().withName(name)
.entities().all()
- .snapshots().beforeOrAtTime(timestamp);
+ .snapshots().beforeOrAtTime(sanitizedTimestamp);
// clang-format on
ARMARX_DEBUG << "Lookup query in reader";
@@ -670,7 +673,7 @@ namespace armarx::armem::robot_state
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
.getCoreSegment(constants::descriptionCoreSegment)
- .getProviderSegment(name); // TODO(fabian.reister): all
+ .getProviderSegment(name);
// clang-format on
const armem::wm::EntityInstance* instance = nullptr;
@@ -678,7 +681,7 @@ namespace armarx::armem::robot_state
{ instance = &i; });
if (instance == nullptr)
{
- ARMARX_WARNING << "No entity snapshots found";
+ ARMARX_WARNING << "No entity snapshots found in provider segment `" << name << "`";
return std::nullopt;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
index 0eb7e2dde3c4e9e56342325c1069f499230186ee..4aa5969dd34e247c2abcce2500f9c15c33353098 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp
@@ -78,16 +78,10 @@ namespace armarx::armem::robot_state
<< xmlFilename << "'";
auto robot = VirtualRobot::RobotIO::loadRobot(xmlFilename, loadMode);
+ ARMARX_CHECK_NOT_NULL(robot) << "Could not load robot from file `" << xmlFilename << "`";
+
robot->setName(name);
- const bool success = synchronizeRobot(*robot, timestamp);
-
- if (not success)
- {
- ARMARX_WARNING << "Could not synchronize robot `" << name << "` with the memory!";
- return nullptr;
- }
-
return robot;
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
index ea1d87b62fd2ad54b536b607d4ba316ae16e3777..19a184e8c4cee71e2a7866206f2ae55cd51f0d27 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h
@@ -51,7 +51,7 @@ namespace armarx::armem::robot_state
[[nodiscard]] VirtualRobot::RobotPtr
getRobot(const std::string& name,
- const armem::Time& timestamp,
+ const armem::Time& timestamp = armem::Time::Invalid(),
const VirtualRobot::RobotIO::RobotDescription& loadMode =
VirtualRobot::RobotIO::RobotDescription::eStructure);
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index 9c194be892458bdac83ce2291cc0eff9cd40dbe9..5ba3398f4ea1e8a00b4249fe411c213dda659c93 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -151,9 +151,9 @@ namespace armarx::armem::common::robot_state::localization
tfLookup[frames.front()] = frames.back();
}
- std::string currentFrame = GlobalFrame;
+ std::string currentFrame = query.header.parentFrame;
chain.push_back(currentFrame);
- while(tfLookup.count(currentFrame) > 0 and currentFrame != ::armarx::armem::robot_state::constants::robotRootNodeName)
+ while(tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
{
currentFrame = tfLookup.at(currentFrame);
chain.push_back(currentFrame);
@@ -161,7 +161,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_VERBOSE << VAROUT(chain);
- if (chain.empty() or chain.back() != ::armarx::armem::robot_state::constants::robotRootNodeName)
+ if (chain.empty() or chain.back() != query.header.frame)
{
ARMARX_WARNING << deactivateSpam(60) << "Cannot create tf lookup chain '" << query.header.parentFrame
<< " -> " << query.header.frame << "' for robot `" + query.header.agent + "`.";