Go to the documentation of this file.
28 #include "../RobotUnit.h"
50 friend class ::armarx::NJointControllerBase;
54 const std::string& deviceName,
55 const std::string& controlMode)
57 if (d.controlDevices.
has(deviceName))
59 auto& dev = d.controlDevices.
at(deviceName);
60 if (dev->hasJointController(controlMode))
64 return dev->getJointController(controlMode);
78 NJointControllerDescription
82 NJointControllerDescription d;
85 d.controller = NJointControllerInterfacePrx::uncheckedCast(
getProxy(-1));
86 d.controlModeAssignment = controlDeviceControlModeMap;
88 d.deletable = deletable;
89 d.internal =
internal;
93 std::optional<std::vector<char>>
99 for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
101 if (controlDeviceUsedBitmap.at(i))
113 NJointControllerStatus
117 NJointControllerStatus
s;
120 s.requested = isRequested;
121 s.error = deactivatedBecauseOfError;
122 s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(
123 std::chrono::high_resolution_clock::now().time_since_epoch())
128 NJointControllerDescriptionWithStatus
131 NJointControllerDescriptionWithStatus ds;
172 const bool active = isActive;
173 if (publishActive.exchange(active) != active)
182 catch (std::exception& e)
185 <<
"' threw an exception!\nWhat:\n"
191 <<
"' threw an exception!";
201 catch (std::exception& e)
204 <<
"' threw an exception!\nWhat:\n"
210 <<
"' threw an exception!";
220 catch (std::exception& e)
223 <<
"' threw an exception!\nWhat:\n"
238 if (publishActive.exchange(
false))
246 NJointControllerBase::rtActivateController()
250 deactivatedBecauseOfError =
false;
253 errorState.store(
false);
258 NJointControllerBase::rtDeactivateController()
268 NJointControllerBase::rtDeactivateControllerBecauseOfError()
270 deactivatedBecauseOfError =
true;
271 rtDeactivateController();
278 <<
"The function '" << BOOST_CURRENT_FUNCTION
279 <<
"' must only be called during the construction of an NJointController.";
287 <<
"The function '" << BOOST_CURRENT_FUNCTION
288 <<
"' must only be called during the construction of an NJointController.";
296 <<
"The function '" << BOOST_CURRENT_FUNCTION
297 <<
"' must only be called during the construction of an NJointController.";
300 rtRobotNodes = rtRobot->getRobotNodes();
338 auto& name = pair.first;
339 auto& handle = pair.second;
340 if (!handle || !handle->isValid())
345 if (handle->isDetached())
349 std::future_status
status;
352 status = handle->getFuture().wait_for(std::chrono::seconds(3));
353 if (
status == std::future_status::timeout)
355 ARMARX_INFO <<
"Still waiting for " << name <<
" thread handle!";
357 else if (
status == std::future_status::ready ||
358 status == std::future_status::deferred)
364 }
while (
status != std::future_status::ready);
387 <<
"The function '" << BOOST_CURRENT_FUNCTION
388 <<
"' must only be called during the construction of an NJointController.";
395 return dev->getSensorValue();
399 robotUnit(RobotUnitModule::ModuleBase::Instance<::
armarx::
RobotUnit>()),
400 controlDeviceUsedBitmap(robotUnit.getNumberOfControlDevices(), 0)
411 const std::string& controlMode)
415 <<
"The function '" << BOOST_CURRENT_FUNCTION
416 <<
"' must only be called during the construction of an NJointController.";
418 << BOOST_CURRENT_FUNCTION
419 <<
": Must not request two ControlTargets for the same device. (got = "
420 << controlDeviceControlModeMap.at(deviceName) <<
", requested " + controlMode +
") "
421 <<
"(You can only have a ControlDevice in one ControlMode. "
422 <<
"To check all available ControlModes for this device, get the ControlDevice via "
423 "peekControlDevice(" +
424 deviceName +
") and querry it)";
428 RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(
437 controlDeviceUsedJointController[deviceName] = jointCtrl;
439 controlDeviceControlModeMap[deviceName] = controlMode;
442 controlDeviceUsedBitmap.at(devIndex) = 1;
444 controlDeviceUsedIndices.insert(std::upper_bound(controlDeviceUsedIndices.begin(),
445 controlDeviceUsedIndices.end(),
ConstSensorDevicePtr peekSensorDevice(const std::string &deviceName) const
Get a const ptr to the given SensorDevice.
~NJointControllerBase() override
std::string getInstanceName(const Ice::Current &=Ice::emptyCurrent) const final override
static bool ConstructorIsRunning()
ConstControlDevicePtr getControlDevice(const std::string &deviceName) const
Returns the ControlDevice.
const VirtualRobot::RobotPtr & useSynchronizedRtRobot(bool updateCollisionModel=false)
Requests a VirtualRobot for use in rtRun *.
void deactivateController(const Ice::Current &=Ice::emptyCurrent) final override
The SensorValueBase class.
void activateController(const Ice::Current &=Ice::emptyCurrent) final override
void onInitComponent() final
Brief description of class JointControlTargetBase.
std::map< std::string, std::shared_ptr< ThreadPool::Handle > > threadHandles
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
#define ARMARX_CHECK_NOT_NULL(ptr)
This macro evaluates whether ptr is not null and if it turns out to be false it will throw an Express...
void deactivateAndDeleteNJointController(const std::string &name, const Ice::Current &=Ice::emptyCurrent) override
Queues the given NJointControllerBase for deletion and deactivates it if necessary.
bool has(const KeyT &k) const
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override=0
virtual void onDisconnectNJointController()
void onDisconnectComponent() final
std::mutex threadHandlesMutex
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
void onExitComponent() final
The JointController class represents one joint in one control mode.
NJointControllerDescription getControllerDescription(const Ice::Current &=Ice::emptyCurrent) const final override
virtual void onPublishDeactivation(const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
virtual void onConnectNJointController()
void deleteNJointController(const std::string &name, const Ice::Current &=Ice::emptyCurrent) override
Queues the given NJointControllerBase for deletion.
std::optional< std::vector< char > > isNotInConflictWith(const NJointControllerBasePtr &other) const
void deactivateNJointController(const std::string &name, const Ice::Current &=Ice::emptyCurrent) override
Requests deactivation for the given NJointControllerBase.
static const NJointControllerBasePtr NullPtr
std::shared_ptr< ThreadPool > ThreadPoolPtr
RobotUnit * getRobotUnit(RobotUnitModule::ControllerManagement *cmngr)
virtual void onPublishActivation(const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current &=Ice::emptyCurrent) const final override
void onConnectComponent() final
void activateNJointController(const std::string &name, const Ice::Current &=Ice::emptyCurrent) override
Requests activation for the given NJointControllerBase.
#define ARMARX_CHECK_IS_NULL(ptr)
This macro evaluates whether ptr is null and if it turns out to be false it will throw an ExpressionE...
void deactivateAndDeleteController(const Ice::Current &=Ice::emptyCurrent) final override
virtual void rtPostDeactivateController()
This function is called after the controller is deactivated.
ConstSensorDevicePtr getSensorDevice(const std::string &deviceName) const
TODO move to attorney for NJointControllerBase.
This class allows minimal access to private members of armarx::RobotUnitModule::Devices in a sane fas...
ThreadPoolPtr getThreadPool() const
NJointControllerStatus getControllerStatus(const Ice::Current &=Ice::emptyCurrent) const final override
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
static Devices & Instance()
Returns the singleton instance of this class.
virtual void onExitNJointController()
static ApplicationPtr getInstance()
Retrieve shared pointer to the application object.
ConstControlDevicePtr peekControlDevice(const std::string &deviceName) const
Get a const ptr to the given ControlDevice.
void deleteController(const Ice::Current &=Ice::emptyCurrent) final override
std::size_t getControlDeviceIndex(const std::string &deviceName) const
Returns the ControlDevice's index.
static RobotData & Instance()
Returns the singleton instance of this class.
This Module manages sensor and control devices for a RobotUnit and only allows save and sane access.
T * _modulePtr()
Returns this as ptr to the given type.
virtual void rtPreActivateController()
This function is called before the controller is activated.
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
The RobotUnit class manages a robot and its controllers.
This Module manages NJointControllers.
virtual void onInitNJointController()
std::shared_ptr< const class SensorDevice > ConstSensorDevicePtr
virtual ControlTargetBase * getControlTarget()=0
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
static thread_local bool ConstructorIsRunning_
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
VirtualRobot::RobotPtr cloneRobot(bool updateCollisionModel=false) const
Returns a clone of the robot's model.
double s(double t, double s0, double v0, double a0, double j)
std::shared_ptr< const class ControlDevice > ConstControlDevicePtr
This file offers overloads of toIce() and fromIce() functions for STL container types.
std::size_t getNumberOfControlDevices() const
Returns the number of ControlDevices.
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
std::shared_ptr< class Robot > RobotPtr