diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp index 9638c020ae3f296ed139e050c086544df29ceaa6..a2825105875a0e72454972692a388f0cd8721f62 100644 --- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp @@ -31,3 +31,8 @@ ControlDevice& JointController::getParent() const ARMARX_CHECK_EXPRESSION_W_HINT(parent, "This JointController is not owned by a ControlDevice"); return *parent; } + +StringVariantBaseMap JointController::publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const +{ + return StringVariantBaseMap {}; +} diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h index 4d12be87a3b3a9fb92b7abdf7f4fdb8f5cfc3158..a82cc1c95055a28c047afdfa10d9947fa7a843fd 100644 --- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h +++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.h @@ -28,6 +28,8 @@ #include <memory> #include <atomic> +#include <RobotAPI/interface/visualization/DebugDrawerInterface.h> + namespace armarx { class ControlDevice; @@ -67,6 +69,19 @@ namespace armarx std::size_t rtGetHardwareControlModeHash() const; ControlDevice& getParent() const; + + /** + * Hook for publishing data from JointController, mainly for debugging purposes. The preferred way is to use the + * return value of the function to publish the data. This function is called in the publish thread, **not** the RT thread. + * Thus, appropriate lock-free synchronization (e.g. atomic variables or TrippleBuffer) must be used to move the data from RT + * thread to the publish thread. + * @param draw Interface proxy to the DebugDrawer topic. + * @param observer Interface proxy to DebugObserver topic. + * @return These values are published on the RobotUnitObserver in the channel "ControlDevices". The keys of the map are prepended + * with "{ControlDeviceName}_{ControlModeName}_" and are used as keys of the datafields. + * + */ + virtual StringVariantBaseMap publish(const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer) const; protected: ControlDevice& rtGetParent() const; diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp index 7a8044b855889409a577ee467d8e0d4f761aff6a..a895019046916bfd13d613214c5c80adc20c4e2a 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp @@ -238,6 +238,14 @@ namespace armarx const auto activeJointCtrl = activatedControllers.jointControllers.at(ctrlidx); status.activeControlMode = activeJointCtrl ? activeJointCtrl->getControlMode() : std::string {"!!JointController is nullptr!!"}; status.deviceName = ctrlDev.getDeviceName(); + if (activeJointCtrl) + { + auto additionalDatafields = activeJointCtrl->publish(debugDrawerPrx, debugObserverPrx); + for (auto& pair : additionalDatafields) + { + ctrlDevMap[ctrlDev.getDeviceName() + "_" + activeJointCtrl->getControlMode() + "_" + pair.first] = pair.second; + } + } for (const auto& ctrlVal : controlThreadOutputBuffer.control.at(ctrlidx)) {