dynamic scene provider component

source/armarx/navigation/components/CMakeLists.txt

@@ -3,3 +3,5 @@ add_subdirectory(NavigationMemory)
 add_subdirectory(Navigator)
 
 add_subdirectory(dynamic_distance_to_obstacle_costmap_provider)
+
+add_subdirectory(dynamic_scene_provider)
\ No newline at end of file

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

+armarx_add_component(dynamic_scene_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
+        armem
+        armem_robot
+        armem_robot_state
+        # VisionX
+        armem_human
+        armem_vision
+        ## 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_scene_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_scene_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/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "ArmarXCore/core/time/Clock.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
+
+#include <armarx/navigation/core/basic_types.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_scene_provider
+{
+
+    const std::string Component::defaultName = "dynamic_scene_provider";
+
+
+    Component::Component()
+    {
+        addPlugin(humanPoseReaderPlugin);
+        addPlugin(laserScannerFeaturesReaderPlugin);
+    }
+
+    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.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
+
+        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();
+        }
+        */
+
+        robot = virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
+        ARMARX_CHECK_NOT_NULL(robot);
+
+        task = new PeriodicTask<Component>(
+            this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+    }
+
+
+    void
+    Component::onDisconnectComponent()
+    {
+    }
+
+
+    void
+    Component::onExitComponent()
+    {
+    }
+
+
+    std::string
+    Component::getDefaultName() const
+    {
+        return Component::defaultName;
+    }
+
+
+    std::string
+    Component::GetDefaultName()
+    {
+        return Component::defaultName;
+    }
+
+    void
+    Component::runPeriodically()
+    {
+        // obtain data from perception
+
+        const DateTime timestamp = Clock::Now();
+
+        //
+        // Robot
+        //
+
+        ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+        const core::Pose global_T_robot(robot->getGlobalPose());
+
+        ARMARX_INFO << "Robot position: " << global_T_robot.translation().head<2>();
+
+        //
+        // Human
+        //
+
+        const armem::human::client::Reader::Query humanPoseQuery{.providerName = "", // all
+                                                        .timestamp = timestamp};
+
+        const armem::human::client::Reader::Result humanPoseResult =
+            humanPoseReaderPlugin->get().query(humanPoseQuery);
+        ARMARX_CHECK_EQUAL(humanPoseResult.status, armem::human::client::Reader::Result::Success);
+
+        ARMARX_INFO << humanPoseResult.humanPoses.size() << " humans in the scene.";
+
+        //
+        // Laser scanner features
+        //
+        
+        const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery
+        {
+            .providerName = properties.laserScannerFeatures.providerName,
+            .name = properties.laserScannerFeatures.name,
+            .timestamp = timestamp
+        };
+
+        const armem::vision::laser_scanner_features::client::Reader::Result laserFeaturesResult =
+            laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+
+        ARMARX_CHECK_EQUAL(laserFeaturesResult.status, armem::vision::laser_scanner_features::client::Reader::Result::Success);
+        
+        ARMARX_INFO << laserFeaturesResult.features.size() << " clusters/features";
+
+
+    }
+
+
+    /* (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_scene_provider

source/armarx/navigation/components/dynamic_scene_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_scene_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 <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/services/tasks/TaskUtil.h"
+#include <ArmarXCore/core/Component.h>
+
+#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+#include "RobotAPI/libraries/armem_vision/client/laser_scanner_features/Reader.h"
+
+#include "VisionX/libraries/armem_human/client/HumanPoseReader.h"
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include <armarx/navigation/components/dynamic_scene_provider/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::dynamic_scene_provider
+{
+
+    class Component :
+        virtual public armarx::Component,
+        virtual public armarx::navigation::components::dynamic_scene_provider::ComponentInterface,
+        // , virtual public armarx::DebugObserverComponentPluginUser
+        // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+        // , virtual public armarx::ArVizComponentPluginUser
+        virtual public armarx::armem::client::plugins::PluginUser
+    {
+    public:
+        Component();
+
+        /// @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);
+        */
+
+        void runPeriodically();
+
+        VirtualRobot::RobotPtr robot = nullptr;
+
+
+    private:
+        static const std::string defaultName;
+
+
+        // Private member variables go here.
+
+
+        /// Properties shown in the Scenario GUI.
+        struct Properties
+        {
+            int taskPeriodMs = 100;
+
+            struct
+            {
+                std::string providerName = "LaserScannerFeatureExtraction";
+                std::string name = ""; // all
+            } laserScannerFeatures;
+
+            struct
+            {
+                std::string name = "Armar6";
+            } robot;
+        };
+        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;
+        */
+
+        PeriodicTask<Component>::pointer_type task;
+
+        armem::client::plugins::ReaderWriterPlugin<armem::human::client::Reader>*
+            humanPoseReaderPlugin = nullptr;
+
+        armem::client::plugins::ReaderWriterPlugin<
+            armem::vision::laser_scanner_features::client::Reader>* laserScannerFeaturesReaderPlugin =
+            nullptr;
+
+        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin = nullptr;
+
+    };
+
+} // namespace armarx::navigation::components::dynamic_scene_provider

source/armarx/navigation/components/dynamic_scene_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_scene_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_scene_provider 
+{
+
+    interface ComponentInterface
+    {
+	// Define your interface here.
+    };
+
+};};};};