new component: VirtualRobotWriterExample

source/RobotAPI/components/armem/client/CMakeLists.txt

@@ -3,3 +3,4 @@ add_subdirectory(ExampleMemoryClient)
 add_subdirectory(GraspProviderExample)
 
 add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(VirtualRobotWriterExample)

source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt

+set(LIB_NAME       VirtualRobotWriterExample)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+# Add the component
+armarx_add_component(
+    COMPONENT_LIBS
+        ArmarXCore 
+        armem_objects
+        armem_robot_state
+    SOURCES
+        VirtualRobotWriterExample.cpp
+    HEADERS
+        VirtualRobotWriterExample.h
+)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+    # If your component is not defined in ::armarx, specify its namespace here:
+    COMPONENT_NAMESPACE "armarx::virtual_robot_writer_example"
+)

source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp

+/*
+ * 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
+ */
+
+#include "VirtualRobotWriterExample.h"
+
+#include <memory>
+
+#include <Eigen/Geometry>
+
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
+namespace armarx::virtual_robot_writer_example
+{
+    VirtualRobotWriterExample::VirtualRobotWriterExample()
+    {
+        addPlugin(virtualRobotWriterPlugin);
+    }
+
+    armarx::PropertyDefinitionsPtr
+    VirtualRobotWriterExample::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr defs =
+            new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        // defs->topic(debugObserver);
+
+        defs->optional(p.updateFrequency, "updateFrequency", "Memory update frequency (write).");
+        defs->optional(p.robot.package, "robot.package", "");
+        defs->optional(p.robot.path, "robot.path", "");
+
+        return defs;
+    }
+
+    std::string
+    VirtualRobotWriterExample::getDefaultName() const
+    {
+        return "VirtualRobotWriterExample";
+    }
+
+    void
+    VirtualRobotWriterExample::onInitComponent()
+    {
+    }
+
+    void
+    VirtualRobotWriterExample::onConnectComponent()
+    {
+        ARMARX_IMPORTANT << "Running example.";
+        start = armem::Time::Now();
+
+        task = new PeriodicTask<VirtualRobotWriterExample>(
+            this, &VirtualRobotWriterExample::run, static_cast<int>(1000.f / p.updateFrequency));
+        task->start();
+    }
+
+    void
+    VirtualRobotWriterExample::onDisconnectComponent()
+    {
+        task->stop();
+    }
+
+    void
+    VirtualRobotWriterExample::onExitComponent()
+    {
+    }
+
+    VirtualRobot::RobotPtr
+    VirtualRobotWriterExample::loadRobot() const
+    {
+        auto robot =
+            VirtualRobot::RobotIO::loadRobot(PackagePath(p.robot.package, p.robot.path).toSystemPath(),
+                                             VirtualRobot::RobotIO::eStructure);
+        return robot;
+    }
+
+
+    void
+    VirtualRobotWriterExample::run()
+    {
+        if (robot == nullptr)
+        {
+            robot = loadRobot();
+
+            if (robot == nullptr) // still
+            {
+                ARMARX_ERROR << "Loading robot failed";
+                return;
+            }
+        }
+
+        ARMARX_DEBUG << "Reporting robot";
+        ARMARX_CHECK(virtualRobotWriterPlugin->get().storeDescription(*robot));
+
+        const armem::Time now = armem::Time::Now();
+        const float t = float((now - start).toSecondsDouble());
+
+        // move joints at certain frequency
+        const float m = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
+
+        auto jointValues = robot->getJointValues();
+        for (auto& [k, v] : jointValues)
+        {
+            const auto node = robot->getRobotNode(k);
+
+            if (node->isLimitless())
+            {
+                continue;
+            }
+
+            v = node->getJointLimitLow() +
+                (node->getJointLimitHigh() - node->getJointLimitLow()) * m;
+        }
+
+        robot->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
+        robot->setJointValues(jointValues);
+
+        ARMARX_CHECK(virtualRobotWriterPlugin->get().storeState(*robot));
+    }
+
+} // namespace armarx::virtual_robot_writer_example

source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h

+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <ArmarXCore/util/tasks.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotWriter.h"
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include <RobotAPI/libraries/armem/client/plugins.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+namespace armarx::virtual_robot_writer_example
+{
+
+    /**
+     * @defgroup Component-ExampleClient ExampleClient
+     * @ingroup RobotAPI-Components
+     * A description of the component ExampleClient.
+     *
+     * @class ExampleClient
+     * @ingroup Component-ExampleClient
+     * @brief Brief description of class ExampleClient.
+     *
+     * Detailed description of class ExampleClient.
+     */
+    class VirtualRobotWriterExample :
+        virtual public armarx::Component,
+        virtual public armarx::armem::client::ComponentPluginUser
+    {
+    public:
+        VirtualRobotWriterExample();
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+    protected:
+        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+        void onInitComponent() override;
+        void onConnectComponent() override;
+        void onDisconnectComponent() override;
+        void onExitComponent() override;
+
+        void run();
+
+    private:
+        VirtualRobot::RobotPtr loadRobot() const;
+        VirtualRobot::RobotPtr robot;
+
+        /// Reference timestamp for object movement
+        armem::Time start;
+
+        armarx::PeriodicTask<VirtualRobotWriterExample>::pointer_type task;
+
+        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotWriter>* virtualRobotWriterPlugin = nullptr;
+
+        struct Properties
+        {
+            float updateFrequency{10.F};
+
+            struct
+            {
+                std::string package;
+                std::string path;
+            } robot;
+        } p;
+    };
+
+}  // namespace armarx::virtual_robot_writer_example