From bbe491040a14aa1eec3bf0b12a19b6f2a3aac9f5 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 23 Jun 2022 18:08:33 +0200
Subject: [PATCH] new component: VirtualRobotWriterExample
---
.../components/armem/client/CMakeLists.txt | 1 +
.../VirtualRobotWriterExample/CMakeLists.txt | 22 +++
.../VirtualRobotWriterExample.cpp | 158 ++++++++++++++++++
.../VirtualRobotWriterExample.h | 78 +++++++++
4 files changed, 259 insertions(+)
create mode 100644 source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
create mode 100644 source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
create mode 100644 source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
diff --git a/source/RobotAPI/components/armem/client/CMakeLists.txt b/source/RobotAPI/components/armem/client/CMakeLists.txt
index 258a28ea0..6eb04b931 100644
--- a/source/RobotAPI/components/armem/client/CMakeLists.txt
+++ b/source/RobotAPI/components/armem/client/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
add_subdirectory(VirtualRobotReaderExampleClient)
+add_subdirectory(VirtualRobotWriterExample)
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
new file mode 100644
index 000000000..a87ba755c
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/CMakeLists.txt
@@ -0,0 +1,22 @@
+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"
+)
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
new file mode 100644
index 000000000..275353128
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.cpp
@@ -0,0 +1,158 @@
+/*
+ * 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
diff --git a/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
new file mode 100644
index 000000000..af91d8354
--- /dev/null
+++ b/source/RobotAPI/components/armem/client/VirtualRobotWriterExample/VirtualRobotWriterExample.h
@@ -0,0 +1,78 @@
+
+#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
--
GitLab