diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
index 88cf11c532113f593f4dfa9056e4469fbc9a7992..6f01acc17ee6c7cf5ede5925d4deffa68ceeca4e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp
@@ -996,14 +996,14 @@ namespace armarx::RobotUnitModule
}
void
- ControlThread::setEmergencyStopState(EmergencyStopState state, const Ice::Current&)
+ ControlThread::setEmergencyStopState(EmergencyStopState state)
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
_module<Units>().getEmergencyStopMaster()->setEmergencyStopState(state);
}
EmergencyStopState
- ControlThread::getEmergencyStopState(const Ice::Current&) const
+ ControlThread::getEmergencyStopState() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return emergencyStop ? EmergencyStopState::eEmergencyStopActive
@@ -1011,7 +1011,7 @@ namespace armarx::RobotUnitModule
}
EmergencyStopState
- ControlThread::getRtEmergencyStopState(const Ice::Current&) const
+ ControlThread::getRtEmergencyStopState() const
{
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
return rtIsInEmergencyStop() ? EmergencyStopState::eEmergencyStopActive
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
index af1990164ed130b335b6e8c26b540d165694b51e..4c06316e0f60f76cac6f1912a739f6109573d656 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.h
@@ -128,20 +128,19 @@ namespace armarx::RobotUnitModule
* @brief Sets the \ref EmergencyStopState
* @param state The \ref EmergencyStopState to set
*/
- void setEmergencyStopState(EmergencyStopState state,
- const Ice::Current& = Ice::emptyCurrent) override;
+ void setEmergencyStopState(EmergencyStopState state);
+
/**
* @brief Returns the \ref ControlThread's target \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState
- getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState getEmergencyStopState() const;
+
/**
* @brief Returns the \ref ControlThread's \ref EmergencyStopState
* @return The \ref EmergencyStopState
*/
- EmergencyStopState
- getRtEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const override;
+ EmergencyStopState getRtEmergencyStopState() const;
// //////////////////////////////////////////////////////////////////////////////////////// //
// /////////////////////////////////// Module interface /////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 34168e9a242f8c43da13eb43e2a809594ebbc226..6b124cdb76197760261a1b851f7a3b188a978490 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -24,6 +24,7 @@
#include <ArmarXCore/core/IceGridAdmin.h>
#include <ArmarXCore/core/IceManager.h>
+#include <ArmarXCore/core/time.h>
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
@@ -78,18 +79,49 @@ namespace armarx::RobotUnitModule
setEmergencyStopState(EmergencyStopState state,
const Ice::Current& = Ice::emptyCurrent) final override
{
- if (getEmergencyStopState() == state)
+ EmergencyStopState currentState = getEmergencyStopState();
+
+ const std::unique_lock lock{ss2StateMutex};
+
+ if (currentState == state)
{
return;
}
+
+ currentState = state;
+
+ if (currentState == EmergencyStopState::eEmergencyStopActive)
+ {
+ lastSS2Activation = armarx::core::time::DateTime::Now();
+ }
+
ControlThreadAttorneyForRobotUnitEmergencyStopMaster::
- SetEmergencyStopStateNoReportToTopic(controlThreadModule, state);
- emergencyStopTopic->reportEmergencyStopState(state);
+ SetEmergencyStopStateNoReportToTopic(controlThreadModule, currentState);
+ emergencyStopTopic->reportEmergencyStopState(currentState);
+ }
+
+ EmergencyStopState
+ trySetEmergencyStopState(EmergencyStopState targetState,
+ const ::Ice::Current&) final override
+ {
+ armarx::core::time::Duration safeDuration =
+ armarx::core::time::Duration::MilliSeconds(500);
+
+ // If the SS2 should be released (target state = inactive) we check whether the last
+ // activation happened at least before safeDuration.
+ if (targetState == EmergencyStopState::eEmergencyStopInactive and
+ armarx::core::time::DateTime::Now() > lastSS2Activation + safeDuration)
+ {
+ setEmergencyStopState(targetState);
+ }
+
+ return getEmergencyStopState();
}
EmergencyStopState
getEmergencyStopState(const Ice::Current& = Ice::emptyCurrent) const final override
{
+ const std::shared_lock lock{ss2StateMutex};
return controlThreadModule->getEmergencyStopState();
}
@@ -118,6 +150,8 @@ namespace armarx::RobotUnitModule
ControlThread* const controlThreadModule;
const std::string emergencyStopTopicName;
EmergencyStopListenerPrx emergencyStopTopic;
+ mutable std::shared_mutex ss2StateMutex;
+ armarx::core::time::DateTime lastSS2Activation;
};
} // namespace armarx::RobotUnitModule
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 97a40339d29419d83e1ce8af190f13bd30cfc516..517ecf266df944b5673f28a8e1742ddbe2ca6841 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -22,30 +22,23 @@
#pragma once
-#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/components/EmergencyStopInterface.ice>
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <ArmarXCore/interface/core/RemoteReferenceCount.ice>
-#include <ArmarXCore/interface/components/EmergencyStopInterface.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXCore/interface/observers/ObserverInterface.ice>
#include <ArmarXGui/interface/WidgetDescription.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/components/RobotHealthInterface.ice>
+#include <RobotAPI/interface/components/TrajectoryPlayerInterface.ice>
#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
#include <RobotAPI/interface/units/TCPControlUnit.ice>
-#include <RobotAPI/interface/components/TrajectoryPlayerInterface.ice>
-
#include <RobotAPI/interface/visualization/DebugDrawerInterface.ice>
-#include <RobotAPI/interface/components/RobotHealthInterface.ice>
-#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
-
-#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <ArmarXCore/interface/core/UserException.ice>
-#include <ArmarXGui/interface/WidgetDescription.ice>
//NJointController
@@ -61,7 +54,9 @@ module armarx
string hardwareControlMode;
string targetType;
};
- dictionary<string, HWControlModeAndTargetType> ControlModeToHWControlModeAndTargetTypeDictionary;
+
+ dictionary<string, HWControlModeAndTargetType>
+ ControlModeToHWControlModeAndTargetTypeDictionary;
struct ControlDeviceDescription
{
@@ -69,7 +64,9 @@ module armarx
ControlModeToHWControlModeAndTargetTypeDictionary contolModeToTargetType;
Ice::StringSeq tags;
};
+
sequence<ControlDeviceDescription> ControlDeviceDescriptionSeq;
+
struct ControlDeviceStatus
{
string deviceName;
@@ -78,6 +75,7 @@ module armarx
StringToStringVariantBaseMapMap controlTargetValues;
long timestampUSec = 0;
};
+
sequence<ControlDeviceStatus> ControlDeviceStatusSeq;
}
//RobotUnit Utility types - SensorDevice
@@ -89,13 +87,16 @@ module armarx
string sensorValueType;
Ice::StringSeq tags;
};
+
sequence<SensorDeviceDescription> SensorDeviceDescriptionSeq;
+
struct SensorDeviceStatus
{
string deviceName;
StringVariantBaseMap sensorValue;
long timestampUSec = 0;
};
+
sequence<SensorDeviceStatus> SensorDeviceStatusSeq;
}
//RobotUnit Utility types - NJointController
@@ -106,6 +107,7 @@ module armarx
string className;
WidgetDescription::Widget configDescription;
};
+
sequence<NJointControllerClassDescription> NJointControllerClassDescriptionSeq;
}
//RobotUnit Utility types - data streaming
@@ -126,9 +128,10 @@ module armarx
struct DataEntry
{
- DataEntryType type;
- long index = -1;
+ DataEntryType type;
+ long index = -1;
};
+
dictionary<string, DataEntry> StringDataEntryMap;
struct DataStreamingDescription
@@ -136,20 +139,20 @@ module armarx
StringDataEntryMap entries;
};
-
struct TimeStep
{
long iterationId;
long timestampUSec;
long timesSinceLastIterationUSec;
- Ice::BoolSeq bools;
- Ice::ByteSeq bytes;
- Ice::ShortSeq shorts;
- Ice::IntSeq ints;
- Ice::LongSeq longs;
- Ice::FloatSeq floats;
+ Ice::BoolSeq bools;
+ Ice::ByteSeq bytes;
+ Ice::ShortSeq shorts;
+ Ice::IntSeq ints;
+ Ice::LongSeq longs;
+ Ice::FloatSeq floats;
Ice::DoubleSeq doubles;
};
+
sequence<TimeStep> TimeStepSeq;
interface Receiver
@@ -193,15 +196,23 @@ module armarx
{
//rt-logging
["cpp:const"] idempotent Ice::StringSeq getLoggingNames();
- SimpleRemoteReferenceCounterBase startRtLogging(string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError, InvalidArgumentException;
- SimpleRemoteReferenceCounterBase startRtLoggingWithAliasNames(string filePathFormatString, StringStringDictionary aliasNames) throws LogicError, InvalidArgumentException;
- void addMarkerToRtLog(SimpleRemoteReferenceCounterBase token, string marker) throws LogicError;
+ SimpleRemoteReferenceCounterBase startRtLogging(
+ string filePathFormatString, Ice::StringSeq loggingNames) throws LogicError,
+ InvalidArgumentException;
+ SimpleRemoteReferenceCounterBase startRtLoggingWithAliasNames(
+ string filePathFormatString, StringStringDictionary aliasNames) throws LogicError,
+ InvalidArgumentException;
+ void addMarkerToRtLog(SimpleRemoteReferenceCounterBase token, string marker)
+ throws LogicError;
void stopRtLogging(SimpleRemoteReferenceCounterBase token) throws LogicError;
- ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
+ ["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString)
+ throws LogicError,
+ InvalidArgumentException;
- RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(RobotUnitDataStreaming::Receiver* receiver, RobotUnitDataStreaming::Config config);
- void stopDataStreaming(RobotUnitDataStreaming::Receiver* receiver);
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
+ RobotUnitDataStreaming::Receiver * receiver, RobotUnitDataStreaming::Config config);
+ void stopDataStreaming(RobotUnitDataStreaming::Receiver * receiver);
};
interface RobotUnitUnitInterface
{
@@ -227,17 +238,29 @@ module armarx
interface RobotUnitDevicesInterface
{
//devices
- ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses() throws LogicError;
-
- ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions() throws LogicError;
- ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name) throws InvalidArgumentException, LogicError;
- ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses() throws LogicError;
+ ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(
+ string name) throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions()
+ throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses()
+ throws LogicError;
+
+ ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions()
+ throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name)
+ throws InvalidArgumentException,
+ LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses()
+ throws LogicError;
};
interface RobotUnitControllerManagementInterface
{
@@ -246,42 +269,76 @@ module armarx
["cpp:const"] idempotent Ice::StringSeq getRequestedNJointControllerNames();
["cpp:const"] idempotent Ice::StringSeq getActivatedNJointControllerNames();
//proxy/information
- ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
- ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
+ ["cpp:const"] idempotent NJointControllerInterface* getNJointController(string name);
+ ["cpp:const"] idempotent StringNJointControllerPrxDictionary getAllNJointControllers();
- ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
+ ["cpp:const"] idempotent NJointControllerStatus getNJointControllerStatus(string name)
+ throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerStatusSeq getNJointControllerStatuses();
- ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionSeq getNJointControllerDescriptions();
+ ["cpp:const"] idempotent NJointControllerDescription getNJointControllerDescription(
+ string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionSeq
+ getNJointControllerDescriptions();
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatus getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq getNJointControllerDescriptionsWithStatuses();
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatus
+ getNJointControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerDescriptionWithStatusSeq
+ getNJointControllerDescriptionsWithStatuses();
//classes
- ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
- ["cpp:const"] idempotent NJointControllerClassDescription getNJointControllerClassDescription(string name) throws InvalidArgumentException;
- ["cpp:const"] idempotent NJointControllerClassDescriptionSeq getNJointControllerClassDescriptions();
+ ["cpp:const"] idempotent Ice::StringSeq getNJointControllerClassNames();
+ ["cpp:const"] idempotent NJointControllerClassDescription
+ getNJointControllerClassDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent NJointControllerClassDescriptionSeq
+ getNJointControllerClassDescriptions();
//switching
- void switchNJointControllerSetup(Ice::StringSeq newSetup) throws InvalidArgumentException, LogicError;
-
- void activateNJointController(string controllerInstanceName) throws InvalidArgumentException, LogicError;
- void activateNJointControllers(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException, LogicError;
- void deactivateNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+ void switchNJointControllerSetup(Ice::StringSeq newSetup)
+ throws InvalidArgumentException,
+ LogicError;
+
+ void activateNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void activateNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
//creting controllers
- NJointControllerInterface* createNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
- NJointControllerInterface* createNJointControllerFromVariantConfig(string className, string instanceName, StringVariantBaseMap config) throws InvalidArgumentException, LogicError;
-
- NJointControllerInterface* createOrReplaceNJointController(string className, string instanceName, NJointControllerConfig config) throws InvalidArgumentException, LogicError;
+ NJointControllerInterface *createNJointController(
+ string className, string instanceName, NJointControllerConfig config)
+ throws InvalidArgumentException,
+ LogicError;
+ NJointControllerInterface *createNJointControllerFromVariantConfig(
+ string className, string instanceName, StringVariantBaseMap config)
+ throws InvalidArgumentException,
+ LogicError;
+
+ NJointControllerInterface *createOrReplaceNJointController(
+ string className, string instanceName, NJointControllerConfig config)
+ throws InvalidArgumentException,
+ LogicError;
//deleting controllers
- void deleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
-
- void deactivateAndDeleteNJointController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
- void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+ void deleteNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deleteNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
+
+ void deactivateAndDeleteNJointController(string controllerInstanceName)
+ throws InvalidArgumentException,
+ LogicError;
+ void deactivateAndDeleteNJointControllers(Ice::StringSeq controllerInstanceNames)
+ throws InvalidArgumentException,
+ LogicError;
};
interface RobotUnitSelfCollisionCheckerInterface
{
@@ -291,11 +348,10 @@ module armarx
idempotent void setSelfCollisionAvoidanceFrequency(float freq);
idempotent void setSelfCollisionAvoidanceDistance(float dist);
};
- interface RobotUnitControlThreadInterface
- {
- idempotent void setEmergencyStopState(EmergencyStopState state);
+ interface RobotUnitControlThreadInterface{
+ /*idempotent void setEmergencyStopState(EmergencyStopState state);
["cpp:const"] idempotent EmergencyStopState getEmergencyStopState();
- ["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();
+ ["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();*/
};
};
}
@@ -303,16 +359,11 @@ module armarx
//RobotUnit
module armarx
{
- interface RobotUnitInterface extends
- RobotUnitModule::RobotUnitUnitInterface,
- RobotUnitModule::RobotUnitDevicesInterface,
- RobotUnitModule::RobotUnitLoggingInterface,
+ interface RobotUnitInterface extends RobotUnitModule::RobotUnitUnitInterface,
+ RobotUnitModule::RobotUnitDevicesInterface, RobotUnitModule::RobotUnitLoggingInterface,
RobotUnitModule::RobotUnitPublishingInterface,
RobotUnitModule::RobotUnitManagementInterface,
RobotUnitModule::RobotUnitControlThreadInterface,
RobotUnitModule::RobotUnitControllerManagementInterface,
- RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
- {
- };
+ RobotUnitModule::RobotUnitSelfCollisionCheckerInterface{};
};
-