diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt index a4ca9b08ad6d3b0937294c5a9050adb6680abfcc..594e05ed14002d7098c2ecaa41a5d627e3f9ea24 100644 --- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt @@ -25,6 +25,7 @@ set(LIB_FILES NaturalIKComponentPlugin.cpp HandUnitComponentPlugin.cpp FrameTrackingComponentPlugin.cpp + HeartbeatComponentPlugin.cpp ) set(LIB_HEADERS RobotStateComponentPlugin.h @@ -40,6 +41,7 @@ set(LIB_HEADERS NaturalIKComponentPlugin.h HandUnitComponentPlugin.h FrameTrackingComponentPlugin.h + HeartbeatComponentPlugin.h ) armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}") diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp new file mode 100644 index 0000000000000000000000000000000000000000..510ff8933b6e045ec96e1020e6914f2b064636f2 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp @@ -0,0 +1,98 @@ +#include "HeartbeatComponentPlugin.h" + +#include "ArmarXCore/core/Component.h" +#include "ArmarXCore/core/exceptions/local/ExpressionException.h" + +#include <RobotAPI/interface/components/RobotHealthInterface.h> + +namespace armarx::plugins +{ + void + HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel, + const RobotHealthHeartbeatArgs& args) + { + channelHeartbeatConfig.emplace(channel, args); + } + + void + HeartbeatComponentPlugin::heartbeat() + { + + if (robotHealthTopic) + { + robotHealthTopic->heartbeat(componentName, heartbeatArgs); + } + else + { + ARMARX_WARNING << "No robot health topic available!"; + } + } + + void + HeartbeatComponentPlugin::heartbeat(const std::string& channel) + { + const auto argsIt = channelHeartbeatConfig.find(channel); + ARMARX_CHECK(argsIt != channelHeartbeatConfig.end()) + << "heartbeat() called for unknown channel '" << channel << "'." + << "You must register the config using configureHeartbeatChannel(channel) first!"; + + const auto& args = argsIt->second; + + if (robotHealthTopic) + { + robotHealthTopic->heartbeat(componentName + "_" + channel, args); + } + else + { + ARMARX_WARNING << "No robot health topic available!"; + } + } + + void + HeartbeatComponentPlugin::preOnInitComponent() + { + if (topicName.empty()) + { + parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName)); + } + parent<Component>().offeringTopic(topicName); + } + + void + HeartbeatComponentPlugin::postOnInitComponent() + { + } + + void + HeartbeatComponentPlugin::preOnConnectComponent() + { + robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName); + } + + void + HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) + { + if (!properties->hasDefinition(makePropertyName(topicPropertyName))) + { + properties->defineOptionalProperty<std::string>( + makePropertyName(topicPropertyName), + "DebugObserver", + "Name of the topic the DebugObserver listens on"); + } + + if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName))) + { + properties->defineRequiredProperty<std::string>( + makePropertyName(maximumCycleTimeWarningMSPropertyName), + "TODO: maximumCycleTimeWarningMS"); + } + + if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName))) + { + properties->defineRequiredProperty<std::string>( + makePropertyName(maximumCycleTimeErrorMSPropertyName), + "TODO: maximumCycleTimeErrorMS"); + } + } + +} // namespace armarx::plugins diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h new file mode 100644 index 0000000000000000000000000000000000000000..6a4a6c2bd692484c94bbd42bdfff399b0f5a0aa9 --- /dev/null +++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h @@ -0,0 +1,89 @@ +/** + * This file is part of ArmarX. + * + * ArmarX is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * ArmarX is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2021 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <ArmarXCore/core/ComponentPlugin.h> + +#include <RobotAPI/interface/components/RobotHealthInterface.h> + +namespace armarx::plugins +{ + + class HeartbeatComponentPlugin : public ComponentPlugin + { + public: + using ComponentPlugin::ComponentPlugin; + + /** + * @brief Configures a heartbeat subchannel. + * + * @param channel Identifier of the heartbeat channel + * @param args Configuration of this channel's heartbeat properties + */ + void configureHeartbeatChannel(const std::string& channel, + const RobotHealthHeartbeatArgs& args); + + /** + * @brief Sends out a heartbeat using the default config + * + */ + void heartbeat(); + + /** + * @brief Sends out a heartbeat for a subchannel. + * + * Note: You must call configureHeartbeatChannel(...) first to register the channel config! + * + * @param channel Identifier of the heartbeat channel + */ + void heartbeat(const std::string& channel); + + protected: + void preOnInitComponent() override; + void postOnInitComponent() override; + void preOnConnectComponent() override; + + void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override; + + private: + //! heartbeat topic name (outgoing) + std::string topicName; + + //! name of this component used as identifier for heartbeats + std::string componentName; + + // + static constexpr auto topicPropertyName = "heartbeat.TopicName"; + static constexpr auto maximumCycleTimeWarningMSPropertyName = + "heartbeat.maximumCycleTimeWarningMS"; + static constexpr auto maximumCycleTimeErrorMSPropertyName = + "heartbeat.maximumCycleTimeErrorMS"; + + RobotHealthInterfacePrx robotHealthTopic; + + //! default config used in heartbeat(), set via properties + RobotHealthHeartbeatArgs heartbeatArgs; + + //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...) + std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig; + }; +} // namespace armarx::plugins