Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

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}")

source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp

+#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

source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h

+/**
+ * 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