From 4ac136c55295f5befb1293005eaba5e7685082f8 Mon Sep 17 00:00:00 2001
From: Johann Mantel <j-mantel@gmx.net>
Date: Thu, 17 Jun 2021 15:02:40 +0200
Subject: [PATCH] create new armarx package for Sick TIM581 LaserScanner driver
---
source/RobotAPI/components/CMakeLists.txt | 3 +-
.../drivers/SickLaserUnit/CMakeLists.txt | 69 ++++++
.../drivers/SickLaserUnit/SickLaserUnit.cpp | 196 ++++++++++++++++++
.../drivers/SickLaserUnit/SickLaserUnit.h | 145 +++++++++++++
.../drivers/SickLaserUnit/test/CMakeLists.txt | 5 +
.../SickLaserUnit/test/SickLaserUnitTest.cpp | 37 ++++
6 files changed, 454 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
create mode 100644 source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index 97d8648b4..0c3653739 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -31,4 +31,5 @@ add_subdirectory(ViewSelection)
add_subdirectory(SkillObserver)
-add_subdirectory(ArticulatedObjectLocalizerExample)
\ No newline at end of file
+add_subdirectory(ArticulatedObjectLocalizerExample)
+add_subdirectory(drivers/SickLaserUnit)
\ No newline at end of file
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
new file mode 100644
index 000000000..2ff404a3f
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -0,0 +1,69 @@
+set(LIB_NAME SickLaserUnit)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+# If your component needs a special ice interface, define it here:
+# armarx_add_component_interface_lib(
+# SLICE_FILES
+# SickLaserUnit.ice
+# ICE_LIBS
+# # RobotAPI
+#)
+
+
+# Add the component
+armarx_add_component(
+ COMPONENT_LIBS
+ # ArmarXCore
+ ArmarXCore
+ ## ArmarXCoreComponentPlugins # For DebugObserver plugin.
+ # ArmarXGui
+ ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ # RobotAPI
+ ## RobotAPICore
+ ## RobotAPIInterfaces
+ ## RobotAPIComponentPlugins # For ArViz and other plugins.
+
+ # This project
+ ## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
+ # This component
+ ## SickLaserUnitInterfaces # If you defined a component ice interface above.
+
+ SOURCES
+ SickLaserUnit.cpp
+
+ HEADERS
+ SickLaserUnit.h
+)
+
+
+# Add dependencies
+#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(SickLaserUnit PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+
+# Add ARON files
+#armarx_enable_aron_file_generation_for_target(
+# TARGET_NAME
+# ${ARMARX_COMPONENT_NAME}
+# ARON_FILES
+# aron/ExampleData.xml
+#)
+
+
+# Add unit tests
+add_subdirectory(test)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+ # If your component is not defined in ::armarx, specify its namespace here:
+ # COMPONENT_NAMESPACE "armarx::mynamespace"
+)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
new file mode 100644
index 000000000..024eda12b
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -0,0 +1,196 @@
+/*
+ * 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::SickLaserUnit
+ * @author Johann Mantel ( j-mantel at gmx dot net )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "SickLaserUnit.h"
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+
+namespace armarx
+{
+
+ armarx::PropertyDefinitionsPtr SickLaserUnit::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
+
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
+
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ // def->component(myComponentProxy)
+
+
+ // Add a required property.
+ def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+ // Add an optionalproperty.
+ def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+
+ return def;
+ }
+
+
+ void SickLaserUnit::onInitComponent()
+ {
+ // Topics and properties defined above are automagically registered.
+
+ // Keep debug observer data until calling `sendDebugObserverBatch()`.
+ // (Requies the armarx::DebugObserverComponentPluginUser.)
+ // setDebugObserverBatchModeEnabled(true);
+ }
+
+
+ void SickLaserUnit::onConnectComponent()
+ {
+ // Do things after connecting to topics and components.
+
+
+ /* (Requies the armarx::DebugObserverComponentPluginUser.)
+ // Use the debug observer to log data over time.
+ // The data can be viewed in the ObserverView and the LivePlotter.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ // Draw boxes in ArViz.
+ // (Before starting any threads, we don't need to lock mutexes.)
+ drawBoxes(properties, arviz);
+ */
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ // Setup the remote GUI.
+ {
+ createRemoteGuiTab();
+ RemoteGui_startRunningTask();
+ }
+ */
+ }
+
+
+ void SickLaserUnit::onDisconnectComponent()
+ {
+
+ }
+
+
+ void SickLaserUnit::onExitComponent()
+ {
+
+ }
+
+
+ std::string SickLaserUnit::getDefaultName() const
+ {
+ return "SickLaserUnit";
+ }
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ void SickLaserUnit::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ // Setup the widgets.
+
+ tab.boxLayerName.setValue(properties.boxLayerName);
+
+ tab.numBoxes.setValue(properties.numBoxes);
+ tab.numBoxes.setRange(0, 100);
+
+ tab.drawBoxes.setLabel("Draw Boxes");
+
+ // Setup the layout.
+
+ GridLayout grid;
+ int row = 0;
+ {
+ grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+ ++row;
+
+ grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ ++row;
+
+ grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ ++row;
+ }
+
+ VBoxLayout root = {grid, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+
+ void SickLaserUnit::RemoteGui_update()
+ {
+ if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ {
+ std::scoped_lock lock(propertiesMutex);
+ properties.boxLayerName = tab.boxLayerName.getValue();
+ properties.numBoxes = tab.numBoxes.getValue();
+
+ {
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
+ }
+ }
+ if (tab.drawBoxes.wasClicked())
+ {
+ // Lock shared variables in methods running in seperate threads
+ // and pass them to functions. This way, the called functions do
+ // not need to think about locking.
+ std::scoped_lock lock(propertiesMutex, arvizMutex);
+ drawBoxes(properties, arviz);
+ }
+ }
+ */
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client& arviz)
+ {
+ // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+ // See the ArVizExample in RobotAPI for more examples.
+
+ viz::Layer layer = arviz.layer(p.boxLayerName);
+ for (int i = 0; i < p.numBoxes; ++i)
+ {
+ layer.add(viz::Box("box_" + std::to_string(i))
+ .position(Eigen::Vector3f(i * 100, 0, 0))
+ .size(20).color(simox::Color::blue()));
+ }
+ arviz.commit(layer);
+ }
+ */
+
+}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
new file mode 100644
index 000000000..8b681b1f5
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -0,0 +1,145 @@
+/*
+ * 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::SickLaserUnit
+ * @author Johann Mantel ( j-mantel at gmx dot net )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+// #include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+
+namespace armarx
+{
+
+ /**
+ * @defgroup Component-SickLaserUnit SickLaserUnit
+ * @ingroup RobotAPI-Components
+ * A description of the component SickLaserUnit.
+ *
+ * @class SickLaserUnit
+ * @ingroup Component-SickLaserUnit
+ * @brief Brief description of class SickLaserUnit.
+ *
+ * Detailed description of class SickLaserUnit.
+ */
+ class SickLaserUnit :
+ virtual public armarx::Component
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ // , virtual public armarx::ArVizComponentPluginUser
+ {
+ public:
+
+ /// @see armarx::ManagedIceObject::getDefaultName()
+ std::string getDefaultName() const override;
+
+
+ protected:
+
+ /// @see PropertyUser::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ /// @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;
+
+
+ /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// This function should be called once in onConnect() or when you
+ /// need to re-create the Remote GUI tab.
+ void createRemoteGuiTab();
+
+ /// After calling `RemoteGui_startRunningTask`, this function is
+ /// called periodically in a separate thread. If you update variables,
+ /// make sure to synchronize access to them.
+ void RemoteGui_update() override;
+ */
+
+
+ private:
+
+ // Private methods go here.
+
+ // Forward declare `Properties` if you used it before its defined.
+ // struct Properties;
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ /// Draw some boxes in ArViz.
+ void drawBoxes(const Properties& p, viz::Client& arviz);
+ */
+
+
+ private:
+
+ // Private member variables go here.
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ std::string boxLayerName;
+ int numBoxes = 10;
+ };
+ Properties properties;
+ /* Use a mutex if you access variables from different threads
+ * (e.g. ice functions and RemoteGui_update()).
+ std::mutex propertiesMutex;
+ */
+
+
+ /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+ /// Tab shown in the Remote GUI.
+ struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
+ {
+ armarx::RemoteGui::Client::LineEdit boxLayerName;
+ armarx::RemoteGui::Client::IntSpinBox numBoxes;
+
+ armarx::RemoteGui::Client::Button drawBoxes;
+ };
+ RemoteGuiTab tab;
+ */
+
+
+ /* (Requires the armarx::ArVizComponentPluginUser.)
+ * When used from different threads, an ArViz client needs to be synchronized.
+ /// Protects the arviz client inherited from the ArViz plugin.
+ std::mutex arvizMutex;
+ */
+
+
+ };
+}
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
new file mode 100644
index 000000000..0e9585259
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore SickLaserUnit)
+
+armarx_add_test(SickLaserUnitTest SickLaserUnitTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp b/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.cpp
new file mode 100644
index 000000000..d6a69c168
--- /dev/null
+++ b/source/RobotAPI/drivers/SickLaserUnit/test/SickLaserUnitTest.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::SickLaserUnit
+ * @author Johann Mantel ( j-mantel at gmx dot net )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE RobotAPI::ArmarXObjects::SickLaserUnit
+
+#define ARMARX_BOOST_TEST
+
+#include <RobotAPI/Test.h>
+#include "../SickLaserUnit.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::SickLaserUnit instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
--
GitLab