diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp index 7b485696f6bf7de22ead231d440991f0bf72e569..5ad285693fd9ec1bdd99e39ab84731e2b417b44d 100644 --- a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp +++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp @@ -33,31 +33,29 @@ namespace armarx JointController* ControlDevice::getJointEmergencyStopController() { - ARMARX_CHECK_EXPRESSION_W_HINT( - jointEmergencyStopController, - "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('" - << getDeviceName() << "') without a JointController " - "with the ControlMode ControlModes::EmergencyStop" - " (add a JointController with this ControlMode)" - ); + ARMARX_CHECK_EXPRESSION( + jointEmergencyStopController) << + "ControlDevice::getJointEmergencyStopController called for a ControlDevice ('" + << getDeviceName() << "') without a JointController " + "with the ControlMode ControlModes::EmergencyStop" + " (add a JointController with this ControlMode)"; return jointEmergencyStopController; } JointController* ControlDevice::getJointStopMovementController() { - ARMARX_CHECK_EXPRESSION_W_HINT( - jointStopMovementController, - "ControlDevice::getJointStopMovementController called for a ControlDevice ('" - << getDeviceName() << "') without a JointController " - "with the ControlMode ControlModes::StopMovement" - " (add a JointController with this ControlMode)" - ); + ARMARX_CHECK_EXPRESSION( + jointStopMovementController) << + "ControlDevice::getJointStopMovementController called for a ControlDevice ('" + << getDeviceName() << "') without a JointController " + "with the ControlMode ControlModes::StopMovement" + " (add a JointController with this ControlMode)"; return jointStopMovementController; } void ControlDevice::rtSetActiveJointController(JointController* jointCtrl) { - ARMARX_CHECK_EXPRESSION_W_HINT(jointCtrl, "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)"); + ARMARX_CHECK_EXPRESSION(jointCtrl) << "Called ControlDevice::rtSetActiveJointController with a nullptr (Don't do this)"; if (activeJointController == jointCtrl) { return; @@ -90,11 +88,10 @@ namespace armarx void ControlDevice::addJointController(JointController* jointCtrl) { - ARMARX_CHECK_IS_NULL_W_HINT( - owner, - "The ControlDevice '" << getDeviceName() - << "' was already added to a RobotUnit! Call addJointController before calling addControlDevice." - ); + ARMARX_CHECK_IS_NULL( + owner) << + "The ControlDevice '" << getDeviceName() + << "' was already added to a RobotUnit! Call addJointController before calling addControlDevice."; ARMARX_DEBUG << "adding Controller " << jointCtrl; if (!jointCtrl) { diff --git a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp index bf0b2bf3493ed82975b95ab40976b95c19ebc067..84e9891176c90ba59443714c444276357f4f30f9 100644 --- a/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/JointControllers/JointController.cpp @@ -28,7 +28,7 @@ namespace armarx { ControlDevice& JointController::getParent() const { - ARMARX_CHECK_EXPRESSION_W_HINT(parent, "This JointController is not owned by a ControlDevice"); + ARMARX_CHECK_EXPRESSION(parent) << "This JointController is not owned by a ControlDevice"; return *parent; } diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp index 08e7a69e7d3db593702ffd6da9c58e2ddecc2582..cb488380e67ad03f0245ea270061870528ceb086 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianTorqueController.cpp @@ -42,7 +42,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty()); useSynchronizedRtRobot(); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << config->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -65,7 +65,7 @@ namespace armarx }; tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName; nodeSetName = config->nodeSetName; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp index 9d5ded10b70b5a0d36cecce0f95f2aac702d952b..eede99223f70e52f7d1a8ab1ed4e9b101901caa1 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp @@ -42,7 +42,7 @@ namespace armarx useSynchronizedRtRobot(); ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << config->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -65,7 +65,7 @@ namespace armarx }; VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName; tcpController.reset(new CartesianVelocityController(rns, tcp)); nodeSetName = config->nodeSetName; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp index 6edb465ad4b6a7a8d25e1663060fbc088b60ccbd..24cafe472b7c53b0548c8221f03206b13534f597 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp @@ -46,7 +46,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty()); useSynchronizedRtRobot(); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(config->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, config->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << config->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -71,7 +71,7 @@ namespace armarx }; VirtualRobot::RobotNodePtr tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? rns->getTCP() : rtGetRobot()->getRobotNode(config->tcpName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName; nodeSetName = config->nodeSetName; jointLimitAvoidanceScale = config->jointLimitAvoidanceScale; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp index e42b68f1d78db459b423fd93e2c6723fa1cdd39e..57e87b49d8e553ff308533f17d2d9d5fc19a7f30 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.cpp @@ -234,29 +234,26 @@ namespace armarx armarx::ConstSensorDevicePtr armarx::NJointControllerBase::peekSensorDevice(const std::string& deviceName) const { - ARMARX_CHECK_EXPRESSION_W_HINT( - NJointControllerRegistryEntry::ConstructorIsRunning(), - "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController." - ); + ARMARX_CHECK_EXPRESSION( + NJointControllerRegistryEntry::ConstructorIsRunning()) << + "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."; return RobotUnitModule::Devices::Instance().getSensorDevice(deviceName); } ConstControlDevicePtr NJointControllerBase::peekControlDevice(const std::string& deviceName) const { - ARMARX_CHECK_EXPRESSION_W_HINT( - NJointControllerRegistryEntry::ConstructorIsRunning(), - "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController." - ); + ARMARX_CHECK_EXPRESSION( + NJointControllerRegistryEntry::ConstructorIsRunning()) << + "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."; return RobotUnitModule::Devices::Instance().getControlDevice(deviceName); } const VirtualRobot::RobotPtr& NJointControllerBase::useSynchronizedRtRobot(bool updateCollisionModel) { - ARMARX_CHECK_EXPRESSION_W_HINT( - NJointControllerRegistryEntry::ConstructorIsRunning(), - "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController." - ); - ARMARX_CHECK_IS_NULL_W_HINT(rtRobot, "useSynchronizedRtRobot was already called"); + ARMARX_CHECK_EXPRESSION( + NJointControllerRegistryEntry::ConstructorIsRunning()) << + "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."; + ARMARX_CHECK_IS_NULL(rtRobot) << "useSynchronizedRtRobot was already called"; rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel); rtRobotNodes = rtRobot->getRobotNodes(); return rtRobot; @@ -334,10 +331,9 @@ namespace armarx const SensorValueBase* NJointControllerBase::useSensorValue(const std::string& deviceName) const { - ARMARX_CHECK_EXPRESSION_W_HINT( - NJointControllerRegistryEntry::ConstructorIsRunning(), - "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController." - ); + ARMARX_CHECK_EXPRESSION( + NJointControllerRegistryEntry::ConstructorIsRunning()) << + "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."; auto dev = peekSensorDevice(deviceName); if (!dev) { @@ -360,17 +356,15 @@ namespace armarx ControlTargetBase* NJointControllerBase::useControlTarget(const std::string& deviceName, const std::string& controlMode) { - ARMARX_CHECK_EXPRESSION_W_HINT( - NJointControllerRegistryEntry::ConstructorIsRunning(), - "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController." - ); - ARMARX_CHECK_EQUAL_W_HINT( - controlDeviceControlModeMap.count(deviceName), 0, - BOOST_CURRENT_FUNCTION << ": Must not request two ControlTargets for the same device. (got = " - << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") " - << "(You can only have a ControlDevice in one ControlMode. " - << "To check all available ControlModes for this device, get the ControlDevice via peekControlDevice(" + deviceName + ") and querry it)" - ); + ARMARX_CHECK_EXPRESSION( + NJointControllerRegistryEntry::ConstructorIsRunning()) << + "The function '" << BOOST_CURRENT_FUNCTION << "' must only be called during the construction of an NJointController."; + ARMARX_CHECK_EQUAL( + controlDeviceControlModeMap.count(deviceName), 0) << + BOOST_CURRENT_FUNCTION << ": Must not request two ControlTargets for the same device. (got = " + << controlDeviceControlModeMap.at(deviceName) << ", requested " + controlMode + ") " + << "(You can only have a ControlDevice in one ControlMode. " + << "To check all available ControlModes for this device, get the ControlDevice via peekControlDevice(" + deviceName + ") and querry it)"; //there is a device and a target was requested: JointController* const jointCtrl = RobotUnitModule::DevicesAttorneyForNJointController::GetJointController(RobotUnitModule::Devices::Instance(), deviceName, controlMode); diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h index 88363b7242e9666883d22351adfb600735deb6f7..057e069963abd69c62f7da0a54c86c7628b34654 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h @@ -407,7 +407,7 @@ namespace armarx * It checks whether the provided config has the correct type. * \code{.cpp} NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config); - ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The provided config has the wrong type! The type is " << config->ice_id()); + ARMARX_CHECK_EXPRESSION(cfg) << "The provided config has the wrong type! The type is " << config->ice_id(); * \endcode * * Then it retrieves the \ref SensorValue1DoFActuatorPosition "SensorValue" and @@ -706,7 +706,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting); ARMARX_VERBOSE << "Adding NJointControllerBase task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount(); auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task)); - ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount()); + ARMARX_CHECK_EXPRESSION(handlePtr->isValid()) << "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount(); threadHandles[taskName] = handlePtr; } std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp index 0b89eaf5f18d12223a623d3b1eba13d672ddfd3b..5a38fe40143a8147e9561b14a184648e095696b9 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.ipp @@ -40,12 +40,12 @@ namespace armarx private: friend class RobotUnitModule::ControllerManagement; virtual NJointControllerBasePtr create( - RobotUnitModule::ControllerManagement* cmngr, - const NJointControllerConfigPtr&, - const VirtualRobot::RobotPtr&, - bool deletable, - bool internal, - const std::string& instanceName) const = 0; + RobotUnitModule::ControllerManagement* cmngr, + const NJointControllerConfigPtr&, + const VirtualRobot::RobotPtr&, + bool deletable, + bool internal, + const std::string& instanceName) const = 0; virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const VirtualRobot::RobotPtr&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0; virtual NJointControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0; virtual bool hasRemoteConfiguration() const = 0; @@ -104,14 +104,14 @@ namespace armarx { static_assert(std::is_base_of<ControlTargetBase, T>::value, "The given type does not derive ControlTargetBase"); ControlTargetBase* const ptr = useControlTarget(deviceName, controlMode); - return ptr ? ptr->asA<T>(): nullptr; + return ptr ? ptr->asA<T>() : nullptr; } template<class T> inline const T* NJointControllerBase::useSensorValue(const std::string& deviceName) const { static_assert(std::is_base_of<SensorValueBase, T>::value, "The given type does not derive SensorValueBase"); const SensorValueBase* const ptr = useSensorValue(deviceName); - return ptr ? ptr->asA<T>(): nullptr; + return ptr ? ptr->asA<T>() : nullptr; } inline void SynchronousNJointController::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) @@ -277,7 +277,7 @@ namespace armarx { if (first == last) { - return std::vector<char>{}; + return std::vector<char> {}; } std::size_t n = (*first)->getControlDeviceUsedBitmap().size(); std::vector<char> inuse(n, false); @@ -312,20 +312,20 @@ namespace armarx::detail static constexpr bool hasRemoteConfiguration_ = hasGenerateConfigDescription<NJointControllerT>::value; NJointControllerBasePtr create(RobotUnitModule::ControllerManagement* cmngr, - const NJointControllerConfigPtr& config, - const VirtualRobot::RobotPtr& rob, - bool deletable, - bool internal, - const std::string& instanceName) const final override + const NJointControllerConfigPtr& config, + const VirtualRobot::RobotPtr& rob, + bool deletable, + bool internal, + const std::string& instanceName) const final override { - ARMARX_CHECK_EXPRESSION_W_HINT(cmngr, "ControllerManagement module is NULL!"); + ARMARX_CHECK_EXPRESSION(cmngr) << "ControllerManagement module is NULL!"; - using ConfigPtrT = typename NJointControllerT::ConfigPtrT; - ConfigPtrT cfg = ConfigPtrT::dynamicCast(config); - ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The configuration is of the wrong type! it has to be an instance of: " - << GetTypeString<ConfigPtrT>()); + using ConfigPtrT = typename NJointControllerT::ConfigPtrT; + ConfigPtrT cfg = ConfigPtrT::dynamicCast(config); + ARMARX_CHECK_EXPRESSION(cfg) << "The configuration is of the wrong type! it has to be an instance of: " + << GetTypeString<ConfigPtrT>(); - ARMARX_CHECK_EXPRESSION_W_HINT(!ConstructorIsRunning(), "Two NJointControllers are created at the same time"); + ARMARX_CHECK_EXPRESSION(!ConstructorIsRunning()) << "Two NJointControllers are created at the same time"; NJointControllerBasePtr ptr; { ConstructorIsRunning_ = true; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp index 1a8943ff48d03c398ffe3d6ac7e37e191b766e51..3cc0e13c37dfaae118d6cb4eba12d3e85a82a7e8 100755 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp @@ -39,7 +39,7 @@ namespace armarx const SensorValueBase* sv = useSensorValue(cfg->platformName); this->sv = sv->asA<SensorValueHolonomicPlatform>(); target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>(); - ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity); + ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity; pid.threadSafe = false; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp index 5fa5360258b34c23ddb0b79948fd8aa93dc77e3e..8ec32aa03d020fc065a2a174756a83061c474c7e 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp @@ -38,7 +38,7 @@ namespace armarx const SensorValueBase* sv = useSensorValue(cfg->platformName); this->sv = sv->asA<SensorValueHolonomicPlatform>(); target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>(); - ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity); + ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity; pid.threadSafe = false; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp index 3bd188efeace82c1848be5b646fffc5653d593f4..35fd7551d0f4dfd9b33de0b2cac71ee427ef5f35 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp @@ -33,7 +33,7 @@ namespace armarx maxCommandDelay(IceUtil::Time::milliSeconds(500)) { target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>(); - ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity); + ARMARX_CHECK_EXPRESSION(target) << "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity; initialSettings.velocityX = cfg->initialVelocityX; initialSettings.velocityY = cfg->initialVelocityY; diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp index 6226de56e9ab0eea48c7697f535ee5bdfea0ecab..23ae6d2299c3874e9f7501ef7e81c124bc4882d6 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTCPController.cpp @@ -160,7 +160,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!config->nodeSetName.empty()); useSynchronizedRtRobot(); auto nodeset = rtGetRobot()->getRobotNodeSet(config->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(nodeset, config->nodeSetName); + ARMARX_CHECK_EXPRESSION(nodeset) << config->nodeSetName; for (size_t i = 0; i < nodeset->getSize(); ++i) { auto jointName = nodeset->getNode(i)->getName(); @@ -171,7 +171,7 @@ namespace armarx }; ik.reset(new VirtualRobot::DifferentialIK(nodeset, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped)); tcp = (config->tcpName.empty() || config->tcpName == DEFAULT_TCP_STRING) ? nodeset->getTCP() : rtGetRobot()->getRobotNode(config->tcpName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, config->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << config->tcpName; nodeSetName = config->nodeSetName; } diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp index bc0c3d694843f9c207a38ce2b30c5f5d8ff75a52..a81af0d3075c73ad053876735a1c0690efa7af88 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTaskSpaceImpedanceController.cpp @@ -42,7 +42,7 @@ NJointTaskSpaceImpedanceController::NJointTaskSpaceImpedanceController(const Rob VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); jointNames.clear(); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp index b51bb59d3907675755f3a65c7278370ea6bb4027..fa0c743d77a225270c44e767c7e1b0843c7601e3 100644 --- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp +++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointTrajectoryController.cpp @@ -10,7 +10,7 @@ namespace armarx NJointTrajectoryController::NJointTrajectoryController(RobotUnitPtr prov, const NJointControllerConfigPtr& config, const VirtualRobot::RobotPtr& robot) { cfg = NJointTrajectoryControllerConfigPtr::dynamicCast(config); - ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointTrajectoryControllerConfigPtr"); + ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointTrajectoryControllerConfigPtr"; for (std::string jointName : cfg->jointNames) { diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp index 0807e71bb1b87c655e2e11925308c5a761155201..8432b4b530cc289e05f0b245121f853df122295c 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.cpp @@ -102,10 +102,9 @@ namespace armarx::RobotUnitModule void ModuleBase::checkDerivedClasses() const { #define check_deriving(Type) \ - ARMARX_CHECK_NOT_NULL_W_HINT( \ - dynamic_cast<const Type*>(this), \ - "This class does not derive from " << GetTypeString<Type>() \ - ); + ARMARX_CHECK_NOT_NULL( \ + dynamic_cast<const Type*>(this)) << \ + "This class does not derive from " << GetTypeString<Type>(); for_each_module(check_deriving) #undef check_deriving } diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp index 4e5a932446f4a424fa04f72f5abb6471e0df5767..c4c7611f379c1ee00c106f2897212c580c25f853 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThread.cpp @@ -668,13 +668,12 @@ namespace armarx::RobotUnitModule const ControlDevicePtr& controlDevice = rtGetControlDevices().at(ctrlDevIdx); JointController* es = controlDevice->rtGetJointEmergencyStopController(); - ARMARX_CHECK_EQUAL_W_HINT( + ARMARX_CHECK_EQUAL( rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIdx), - nJointCtrlIndex, - VAROUT(ctrlDevIdx) << "\n" - << VAROUT(controlDevice->getDeviceName()) << "\n" - << dumpRtState() - ); + nJointCtrlIndex) << + VAROUT(ctrlDevIdx) << "\n" + << VAROUT(controlDevice->getDeviceName()) << "\n" + << dumpRtState(); controlDevice->rtSetActiveJointController(es); rtGetActivatedJointControllers().at(ctrlDevIdx) = es; @@ -729,15 +728,14 @@ namespace armarx::RobotUnitModule void ControlThread::rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex) { std::size_t nJointCtrlIndex = rtGetActivatedJointToNJointControllerAssignement().at(ctrlDevIndex); - ARMARX_CHECK_LESS_W_HINT( - 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() - ); + 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(); rtDeactivateNJointControllerBecauseOfError(nJointCtrlIndex); } @@ -834,7 +832,7 @@ 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_W_HINT(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)); @@ -844,7 +842,7 @@ namespace armarx::RobotUnitModule for (auto& set : setList) { auto rns = rtGetRobot()->getRobotNodeSet(set); - ARMARX_CHECK_EXPRESSION_W_HINT(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"; } @@ -911,7 +909,7 @@ namespace armarx::RobotUnitModule case EmergencyStopStateRequest::RequestNone: break; default: - ARMARX_CHECK_EXPRESSION_W_HINT(!static_cast<int>(state), "Unkown value for EmergencyStopStateRequest"); + ARMARX_CHECK_EXPRESSION(!static_cast<int>(state)) << "Unkown value for EmergencyStopStateRequest"; } } diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp index 8f5960857d371d51d9053113a3b032732f3bb905..3ccb1f9be38f93ae8d7a3ee96ac6cfb58957f631 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControllerManagement.cpp @@ -252,10 +252,9 @@ namespace armarx::RobotUnitModule deletable, internal, instanceName ); - ARMARX_CHECK_NOT_EQUAL_W_HINT( - nJointCtrl->getControlDeviceUsedIndices().size(), 0, - "The NJointControllerBase '" << nJointCtrl->getName() << "' uses no ControlDevice! (It has to use at least one)" - ); + ARMARX_CHECK_NOT_EQUAL( + nJointCtrl->getControlDeviceUsedIndices().size(), 0) << + "The NJointControllerBase '" << nJointCtrl->getName() << "' uses no ControlDevice! (It has to use at least one)"; getArmarXManager()->addObject(nJointCtrl, instanceName, false, false); nJointControllers[instanceName] = std::move(nJointCtrl); diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp index 219ba7815eba0cb2b03c4a5c93ccbe0e0973d175..c0874a7942069cad1330c359750f54baa1c26350 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp @@ -606,19 +606,17 @@ namespace armarx::RobotUnitModule //check other devs for (const auto& devname : group) { - ARMARX_CHECK_EXPRESSION_W_HINT( - controlDevices.has(devname), - "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: " - << devname << "\navailable:\n" << controlDevices.keys() - ); + ARMARX_CHECK_EXPRESSION( + controlDevices.has(devname)) << + "The ControlDeviceHardwareControlModeGroups property contains device names not existent in the robot: " + << devname << "\navailable:\n" << controlDevices.keys(); //Assert all devices in a group have the same control modes with the same hw controle modes const auto controlModeToHWControlModeForDevice = getControlModeToHWControlMode(devname); - ARMARX_CHECK_EXPRESSION_W_HINT( - controlModeToHWControlModeForDevice == controlModeToHWControlMode, - "Error for control modes of device '" << devname << "'\n" - << "it has the modes: " << controlModeToHWControlModeForDevice - << "\n but should have the modes: " << controlModeToHWControlMode - ); + ARMARX_CHECK_EXPRESSION( + controlModeToHWControlModeForDevice == controlModeToHWControlMode) << + "Error for control modes of device '" << devname << "'\n" + << "it has the modes: " << controlModeToHWControlModeForDevice + << "\n but should have the modes: " << controlModeToHWControlMode; //insert the device index into the device indices const auto devIdx = controlDevices.index(devname); ctrlModeGroups.deviceIndices.at(groupIdx).emplace_back(devIdx); @@ -689,14 +687,12 @@ namespace armarx::RobotUnitModule std::set<std::string> group; for (auto& device : strElems) { - ARMARX_CHECK_EXPRESSION_W_HINT( - !device.empty(), - "The ControlDeviceHardwareControlModeGroups property contains empty device names" - ); - ARMARX_CHECK_EXPRESSION_W_HINT( - !ctrlModeGroups.groupsMerged.count(device), - "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device - ); + ARMARX_CHECK_EXPRESSION( + !device.empty()) << + "The ControlDeviceHardwareControlModeGroups property contains empty device names"; + ARMARX_CHECK_EXPRESSION( + !ctrlModeGroups.groupsMerged.count(device)) << + "The ControlDeviceHardwareControlModeGroups property contains duplicate device names: " << device; ctrlModeGroups.groupsMerged.emplace(device); group.emplace(std::move(device)); } diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp index 70da5df0a915823ed517022cd287ce16b5eb057b..1f58f79ae6bb6be402f4b22b45bc5e9ebb06ae7a 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp @@ -431,7 +431,7 @@ namespace armarx::RobotUnitModule { throwIfInControlThread(BOOST_CURRENT_FUNCTION); rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs"); - ARMARX_CHECK_LESS_W_HINT(0, rtLoggingTimestep, "The property RTLoggingPeriodMs must not be 0"); + ARMARX_CHECK_LESS(0, rtLoggingTimestep) << "The property RTLoggingPeriodMs must not be 0"; messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize"); messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize"); diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp index 541147c28728402614fb24648376efd893615746..955a336053a4b7d605ea19cb70ba993b00b517a7 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleRobotData.cpp @@ -39,7 +39,7 @@ namespace armarx::RobotUnitModule const std::string& RobotData::getRobotPlatformName() const { throwIfInControlThread(BOOST_CURRENT_FUNCTION); - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; return robotPlatformName; } @@ -51,21 +51,21 @@ namespace armarx::RobotUnitModule const std::string& RobotData::getRobotNodetSeName() const { throwIfInControlThread(BOOST_CURRENT_FUNCTION); - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; return robotNodeSetName; } const std::string& RobotData::getRobotProjectName() const { throwIfInControlThread(BOOST_CURRENT_FUNCTION); - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; return robotProjectName; } const std::string& RobotData::getRobotFileName() const { throwIfInControlThread(BOOST_CURRENT_FUNCTION); - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; return robotFileName; } @@ -73,7 +73,7 @@ namespace armarx::RobotUnitModule { throwIfInControlThread(BOOST_CURRENT_FUNCTION); std::lock_guard<std::mutex> guard {robotMutex}; - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; return robot->getName(); } @@ -81,7 +81,7 @@ namespace armarx::RobotUnitModule { throwIfInControlThread(BOOST_CURRENT_FUNCTION); std::lock_guard<std::mutex> guard {robotMutex}; - ARMARX_CHECK_EXPRESSION_W_HINT(robot, __FUNCTION__ << " should only be called after _initVirtualRobot was called"); + ARMARX_CHECK_EXPRESSION(robot) << __FUNCTION__ << " should only be called after _initVirtualRobot was called"; ARMARX_CHECK_EXPRESSION(robotPool); const VirtualRobot::RobotPtr clone = robotPool->getRobot(); clone->setUpdateVisualization(false); diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp index 0ff29e89c33667dd2f82fc2eaa1072bf14e1c35c..76e2811dc28ae4aa3246d0fafc7fc032f8483669 100644 --- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp +++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp @@ -248,7 +248,7 @@ namespace armarx::RobotUnitModule ARMARX_IMPORTANT << "No joint devices found - skipping adding of KinematicUnit"; return nullptr; } - ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice"); + ARMARX_CHECK_EXPRESSION(!devs.empty()) << "no 1DoF ControlDevice found with matching SensorDevice"; //add it const std::string configName = getProperty<std::string>("KinematicUnitName"); const std::string confPre = getConfigDomain() + "." + configName + "."; @@ -386,17 +386,15 @@ namespace armarx::RobotUnitModule ftSensorData.sensorIndex = _module<Devices>().getSensorDevices().index(ftSensorDevice->getDeviceName()); ftSensorData.deviceName = ftSensorDevice->getDeviceName(); ftSensorData.frame = ftSensorDevice->getReportingFrame(); - ARMARX_CHECK_EXPRESSION_W_HINT( - !ftSensorData.frame.empty(), - "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex - << ") reports force torque values but returns an empty string as reporting frame" - ); - ARMARX_CHECK_EXPRESSION_W_HINT( - unitCreateRobot->hasRobotNode(ftSensorData.frame), - "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex - << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame - << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'" - ); + ARMARX_CHECK_EXPRESSION( + !ftSensorData.frame.empty()) << + "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex + << ") reports force torque values but returns an empty string as reporting frame"; + ARMARX_CHECK_EXPRESSION( + unitCreateRobot->hasRobotNode(ftSensorData.frame)) << + "The sensor device '" << ftSensorData.deviceName << "' (index: " << ftSensorData.sensorIndex + << ") reports force torque values but returns a reporting frame '" << ftSensorData.frame + << "' which is not present in the robot '" << _module<RobotData>().getRobotName() << "'"; ftdevs.emplace_back(std::move(ftSensorData)); } } @@ -564,7 +562,7 @@ namespace armarx::RobotUnitModule { emergencyStopMaster = new RobotUnitEmergencyStopMaster {&_module<ControlThread>(), getProperty<std::string>("EmergencyStopTopic").getValue()}; ManagedIceObjectPtr obj = ManagedIceObjectPtr::dynamicCast(emergencyStopMaster); - ARMARX_CHECK_EXPRESSION_W_HINT(obj, "RobotUnitEmergencyStopMaster"); + ARMARX_CHECK_EXPRESSION(obj) << "RobotUnitEmergencyStopMaster"; getArmarXManager()->addObject(obj, getProperty<std::string>("EmergencyStopMasterName").getValue(), true); auto prx = obj->getProxy(-1); try diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp index 1c98f6b653589f3a6f6fd611d4695e1933111458..4f320f59ce2d0181473fe7231ed56a5f6ad0a67d 100644 --- a/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp +++ b/source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp @@ -59,7 +59,7 @@ void TCPControllerSubUnit::setCycleTime(Ice::Int, const Ice::Current& c) void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const std::string& tcpName, const FramedDirectionBasePtr& translationVelocity, const FramedDirectionBasePtr& orientationVelocityRPY, const Ice::Current& c) { std::unique_lock lock(dataMutex); - ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName); + ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNodeSet(nodeSetName)) << "The robot does not have the node set: " + nodeSetName; std::string tcp; if (tcpName.empty()) { @@ -67,7 +67,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const } else { - ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName); + ARMARX_CHECK_EXPRESSION(coordinateTransformationRobot->hasRobotNode(tcpName)) << "The robot does not have the node: " + tcpName; tcp = tcpName; } diff --git a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp index 4af60478ff5da6a4a57d055dd919473a64b1d1d5..e22d0319c205a8665bb23222f692b1832b417b41 100644 --- a/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp +++ b/source/RobotAPI/components/units/RobotUnit/Units/TrajectoryControllerSubUnit.cpp @@ -54,7 +54,7 @@ bool TrajectoryControllerSubUnit::startTrajectoryPlayer(const Ice::Current& c) { ARMARX_DEBUG << "Starting TrajectoryPlayer ..."; - ARMARX_CHECK_EXPRESSION_W_HINT(jointTraj, "No trajectory loaded!"); + ARMARX_CHECK_EXPRESSION(jointTraj) << "No trajectory loaded!"; assureTrajectoryController(); jointTrajController->setTrajectory(this->jointTraj, c); diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp index 98fe0b961192861a9fa7df1029afc178cd4db1ce..e753f9c4b60b13533dc2cfa20e6494a1d106ba1a 100644 --- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp +++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp @@ -238,14 +238,13 @@ namespace armarx << "\n" << "\nthis = " << this; }; - ARMARX_CHECK_NOT_NULL_W_HINT(place, hint); - ARMARX_CHECK_LESS_EQUAL_W_HINT(static_cast<void*>(&buffer.front()), static_cast<void*>(place), hint); - ARMARX_CHECK_LESS_EQUAL_W_HINT(static_cast<void*>(place), static_cast<void*>(&buffer.back()), hint); - ARMARX_CHECK_LESS_EQUAL_W_HINT( + ARMARX_CHECK_NOT_NULL(place) << hint; + ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(&buffer.front()), static_cast<void*>(place)) << hint; + ARMARX_CHECK_LESS_EQUAL(static_cast<void*>(place), static_cast<void*>(&buffer.back())) << hint; + ARMARX_CHECK_LESS_EQUAL( static_cast<void*>(static_cast<std::uint8_t*>(place) + entry->_sizeof() - 1), - static_cast<void*>(&buffer.back()), - hint - ); + static_cast<void*>(&buffer.back())) << + hint; ARMARX_CHECK_LESS_EQUAL(place, &buffer.back()); ARMARX_CHECK_LESS(entriesWritten, entries.size()); ARMARX_CHECK_EXPRESSION(!entries.at(entriesWritten)); diff --git a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp index f52c9276e2876c52264a09831c212d5b9db2ffd5..bf5e5e28970861a2ebd6e0fb8c17d052962f5786 100644 --- a/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp +++ b/source/RobotAPI/components/units/RobotUnit/util/DynamicsHelper.cpp @@ -47,7 +47,7 @@ namespace armarx if (selectedJoint) { auto sensorValue = const_cast<SensorValue1DoFActuator*>(selectedJoint->getSensorValue()->asA<SensorValue1DoFActuator>()); - ARMARX_CHECK_EXPRESSION_W_HINT(sensorValue, rnsJoints->getNode(i)->getName()); + ARMARX_CHECK_EXPRESSION(sensorValue) << rnsJoints->getNode(i)->getName(); ARMARX_DEBUG << "will calculate inverse dynamics for robot node " << rnsJoints->getNode(i)->getName(); data.nodeSensorVec.push_back(std::make_pair(rnsJoints->getNode(i), sensorValue)); } @@ -79,7 +79,7 @@ namespace armarx size_t i = 0; for (auto& node : data.nodeSensorVec) { - ARMARX_CHECK_EXPRESSION_W_HINT(node.second, node.first->getName()); + ARMARX_CHECK_EXPRESSION(node.second) << node.first->getName(); data.q(i) = node.second->position; if (i < data.velocityFilter.size() && data.velocityFilter.at(i) && !std::isnan(node.second->velocity)) { diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h index 00800f190fcbbc40b4c774c6387577f89d925c6b..65e8980af45c0a202787d0220e89bf7cf0313ea1 100644 --- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h +++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h @@ -100,11 +100,11 @@ namespace armarx::introspection { if (!addedBases.count(e.getClassName())) { - ARMARX_CHECK_EXPRESSION_W_HINT( - !entries.count(e.getMemberName()), - "Adding the base class '" << GetTypeString<BaseClassType>() - << "' adds an entry for the member '" << e.getMemberName() - << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'"); + ARMARX_CHECK_EXPRESSION( + !entries.count(e.getMemberName())) << + "Adding the base class '" << GetTypeString<BaseClassType>() + << "' adds an entry for the member '" << e.getMemberName() + << "' which already was added by class '" << entries.at(e.getMemberName()).getClassName() << "'"; basesAddedInCall.emplace(e.getClassName()); entries.add(e.getMemberName(), e); } diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp index 60f62a097be4da24c4f1808b5c6c7c0c385c4f30..cf3a4e9b0ba5db52287a006a879c0b841778bf29 100644 --- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp +++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp @@ -53,7 +53,7 @@ namespace armarx::introspection { return {{name, {new TimedVariant(FramedPosition{value, frame, agent}, timestamp)}}}; } - ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given"); + ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given"; return {{name, {new TimedVariant(Vector3{value}, timestamp)}}}; } } @@ -73,7 +73,7 @@ namespace armarx::introspection const std::string& frame, \ const std::string& agent) \ { \ - ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant"); \ + ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \ std::map<std::string, VariantBasePtr> result; \ for(int i = 0; i < Num; ++i) \ { \ @@ -141,7 +141,7 @@ namespace armarx::introspection const std::string& frame, const std::string& agent) { - ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for JointStatus"); + ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus"; static const auto fnames = GetFieldNames(); return { @@ -189,7 +189,7 @@ namespace armarx::introspection { return {{name, {new TimedVariant(FramedOrientation{value, frame, agent}, timestamp)}}}; } - ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given"); + ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given"; return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}}; } } @@ -213,7 +213,7 @@ namespace armarx::introspection { return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}}; } - ARMARX_CHECK_EXPRESSION_W_HINT(agent.empty(), "No frame but an agent given"); + ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given"; return {{name, {new TimedVariant(Pose{value}, timestamp)}}}; } } @@ -227,7 +227,7 @@ namespace armarx::introspection const std::string& frame, const std::string& agent) { - ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant"); + ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}}; } @@ -238,7 +238,7 @@ namespace armarx::introspection const std::string& frame, const std::string& agent) { - ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version of TimestampVariant"); + ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}}; } } diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h index c29199ada8042ed04c30a83e48fe9da77444010e..c122c55b0361fbe82f7e7ce6bbffcb2fdbf9bc84 100644 --- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h +++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h @@ -67,7 +67,7 @@ namespace armarx::introspection const std::string& frame = "", const std::string& agent = "") { - ARMARX_CHECK_EXPRESSION_W_HINT(frame.empty() && agent.empty(), "There is no framed version for fundamental types"); + ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for fundamental types"; return {{name, {new TimedVariant(value, timestamp)}}}; } }; diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp index 37e1df461dc90453d02567bc65feb6deeb744daf..9c434042b75b23ab4393ed3e7c3a65a0658181e5 100644 --- a/source/RobotAPI/components/units/TCPControlUnit.cpp +++ b/source/RobotAPI/components/units/TCPControlUnit.cpp @@ -191,7 +191,7 @@ namespace armarx } std::unique_lock lock(dataMutex); - ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName), "The robot does not have the node set: " + nodeSetName); + ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNodeSet(nodeSetName)) << "The robot does not have the node set: " + nodeSetName; if (translationVelocity) @@ -215,7 +215,7 @@ namespace armarx } else { - ARMARX_CHECK_EXPRESSION_W_HINT(jointExistenceCheckRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName); + ARMARX_CHECK_EXPRESSION(jointExistenceCheckRobot->hasRobotNode(tcpName)) << "The robot does not have the node: " + tcpName; data.tcpName = tcpName; } @@ -687,15 +687,15 @@ namespace armarx { if (mode <= IKSolver::Z) { - ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 1, "The tcpWeight vector must be of size 1"); + ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 1) << "The tcpWeight vector must be of size 1"; } else if (mode <= IKSolver::Orientation) { - ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 3, "The tcpWeight vector must be of size 3"); + ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 3) << "The tcpWeight vector must be of size 3"; } else if (mode == IKSolver::All) { - ARMARX_CHECK_EXPRESSION_W_HINT(tcpWeight.rows() == 6, "The tcpWeight vector must be of size 6"); + ARMARX_CHECK_EXPRESSION(tcpWeight.rows() == 6) << "The tcpWeight vector must be of size 6"; } tcpWeights[tcp] = tcpWeight; diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp index 17ce02826bd5eb2ab427cfc88310292b82d2d6f3..e1d31b40ab9d3b02d838f9f7d6156fed3f38d14b 100644 --- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp +++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp @@ -761,7 +761,7 @@ bool KinematicUnitWidgetController::initGUIJointListTable(VirtualRobot::RobotNod << "Enabled" << "Emergency Stop"; ui.tableJointList->setHorizontalHeaderLabels(s); - ARMARX_CHECK_EXPRESSION_W_HINT(ui.tableJointList->columnCount() == eTabelColumnCount, "Current table size: " << ui.tableJointList->columnCount()); + ARMARX_CHECK_EXPRESSION(ui.tableJointList->columnCount() == eTabelColumnCount) << "Current table size: " << ui.tableJointList->columnCount(); // fill in joint names diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h index 50207f96b4df5d3cfc58d302564c6ca188dacd97..5123ac0df92bd4488e3f1c20fc8306af233d9eb4 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractData.h @@ -60,17 +60,16 @@ namespace armarx void init(T* raw, float factor, float offset, float defaultValue = std::nan("1"), bool offsetBeforeFactor = true, const char* nameForDebugging = "") { const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw); - ARMARX_CHECK_EXPRESSION_W_HINT( - (rawAsInt % alignof(T)) == 0, - "\nThe alignment is wrong!\nIt has to be " - << alignof(T) << ", but the data is aligned with " - << rawAsInt % alignof(std::max_align_t) - << "!\nThis is an offset of " << (rawAsInt % alignof(T)) - << " bytes!\nThe datatype is " << GetTypeString<T>() - << "\nIts size is " << sizeof(T) - << "\nraw = " << raw - << " bytes\nThe name is " << nameForDebugging - ); + ARMARX_CHECK_EXPRESSION( + (rawAsInt % alignof(T)) == 0) << + "\nThe alignment is wrong!\nIt has to be " + << alignof(T) << ", but the data is aligned with " + << rawAsInt % alignof(std::max_align_t) + << "!\nThis is an offset of " << (rawAsInt % alignof(T)) + << " bytes!\nThe datatype is " << GetTypeString<T>() + << "\nIts size is " << sizeof(T) + << "\nraw = " << raw + << " bytes\nThe name is " << nameForDebugging; this->offsetBeforeFactor = offsetBeforeFactor; this->factor = factor; this->offset = offset; diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h index fb8868b0d0044514b29702b9d144af9e75209531..f9064b08c179e984fb7eb6cd9f95e32c5e96ae76 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/AbstractSlave.h @@ -113,31 +113,29 @@ namespace armarx void setInputPDO(void* ptr) override { const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr); - ARMARX_CHECK_EXPRESSION_W_HINT( - (ptrAsInt % alignof(InputT)) == 0, - "\nThe alignment is wrong!\nIt has to be " - << alignof(InputT) << ", but the data is aligned with " - << ptrAsInt % alignof(std::max_align_t) - << "!\nThis is an offset of " << (ptrAsInt % alignof(InputT)) - << " bytes!\nThe datatype is " << GetTypeString<InputT>() - << "\nIts size is " << sizeof(InputT) - << " bytes" - ); + ARMARX_CHECK_EXPRESSION( + (ptrAsInt % alignof(InputT)) == 0) << + "\nThe alignment is wrong!\nIt has to be " + << alignof(InputT) << ", but the data is aligned with " + << ptrAsInt % alignof(std::max_align_t) + << "!\nThis is an offset of " << (ptrAsInt % alignof(InputT)) + << " bytes!\nThe datatype is " << GetTypeString<InputT>() + << "\nIts size is " << sizeof(InputT) + << " bytes"; inputs = static_cast<InputT*>(ptr); } void setOutputPDO(void* ptr) override { const auto ptrAsInt = reinterpret_cast<std::uint64_t>(ptr); - ARMARX_CHECK_EXPRESSION_W_HINT( - (ptrAsInt % alignof(OutputT)) == 0, - "\nThe alignment is wrong!\nIt has to be " - << alignof(OutputT) << ", but the data is aligned with " - << ptrAsInt % alignof(std::max_align_t) - << "!\nThis is an offset of " << (ptrAsInt % alignof(OutputT)) - << " bytes!\nThe datatype is " << GetTypeString<OutputT>() - << "\nIts size is " << sizeof(OutputT) - << " bytes" - ); + ARMARX_CHECK_EXPRESSION( + (ptrAsInt % alignof(OutputT)) == 0) << + "\nThe alignment is wrong!\nIt has to be " + << alignof(OutputT) << ", but the data is aligned with " + << ptrAsInt % alignof(std::max_align_t) + << "!\nThis is an offset of " << (ptrAsInt % alignof(OutputT)) + << " bytes!\nThe datatype is " << GetTypeString<OutputT>() + << "\nIts size is " << sizeof(OutputT) + << " bytes"; outputs = static_cast<OutputT*>(ptr); } bool hasPDOMapping() const final override diff --git a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp index d85ca3490aa355e853dc7b65ec873a0710b4e668..3ba0cd1a3b3e45be76353f5357894beff10b7163 100644 --- a/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp +++ b/source/RobotAPI/libraries/ArmarXEtherCAT/DeviceContainer.cpp @@ -70,7 +70,7 @@ namespace armarx auto name = node.attribute_value("name"); ARMARX_DEBUG << "Handling: " << node.name() << " name: " << name; // auto obj = getSceneObject(name); - // ARMARX_CHECK_EXPRESSION_W_HINT(obj, name); + // ARMARX_CHECK_EXPRESSION(obj) << name; auto tuple = std::make_tuple(node, defaultNode, robot); auto instance = VirtualDeviceFactory::fromName(node.name(), tuple); if (!instance) diff --git a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp index 87973287030bf4dac0f45a0a0af25c8fb930afe7..d85b1a78d4bd192e56e95c1c5eef0ed1350138c2 100644 --- a/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp +++ b/source/RobotAPI/libraries/DSControllers/DSJointCarryController.cpp @@ -77,8 +77,8 @@ DSJointCarryController::DSJointCarryController(const RobotUnitPtr& robotUnit, co left_jointNames.clear(); right_jointNames.clear(); - ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm"); - ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm"); + ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm"; + ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm"; // for left arm for (size_t i = 0; i < left_ns->getSize(); ++i) diff --git a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp index b27e2d95809ae52f73c5c6e341f123bef6e6b941..6f0b438717a13aefdbc6c68a6e949808a6243bd8 100644 --- a/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp +++ b/source/RobotAPI/libraries/DSControllers/DSRTBimanualController.cpp @@ -77,8 +77,8 @@ namespace armarx left_jointNames.clear(); right_jointNames.clear(); - ARMARX_CHECK_EXPRESSION_W_HINT(left_ns, "LeftArm"); - ARMARX_CHECK_EXPRESSION_W_HINT(right_ns, "RightArm"); + ARMARX_CHECK_EXPRESSION(left_ns) << "LeftArm"; + ARMARX_CHECK_EXPRESSION(right_ns) << "RightArm"; // for left arm for (size_t i = 0; i < left_ns->getSize(); ++i) diff --git a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp index 53d89208654b540280009cc687fcc47194e2ee71..2c9e75497c9730f7cc5b89a6bd67e12eb5a9852d 100644 --- a/source/RobotAPI/libraries/DSControllers/DSRTController.cpp +++ b/source/RobotAPI/libraries/DSControllers/DSRTController.cpp @@ -63,7 +63,7 @@ namespace armarx VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); jointNames.clear(); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -96,7 +96,7 @@ namespace armarx }; ARMARX_INFO << "Initialized " << targets.size() << " targets"; tcp = rns->getTCP(); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName; ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped)); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp index 630e11b7630c28fa15807a364a30e867fdf881f2..a79f9120177daef4a3d51d1e42c838927ceb58d5 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAdaptiveWipingController.cpp @@ -13,7 +13,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp index 843a8704145eef3c80b133bd2f1e225e012f5fd9..2e3ec38e5e4cf64955a6ca6107b32e779aa1418e 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointAnomalyDetectionAdaptiveWipingController.cpp @@ -13,7 +13,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp index 837c624dba875a35c46c13c9769664ab9e6ee674..98e91a6fe769b1c81cfbd313bf73a5ee8fc7a0d0 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointCCDMPController.cpp @@ -13,7 +13,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -37,7 +37,7 @@ namespace armarx }; tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName; // set tcp controller tcpController.reset(new CartesianVelocityController(rns, tcp)); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp index 11d0637a53ea10892ddf40f82b3f3cb64b6d61fd..e5f7df8ef3a01bc95d68f4e59f8fe39b043a176a 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointJSDMPController.cpp @@ -23,7 +23,7 @@ namespace armarx useSynchronizedRtRobot(); cfg = NJointJointSpaceDMPControllerConfigPtr::dynamicCast(config); - ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: NJointJointSpaceDMPControllerConfigPtr"); + ARMARX_CHECK_EXPRESSION(cfg) << "Needed type: NJointJointSpaceDMPControllerConfigPtr"; for (std::string jointName : cfg->jointNames) { diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp index b8e560bb9dd0f341cde6a8ae57bf65ae58ff196a..1a173c43b48400f9796f92c437ff1a1ec934918e 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp @@ -13,7 +13,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp index 24b98a2ea15d0919be10196f834482d385983dcb..abbebd242dd5b6b8755eff620d06af9dcf854922 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPForwardVelController.cpp @@ -13,7 +13,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp index b63554a7dc4ae326a6d1e80af6210581590c6594..efbb78ce6ac40d4104e059167446028c4a4dceb0 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTSDMPController.cpp @@ -16,7 +16,7 @@ namespace armarx ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); jointNames = rns->getNodeNames(); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { std::string jointName = rns->getNode(i)->getName(); @@ -45,7 +45,7 @@ namespace armarx tcp = (cfg->tcpName.empty()) ? rns->getTCP() : rtGetRobot()->getRobotNode(cfg->tcpName); refFrame = (cfg->frameName.empty()) ? rns->getRobot()->getRootNode() : rtGetRobot()->getRobotNode(cfg->frameName); - ARMARX_CHECK_EXPRESSION_W_HINT(tcp, cfg->tcpName); + ARMARX_CHECK_EXPRESSION(tcp) << cfg->tcpName; // set tcp controller tcpController.reset(new CartesianVelocityController(rns, tcp)); diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp index e284a6c0866fa35d34baa580ede3976b0049afef..e52e8c26f863b165dca8605226747e66e68a7aee 100644 --- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp +++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp @@ -12,7 +12,7 @@ namespace armarx cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config); useSynchronizedRtRobot(); rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); - ARMARX_CHECK_EXPRESSION_W_HINT(rns, cfg->nodeSetName); + ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName; for (size_t i = 0; i < rns->getSize(); ++i) { diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp index a1b0464715739f9ca19a88570e311576f24c980e..2e0e792d0f9d4f817eb9efa723f7991bebef812f 100644 --- a/source/RobotAPI/libraries/core/FramedPose.cpp +++ b/source/RobotAPI/libraries/core/FramedPose.cpp @@ -228,15 +228,15 @@ namespace armarx if (oldFrame == GlobalFrame) { auto node = robotState->getRobotNode(newFrame); - ARMARX_CHECK_EXPRESSION_W_HINT(node, newFrame); + ARMARX_CHECK_EXPRESSION(node) << newFrame; toNewFrame = node->getGlobalPose(); } else { auto node = robotState->getRobotNode(oldFrame); - ARMARX_CHECK_EXPRESSION_W_HINT(node, "old frame: " + oldFrame + ValueToString(robotState->getConfig()->getRobotNodeJointValueMap())); + ARMARX_CHECK_EXPRESSION(node) << "old frame: " + oldFrame + ValueToString(robotState->getConfig()->getRobotNodeJointValueMap()); auto newNode = robotState->getRobotNode(newFrame); - ARMARX_CHECK_EXPRESSION_W_HINT(newNode, newFrame); + ARMARX_CHECK_EXPRESSION(newNode) << newFrame; toNewFrame = node->getTransformationTo(newNode); } diff --git a/source/RobotAPI/libraries/core/LinkedPose.cpp b/source/RobotAPI/libraries/core/LinkedPose.cpp index 79437b4a38987faa9ed4d88b79e093e339868ffd..00391df0cba8d2a9b94cb368cdf16738c7e4c2c8 100644 --- a/source/RobotAPI/libraries/core/LinkedPose.cpp +++ b/source/RobotAPI/libraries/core/LinkedPose.cpp @@ -68,7 +68,7 @@ namespace armarx Pose(other), FramedPose(other) { - ARMARX_CHECK_EXPRESSION_W_HINT(referenceRobot, "The robot proxy must not be zero"); + ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero"; this->referenceRobot = referenceRobot; if (referenceRobot) @@ -83,7 +83,7 @@ namespace armarx Pose(m, v), FramedPose(m, v, s, referenceRobot->getName()) { - ARMARX_CHECK_EXPRESSION_W_HINT(referenceRobot, "The robot proxy must not be zero"); + ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero"; referenceRobot->ref(); this->referenceRobot = referenceRobot; } @@ -92,7 +92,7 @@ namespace armarx Pose(m), FramedPose(m, s, referenceRobot->getName()) { - ARMARX_CHECK_EXPRESSION_W_HINT(referenceRobot, "The robot proxy must not be zero"); + ARMARX_CHECK_EXPRESSION(referenceRobot) << "The robot proxy must not be zero"; referenceRobot->ref(); this->referenceRobot = referenceRobot; } diff --git a/source/RobotAPI/libraries/core/Trajectory.cpp b/source/RobotAPI/libraries/core/Trajectory.cpp index 90c68c71c1fd0ca963f8f4eb1bc05c1d926be1f7..f8068131cc9a3d0b939ff06242505195310f830a 100644 --- a/source/RobotAPI/libraries/core/Trajectory.cpp +++ b/source/RobotAPI/libraries/core/Trajectory.cpp @@ -273,7 +273,7 @@ namespace armarx } - ARMARX_CHECK_GREATER_W_HINT(vec.at(dim)->size(), derivation, VAROUT(t) << VAROUT(dim) << VAROUT(*this)); + ARMARX_CHECK_GREATER(vec.at(dim)->size(), derivation) << VAROUT(t) << VAROUT(dim) << VAROUT(*this); // std::cout << "dimensions: " <<it->data.size() << " derivations: " << it->data.at(dim)->size() << std::endl; double result = vec.at(dim)->at(derivation); // checkValue(result); @@ -1495,7 +1495,7 @@ namespace armarx if (itPrev != ordView.end()) { // ARMARX_INFO << "Found prev state at " << itPrev->timestamp; - ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itPrev->timestamp, VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this)); + ARMARX_CHECK_NOT_EQUAL(t, itPrev->timestamp) << VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this); previous = getState(itPrev->timestamp, dimension, derivation); } @@ -1508,7 +1508,7 @@ namespace armarx if (itNext != ordView.end()) { // ARMARX_INFO << "Found next state at " << itNext->timestamp; - ARMARX_CHECK_NOT_EQUAL_W_HINT(t, itNext->timestamp, VAROUT(t) << VAROUT(itNext->timestamp)); + ARMARX_CHECK_NOT_EQUAL(t, itNext->timestamp) << VAROUT(t) << VAROUT(itNext->timestamp); next = getState(itNext->timestamp, dimension, derivation); } @@ -1679,7 +1679,7 @@ namespace armarx currentTimestamp += stepSize; i++; } - ARMARX_CHECK_EQUAL_W_HINT(result.size(), size, VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize)); + ARMARX_CHECK_EQUAL(result.size(), size) << VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize); return result; } @@ -1934,7 +1934,7 @@ namespace armarx } else { - ARMARX_CHECK_EXPRESSION_W_HINT(timestamps.size() == values.size(), timestamps.size() << ", " << values.size()); + ARMARX_CHECK_EXPRESSION(timestamps.size() == values.size()) << timestamps.size() << ", " << values.size(); for (size_t i = 0; i < timestamps.size(); ++i) { diff --git a/source/RobotAPI/libraries/core/test/RobotTest.cpp b/source/RobotAPI/libraries/core/test/RobotTest.cpp index c34a4d38a05e660e016bc768612dd12956221f79..b16bf6cbc4528994969fdc60b2f786f44f528252 100644 --- a/source/RobotAPI/libraries/core/test/RobotTest.cpp +++ b/source/RobotAPI/libraries/core/test/RobotTest.cpp @@ -41,7 +41,7 @@ BOOST_AUTO_TEST_CASE(RobotPoolTest) const std::string project = "RobotAPI"; armarx::CMakePackageFinder finder(project); - ARMARX_CHECK_EXPRESSION_W_HINT(finder.packageFound(), project); + ARMARX_CHECK_EXPRESSION(finder.packageFound()) << project; armarx::ArmarXDataPath::addDataPaths(finder.getDataDir()); diff --git a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp index 10cf524b783e1c29d1abc590143e97e1dec2734a..08a05bfdd371bad4071f92ff3530d375578a145a 100644 --- a/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp +++ b/source/RobotAPI/statecharts/StatechartProfilesTestGroup/TestState.cpp @@ -48,7 +48,7 @@ void TestState::onEnter() ARMARX_IMPORTANT << "TestParam2: " << TestParam2; ARMARX_IMPORTANT << "TestParam3: " << TestParam3; - ARMARX_CHECK_EXPRESSION_W_HINT(emptyString == "", "EmptyStringTest is not empty."); + ARMARX_CHECK_EXPRESSION(emptyString == "") << "EmptyStringTest is not empty."; ARMARX_CHECK_EXPRESSION(TestParam1 == "OnlyRootSet"); ARMARX_CHECK_EXPRESSION(TestParam2 == "Armar3BaseSet"); ARMARX_CHECK_EXPRESSION(TestParam3 == "Armar3aSet" || TestParam3 == "Armar3SimulationSet");