From 09a3a0cd9ee5c14def6583fdf383055aa9910fcc Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 3 Apr 2020 17:36:01 +0200
Subject: [PATCH] Add and implement component RobotToArViz
---
source/RobotAPI/components/CMakeLists.txt | 1 +
.../components/RobotToArViz/CMakeLists.txt | 35 ++++++
.../components/RobotToArViz/RobotToArViz.cpp | 93 +++++++++++++++
.../components/RobotToArViz/RobotToArViz.h | 108 ++++++++++++++++++
.../RobotAPI/components/RobotToArViz/main.cpp | 33 ++++++
.../RobotToArViz/test/CMakeLists.txt | 5 +
.../RobotToArViz/test/RobotToArVizTest.cpp | 37 ++++++
7 files changed, 312 insertions(+)
create mode 100644 source/RobotAPI/components/RobotToArViz/CMakeLists.txt
create mode 100644 source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
create mode 100644 source/RobotAPI/components/RobotToArViz/RobotToArViz.h
create mode 100644 source/RobotAPI/components/RobotToArViz/main.cpp
create mode 100644 source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
create mode 100644 source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 679bc2f79..34bc50559 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -14,6 +14,7 @@ add_subdirectory(NaturalIKTest)
add_subdirectory(RobotHealth)
add_subdirectory(RobotNameService)
add_subdirectory(RobotState)
+add_subdirectory(RobotToArViz)
add_subdirectory(StatechartExecutorExample)
add_subdirectory(TopicTimingTest)
add_subdirectory(ViewSelection)
diff --git a/source/RobotAPI/components/RobotToArViz/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
new file mode 100644
index 000000000..1f07bddf7
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/CMakeLists.txt
@@ -0,0 +1,35 @@
+armarx_component_set_name("RobotToArViz")
+
+
+set(COMPONENT_LIBS
+ ArmarXCore
+ RobotAPIComponentPlugins
+)
+
+set(SOURCES
+ RobotToArViz.cpp
+)
+set(HEADERS
+ RobotToArViz.h
+)
+
+
+armarx_add_component("${SOURCES}" "${HEADERS}")
+
+#find_package(MyLib QUIET)
+#armarx_build_if(MyLib_FOUND "MyLib not available")
+# all target_include_directories must be guarded by if(Xyz_FOUND)
+# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
+#if(MyLib_FOUND)
+# target_include_directories(RobotToArViz PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+# add unit tests
+add_subdirectory(test)
+
+
+armarx_component_set_name("RobotToArVizApp")
+set(COMPONENT_LIBS RobotToArViz)
+armarx_add_component_executable(main.cpp)
+
+
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
new file mode 100644
index 000000000..f0e7ae58e
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -0,0 +1,93 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotToArViz.h"
+
+#include <ArmarXCore/core/time/CycleUtil.h>
+
+
+namespace armarx
+{
+ RobotToArVizPropertyDefinitions::RobotToArVizPropertyDefinitions(std::string prefix) :
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ }
+
+ armarx::PropertyDefinitionsPtr RobotToArViz::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr defs(new RobotToArVizPropertyDefinitions(getConfigIdentifier()));
+
+ defs->optional(updateFrequency, "updateFrequency", "Target number of updates per second.");
+
+ return defs;
+ }
+
+
+ std::string RobotToArViz::getDefaultName() const
+ {
+ return "RobotToArViz";
+ }
+
+
+ void RobotToArViz::onInitComponent()
+ {
+ }
+
+
+ void RobotToArViz::onConnectComponent()
+ {
+ // Load robot.
+ this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+
+ // Initialize robot visu element.
+ this->robotViz = viz::Robot(robot->getName()).file("", robot->getFilename());
+
+ task = new SimplePeriodicTask<>([this]()
+ {
+ this->updateRobot();
+ }, int(1000 / updateFrequency));
+ task->start();
+ }
+
+
+ void RobotToArViz::onDisconnectComponent()
+ {
+ task->stop();
+ task = nullptr;
+ }
+
+
+ void RobotToArViz::onExitComponent()
+ {
+ }
+
+
+ void RobotToArViz::updateRobot()
+ {
+ RobotState::synchronizeLocalClone(robotName);
+
+ robotViz.joints(robot->getConfig()->getRobotNodeJointValueMap())
+ .pose(robot->getGlobalPose());
+
+ arviz.commitLayerContaining(robot->getName(), robotViz);
+ }
+}
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.h b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
new file mode 100644
index 000000000..4e6a64ca0
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.h
@@ -0,0 +1,108 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
+
+
+namespace armarx
+{
+ /**
+ * @class RobotToArVizPropertyDefinitions
+ * @brief Property definitions of `RobotToArViz`.
+ */
+ class RobotToArVizPropertyDefinitions :
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ RobotToArVizPropertyDefinitions(std::string prefix);
+ };
+
+
+
+ /**
+ * @defgroup Component-RobotToArViz RobotToArViz
+ * @ingroup RobotAPI-Components
+ *
+ * Visualizes a robot via ArViz.
+ *
+ * @class RobotToArViz
+ * @ingroup Component-RobotToArViz
+ * @brief Brief description of class RobotToArViz.
+ *
+ * Detailed description of class RobotToArViz.
+ */
+ class RobotToArViz :
+ virtual public armarx::Component,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::RobotStateComponentPluginUser
+ {
+ using RobotState = RobotStateComponentPluginUser;
+
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ /// @see armarx::ManagedIceObject::onInitComponent()
+ void onInitComponent() override;
+
+ /// @see armarx::ManagedIceObject::onConnectComponent()
+ void onConnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onDisconnectComponent()
+ void onDisconnectComponent() override;
+
+ /// @see armarx::ManagedIceObject::onExitComponent()
+ void onExitComponent() override;
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+
+ private:
+
+ void updateRobot();
+
+
+ private:
+
+ float updateFrequency = 100;
+ SimplePeriodicTask<>::pointer_type task;
+
+ std::string robotName = "robot";
+
+ VirtualRobot::RobotPtr robot;
+
+ viz::Robot robotViz { "" };
+
+ };
+}
diff --git a/source/RobotAPI/components/RobotToArViz/main.cpp b/source/RobotAPI/components/RobotToArViz/main.cpp
new file mode 100644
index 000000000..f0d3c514d
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/main.cpp
@@ -0,0 +1,33 @@
+/*
+ * 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/>.
+ *
+ * @package OML::application::SceneVisualizer
+ * @author Fabian Paus ( fabian dot paus at kit dot edu )
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+
+int main(int argc, char* argv[])
+{
+ return armarx::runSimpleComponentApp < armarx::RobotToArViz > (argc, argv, "RobotToArViz");
+}
diff --git a/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
new file mode 100644
index 000000000..6ddf1aed8
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore RobotToArViz)
+
+armarx_add_test(RobotToArVizTest RobotToArVizTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
new file mode 100644
index 000000000..8a7082d68
--- /dev/null
+++ b/source/RobotAPI/components/RobotToArViz/test/RobotToArVizTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotToArViz
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2020
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::RobotToArViz
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include <RobotAPI/components/RobotToArViz/RobotToArViz.h>
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::RobotToArViz instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
--
GitLab