diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index 2c6d48126cf3dc968b305c3f04e78d92eef23d0a..144f9edcd76acbbe536084229dfe440804add7e2 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -1,3 +1,4 @@
+
#include "GraspMemory.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -37,6 +38,7 @@ namespace armarx::armem::server::grasp
void GraspMemory::onInitComponent()
{
workingMemory().name() = memoryName;
+
workingMemory().addCoreSegment("GraspCandidate",
armarx::grasping::arondto::GraspCandidate::toAronType());
workingMemory().addCoreSegment("BimanualGraspCandidate",
@@ -49,8 +51,10 @@ namespace armarx::armem::server::grasp
void GraspMemory::onConnectComponent()
{
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
+
}
void GraspMemory::onDisconnectComponent()
@@ -81,24 +85,27 @@ namespace armarx::armem::server::grasp
// REMOTE GUI
+
void GraspMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
- {
- tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
- }
- VBoxLayout root = {tab.memoryGroup, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
+// {
+//
+// tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
+// }
+//
+// VBoxLayout root = {tab.memoryGroup, VSpacer()};
+// RemoteGui_createTab(getName(), root, &tab);
}
void GraspMemory::RemoteGui_update()
{
- if (tab.rebuild.exchange(false))
- {
- createRemoteGuiTab();
- }
+// if (tab.rebuild.exchange(false))
+// {
+//// createRemoteGuiTab();
+// }
}
}
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 3fa53590170e0ff9d288f7f130adae9814ee7446..f6710bc22978c2816aa8056da5795b0a84b5578a 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -6,6 +6,7 @@ set(LIBS
RobotAPICore
ArmarXCoreObservers
ArmarXCoreEigen3Variants
+ GraspingUtility
)
set(LIB_HEADERS
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.cpp b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
index e780bbfe388f6e484673bbd44d5cc7c3bf5f0570..d4b5ea81cab4f670ba0a073b7ae120b82295de49 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.cpp
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.cpp
@@ -38,7 +38,7 @@
using namespace armarx;
using namespace armarx::grasping;
-GraspCandidateObserver::GraspCandidateObserver()
+GraspCandidateObserver::GraspCandidateObserver() : graspCandidateWriter(memoryNameSystem())
{
}
@@ -53,6 +53,7 @@ void GraspCandidateObserver::onInitObserver()
void GraspCandidateObserver::onConnectObserver()
{
configTopic = getTopic<GraspCandidateProviderInterfacePrx>(getProperty<std::string>("ConfigTopicName").getValue());
+ graspCandidateWriter.connect();
}
PropertyDefinitionsPtr GraspCandidateObserver::createPropertyDefinitions()
@@ -125,6 +126,7 @@ void GraspCandidateObserver::reportGraspCandidates(const std::string& providerNa
{
std::unique_lock lock(dataMutex);
this->candidates[providerName] = candidates;
+ graspCandidateWriter.commitGraspCandidateSeq(candidates, armarx::armem::Time::now(), providerName);
handleProviderUpdate(providerName, candidates.size());
}
diff --git a/source/RobotAPI/components/units/GraspCandidateObserver.h b/source/RobotAPI/components/units/GraspCandidateObserver.h
index f4bb0ba3e7886d970cd33a82a03fe9525057b6c9..91169049b9376b41a8deaddf7f93ed9100979c28 100644
--- a/source/RobotAPI/components/units/GraspCandidateObserver.h
+++ b/source/RobotAPI/components/units/GraspCandidateObserver.h
@@ -25,6 +25,8 @@
#include <ArmarXCore/observers/Observer.h>
#include <RobotAPI/interface/observers/GraspCandidateObserverInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateWriter.h>
#include <mutex>
@@ -52,6 +54,7 @@ namespace armarx
*/
class GraspCandidateObserver :
virtual public Observer,
+ virtual public armarx::armem::ClientPluginUser,
virtual public grasping::GraspCandidateObserverInterface
{
public:
@@ -124,7 +127,7 @@ namespace armarx
grasping::GraspCandidateSeq selectedCandidates;
grasping::BimanualGraspCandidateSeq selectedBimanualCandidates;
-
+ armarx::armem::GraspCandidateWriter graspCandidateWriter;
void handleProviderUpdate(const std::string& providerName, int candidateCount);
};
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
index 2ba5dd359b22e50420b4da931b3344d884f52e2d..35958d52d5f984b14e5644e2372990d103eb935b 100644
--- a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -30,12 +30,14 @@ namespace armarx
{
public:
/// @brief Create a Device with the given name
- DeviceBase(const std::string& name): deviceName {name} {}
+ DeviceBase(const std::string& name) : deviceName{name}
+ {
+ }
virtual ~DeviceBase() = default;
/// @return The Device's name
const std::string& getDeviceName() const;
/// @return The Device's name
- const std::string& rtGetDeviceName() const;
+ const char* rtGetDeviceName() const;
//tags
/// @return Thes set of tags for this Device
const std::set<std::string>& getTags() const;
@@ -45,38 +47,43 @@ namespace armarx
protected:
/// @brief adds the given tag to the Device
void addDeviceTag(const std::string& tag);
+
private:
std::set<std::string> tags;
const std::string deviceName;
};
-}
+} // namespace armarx
//inline functions
namespace armarx
{
- inline bool DeviceBase::hasTag(const std::string& tag) const
+ inline bool
+ DeviceBase::hasTag(const std::string& tag) const
{
return getTags().count(tag);
}
- inline void DeviceBase::addDeviceTag(const std::string& tag)
+ inline void
+ DeviceBase::addDeviceTag(const std::string& tag)
{
tags.emplace(tag);
}
- inline const std::string& DeviceBase::getDeviceName() const
+ inline const std::string&
+ DeviceBase::getDeviceName() const
{
return deviceName;
}
- inline const std::string& DeviceBase::rtGetDeviceName() const
+ inline const char*
+ DeviceBase::rtGetDeviceName() const
{
- return deviceName;
+ return deviceName.c_str();
}
- inline const std::set<std::string>& DeviceBase::getTags() const
+ inline const std::set<std::string>&
+ DeviceBase::getTags() const
{
return tags;
}
-}
-
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index c4c7611f379c1ee00c106f2897212c580c25f853..6b251b9cbaa0db977e102580deeb17b4b0d2165e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -20,18 +20,18 @@
* GNU General Public License
*/
+#include "RobotUnitModuleControlThread.h"
+
#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/core/time/TimeUtil.h>
-#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
#include <ArmarXCore/observers/filters/rtfilters/AverageFilter.h>
#include <ArmarXCore/observers/filters/rtfilters/ButterworthFilter.h>
-
-#include "RobotUnitModuleControlThread.h"
-#include "../NJointControllers/NJointController.h"
+#include <RobotAPI/components/units/RobotUnit/util/DynamicsHelper.h>
#include "../Devices/RTThreadTimingsSensorDevice.h"
+#include "../NJointControllers/NJointController.h"
namespace armarx::RobotUnitModule
{
@@ -42,15 +42,18 @@ namespace armarx::RobotUnitModule
class NJointControllerAttorneyForControlThread
{
friend class ControlThread;
- static void RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtDeactivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateController();
}
- static void RtActivateController(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtActivateController(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtActivateController();
}
- static void RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
+ static void
+ RtDeactivateControllerBecauseOfError(const NJointControllerBasePtr& nJointCtrl)
{
nJointCtrl->rtDeactivateControllerBecauseOfError();
}
@@ -62,73 +65,111 @@ namespace armarx::RobotUnitModule
class ControlThreadDataBufferAttorneyForControlThread
{
friend class ControlThread;
- static std::vector<JointController*>& GetActivatedJointControllers(ControlThread* p)
+ static std::vector<JointController*>&
+ GetActivatedJointControllers(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointControllers;
}
- static std::vector<NJointControllerBase*>& GetActivatedNJointControllers(ControlThread* p)
+ static std::vector<NJointControllerBase*>&
+ GetActivatedNJointControllers(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .nJointControllers;
}
- static std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(ControlThread* p)
+ static std::vector<std::size_t>&
+ GetActivatedJointToNJointControllerAssignement(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement;
}
- static const std::vector<JointController*>& GetActivatedJointControllers(const ControlThread* p)
+ static const std::vector<JointController*>&
+ GetActivatedJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointControllers;
}
- static const std::vector<NJointControllerBase*>& GetActivatedNJointControllers(const ControlThread* p)
+ static const std::vector<NJointControllerBase*>&
+ GetActivatedNJointControllers(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .nJointControllers;
}
- static const std::vector<std::size_t>& GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
+ static const std::vector<std::size_t>&
+ GetActivatedJointToNJointControllerAssignement(const ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement;
}
- static void AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
+ static void
+ AcceptRequestedJointToNJointControllerAssignement(ControlThread* p)
{
- p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().jointToNJointControllerAssignement =
- p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .jointToNJointControllerAssignement = p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointToNJointControllerAssignement;
}
- static void CommitActivatedControllers(ControlThread* p)
+ static void
+ CommitActivatedControllers(ControlThread* p)
{
return p->_module<ControlThreadDataBuffer>().controllersActivated.commitWrite();
}
- static void ResetActivatedControllerAssignement(ControlThread* p)
+ static void
+ ResetActivatedControllerAssignement(ControlThread* p)
{
- return p->_module<ControlThreadDataBuffer>().controllersActivated.getWriteBuffer().resetAssignement();
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersActivated.getWriteBuffer()
+ .resetAssignement();
}
- static const std::vector<JointController*>& GetRequestedJointControllers(const ControlThread* p)
+ static const std::vector<JointController*>&
+ GetRequestedJointControllers(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointControllers;
}
- static const std::vector<NJointControllerBase*>& GetRequestedNJointControllers(const ControlThread* p)
+ static const std::vector<NJointControllerBase*>&
+ GetRequestedNJointControllers(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().nJointControllers;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .nJointControllers;
}
- static const std::vector<std::size_t>& GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
+ static const std::vector<std::size_t>&
+ GetRequestedJointToNJointControllerAssignement(const ControlThread* p)
{
//do NOT update here!
- return p->_module<ControlThreadDataBuffer>().controllersRequested.getReadBuffer().jointToNJointControllerAssignement;
+ return p->_module<ControlThreadDataBuffer>()
+ .controllersRequested.getReadBuffer()
+ .jointToNJointControllerAssignement;
}
- static bool RequestedControllersChanged(const ControlThread* p)
+ static bool
+ RequestedControllersChanged(const ControlThread* p)
{
//only place allowed to update this buffer!
return p->_module<ControlThreadDataBuffer>().controllersRequested.updateReadBuffer();
}
/// Activate a joint controller from the rt loop (only works in switch mode RTThread)
- static void RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
+ static void
+ RTSetJointController(ControlThread* p, JointController* c, std::size_t index)
{
ARMARX_CHECK_NOT_NULL(c);
//do NOT update here!
- auto& readbuf = p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
+ auto& readbuf =
+ p->_module<ControlThreadDataBuffer>().controllersRequested._getNonConstReadBuffer();
auto& j = readbuf.jointControllers;
auto& assig = readbuf.jointToNJointControllerAssignement;
auto& nj = readbuf.nJointControllers;
@@ -156,31 +197,40 @@ namespace armarx::RobotUnitModule
class DevicesAttorneyForControlThread
{
friend class ControlThread;
- static const std::vector<ControlDevicePtr>& GetControlDevices(const ControlThread* p)
+ static const std::vector<ControlDevicePtr>&
+ GetControlDevices(const ControlThread* p)
{
return p->_module<Devices>().controlDevices.values();
}
- static const std::vector<SensorDevicePtr>& GetSensorDevices(const ControlThread* p)
+ static const std::vector<SensorDevicePtr>&
+ GetSensorDevices(const ControlThread* p)
{
return p->_module<Devices>().sensorDevices.values();
}
- static const std::vector<const SensorValueBase*>& GetSensorValues(const ControlThread* p)
+ static const std::vector<const SensorValueBase*>&
+ GetSensorValues(const ControlThread* p)
{
return p->_module<Devices>().sensorValues;
}
- static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>& GetControlTargets(const ControlThread* p)
+ static const std::vector<std::vector<PropagateConst<ControlTargetBase*>>>&
+ GetControlTargets(const ControlThread* p)
{
return p->_module<Devices>().controlTargets;
}
- static RTThreadTimingsSensorDevice& GetThreadTimingsSensorDevice(const ControlThread* p)
+ static RTThreadTimingsSensorDevice&
+ GetThreadTimingsSensorDevice(const ControlThread* p)
{
return *(p->_module<Devices>().rtThreadTimingsSensorDevice);
}
- static void UpdateRobotWithSensorValues(const ControlThread* p, const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
+ static void
+ UpdateRobotWithSensorValues(const ControlThread* p,
+ const VirtualRobot::RobotPtr& robot,
+ const std::vector<VirtualRobot::RobotNodePtr>& robotNodes)
{
- p->_module<Devices>().updateVirtualRobotFromSensorValues(robot, robotNodes, p->_module<Devices>().sensorValues);
+ p->_module<Devices>().updateVirtualRobotFromSensorValues(
+ robot, robotNodes, p->_module<Devices>().sensorValues);
}
};
/**
@@ -190,7 +240,8 @@ namespace armarx::RobotUnitModule
class ManagementAttorneyForControlThread
{
friend class ControlThread;
- static bool HeartbeatMissing(const ControlThread* p)
+ static bool
+ HeartbeatMissing(const ControlThread* p)
{
const Management& m = p->_module<Management>();
long now = TimeUtil::GetTime(true).toMilliSeconds();
@@ -201,14 +252,18 @@ namespace armarx::RobotUnitModule
return (now - m.lastHeartbeat) > m.heartbeatMaxCycleMS;
}
};
-}
+} // namespace armarx::RobotUnitModule
namespace armarx::RobotUnitModule
{
- bool ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
+ bool
+ ControlThread::rtSwitchControllerSetup(SwitchControllerMode mode)
{
rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupStart();
- ARMARX_ON_SCOPE_EXIT { rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd(); };
+ ARMARX_ON_SCOPE_EXIT
+ {
+ rtGetThreadTimingsSensorDevice().rtMarkRtSwitchControllerSetupEnd();
+ };
//store controllers activated before switching controllers
{
@@ -250,27 +305,25 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_RT_LOGF_ERROR("NJoint controller that is neither SynchronousNJointController"
- " nor AsynchronousNJointController: %s", actNJC.at(i)->rtGetClassName().c_str());
+ ARMARX_RT_LOGF_ERROR(
+ "NJoint controller that is neither SynchronousNJointController"
+ " nor AsynchronousNJointController: %s",
+ actNJC.at(i)->rtGetClassName().c_str());
// Throwing exceptions in a destructor causes std::abort to be called
//throw std::logic_error{};
}
}
- for (
- std::size_t i = numSyncNj;
- i < _maxControllerCount &&
- _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
- ++i
- )
+ for (std::size_t i = numSyncNj;
+ i < _maxControllerCount &&
+ _activatedSynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i)
{
_activatedSynchronousNJointControllersIdx.at(i) = _maxControllerCount;
}
- for (
- std::size_t i = numAsyncNj;
- i < _maxControllerCount &&
- _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
- ++i
- )
+ for (std::size_t i = numAsyncNj;
+ i < _maxControllerCount &&
+ _activatedAsynchronousNJointControllersIdx.at(i) != _maxControllerCount;
+ ++i)
{
_activatedAsynchronousNJointControllersIdx.at(i) = _maxControllerCount;
}
@@ -283,7 +336,8 @@ namespace armarx::RobotUnitModule
if (!emergencyStop && ManagementAttorneyForControlThread::HeartbeatMissing(this))
{
rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
- ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!").deactivateSpam(1);
+ ARMARX_RT_LOGF_ERROR("Emergency Stop was activated because heartbeat is missing!")
+ .deactivateSpam(1);
}
const bool rtThreadOverridesControl = mode != SwitchControllerMode::IceRequests;
@@ -325,17 +379,16 @@ namespace armarx::RobotUnitModule
}
if (rtSwitchControllerSetupChangedControllers)
{
- ControlThreadDataBufferAttorneyForControlThread::ResetActivatedControllerAssignement(this);
+ ControlThreadDataBufferAttorneyForControlThread::
+ ResetActivatedControllerAssignement(this);
// the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
}
return rtSwitchControllerSetupChangedControllers;
}
- if (
- !rtThreadOverridesControl &&
+ if (!rtThreadOverridesControl &&
!ControlThreadDataBufferAttorneyForControlThread::RequestedControllersChanged(this) &&
- !rtIsInEmergencyStop()
- )
+ !rtIsInEmergencyStop())
{
return false;
}
@@ -356,8 +409,10 @@ namespace armarx::RobotUnitModule
{
++idxAct;
}
- const NJointControllerBasePtr& req = idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
- const NJointControllerBasePtr& act = idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
+ const NJointControllerBasePtr& req =
+ idxReq < n ? allReqNJ.at(idxReq) : nullptr; //may be null
+ const NJointControllerBasePtr& act =
+ idxAct < n ? allActdNJ.at(idxAct) : nullptr; //may be null if it is the last
const auto reqId = reinterpret_cast<std::uintptr_t>(req.get());
const auto actId = reinterpret_cast<std::uintptr_t>(act.get());
@@ -402,12 +457,14 @@ namespace armarx::RobotUnitModule
allActdJ.at(i) = requestedJointCtrl;
}
}
- ControlThreadDataBufferAttorneyForControlThread::AcceptRequestedJointToNJointControllerAssignement(this);
+ ControlThreadDataBufferAttorneyForControlThread::
+ AcceptRequestedJointToNJointControllerAssignement(this);
// the activated controllers are committed in rtUpdateSensorAndControlBuffer(...)
return true;
}
- void ControlThread::rtResetAllTargets()
+ void
+ ControlThread::rtResetAllTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsStart();
for (const ControlDevicePtr& controlDev : rtGetControlDevices())
@@ -417,7 +474,8 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtResetAllTargetsEnd();
}
- void ControlThread::rtHandleInvalidTargets()
+ void
+ ControlThread::rtHandleInvalidTargets()
{
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsStart();
numberOfInvalidTargetsInThisIteration = 0;
@@ -426,8 +484,11 @@ namespace armarx::RobotUnitModule
{
if (!rtGetActivatedJointControllers().at(i)->rtIsTargetValid())
{
- ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'", i, cdevs.at(i)->rtGetDeviceName().c_str());
- ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '" << cdevs.at(i)->rtGetDeviceName().c_str() << "'";
+ ARMARX_RT_LOGF_ERROR("INVALID TARGET for JointController (idx = %lu) '%s'",
+ i,
+ cdevs.at(i)->rtGetDeviceName());
+ ARMARX_ERROR << ">>>INVALID TARGET for JointController (idx = " << i << ") '"
+ << cdevs.at(i)->getDeviceName() << "'";
rtDeactivateAssignedNJointControllerBecauseOfError(i);
++numberOfInvalidTargetsInThisIteration;
}
@@ -436,7 +497,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtHandleInvalidTargetsEnd();
}
- void ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesStart();
for (const SensorDevicePtr& device : rtGetSensorDevices())
@@ -448,7 +511,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtReadSensorDeviceValuesEnd();
}
- void ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
if (dynamicsHelpers)
{
@@ -459,7 +524,9 @@ namespace armarx::RobotUnitModule
}
}
- void ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersStart();
for (const ControlDevicePtr& device : rtGetControlDevices())
@@ -469,7 +536,9 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtRunJointControllersEnd();
}
- void ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersStart();
// bool activeControllersChanged = false;
@@ -481,7 +550,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(
+ activatedNjointCtrls.at(nJointCtrlIndex));
if (!nJointCtrl)
{
continue;
@@ -492,8 +562,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
auto start = TimeUtil::GetTime(true);
@@ -504,25 +573,24 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
catch (...)
@@ -541,7 +609,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl =
+ static_cast<SynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
try
{
if (nJointCtrl)
@@ -550,8 +619,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
// activeControllersChanged = true;
}
@@ -564,26 +632,25 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
// activeControllersChanged = true;
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
}
@@ -603,7 +670,8 @@ namespace armarx::RobotUnitModule
{
break;
}
- auto nJointCtrl = static_cast<AsynchronousNJointController*>(activatedNjointCtrls.at(nJointCtrlIndex));
+ auto nJointCtrl = static_cast<AsynchronousNJointController*>(
+ activatedNjointCtrls.at(nJointCtrlIndex));
if (!nJointCtrl)
{
continue;
@@ -614,8 +682,7 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while activating it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
auto start = TimeUtil::GetTime(true);
@@ -626,25 +693,24 @@ namespace armarx::RobotUnitModule
{
ARMARX_RT_LOGF_ERROR(
"NJointControllerBase '%s' requested deactivation while running it",
- nJointCtrl->rtGetInstanceName().c_str()
- );
+ nJointCtrl->rtGetInstanceName().c_str());
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
+ if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilError)
{
- ARMARX_RT_LOGF_ERROR(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_ERROR("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
- else if (static_cast<std::size_t>(duration.toMicroSeconds()) > nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
+ else if (static_cast<std::size_t>(duration.toMicroSeconds()) >
+ nJointCtrl->rtGetNumberOfUsedControlDevices() * usPerDevUntilWarn)
{
- ARMARX_RT_LOGF_WARNING(
- "The NJointControllerBase '%s' took %ld µs to run!",
- nJointCtrl->rtGetInstanceName().c_str(),
- duration.toMicroSeconds()
- ).deactivateSpam(5);
+ ARMARX_RT_LOGF_WARNING("The NJointControllerBase '%s' took %ld µs to run!",
+ nJointCtrl->rtGetInstanceName().c_str(),
+ duration.toMicroSeconds())
+ .deactivateSpam(5);
}
}
catch (...)
@@ -659,21 +725,22 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtRunNJointControllersEnd();
}
- void ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
+ void
+ ControlThread::rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex)
{
- const NJointControllerBasePtr& nJointCtrl = rtGetActivatedNJointControllers().at(nJointCtrlIndex);
+ const NJointControllerBasePtr& nJointCtrl =
+ rtGetActivatedNJointControllers().at(nJointCtrlIndex);
NJointControllerAttorneyForControlThread::RtDeactivateControllerBecauseOfError(nJointCtrl);
for (auto ctrlDevIdx : nJointCtrl->rtGetControlDeviceUsedIndices())
{
const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx);
JointController* es = controlDevice->rtGetJointEmergencyStopController();
- ARMARX_CHECK_EQUAL(
- rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
- nJointCtrlIndex) <<
- VAROUT(ctrlDevIdx) << "\n"
- << VAROUT(controlDevice->getDeviceName()) << "\n"
- << dumpRtState();
+ ARMARX_CHECK_EQUAL(rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx),
+ nJointCtrlIndex)
+ << VAROUT(ctrlDevIdx) << "\n"
+ << VAROUT(controlDevice->getDeviceName()) << "\n"
+ << dumpRtState();
controlDevice->rtSetActiveJointController(es);
rtGetActivatedJointControllers().at(ctrlDevIdx) = es;
@@ -690,14 +757,14 @@ namespace armarx::RobotUnitModule
{
continue;
}
- ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
+ ControlDevice* const dev = rtGetControlDevices().at(ctrlDevIdx).get();
JointController* const jointCtrl = dev->rtGetActiveJointController();
const auto hwModeHash = jointCtrl->rtGetHardwareControlModeHash();
//this device is in a group!
// -> check all other devices
for (const auto otherIdx : ctrlModeGroups.deviceIndices.at(groupIdx))
{
- ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
+ ControlDevice* const otherDev = rtGetControlDevices().at(otherIdx).get();
JointController* const otherJointCtrl = otherDev->rtGetActiveJointController();
const auto otherHwModeHash = otherJointCtrl->rtGetHardwareControlModeHash();
if (hwModeHash == otherHwModeHash)
@@ -705,11 +772,13 @@ namespace armarx::RobotUnitModule
//the assigend ctrl has the same hwMode -> don't do anything
continue;
}
- const auto otherNJointCtrl1Idx = rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
+ const auto otherNJointCtrl1Idx =
+ rtGetActivatedJointToNJointControllerAssignement().at(otherIdx);
if (otherNJointCtrl1Idx == IndexSentinel())
{
//the hwmodes are different! (hence the other ctrl must be in stop movement
- ARMARX_CHECK_EXPRESSION(otherJointCtrl == otherDev->rtGetJointStopMovementController());
+ ARMARX_CHECK_EXPRESSION(otherJointCtrl ==
+ otherDev->rtGetJointStopMovementController());
//we need to activate the es contoller
JointController* const es = otherDev->rtGetJointEmergencyStopController();
otherDev->rtSetActiveJointController(es);
@@ -725,22 +794,26 @@ namespace armarx::RobotUnitModule
rtDeactivatedNJointControllerBecauseOfError(nJointCtrl);
}
- void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
+ void
+ ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex)
{
- std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
- ARMARX_CHECK_LESS(
- nJointCtrlIndex, rtGetControlDevices().size()) <<
- "no NJoint controller controls this device (name = "
- << rtGetControlDevices().at(ctrlDevIndex)->rtGetDeviceName()
- << ", ControlMode = " << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
- << "This means an invariant is violated! Dumping data for debugging:\n"
- << VAROUT(ctrlDevIndex) << "\n"
- << dumpRtState();
+ std::size_t nJointCtrlIndex =
+ rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex);
+ ARMARX_CHECK_LESS(nJointCtrlIndex, rtGetControlDevices().size())
+ << "no NJoint controller controls this device (name = "
+ << rtGetControlDevices().at(ctrlDevIndex)->getDeviceName() << ", ControlMode = "
+ << rtGetActivatedJointControllers().at(ctrlDevIndex)->getControlMode() << ")!"
+ << "\n"
+ << "This means an invariant is violated! Dumping data for debugging:\n"
+ << VAROUT(ctrlDevIndex) << "\n"
+ << dumpRtState();
rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex);
}
- void ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void
+ ControlThread::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
{
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferStart();
//commit all changes to activated controllers (joint, njoint, assignement)
@@ -762,8 +835,10 @@ namespace armarx::RobotUnitModule
for (std::size_t ctrlIdx = 0; ctrlIdx < rtGetControlDevices().size(); ++ctrlIdx)
{
ControlDevice& controlDevice = *rtGetControlDevices().at(ctrlIdx);
- ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(), sc.control.at(ctrlIdx).size());
- for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size(); ++targIdx)
+ ARMARX_CHECK_EQUAL(controlDevice.rtGetJointControllers().size(),
+ sc.control.at(ctrlIdx).size());
+ for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetJointControllers().size();
+ ++targIdx)
{
JointController& jointCtrl = *controlDevice.rtGetJointControllers().at(targIdx);
jointCtrl.getControlTarget()->_copyTo(sc.control.at(ctrlIdx).at(targIdx));
@@ -775,22 +850,26 @@ namespace armarx::RobotUnitModule
rtGetThreadTimingsSensorDevice().rtMarkRtUpdateSensorAndControlBufferEnd();
}
- const std::vector<ControlDevicePtr>& ControlThread::rtGetControlDevices() const
+ const std::vector<ControlDevicePtr>&
+ ControlThread::rtGetControlDevices() const
{
return DevicesAttorneyForControlThread::GetControlDevices(this);
}
- const std::vector<SensorDevicePtr>& ControlThread::rtGetSensorDevices()
+ const std::vector<SensorDevicePtr>&
+ ControlThread::rtGetSensorDevices()
{
return DevicesAttorneyForControlThread::GetSensorDevices(this);
}
- RTThreadTimingsSensorDevice& ControlThread::rtGetThreadTimingsSensorDevice()
+ RTThreadTimingsSensorDevice&
+ ControlThread::rtGetThreadTimingsSensorDevice()
{
return DevicesAttorneyForControlThread::GetThreadTimingsSensorDevice(this);
}
- void ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
+ void
+ ControlThread::rtSetEmergencyStopState(EmergencyStopState state)
{
if (state == EmergencyStopState::eEmergencyStopActive)
{
@@ -802,14 +881,16 @@ namespace armarx::RobotUnitModule
}
}
- void ControlThread::_preFinishControlThreadInitialization()
+ void
+ ControlThread::_preFinishControlThreadInitialization()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = std::this_thread::get_id();
_maxControllerCount = rtGetActivatedJointControllers().size();
- ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedJointToNJointControllerAssignement().size());
+ ARMARX_CHECK_EQUAL(_maxControllerCount,
+ rtGetActivatedJointToNJointControllerAssignement().size());
ARMARX_CHECK_EQUAL(_maxControllerCount, rtGetActivatedNJointControllers().size());
//resize buffers used for error oputput
preSwitchSetup_ActivatedJointControllers.resize(_maxControllerCount);
@@ -832,32 +913,40 @@ namespace armarx::RobotUnitModule
std::shared_ptr<DynamicsHelper> dynamicsHelpers(new DynamicsHelper(robotUnit));
auto bodySetName = getProperty<std::string>("InverseDynamicsRobotBodySet").getValue();
auto rtRobotBodySet = rtGetRobot()->getRobotNodeSet(bodySetName);
- ARMARX_CHECK_EXPRESSION(rtRobotBodySet) << "could not find robot node set with name: " << bodySetName << " - Check property InverseDynamicsRobotBodySet";
+ ARMARX_CHECK_EXPRESSION(rtRobotBodySet)
+ << "could not find robot node set with name: " << bodySetName
+ << " - Check property InverseDynamicsRobotBodySet";
// rtfilters::RTFilterBasePtr accFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
// rtfilters::RTFilterBasePtr velFilter(new rtfilters::ButterworthFilter(100, 1000, Lowpass, 1));
rtfilters::RTFilterBasePtr accFilter(new rtfilters::AverageFilter(30));
rtfilters::RTFilterBasePtr velFilter(new rtfilters::AverageFilter(30));
- auto setList = armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(), ",", true, true);
+ auto setList =
+ armarx::Split(getProperty<std::string>("InverseDynamicsRobotJointSets").getValue(),
+ ",",
+ true,
+ true);
for (auto& set : setList)
{
auto rns = rtGetRobot()->getRobotNodeSet(set);
- ARMARX_CHECK_EXPRESSION(rns) << "could not find robot node set with name: " << set << " - Check property InverseDynamicsRobotJointSets";
+ ARMARX_CHECK_EXPRESSION(rns) << "could not find robot node set with name: " << set
+ << " - Check property InverseDynamicsRobotJointSets";
dynamicsHelpers->addRobotPart(rns, rtRobotBodySet, velFilter, accFilter);
ARMARX_VERBOSE << "Added nodeset " << set << " for inverse dynamics";
}
this->dynamicsHelpers = dynamicsHelpers;
}
-
}
- void ControlThread::_preOnInitRobotUnit()
+ void
+ ControlThread::_preOnInitRobotUnit()
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
try
{
- rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(), VirtualRobot::RobotIO::eStructure);
+ rtRobot = VirtualRobot::RobotIO::loadRobot(_module<RobotData>().getRobotFileName(),
+ VirtualRobot::RobotIO::eStructure);
rtRobot->setUpdateCollisionModel(false);
rtRobot->setUpdateVisualization(false);
rtRobot->setThreadsafe(false);
@@ -867,37 +956,49 @@ namespace armarx::RobotUnitModule
{
throw UserException(e.what());
}
- usPerDevUntilWarn = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilWarning").getValue();
- usPerDevUntilError = getProperty<std::size_t>("NjointController_AllowedExecutionTimePerControlDeviceUntilError").getValue();
+ usPerDevUntilWarn = getProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning")
+ .getValue();
+ usPerDevUntilError = getProperty<std::size_t>(
+ "NjointController_AllowedExecutionTimePerControlDeviceUntilError")
+ .getValue();
}
- void ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ void
+ ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
- EmergencyStopState ControlThread::getEmergencyStopState(const Ice::Current&) const
+ EmergencyStopState
+ ControlThread::getEmergencyStopState(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return emergencyStop ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return emergencyStop ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
- EmergencyStopState ControlThread::getRtEmergencyStopState(const Ice::Current&) const
+ EmergencyStopState
+ ControlThread::getRtEmergencyStopState(const Ice::Current&) const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive : EmergencyStopState::eEmergencyStopInactive;
+ return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive
+ : EmergencyStopState::eEmergencyStopInactive;
}
- void ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
+ void
+ ControlThread::setEmergencyStopStateNoReportToTopic(EmergencyStopState state)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
emergencyStop = (state == EmergencyStopState::eEmergencyStopActive);
}
- void ControlThread::processEmergencyStopRequest()
+ void
+ ControlThread::processEmergencyStopRequest()
{
- const auto state = emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
+ const auto state =
+ emergencyStopStateRequest.exchange(EmergencyStopStateRequest::RequestNone);
switch (state)
{
case EmergencyStopStateRequest::RequestActive:
@@ -909,56 +1010,70 @@ namespace armarx::RobotUnitModule
case EmergencyStopStateRequest::RequestNone:
break;
default:
- ARMARX_CHECK_EXPRESSION(!static_cast<int>(state)) << "Unkown value for EmergencyStopStateRequest";
+ ARMARX_CHECK_EXPRESSION(!static_cast<int>(state))
+ << "Unkown value for EmergencyStopStateRequest";
}
}
- const std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers() const
+ const std::vector<JointController*>&
+ ControlThread::rtGetActivatedJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
}
- const std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers() const
+ const std::vector<NJointControllerBase*>&
+ ControlThread::rtGetActivatedNJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
- const std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
+ const std::vector<std::size_t>&
+ ControlThread::rtGetActivatedJointToNJointControllerAssignement() const
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetActivatedJointToNJointControllerAssignement(this);
}
- std::vector<JointController*>& ControlThread::rtGetActivatedJointControllers()
+ std::vector<JointController*>&
+ ControlThread::rtGetActivatedJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointControllers(this);
}
- std::vector<NJointControllerBase*>& ControlThread::rtGetActivatedNJointControllers()
+ std::vector<NJointControllerBase*>&
+ ControlThread::rtGetActivatedNJointControllers()
{
return ControlThreadDataBufferAttorneyForControlThread::GetActivatedNJointControllers(this);
}
- std::vector<std::size_t>& ControlThread::rtGetActivatedJointToNJointControllerAssignement()
+ std::vector<std::size_t>&
+ ControlThread::rtGetActivatedJointToNJointControllerAssignement()
{
- return ControlThreadDataBufferAttorneyForControlThread::GetActivatedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetActivatedJointToNJointControllerAssignement(this);
}
- const std::vector<JointController*>& ControlThread::rtGetRequestedJointControllers() const
+ const std::vector<JointController*>&
+ ControlThread::rtGetRequestedJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointControllers(this);
}
- const std::vector<NJointControllerBase*>& ControlThread::rtGetRequestedNJointControllers() const
+ const std::vector<NJointControllerBase*>&
+ ControlThread::rtGetRequestedNJointControllers() const
{
return ControlThreadDataBufferAttorneyForControlThread::GetRequestedNJointControllers(this);
}
- const std::vector<std::size_t>& ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
+ const std::vector<std::size_t>&
+ ControlThread::rtGetRequestedJointToNJointControllerAssignement() const
{
- return ControlThreadDataBufferAttorneyForControlThread::GetRequestedJointToNJointControllerAssignement(this);
+ return ControlThreadDataBufferAttorneyForControlThread::
+ GetRequestedJointToNJointControllerAssignement(this);
}
- void ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
+ void
+ ControlThread::rtSyncNJointControllerRobot(NJointControllerBase* njctrl)
{
if (njctrl->rtGetRobot())
{
@@ -972,7 +1087,8 @@ namespace armarx::RobotUnitModule
}
- void ControlThread::dumpRtControllerSetup(
+ void
+ ControlThread::dumpRtControllerSetup(
std::ostream& out,
const std::string& indent,
const std::vector<JointController*>& activeJCtrls,
@@ -985,8 +1101,9 @@ namespace armarx::RobotUnitModule
for (std::size_t i = 0; i < cdevs.size(); ++i)
{
const JointController* jctrl = activeJCtrls.at(i);
- out << indent << "\t(" << i << ")\t" << cdevs.at(i)->rtGetDeviceName() << ":\n"
- << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl << ")\n"
+ out << indent << "\t(" << i << ")\t" << cdevs.at(i)->getDeviceName() << ":\n"
+ << indent << "\t\t Controller: " << jctrl->getControlMode() << " (" << jctrl
+ << ")\n"
<< indent << "\t\t Assigned NJoint: " << assignement.at(i) << "\n";
}
}
@@ -995,7 +1112,7 @@ namespace armarx::RobotUnitModule
for (std::size_t i = 0; i < activeNJCtrls.size(); ++i)
{
const auto* njctrl = activeNJCtrls.at(i);
- out << indent << "\t(" << i << ")\t";
+ out << indent << "\t(" << i << ")\t";
if (njctrl)
{
out << njctrl->rtGetInstanceName() << " (" << njctrl << "):"
@@ -1003,57 +1120,58 @@ namespace armarx::RobotUnitModule
}
else
{
- out << " (" << njctrl << ")\n";
+ out << " (" << njctrl << ")\n";
}
}
}
-
}
- std::string ControlThread::dumpRtState() const
+ std::string
+ ControlThread::dumpRtState() const
{
std::stringstream str;
str << "state requested\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetRequestedJointControllers(),
- rtGetRequestedJointToNJointControllerAssignement(),
- rtGetRequestedNJointControllers());
+ dumpRtControllerSetup(str,
+ "\t",
+ rtGetRequestedJointControllers(),
+ rtGetRequestedJointToNJointControllerAssignement(),
+ rtGetRequestedNJointControllers());
str << "state before rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- preSwitchSetup_ActivatedJointControllers,
- preSwitchSetup_ActivatedJointToNJointControllerAssignement,
- preSwitchSetup_ActivatedNJointControllers);
+ dumpRtControllerSetup(str,
+ "\t",
+ preSwitchSetup_ActivatedJointControllers,
+ preSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ preSwitchSetup_ActivatedNJointControllers);
str << "state after rtSwitchControllerSetup() was called\n";
- dumpRtControllerSetup(
- str, "\t",
- postSwitchSetup_ActivatedJointControllers,
- postSwitchSetup_ActivatedJointToNJointControllerAssignement,
- postSwitchSetup_ActivatedNJointControllers);
+ dumpRtControllerSetup(str,
+ "\t",
+ postSwitchSetup_ActivatedJointControllers,
+ postSwitchSetup_ActivatedJointToNJointControllerAssignement,
+ postSwitchSetup_ActivatedNJointControllers);
str << "state now\n";
- dumpRtControllerSetup(
- str, "\t",
- rtGetActivatedJointControllers(),
- rtGetActivatedJointToNJointControllerAssignement(),
- rtGetActivatedNJointControllers());
+ dumpRtControllerSetup(str,
+ "\t",
+ rtGetActivatedJointControllers(),
+ rtGetActivatedJointToNJointControllerAssignement(),
+ rtGetActivatedNJointControllers());
str << VAROUT(rtSwitchControllerSetupChangedControllers) << "\n";
str << VAROUT(numberOfInvalidTargetsInThisIteration) << "\n";
return str.str();
}
- void ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
+ void
+ ControlThread::rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot)
{
rtRobot->setGlobalPose(gp, updateRobot);
}
- void ControlThread::rtSetJointController(JointController* c, std::size_t index)
+ void
+ ControlThread::rtSetJointController(JointController* c, std::size_t index)
{
ControlThreadDataBufferAttorneyForControlThread::RTSetJointController(this, c, index);
-
}
-}
+} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 1ca557e8d84cd565d689445a3f4b0c5e79a256aa..5cf5fed082abdcb79f4f1de97ec9982c90f10d92 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -21,19 +21,18 @@
*/
-#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <ArmarXCore/util/CPPUtility/Iterator.h>
-#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-
#include "RobotUnitModuleLogging.h"
-#include "RobotUnitModuleControlThreadDataBuffer.h"
-#include "RobotUnitModuleDevices.h"
+#include <regex>
-#include "../util/ControlThreadOutputBuffer.h"
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
-#include <regex>
+#include "../util/ControlThreadOutputBuffer.h"
+#include "RobotUnitModuleControlThreadDataBuffer.h"
+#include "RobotUnitModuleDevices.h"
namespace armarx::RobotUnitModule::details
{
@@ -46,16 +45,19 @@ namespace armarx::RobotUnitModule::details
t = IceUtil::Time::seconds(0);
}
IceUtil::Time t;
- void start()
+ void
+ start()
{
t -= IceUtil::Time::now();
++n;
}
- void stop()
+ void
+ stop()
{
t += IceUtil::Time::now();
}
- double ms() const
+ double
+ ms() const
{
return t.toMilliSecondsDouble();
}
@@ -75,22 +77,28 @@ namespace armarx::RobotUnitModule::details
DurationsEntry backlog;
DurationsEntry msg;
};
-}
+} // namespace armarx::RobotUnitModule::details
namespace armarx::RobotUnitModule
{
- void Logging::addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
+ void
+ Logging::addMarkerToRtLog(const SimpleRemoteReferenceCounterBasePtr& token,
+ const std::string& marker,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
+ throw InvalidArgumentException{"addMarkerToRtLog called for a nonexistent log"};
}
rtLoggingEntries.at(token->getId())->marker = marker;
}
- SimpleRemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
+ SimpleRemoteReferenceCounterBasePtr
+ Logging::startRtLogging(const std::string& formatString,
+ const Ice::StringSeq& loggingNames,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -102,28 +110,31 @@ namespace armarx::RobotUnitModule
return startRtLoggingWithAliasNames(formatString, alias, Ice::emptyCurrent);
}
- void Logging::stopRtLogging(const SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current&)
+ void
+ Logging::stopRtLogging(const SimpleRemoteReferenceCounterBasePtr& token, const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
ARMARX_CHECK_NOT_NULL(token);
- try {
+ try
+ {
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"stopRtLogging called for a nonexistent log"};
+ throw InvalidArgumentException{"stopRtLogging called for a nonexistent log"};
}
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file "
+ << rtLoggingEntries.at(token->getId())->filename;
rtLoggingEntries.at(token->getId())->stopLogging = true;
}
catch (...)
{
ARMARX_WARNING << "Error during attempting to stop rt logging";
}
-
}
- Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
+ Ice::StringSeq
+ Logging::getLoggingNames(const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -148,18 +159,19 @@ namespace armarx::RobotUnitModule
return result;
}
- SimpleRemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(
- const std::string& formatString,
- const StringStringDictionary& aliasNames,
- const Ice::Current&)
+ SimpleRemoteReferenceCounterBasePtr
+ Logging::startRtLoggingWithAliasNames(const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- FileSystemPathBuilder pb {formatString};
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ FileSystemPathBuilder pb{formatString};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (rtLoggingEntries.count(pb.getPath()))
{
- throw InvalidArgumentException {"There already is a logger for the path '" + pb.getPath() + "'"};
+ throw InvalidArgumentException{"There already is a logger for the path '" +
+ pb.getPath() + "'"};
}
rtLoggingEntries[pb.getPath()].reset(new CSVLoggingEntry());
auto ptr = rtLoggingEntries[pb.getPath()];
@@ -170,14 +182,14 @@ namespace armarx::RobotUnitModule
if (!e.stream)
{
rtLoggingEntries.erase(pb.getPath());
- throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
+ throw LogicError{"RtLogging could not open filestream for '" + pb.getPath() + "'"};
}
- ARMARX_INFO << "Start logging to " << e.filename
- << ". Names (pattern, replacement name): " << aliasNames;
+ ARMARX_VERBOSE << "Start logging to " << e.filename
+ << ". Names (pattern, replacement name): " << aliasNames;
std::stringstream header;
header << "marker;iteration;timestamp;TimeSinceLastIteration";
- auto logDev = [&](const std::string & dev)
+ auto logDev = [&](const std::string& dev)
{
ARMARX_TRACE_LITE;
for (const auto& [key, value] : aliasNames)
@@ -231,38 +243,45 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
//write header
- e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
+ e.stream << header.str()
+ << std::flush; // newline is written at the beginning of each log line
//create and return handle
auto block = getArmarXManager()->createSimpleRemoteReferenceCount(
- [ptr]()
- {
- ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
- ptr->stopLogging = true;
- }, e.filename, IceUtil::Time::milliSeconds(100));
+ [ptr]()
+ {
+ ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << ptr->filename;
+ ptr->stopLogging = true;
+ },
+ e.filename,
+ IceUtil::Time::milliSeconds(100));
auto counter = block->getReferenceCounter();
block->activateCounting();
ARMARX_DEBUG_S << "RobotUnit: start RtLogging for file " << ptr->filename;
return counter;
}
- void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
+ void
+ Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
- FileSystemPathBuilder pb {formatString};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
+ FileSystemPathBuilder pb{formatString};
pb.createParentDirectories();
- std::ofstream outCSV {pb.getPath() + ".csv"};
+ std::ofstream outCSV{pb.getPath() + ".csv"};
if (!outCSV)
{
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".csv'"};
+ throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
+ pb.getPath() + ".csv'"};
}
- std::ofstream outMsg {pb.getPath() + ".messages"};
+ std::ofstream outMsg{pb.getPath() + ".messages"};
if (!outMsg)
{
- throw LogicError {"writeRecentIterationsToFile could not open filestream for '" + pb.getPath() + ".messages'"};
+ throw LogicError{"writeRecentIterationsToFile could not open filestream for '" +
+ pb.getPath() + ".messages'"};
}
- ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath() << ".{csv, messages}";
+ ARMARX_INFO << "writing the last " << backlog.size() << " iterations to " << pb.getPath()
+ << ".{csv, messages}";
//write csv header
{
outCSV << "iteration;timestamp";
@@ -290,16 +309,17 @@ namespace armarx::RobotUnitModule
{
//write csv data
{
- outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
+ outCSV << iteration.iteration << ";" << iteration.sensorValuesTimestamp;
//sens
{
for (const SensorValueBase* val : iteration.sensors)
{
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
std::string s;
val->getDataFieldAs(idxField, s);
- outCSV << ";" << s;
+ outCSV << ";" << s;
}
}
}
@@ -309,11 +329,12 @@ namespace armarx::RobotUnitModule
{
for (const ControlTargetBase* val : vals)
{
- for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
+ for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
+ ++idxField)
{
std::string s;
val->getDataFieldAs(idxField, s);
- outCSV << ";" << s;
+ outCSV << ";" << s;
}
}
}
@@ -323,7 +344,8 @@ namespace armarx::RobotUnitModule
//write message data
{
bool atLeastOneMessage = false;
- for (const ::armarx::detail::RtMessageLogEntryBase* msg : iteration.messages.getEntries())
+ for (const ::armarx::detail::RtMessageLogEntryBase* msg :
+ iteration.messages.getEntries())
{
if (!msg)
{
@@ -339,27 +361,28 @@ namespace armarx::RobotUnitModule
outMsg << "\nmessages lost: " << iteration.messages.messagesLost
<< " (required additional "
<< iteration.messages.requiredAdditionalBufferSpace << " bytes, "
- << iteration.messages.requiredAdditionalEntries << " message entries)\n" << std::endl;
+ << iteration.messages.requiredAdditionalEntries << " message entries)\n"
+ << std::endl;
}
}
}
}
- RobotUnitDataStreaming::DataStreamingDescription Logging::startDataStreaming(
- const RobotUnitDataStreaming::ReceiverPrx& receiver,
- const RobotUnitDataStreaming::Config& config,
- const Ice::Current&)
+ RobotUnitDataStreaming::DataStreamingDescription
+ Logging::startDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
if (!receiver)
{
- throw InvalidArgumentException {"Receiver proxy is NULL!"};
+ throw InvalidArgumentException{"Receiver proxy is NULL!"};
}
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (rtDataStreamingEntry.count(receiver))
{
- throw InvalidArgumentException {"There already is a logger for the given receiver"};
+ throw InvalidArgumentException{"There already is a logger for the given receiver"};
}
RobotUnitDataStreaming::DataStreamingDescription result;
@@ -369,7 +392,7 @@ namespace armarx::RobotUnitModule
ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
<< ". Values: " << config.loggingNames;
- auto devMatchesAnyKey = [&](const std::string & dev)
+ auto devMatchesAnyKey = [&](const std::string& dev)
{
for (const auto& key : config.loggingNames)
{
@@ -381,11 +404,10 @@ namespace armarx::RobotUnitModule
return false;
};
- const auto handleVal = [&](
- const ValueMetaData & valData,
- DataStreamingEntry & streamingEntry,
- RobotUnitDataStreaming::DataStreamingDescription & descr
- ) -> std::vector<DataStreamingEntry::OutVal>
+ const auto handleVal = [&](const ValueMetaData& valData,
+ DataStreamingEntry& streamingEntry,
+ RobotUnitDataStreaming::DataStreamingDescription& descr)
+ -> std::vector<DataStreamingEntry::OutVal>
{
ARMARX_TRACE_LITE;
std::vector<DataStreamingEntry::OutVal> result;
@@ -397,35 +419,34 @@ namespace armarx::RobotUnitModule
continue; //do not add to result and skipp during processing
}
auto& descrEntr = descr.entries[valData.fields.at(i).name];
- //formatter failes here!
- //*INDENT-OFF*
- #define make_case(Type, TName) \
- (typeid(Type) == *valData.fields.at(i).type) \
- { \
- descrEntr.index = streamingEntry.num##TName##s; \
- descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
- result.at(i).idx = streamingEntry.num##TName##s; \
- result.at(i).value = DataStreamingEntry::ValueT::TName; \
- ++streamingEntry.num##TName##s; \
- }
- if make_case(bool, Bool)
- else if make_case(Ice::Byte, Byte)
- else if make_case(Ice::Short, Short)
- else if make_case(Ice::Int, Int)
- else if make_case(Ice::Long, Long)
- else if make_case(Ice::Float, Float)
- else if make_case(Ice::Double, Double)
- else if make_case(std::uint16_t, Long)
- else if make_case(std::uint32_t, Long)
- else
- {
- ARMARX_CHECK_EXPRESSION(false)
+//formatter failes here!
+//*INDENT-OFF*
+#define make_case(Type, TName) \
+ (typeid(Type) == *valData.fields.at(i).type) \
+ { \
+ descrEntr.index = streamingEntry.num##TName##s; \
+ descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
+ result.at(i).idx = streamingEntry.num##TName##s; \
+ result.at(i).value = DataStreamingEntry::ValueT::TName; \
+ ++streamingEntry.num##TName##s; \
+ }
+ if make_case (bool, Bool)
+ else if make_case (Ice::Byte, Byte) else if make_case (Ice::Short, Short) else if make_case (Ice::Int, Int) else if make_case (
+ Ice::Long,
+ Long) else if make_case (Ice::Float,
+ Float) else if make_case (Ice::Double,
+ Double) else if make_case (std::
+ uint16_t,
+ Long) else if make_case (std::
+ uint32_t,
+ Long) else
+ {
+ ARMARX_CHECK_EXPRESSION(false)
<< "This code sould be unreachable! "
"The type of "
- << valData.fields.at(i).name
- << " is not handled correctly!";
- }
- #undef make_case
+ << valData.fields.at(i).name << " is not handled correctly!";
+ }
+#undef make_case
//*INDENT-ON*
}
return result;
@@ -437,8 +458,7 @@ namespace armarx::RobotUnitModule
streamingEntry.sensDevs.reserve(sensorDeviceValueMetaData.size());
for (const auto& valData : sensorDeviceValueMetaData)
{
- streamingEntry.sensDevs.emplace_back(
- handleVal(valData, streamingEntry, result));
+ streamingEntry.sensDevs.emplace_back(handleVal(valData, streamingEntry, result));
}
}
//get logged control device values
@@ -452,8 +472,7 @@ namespace armarx::RobotUnitModule
ctrlDevEntrs.reserve(devData.size());
for (const auto& valData : devData)
{
- ctrlDevEntrs.emplace_back(
- handleVal(valData, streamingEntry, result));
+ ctrlDevEntrs.emplace_back(handleVal(valData, streamingEntry, result));
}
}
}
@@ -461,20 +480,23 @@ namespace armarx::RobotUnitModule
return result;
}
- void Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver, const Ice::Current&)
+ void
+ Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current&)
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (!rtDataStreamingEntry.count(receiver))
{
- throw InvalidArgumentException {"stopDataStreaming called for a nonexistent log"};
+ throw InvalidArgumentException{"stopDataStreaming called for a nonexistent log"};
}
ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
rtDataStreamingEntry.at(receiver).stopStreaming = true;
}
- void Logging::_preFinishRunning()
+ void
+ Logging::_preFinishRunning()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
@@ -488,46 +510,56 @@ namespace armarx::RobotUnitModule
}
}
- void Logging::_preFinishControlThreadInitialization()
+ void
+ Logging::_preFinishControlThreadInitialization()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = LogSender::getThreadId();
- ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
+ ControlThreadOutputBuffer::RtLoggingInstance =
+ &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
ARMARX_INFO << "starting rt logging with timestep " << rtLoggingTimestepMs;
stopRtLoggingTask = false;
- rtLoggingTask = std::thread{[&]{
+ rtLoggingTask = std::thread{
+ [&]
+ {
using clock_t = std::chrono::steady_clock;
- const auto now = []{return clock_t::now();};
+ const auto now = [] { return clock_t::now(); };
while (!stopRtLoggingTask)
{
const auto start = now();
doLogging();
- const std::uint64_t ms = std::chrono::duration_cast<std::chrono::milliseconds>(now() - start).count();
+ const std::uint64_t ms =
+ std::chrono::duration_cast<std::chrono::milliseconds>(now() - start)
+ .count();
if (ms < rtLoggingTimestepMs)
{
- std::this_thread::sleep_for(std::chrono::milliseconds{rtLoggingTimestepMs - ms});
+ std::this_thread::sleep_for(
+ std::chrono::milliseconds{rtLoggingTimestepMs - ms});
continue;
}
- ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms
- << " ms > " << rtLoggingTimestepMs << " ms (message printed every 10 seconds)";
+ ARMARX_WARNING << deactivateSpam(10) << "logging thread took " << ms << " ms > "
+ << rtLoggingTimestepMs
+ << " ms (message printed every 10 seconds)";
}
}};
}
- void Logging::doLogging()
+ void
+ Logging::doLogging()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
- std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ std::lock_guard<std::mutex> guard{rtLoggingMutex};
const auto now = IceUtil::Time::now();
// entries are removed last
//remove backlog entries
const auto start_time_remove_backlog_entries = IceUtil::Time::now();
{
- while (!backlog.empty() && backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
+ while (!backlog.empty() &&
+ backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
{
backlog.pop_front();
}
@@ -539,19 +571,21 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
{
- ARMARX_INFO << deactivateSpam()
- << "Number of logs " << rtLoggingEntries.size() << '\n'
- << "Number of streams " << rtDataStreamingEntry.size();
+ ARMARX_DEBUG << deactivateSpam() << "Number of logs " << rtLoggingEntries.size()
+ << '\n'
+ << "Number of streams " << rtDataStreamingEntry.size();
}
- _module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
- [&](const auto & data, auto i, auto num)
- {
- ARMARX_TRACE;
- doLogging(dlogdurs, now, data, i, num);
- }
- );
+ _module<ControlThreadDataBuffer>()
+ .getControlThreadOutputBuffer()
+ .foreachNewLoggingEntry(
+ [&](const auto& data, auto i, auto num)
+ {
+ ARMARX_TRACE;
+ doLogging(dlogdurs, now, data, i, num);
+ });
}
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
+ << " iterations are stored";
//flush all files
const auto start_time_flush_all_files = IceUtil::Time::now();
{
@@ -604,33 +638,38 @@ namespace armarx::RobotUnitModule
rtDataStreamingEntry.erase(prx);
}
}
+ // clang-format off
const auto end_time = IceUtil::Time::now();
const auto time_total = (end_time - now).toMilliSecondsDouble();
- ARMARX_DEBUG_S << "rtlogging time required: " << time_total << "ms\n"
- << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
- << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
- << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
- << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
- << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
- << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
- << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
- << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
- << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
- << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
- << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
- << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
- << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
- << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
- << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
- << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
- << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
- << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
+ ARMARX_DEBUG << deactivateSpam(1)
+ << "rtlogging time required: " << time_total << "ms\n"
+ << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
+ << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
+ << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
+ << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
+ << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
+ << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
+ << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
+ << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
+ << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
+ << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
+ << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
+ << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
+ << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
+ << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
+ // clang-format on
}
- void Logging::doLogging(details::DoLoggingDurations& durations,
- const IceUtil::Time& now, const
- ControlThreadOutputBuffer::Entry& data,
- std::size_t i, std::size_t num)
+ void
+ Logging::doLogging(details::DoLoggingDurations& durations,
+ const IceUtil::Time& now,
+ const ControlThreadOutputBuffer::Entry& data,
+ std::size_t i,
+ std::size_t num)
{
ARMARX_TRACE;
@@ -645,8 +684,7 @@ namespace armarx::RobotUnitModule
for (auto& [_, e] : rtLoggingEntries)
{
e->stream << "\n"
- << e->marker << ";"
- << data.iteration << ";"
+ << e->marker << ";" << data.iteration << ";"
<< data.sensorValuesTimestamp.toMicroSeconds() << ";"
<< data.timeSinceLastIteration.toMicroSeconds();
e->marker.clear();
@@ -666,416 +704,425 @@ 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)
{
- //sens
+ 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)
{
- ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ if (!rtLoggingEntries.empty())
{
- 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)
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- if (!rtLoggingEntries.empty())
- {
- 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())
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- 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();
+ entry->stream << ";" << str;
}
}
+ durations.sens_csv.stop();
}
- durations.sens.stop();
- }
- //ctrl
- {
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
+ if (!rtDataStreamingEntry.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.sens_stream.start();
+ for (auto& [_, data] : rtDataStreamingEntry)
{
- 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 (!rtLoggingEntries.empty())
- {
- 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.sens_stream_elem.start();
+ data.processSens(*val, idxDev, idxField);
+ durations.sens_stream_elem.stop();
}
+ durations.sens_stream.stop();
}
- durations.ctrl.stop();
}
}
- //finish processing
+ durations.sens.stop();
+ }
+ //ctrl
+ {
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
- //store data to backlog
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.backlog.start();
- ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
+ 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)
{
- 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)
+ if (!rtLoggingEntries.empty())
{
- break;
+ 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();
}
- ptr->print(controlThreadId);
}
- durations.msg.stop();
}
}
+ durations.ctrl.stop();
}
-
- bool Logging::MatchName(const std::string& pattern, const std::string& name)
+} // namespace armarx::RobotUnitModule
+//finish processing
+{
+ //store data to backlog
{
+ durations.backlog.start();
ARMARX_TRACE;
- if (pattern.empty())
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- return false;
+ backlog.emplace_back(data, true); //true for minimal copy
}
- static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
- if (!std::regex_match(pattern, pattern_regex))
+ durations.backlog.stop();
+ }
+ //print + reset messages
+ {
+ durations.msg.start();
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- throw InvalidArgumentException {"Pattern '" + pattern + "' is invalid"};
+ if (!ptr)
+ {
+ break;
+ }
+ ptr->print(controlThreadId);
}
- 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);
+ durations.msg.stop();
}
+}
+}
- void Logging::_postOnInitRobotUnit()
+bool
+Logging::MatchName(const std::string& pattern, const std::string& name)
+{
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- 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";
+ return false;
+ }
+ static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
+ if (!std::regex_match(pattern, pattern_regex))
+ {
+ 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);
+}
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+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";
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- rtLoggingBacklogRetentionTime = IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- }
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
- void Logging::_postFinishDeviceInitialization()
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+}
+
+void
+Logging::_postFinishDeviceInitialization()
+{
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
{
ARMARX_TRACE;
- 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;
+ 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
+ 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)
{
- ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto * val, const std::string & namePre)
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
+ for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
{
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
- for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
- {
- 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())
- {
- 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()));
- }
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ 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())
{
- ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(),
- "sens." +
- sd->getDeviceName()));
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
- {
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
- }
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
-
- void Logging::DataStreamingEntry::clearResult()
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- for (auto& e : result)
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
{
- entryBuffer.emplace_back(std::move(e));
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
}
- result.clear();
}
+}
- RobotUnitDataStreaming::TimeStep Logging::DataStreamingEntry::allocateResultElement() const
+void
+Logging::DataStreamingEntry::clearResult()
+{
+ ARMARX_TRACE;
+ for (auto& e : result)
{
- 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;
+ entryBuffer.emplace_back(std::move(e));
}
+ 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()
+RobotUnitDataStreaming::TimeStep
+Logging::DataStreamingEntry::getResultElement()
+{
+ ARMARX_TRACE;
+ if (entryBuffer.empty())
{
- ARMARX_TRACE;
- if (entryBuffer.empty())
- {
- return allocateResultElement();
- }
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
+ return allocateResultElement();
}
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
+}
- void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+void
+Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- 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();
+ return;
}
+ result.emplace_back(getResultElement());
- void WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
+ 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
{
ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
+ switch (out.value)
{
- 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;
+ 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;
}
}
-
- void Logging::DataStreamingEntry::processCtrl(
- const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
+ catch (...)
{
- 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);
+ 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::processSens(
- const SensorValueBase& val,
- std::size_t didx,
- std::size_t fidx)
+void
+Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
+ return;
}
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(*this, o, val, fidx, data);
+}
- void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
- std::uint64_t msgId)
+void
+Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- 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";
+ return;
+ }
+ 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)
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
+ {
+ try
{
- try
- {
- if (!updateCalls.at(i)->isCompleted())
- {
- break;
- }
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
+ if (!updateCalls.at(i)->isCompleted())
{
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
- {
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
- }
+ break;
}
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- if (!updateCalls.empty())
+ catch (...)
{
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ 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);
+ }
+}
}
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 638e9737b6e47043b1f6c253f6bb640b5f596a00..1d5bd4be73c9215365c746f875731042e4a54f48 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -72,6 +72,7 @@ set(SLICE_FILES
units/RobotUnit/NJointCartesianWaypointController.ice
units/RobotUnit/NJointCartesianNaturalPositionController.ice
units/RobotUnit/RobotUnitInterface.ice
+ units/RobotUnit/GazeController.ice
units/RobotUnit/NJointBimanualForceController.ice
units/RobotUnit/NJointBimanualObjLevelController.ice
diff --git a/source/RobotAPI/interface/units/RobotUnit/GazeController.ice b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
new file mode 100644
index 0000000000000000000000000000000000000000..c09e65097f5bd4458a2c074177b7652a8181ad03
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/GazeController.ice
@@ -0,0 +1,63 @@
+
+/*
+* 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 ArmarX::RobotAPI
+* @author Raphael Grimm
+* @copyright 2019 Humanoids Group, H2T, KIT
+* @license http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include <RobotAPI/interface/core/FramedPoseBase.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+
+module armarx { module control { module gaze_controller {
+
+ class GazeControllerConfig extends NJointControllerConfig
+ {
+ string cameraFrameName = "DepthCamera";
+ string yawNodeName = "Neck_1_Yaw";
+ string pitchNodeName = "Neck_2_Pitch";
+ string cameraNodeName = "DepthCamera";
+ string torsoNodeName = "TorsoJoint";
+
+ float Kp = 1.9f;
+ float Ki = 0.0f;
+ float Kd = 0.0f;
+ double maxControlValue = 1.0;
+ double maxDerivation = 0.5;
+
+ float yawAngleTolerance = 0.005;
+ float pitchAngleTolerance = 0.005;
+ bool abortIfUnreachable = false;
+ };
+
+ interface GazeControllerInterface extends
+ NJointControllerInterface
+ {
+ void submitTarget(FramedPositionBase target);
+ void removeTarget();
+ void removeTargetAfter(long durationMilliSeconds);
+ };
+
+ interface GazeControllerListener
+ {
+ void reportGazeTarget(long requestTimestamp, long reachedTimestamp, long releasedTimestamp, long abortedTimestamp, FramedPositionBase target);
+ };
+
+};};};
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 9ff7402b4b2db5b833adabcb48f6d5ffb554ec39..4e742c96f69db5d5abeabe58f7cbc69684e740bb 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -181,7 +181,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- if (provider != "")
+ if (!provider.empty())
{
qb
.coreSegments().withName(properties.graspCandidateMemoryName)
@@ -212,7 +212,7 @@ namespace armarx::armem
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- if (provider != "")
+ if (!provider.empty())
{
qb
.coreSegments().withName(properties.bimanualGraspCandidateMemoryName)
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
index 592c9fe677ecb0e8ad9b4e55c59a9560f336f439..070806e0cd1b9718999b4034b5bd5ee3dc046b62 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
+++ b/source/RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.xml
@@ -40,7 +40,7 @@
<Object name="armarx::grasping::arondto::GraspCandidateSourceInfo">
<ObjectChild key='referenceObjectPose'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='referenceObjectName'>
<string />
@@ -49,7 +49,7 @@
<int />
</ObjectChild>
<ObjectChild key='bbox'>
- <armarx::grasping::arondto::BoundingBox />
+ <armarx::grasping::arondto::BoundingBox optional="true" />
</ObjectChild>
</Object>
@@ -96,11 +96,11 @@
</ObjectChild>
<ObjectChild key='tcpPoseInHandRoot'>
- <Pose />
+ <Pose optional="true" />
</ObjectChild>
<ObjectChild key='approachVector'>
- <Position />
+ <Position optional="true" />
</ObjectChild>
<ObjectChild key='sourceFrame'>
@@ -116,7 +116,7 @@
</ObjectChild>
<ObjectChild key='graspSuccessProbability'>
- <float />
+ <float optional="true" />
</ObjectChild>
<ObjectChild key='objectType'>
@@ -124,7 +124,7 @@
</ObjectChild>
<ObjectChild key='groupNr'>
- <int />
+ <int optional="true" />
</ObjectChild>
<ObjectChild key='providerName'>
@@ -135,21 +135,21 @@
<bool />
</ObjectChild>
<ObjectChild key='sourceInfo'>
- <armarx::grasping::arondto::GraspCandidateSourceInfo />
+ <armarx::grasping::arondto::GraspCandidateSourceInfo optional="true" />
</ObjectChild>
<ObjectChild key='executionHintsValid'>
<bool />
</ObjectChild>
<ObjectChild key='executionHints'>
- <armarx::grasping::arondto::GraspCandidateExecutionHints />
+ <armarx::grasping::arondto::GraspCandidateExecutionHints optional="true" />
</ObjectChild>
<ObjectChild key='reachabilityInfoValid'>
<bool />
</ObjectChild>
<ObjectChild key='reachabilityInfo'>
- <armarx::grasping::arondto::GraspCandidateReachabilityInfo />
+ <armarx::grasping::arondto::GraspCandidateReachabilityInfo optional="true" />
</ObjectChild>
</Object>
diff --git a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
index 01f6adc1a9fe479580104e9e7de110de6b09aaef..0b367d3cbd4203474aeeeaa9cf8bd30c08c0fc30 100644
--- a/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/aron_conversions.cpp
@@ -5,18 +5,18 @@
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/core/logging/Logging.h>
-
-void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto,
- armarx::grasping::BoundingBox& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::BoundingBox& dto, armarx::grasping::BoundingBox& bo)
{
+ ARMARX_TRACE;
bo = BoundingBox(toIce(dto.center), toIce(dto.ha1), toIce(dto.ha2), toIce(dto.ha3));
}
-void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
- const armarx::grasping::BoundingBox& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto, const armarx::grasping::BoundingBox& bo)
{
+ ARMARX_TRACE;
dto.center = fromIce(bo.center);
dto.ha1 = fromIce(bo.ha1);
dto.ha2 = fromIce(bo.ha2);
@@ -26,26 +26,41 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BoundingBox& dto,
void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
armarx::grasping::GraspCandidateSourceInfo& bo)
{
+ ARMARX_TRACE;
bo.bbox = new BoundingBox();
- fromAron(dto.bbox, *bo.bbox);
+ if (dto.bbox)
+ {
+ fromAron(dto.bbox.value(), *bo.bbox);
+ } else bo.bbox = nullptr;
bo.referenceObjectName = dto.referenceObjectName;
- bo.referenceObjectPose = toIce(dto.referenceObjectPose);
+ if (dto.referenceObjectPose)
+ {
+ bo.referenceObjectPose = toIce(dto.referenceObjectPose.value());
+ } else bo.referenceObjectPose = nullptr;
bo.segmentationLabelID = dto.segmentationLabelID;
}
void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateSourceInfo& dto,
const armarx::grasping::GraspCandidateSourceInfo& bo)
{
- toAron(dto.bbox, *bo.bbox);
+ ARMARX_TRACE;
+ if (bo.bbox)
+ {
+ dto.bbox = arondto::BoundingBox();
+ toAron(dto.bbox.value(), *bo.bbox);
+ }
dto.referenceObjectName = bo.referenceObjectName;
- dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ if (bo.referenceObjectPose)
+ {
+ dto.referenceObjectPose = fromIce(bo.referenceObjectPose);
+ }
dto.segmentationLabelID = bo.segmentationLabelID;
-
}
void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
+ ARMARX_TRACE;
bo = GraspCandidateReachabilityInfo(dto.reachable, dto.minimumJointLimitMargin, dto.jointLimitMargins,
dto.maxPosError, dto.maxOriError);
}
@@ -53,6 +68,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidateR
void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabilityInfo& dto,
const armarx::grasping::GraspCandidateReachabilityInfo& bo)
{
+ ARMARX_TRACE;
dto.jointLimitMargins = bo.jointLimitMargins;
dto.maxOriError = bo.maxOriError;
dto.maxPosError = bo.maxPosError;
@@ -60,92 +76,110 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateReachabil
dto.reachable = bo.reachable;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto,
- armarx::grasping::GraspCandidate& bo)
+void
+armarx::grasping::fromAron(const armarx::grasping::arondto::GraspCandidate& dto, armarx::grasping::GraspCandidate& bo)
{
+ ARMARX_TRACE;
bo = GraspCandidate();
bo.graspPose = toIce(dto.graspPose);
bo.robotPose = toIce(dto.robotPose);
- bo.tcpPoseInHandRoot = toIce(dto.tcpPoseInHandRoot);
- bo.approachVector = toIce(dto.approachVector);
+ bo.tcpPoseInHandRoot = dto.tcpPoseInHandRoot ? toIce(dto.tcpPoseInHandRoot.value()) : nullptr;
+ bo.approachVector = dto.approachVector ? toIce(dto.approachVector.value()) : nullptr;
bo.sourceFrame = dto.sourceFrame;
bo.targetFrame = dto.targetFrame;
bo.side = dto.side;
- bo.graspSuccessProbability = dto.graspSuccessProbability;
+ bo.graspSuccessProbability = dto.graspSuccessProbability ? dto.graspSuccessProbability.value() : -1.0;
fromAron(dto.objectType, bo.objectType);
- if (dto.executionHintsValid)
+ if (dto.executionHints && dto.executionHintsValid)
{
bo.executionHints = new GraspCandidateExecutionHints();
- fromAron(dto.executionHints, *bo.executionHints);
- }
- else
+ fromAron(dto.executionHints.value(), *bo.executionHints);
+ } else
{
bo.executionHints = nullptr;
}
- bo.groupNr = dto.groupNr;
+ bo.groupNr = dto.groupNr ? dto.groupNr.value() : -1;
bo.providerName = dto.providerName;
- if (dto.reachabilityInfoValid)
+ if (dto.reachabilityInfo && dto.reachabilityInfoValid)
{
bo.reachabilityInfo = new GraspCandidateReachabilityInfo();
- fromAron(dto.reachabilityInfo, *bo.reachabilityInfo);
- }
- else
+ fromAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
+ } else
{
bo.reachabilityInfo = nullptr;
}
- if (dto.sourceInfoValid)
+ if (dto.sourceInfo && dto.sourceInfoValid)
{
bo.sourceInfo = new GraspCandidateSourceInfo();
- fromAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ fromAron(dto.sourceInfo.value(), *bo.sourceInfo);
+ } else
{
bo.sourceInfo = nullptr;
}
}
-void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
- const armarx::grasping::GraspCandidate& bo)
+void
+armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto, const armarx::grasping::GraspCandidate& bo)
{
- dto.approachVector = fromIce(bo.approachVector);
+ ARMARX_TRACE;
+ if (bo.approachVector)
+ {
+ dto.approachVector = fromIce(bo.approachVector);
+ }
if (bo.executionHints)
{
+ dto.executionHints = arondto::GraspCandidateExecutionHints();
dto.executionHintsValid = true;
- toAron(dto.executionHints, *bo.executionHints);
- }
- else
+ toAron(dto.executionHints.value(), *bo.executionHints);
+ } else
{
dto.executionHintsValid = false;
- dto.executionHints.toAron();
+ dto.executionHints = std::nullopt;
}
dto.graspPose = fromIce(bo.graspPose);
- dto.graspSuccessProbability = bo.graspSuccessProbability;
- dto.groupNr = bo.groupNr;
+ if (bo.graspSuccessProbability < 0 || bo.graspSuccessProbability > 1.0)
+ {
+ dto.graspSuccessProbability = std::nullopt;
+ } else
+ {
+ dto.graspSuccessProbability = bo.graspSuccessProbability;
+ }
+ if (bo.groupNr < 0)
+ {
+ dto.groupNr = std::nullopt;
+ } else
+ {
+ dto.groupNr = bo.groupNr;
+ }
toAron(dto.objectType, bo.objectType);
dto.providerName = bo.providerName;
if (bo.reachabilityInfo)
{
+ dto.reachabilityInfo = arondto::GraspCandidateReachabilityInfo();
dto.reachabilityInfoValid = true;
- toAron(dto.reachabilityInfo, *bo.reachabilityInfo);
- }
- else
+ toAron(dto.reachabilityInfo.value(), *bo.reachabilityInfo);
+ } else
{
dto.reachabilityInfoValid = false;
- dto.reachabilityInfo.toAron();
+ dto.reachabilityInfo = std::nullopt;
}
dto.robotPose = fromIce(bo.robotPose);
- dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
+ if (bo.tcpPoseInHandRoot)
+ {
+ dto.tcpPoseInHandRoot = fromIce(bo.tcpPoseInHandRoot);
+ }
+
dto.side = bo.side;
dto.sourceFrame = bo.sourceFrame;
if (bo.sourceInfo)
{
+ dto.sourceInfo = arondto::GraspCandidateSourceInfo();
dto.sourceInfoValid = true;
- toAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ toAron(dto.sourceInfo.value(), *bo.sourceInfo);
+ } else
{
dto.sourceInfoValid = false;
- dto.sourceInfo.toAron();
+ dto.sourceInfo = std::nullopt;
}
dto.targetFrame = bo.targetFrame;
}
@@ -153,6 +187,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidate& dto,
void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCandidate& dto,
armarx::grasping::BimanualGraspCandidate& bo)
{
+ ARMARX_TRACE;
bo = BimanualGraspCandidate();
bo.graspPoseRight = toIce(dto.graspPoseRight);
bo.graspPoseLeft = toIce(dto.graspPoseLeft);
@@ -170,8 +205,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsRight = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsRight, *bo.executionHintsRight);
- }
- else
+ } else
{
bo.executionHintsRight = nullptr;
}
@@ -179,8 +213,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.executionHintsLeft = new GraspCandidateExecutionHints();
fromAron(dto.executionHintsLeft, *bo.executionHintsLeft);
- }
- else
+ } else
{
bo.executionHintsLeft = nullptr;
}
@@ -190,8 +223,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoRight = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
- }
- else
+ } else
{
bo.reachabilityInfoRight = nullptr;
}
@@ -199,8 +231,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.reachabilityInfoLeft = new GraspCandidateReachabilityInfo();
fromAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
- }
- else
+ } else
{
bo.reachabilityInfoLeft = nullptr;
}
@@ -208,8 +239,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::BimanualGraspCa
{
bo.sourceInfo = new GraspCandidateSourceInfo();
fromAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ } else
{
bo.sourceInfo = nullptr;
}
@@ -225,8 +255,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsRightValid = true;
toAron(dto.executionHintsRight, *bo.executionHintsRight);
- }
- else
+ } else
{
dto.executionHintsRightValid = false;
dto.executionHintsRight.toAron();
@@ -235,8 +264,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.executionHintsLeftValid = true;
toAron(dto.executionHintsLeft, *bo.executionHintsLeft);
- }
- else
+ } else
{
dto.executionHintsLeftValid = false;
dto.executionHintsLeft.toAron();
@@ -251,8 +279,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoRightValid = true;
toAron(dto.reachabilityInfoRight, *bo.reachabilityInfoRight);
- }
- else
+ } else
{
dto.reachabilityInfoRightValid = false;
dto.reachabilityInfoRight.toAron();
@@ -261,8 +288,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.reachabilityInfoLeftValid = true;
toAron(dto.reachabilityInfoLeft, *bo.reachabilityInfoLeft);
- }
- else
+ } else
{
dto.reachabilityInfoLeftValid = false;
dto.reachabilityInfoLeft.toAron();
@@ -273,8 +299,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::BimanualGraspCandidate&
{
dto.sourceInfoValid = true;
toAron(dto.sourceInfo, *bo.sourceInfo);
- }
- else
+ } else
{
dto.sourceInfoValid = false;
dto.sourceInfo.toAron();
@@ -302,8 +327,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::GraspCandidateExecution
dto.graspTrajectoryName = bo.graspTrajectoryName;
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto,
- armarx::grasping::ApproachType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& dto, armarx::grasping::ApproachType& bo)
{
switch (dto.value)
{
@@ -321,8 +345,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApproachType& d
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
- const armarx::grasping::ApproachType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto, const armarx::grasping::ApproachType& bo)
{
switch (bo)
{
@@ -340,8 +363,7 @@ void armarx::grasping::toAron(armarx::grasping::arondto::ApproachType& dto,
}
-void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto,
- armarx::grasping::ApertureType& bo)
+void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& dto, armarx::grasping::ApertureType& bo)
{
switch (dto.value)
{
@@ -358,8 +380,7 @@ void armarx::grasping::fromAron(const armarx::grasping::arondto::ApertureType& d
ARMARX_UNEXPECTED_ENUM_VALUE(arondto::ObjectType, dto.value);
}
-void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto,
- const armarx::grasping::ApertureType& bo)
+void armarx::grasping::toAron(armarx::grasping::arondto::ApertureType& dto, const armarx::grasping::ApertureType& bo)
{
switch (bo)
{
diff --git a/source/RobotAPI/libraries/armem/client/Reader.cpp b/source/RobotAPI/libraries/armem/client/Reader.cpp
index 2662f86e0d7f36827f9d5cff67f963ac29613de9..b7a1c85dd5925353a360058cc5359a312f6e42c0 100644
--- a/source/RobotAPI/libraries/armem/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem/client/Reader.cpp
@@ -39,6 +39,7 @@ namespace armarx::armem::client
};
armem::query::data::Result result;
+ ARMARX_CHECK_NOT_NULL(memoryPrx);
try
{
result = memoryPrx->query(input);
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index 3399214dc4957654f5e8cdefa76a99bdee29640f..e84ad2fc735ae6341a573f54ad4103e6bbb1bb9a 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -26,6 +26,8 @@
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <VirtualRobot/Robot.h>
@@ -33,6 +35,10 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+
using namespace Eigen;
@@ -273,6 +279,31 @@ namespace armarx
}
}
+ Ice::ObjectPtr FramedDirection::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedDirection::clone(const Ice::Current& c) const
+ {
+ return new FramedDirection(*this);
+ }
+
+ VariantTypeId FramedDirection::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedDirection;
+ }
+
+ bool FramedDirection::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
+ {
+ stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
FramedPose::FramedPose() :
@@ -692,6 +723,51 @@ namespace armarx
}
}
+ FramedPosition::FramedPosition(const FramedPosition& other) :
+ Shared(other),
+ armarx::Serializable(other),
+ Vector3Base(other.x, other.y, other.z),
+ FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
+ Vector3(other.x, other.y, other.z)
+ {
+ }
+
+ FramedPosition& FramedPosition::operator=(const FramedPosition& other)
+ {
+ x = other.x;
+ y = other.y;
+ z = other.z;
+ frame = other.frame;
+ agent = other.agent;
+ return *this;
+ }
+
+ Ice::ObjectPtr FramedPosition::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedPosition::clone(const Ice::Current& c) const
+ {
+ return new FramedPosition(*this);
+ }
+
+ VariantTypeId FramedPosition::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedPosition;
+ }
+
+ bool FramedPosition::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
+ {
+ stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
FramedOrientation::FramedOrientation() :
Quaternion()
@@ -890,6 +966,32 @@ namespace armarx
}
}
+ Ice::ObjectPtr FramedOrientation::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedOrientation::clone(const Ice::Current& c) const
+ {
+ return new FramedOrientation(*this);
+ }
+
+ VariantTypeId FramedOrientation::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedOrientation;
+ }
+
+ bool FramedOrientation::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
+ {
+ stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
VirtualRobot::LinkedCoordinate FramedPose::createLinkedCoordinate(const VirtualRobot::RobotPtr& virtualRobot, const FramedPositionPtr& position, const FramedOrientationPtr& orientation)
{
@@ -963,5 +1065,31 @@ namespace armarx
return toFrame(referenceRobot, newFrame)->toEigen();
}
+ Ice::ObjectPtr FramedPose::ice_clone() const
+ {
+ return this->clone();
+ }
+
+ VariantDataClassPtr FramedPose::clone(const Ice::Current& c) const
+ {
+ return new FramedPose(*this);
+ }
+
+ VariantTypeId FramedPose::getType(const Ice::Current& c) const
+ {
+ return VariantType::FramedPose;
+ }
+
+ bool FramedPose::validate(const Ice::Current& c)
+ {
+ return true;
+ }
+
+ std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
+ {
+ stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
+ return stream;
+ }
+
}
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index 27a04fccb69d3b8bf89d91a4e33c45d0d0b36885..e0ca6fdaa8c6aa51880ade434d232929f15d769e 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -25,21 +25,11 @@
#pragma once
#include "Pose.h"
+#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/interface/core/FramedPoseBase.h>
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <VirtualRobot/VirtualRobot.h>
-#include <VirtualRobot/LinkedCoordinate.h>
-#include <Eigen/Core>
-#include <Eigen/Geometry>
-#include <sstream>
namespace armarx::VariantType
{
@@ -50,8 +40,21 @@ namespace armarx::VariantType
const VariantTypeId FramedOrientation = Variant::addTypeName("::armarx::FramedOrientationBase");
}
+namespace VirtualRobot
+{
+ class LinkedCoordinate;
+}
+namespace IceProxy::armarx
+{
+ class SharedRobotInterface;
+}
+namespace Eigen
+{
+ using Matrix6f = Matrix<Ice::Float, 6, 6>;
+}
namespace armarx
{
+ using SharedRobotInterfacePrx = ::IceInternal::ProxyHandle< ::IceProxy::armarx::SharedRobotInterface> ;
/**
* @ingroup RobotAPI-FramedPose
* Variable of the global coordinate system. use this if you are specifying a global pose.
@@ -104,29 +107,13 @@ namespace armarx
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedDirection(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedDirection;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
-
- friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs)
- {
- stream << "FramedDirection: " << std::endl << rhs.output() << std::endl;
- return stream;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ friend std::ostream& operator<<(std::ostream& stream, const FramedDirection& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -154,24 +141,9 @@ namespace armarx
FramedPosition(const Eigen::Vector3f&, const std::string& frame, const std::string& agent);
FramedPosition(const Eigen::Matrix4f&, const std::string& frame, const std::string& agent);
//FramedPosition(const Vector3BasePtr pos, const std::string &frame ); // this doesnt work for unknown reasons
- FramedPosition(const FramedPosition& other):
- Shared(other),
- armarx::Serializable(other),
- Vector3Base(other.x, other.y, other.z),
- FramedPositionBase(other.x, other.y, other.z, other.frame, other.agent),
- Vector3(other.x, other.y, other.z)
- {
- }
-
- FramedPosition& operator=(const armarx::FramedPosition& other)
- {
- x = other.x;
- y = other.y;
- z = other.z;
- frame = other.frame;
- agent = other.agent;
- return *this;
- }
+ FramedPosition(const FramedPosition& other);
+
+ FramedPosition& operator=(const armarx::FramedPosition& other);
std::string getFrame() const;
@@ -189,29 +161,13 @@ namespace armarx
Eigen::Vector3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedPosition(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedPosition;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
-
- friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs)
- {
- stream << "FramedPosition: " << std::endl << rhs.output() << std::endl;
- return stream;
- };
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
+
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPosition& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -243,23 +199,11 @@ namespace armarx
std::string getFrame()const ;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedOrientation(*this);
- }
+ Ice::ObjectPtr ice_clone() const override;
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedOrientation;
- }
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
@@ -274,11 +218,7 @@ namespace armarx
Eigen::Matrix3f toRootEigen(const SharedRobotInterfacePrx& referenceRobot) const;
Eigen::Matrix3f toRootEigen(const VirtualRobot::RobotPtr& referenceRobot) const;
- friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs)
- {
- stream << "FramedOrientation: " << std::endl << rhs.output() << std::endl;
- return stream;
- };
+ friend std::ostream& operator<<(std::ostream& stream, const FramedOrientation& rhs);
public: // serialization
void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = Ice::emptyCurrent) const override;
@@ -312,27 +252,15 @@ namespace armarx
std::string getFrame() const;
// inherited from VariantDataClass
- Ice::ObjectPtr ice_clone() const override
- {
- return this->clone();
- }
+ Ice::ObjectPtr ice_clone() const override;
- VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return new FramedPose(*this);
- }
+ VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
- VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override
- {
- return VariantType::FramedPose;
- }
+ VariantTypeId getType(const Ice::Current& c = Ice::emptyCurrent) const override;
- bool validate(const Ice::Current& c = Ice::emptyCurrent) override
- {
- return true;
- }
+ bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
void changeFrame(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame);
void changeFrame(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame);
@@ -351,11 +279,7 @@ namespace armarx
Eigen::Matrix4f toFrameEigen(const SharedRobotInterfacePrx& referenceRobot, const std::string& newFrame) const;
Eigen::Matrix4f toFrameEigen(const VirtualRobot::RobotPtr& referenceRobot, const std::string& newFrame) const;
- friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs)
- {
- stream << "FramedPose: " << std::endl << rhs.output() << std::endl;
- return stream;
- }
+ friend std::ostream& operator<<(std::ostream& stream, const FramedPose& rhs);
FramedPositionPtr getPosition() const;
FramedOrientationPtr getOrientation() const;
diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp
index 4a4d285d97a3001b2d5d3eb1ab504604809806c6..1f4a2b6fe5f66815fb87eb53d60bcf876e6a39ed 100644
--- a/source/RobotAPI/libraries/core/LinkedPose.cpp
+++ b/source/RobotAPI/libraries/core/LinkedPose.cpp
@@ -26,6 +26,7 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
diff --git a/source/RobotAPI/libraries/core/Pose.cpp b/source/RobotAPI/libraries/core/Pose.cpp
index 39968afc088d5ed82cad8461cfd89cb96a75e321..f837f87117f5c8934ab5430af9491277010cb4e9 100644
--- a/source/RobotAPI/libraries/core/Pose.cpp
+++ b/source/RobotAPI/libraries/core/Pose.cpp
@@ -28,7 +28,12 @@
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <sstream>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
template class ::IceInternal::Handle<::armarx::Pose>;
diff --git a/source/RobotAPI/libraries/core/Pose.h b/source/RobotAPI/libraries/core/Pose.h
index 16b70957d4a336a57d0eeab072525c2dce08effc..268b6ba846022f0586fbdfafb5cf8d2999665c55 100644
--- a/source/RobotAPI/libraries/core/Pose.h
+++ b/source/RobotAPI/libraries/core/Pose.h
@@ -27,13 +27,10 @@
#include <RobotAPI/interface/core/PoseBase.h>
#include <ArmarXCore/observers/variant/Variant.h>
-#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
-#include <sstream>
-
namespace armarx::VariantType
{
// variant types
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 60577ef2dfb7b01eaf648a1a86008f7a969061dd..4f0f563e7960efbb4cc8055f2c198803105b841e 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -34,6 +34,7 @@
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <ArmarXCore/core/logging/Logging.h>
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
index e6cc65c23c13e154382042424db8483d879a89b4..28bb76573e925f3f6135ba0f3d75be2aa060d9f4 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobotNode.cpp
@@ -27,7 +27,7 @@
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/core/BasicTypes.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <VirtualRobot/VirtualRobot.h>