new component dynamic_distance_to_obstacle_costmap_provider

source/armarx/navigation/components/CMakeLists.txt

@@ -4,3 +4,5 @@ add_subdirectory(Navigator)
 
 # Examples
 add_subdirectory(example_client)
+
+add_subdirectory(dynamic_distance_to_obstacle_costmap_provider)
\ No newline at end of file

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/CMakeLists.txt

+armarx_add_component(dynamic_distance_to_obstacle_costmap_provider
+    ICE_FILES
+        ComponentInterface.ice
+    ICE_DEPENDENCIES
+        ArmarXCoreInterfaces
+        # RobotAPIInterfaces
+    # ARON_FILES
+        # aron/my_type.xml
+    SOURCES
+        Component.cpp
+    HEADERS
+        Component.h
+    DEPENDENCIES
+        # ArmarXCore
+        ArmarXCore
+        ## ArmarXCoreComponentPlugins  # For DebugObserver plugin.
+        # ArmarXGui
+        ## ArmarXGuiComponentPlugins  # For RemoteGui plugin.
+        # RobotAPI
+        ## RobotAPICore
+        ## RobotAPIInterfaces
+        ## RobotAPIComponentPlugins  # For ArViz and other plugins.
+    # DEPENDENCIES_LEGACY
+        ## Add libraries that do not provide any targets but ${FOO_*} variables.
+        # FOO
+    # If you need a separate shared component library you can enable it with the following flag.
+    # SHARED_COMPONENT_LIBRARY
+)

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.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/>.
+ *
+ * @package    navigation::ArmarXObjects::dynamic_distance_to_obstacle_costmap_provider
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#include "Component.h"
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+
+namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+{
+
+    const std::string
+    Component::defaultName = "dynamic_distance_to_obstacle_costmap_provider";
+
+
+    armarx::PropertyDefinitionsPtr
+    Component::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr def = new armarx::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. (The component won't start without a value being set.)
+        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+
+        // Add an optionalproperty.
+        def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+        def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+
+        return def;
+    }
+
+
+    void
+    Component::onInitComponent()
+    {
+        // Topics and properties defined above are automagically registered.
+
+        // Keep debug observer data until calling `sendDebugObserverBatch()`.
+        // (Requies the armarx::DebugObserverComponentPluginUser.)
+        // setDebugObserverBatchModeEnabled(true);
+    }
+
+
+    void
+    Component::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
+    Component::onDisconnectComponent()
+    {
+    }
+
+
+    void
+    Component::onExitComponent()
+    {
+    }
+
+
+    std::string
+    Component::getDefaultName() const
+    {
+        return Component::defaultName;
+    }
+
+
+    std::string
+    Component::GetDefaultName()
+    {
+        return Component::defaultName;
+    }
+
+
+    /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+    void
+    Component::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
+    Component::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
+    Component::drawBoxes(const Component::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);
+    }
+    */
+
+
+    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+}  // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.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/>.
+ *
+ * @package    navigation::ArmarXObjects::dynamic_distance_to_obstacle_costmap_provider
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @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>
+
+#include <armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
+{
+
+    class Component :
+        virtual public armarx::Component,
+        virtual public armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider::ComponentInterface
+        // , virtual public armarx::DebugObserverComponentPluginUser
+        // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+        // , virtual public armarx::ArVizComponentPluginUser
+    {
+    public:
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+        /// Get the component's default name.
+        static std::string GetDefaultName();
+
+
+    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:
+
+        static const std::string defaultName;
+
+
+        // Private member variables go here.
+
+
+        /// Properties shown in the Scenario GUI.
+        struct Properties
+        {
+            std::string boxLayerName = "boxes";
+            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;
+        */
+
+    };
+
+}  // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/ComponentInterface.ice

+/*
+ * 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    navigation::dynamic_distance_to_obstacle_costmap_provider
+ * author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * date       2022
+ * copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *            GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armarx {  module navigation {  module components {  module dynamic_distance_to_obstacle_costmap_provider 
+{
+
+    interface ComponentInterface
+    {
+	// Define your interface here.
+    };
+
+};};};};