Component.cpp

Go to the documentation of this file.

/**
 * 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::costmap_example
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
#include <cstdlib>
#include <string>
 
#include <Eigen/Core>
 
#include <VirtualRobot/Random.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/types.h>
 
namespace armarx::navigation::components::costmap_example
{
 
    const std::string Component::defaultName = "costmap_example";
 
    Component::Component()
    {
        addPlugin(costmapWriterPlugin);
    }
 
    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 optional property.
        def->optional(properties.updateSize,
                      "p.updateSize",
                      "If enabled, the costmap size is updated randomly in each iteration.");
        def->optional(properties.size,
                      "p.size",
                      "If updateSize is false, this is the size used for the costmap.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requires the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    void
    Component::onConnectComponent()
    {
        // Do things after connecting to topics and components.
 
        /* (Requires 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);
        */
 
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
 
        costmapUpdateTask = new RunningTask<Component>(this, &Component::run, "CostmapUpdateTask");
 
        costmapUpdateTask->start();
    }
 
    void
    Component::onDisconnectComponent()
    {
        costmapUpdateTask->stop();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        // Setup the widgets.
 
        tab.clearAll.setLabel("Clear all areas");
 
        const auto setupSpinner = [](FloatSpinBox& spinner)
        {
            spinner.setValue(0);
            spinner.setRange(0, 50000);
            spinner.setDecimals(1);
        };
        setupSpinner(tab.minX);
        setupSpinner(tab.minY);
        setupSpinner(tab.maxX);
        setupSpinner(tab.maxY);
 
        tab.addArea.setLabel("Add blocked area");
 
        // Setup the layout.
 
        GridLayout grid;
        int row = 0;
        {
            grid.add(Label("Min X"), {row, 0}).add(tab.minX, {row, 1});
            ++row;
 
            grid.add(Label("Min Y"), {row, 0}).add(tab.minY, {row, 1});
            ++row;
 
            grid.add(Label("Max X"), {row, 0}).add(tab.maxX, {row, 1});
            ++row;
 
            grid.add(Label("Max Y"), {row, 0}).add(tab.maxY, {row, 1});
            ++row;
 
            grid.add(tab.addArea, {row, 0});
            ++row;
        }
 
        VBoxLayout root = {tab.clearAll, VSpacer(), grid};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        if (tab.clearAll.wasClicked())
        {
            std::scoped_lock lock(areasMutex);
            blockedAreas.clear();
            ARMARX_INFO << "Cleared all blocked areas";
        }
 
        if (tab.addArea.wasClicked())
        {
            std::scoped_lock lock(areasMutex);
            Eigen::AlignedBox2f area(Eigen::Vector2f(tab.minX.getValue(), tab.minY.getValue()),
                                     Eigen::Vector2f(tab.maxX.getValue(), tab.maxY.getValue()));
            ARMARX_INFO << "Add blocked area with min: " << area.min() << " and max " << area.max();
            blockedAreas.emplace_back(area);
        }
    }
 
    /* (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());
 
    void
    Component::run()
    {
        armarx::Metronome metronome(armarx::Duration::MilliSeconds(100));
 
        while (not costmapUpdateTask->isStopped())
        {
 
            const int size = properties.updateSize
                                 ? static_cast<int>(VirtualRobot::RandomFloat(500, 2000))
                                 : properties.size;
 
            const algorithms::Costmap::Parameters params{.cellSize = 100, .sceneBoundsMargin = 0};
 
            const algorithms::SceneBounds sceneBounds{.min = Eigen::Vector2f::Zero(),
                                                      .max = Eigen::Vector2f(size, size)};
 
            auto grid = algorithms::CostmapBuilder::createUniformGrid(sceneBounds, params);
 
            // fill grid with some example data
            {
                // just a simple distance field example with varying center
                const float centerX = VirtualRobot::RandomFloat(0, size - 1);
                const float centerY = VirtualRobot::RandomFloat(0, size - 1);
                const Eigen::Vector2f center(centerX, centerY);
 
                for (int x = 0; x < grid.rows(); ++x)
                {
                    for (int y = 0; y < grid.cols(); ++y)
                    {
                        const Eigen::Vector2f cellPos(x * params.cellSize, y * params.cellSize);
 
                        const float distance = (center - cellPos).norm();
                        grid(x, y) = distance;
                    }
                }
            }
 
            const algorithms::Costmap::Mask mask = grid.array() > 0;
 
            algorithms::Costmap costmap(grid, params, sceneBounds, mask);
 
            // mask out any blocked areas
            {
                std::scoped_lock lock(areasMutex);
                for (const auto& area : blockedAreas)
                {
                    const auto minVertex = costmap.toVertex(area.min()).index;
                    const auto maxVertex = costmap.toVertex(area.max()).index;
 
                    for (int x = minVertex.x(); x <= maxVertex.x(); ++x)
                    {
                        for (int y = minVertex.y(); y <= maxVertex.y(); ++y)
                        {
                            costmap.getMutableGrid()(x, y) = 0;
                        }
                    }
                }
            }
 
 
            ARMARX_INFO << "Storing costmap in memory";
            ARMARX_CHECK(costmapWriterPlugin->get().store(
                costmap, "example_costmap", getName(), Clock::Now()));
 
 
            metronome.waitForNextTick();
        }
    }
 
} // namespace armarx::navigation::components::costmap_example