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::handover_costmap_provider
 * @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 <string>
#include <vector>
 
#include <range/v3/iterator/insert_iterators.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/drop.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.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 <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Robot.h>
 
#include <armarx/navigation/algorithms/costmap/HandoverCostmapBuilder.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
 
#include <VisionX/libraries/armem_human/client/PersonInstanceReader.h>
#include <VisionX/libraries/armem_human/types.h>
 
namespace armarx::navigation::components::handover_costmap_provider
{
 
    const std::string Component::defaultName = "handover_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 optional property.
        // 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()`.
        // (Requires the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
 
        // Create the task that updates the costmap.
        costmapUpdateTask =
            new armarx::RunningTask<Component>(this, &Component::run, "HandoverCostmapUpdateTask");
    }
 
    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);
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
        */
 
        if (not costmapUpdateTask->isRunning())
        {
            ARMARX_INFO << "Starting handover costmap update task.";
            costmapUpdateTask->start();
        }
    }
 
    void
    Component::onDisconnectComponent()
    {
        if (costmapUpdateTask->isRunning())
        {
            ARMARX_INFO << "Stopping handover costmap update task.";
            costmapUpdateTask->stop();
        }
    }
 
    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 separate threads
            // and pass them to functions. This way, the called functions do
            // not need to think about locking.
            std::scoped_lock lock(propertiesMutex);
            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);
    }
    */
 
 
    void
    Component::run()
    {
        Metronome metronome(Duration::MilliSeconds(500));
        while (costmapUpdateTask->isRunning())
        {
            // read data from memory
            readObstacleDistanceMap();
            queryHumans();
 
            // compute costmap
            computeHandoverCostmap();
 
            metronome.waitForNextTick();
        }
    }
 
    void
    Component::computeHandoverCostmap()
    {
        if (!obstacleDistanceMap.has_value())
        {
            ARMARX_INFO << deactivateSpam(1)
                        << "Cannot compute handover costmap: no obstacle distance map available";
            return;
        }
 
        if (detectedHumans.empty())
        {
            ARMARX_INFO << deactivateSpam(1)
                        << "No humans detected, skipping handover costmap computation";
            return;
        }
 
        algorithms::costmap::HandoverCostmapBuilder builder{obstacleDistanceMap->params(),
                                                            properties.handoverCostmapParams};
 
        std::vector<armem::human::HumanPose> otherHumans;
        if (detectedHumans.size() > 1)
        {
            otherHumans =
                detectedHumans | ranges::views::drop(1) |
                ranges::views::filter(
                    [](const armem::human::MemoryResolvedPersonInstance& personInstance) noexcept
                    { return personInstance.humanPose.has_value(); }) |
                ranges::views::transform(
                    [](const armem::human::MemoryResolvedPersonInstance& personInstance) noexcept
                    { return personInstance.humanPose.value(); }) |
                ranges::to_vector;
        }
 
        algorithms::costmap::HandoverCostmapBuilder::SceneInformation sceneInfo{
            .humanPose = detectedHumans.front().humanPose.value(),
            .otherHumans = otherHumans,
            .distanceMap = obstacleDistanceMap.value(),
            .humanPose2DOverride = std::nullopt,
            .robotPosition = std::nullopt,
        };
 
        const auto ts1 = armarx::Clock::Now();
        const auto handoverCostmap = builder.create(sceneInfo);
        const auto ts2 = armarx::Clock::Now();
 
        const auto global_T_robot_opt = builder.getOptimalHandoverPose(sceneInfo);
        const auto ts3 = armarx::Clock::Now();
 
        // log timings
        {
            const auto costmapDuration = ts2 - ts1;
            const auto optimalPoseDuration = ts3 - ts2;
 
            ARMARX_INFO << "Handover costmap computed in " << costmapDuration.toMilliSeconds()
                        << " ms";
            ARMARX_INFO << "Optimal pose computed in " << optimalPoseDuration.toMilliSeconds()
                        << " ms.";
        }
 
        if (!global_T_robot_opt.has_value())
        {
            ARMARX_INFO << "No optimal handover pose found.";
            return;
        }
 
        // visualize optimal placement
        {
            const auto robotDescription = services.virtualRobotReaderPlugin->get().queryDescription(
                properties.robot.name, armarx::Clock::Now());
 
            const auto robot =
                services.virtualRobotReaderPlugin->get().getRobot(properties.robot.name);
            ARMARX_CHECK_NOT_NULL(robot)
                << "Robot '" << properties.robot.name << "' not found in RobotStateMemory.";
 
            ARMARX_CHECK(services.virtualRobotReaderPlugin->get().synchronizeRobot(
                *robot, armarx::Clock::Now()))
                << "Failed to synchronize robot '" << properties.robot.name << "'.";
 
            viz::Layer layer = arviz.layer("optimal_handover_pose");
 
            viz::Robot vizRobot("optimal_handover_robot");
            vizRobot.useCollisionModel();
            vizRobot.file(robotDescription->xml);
            vizRobot.joints(robot->getJointValues());
            vizRobot.pose(conv::to3D(global_T_robot_opt.value()));
 
            layer.add(vizRobot);
 
            arviz.commit(layer);
        }
    }
 
    bool
    Component::readObstacleDistanceMap()
    {
        const memory::client::costmap::Reader::Query query{
            .providerName = properties.obstacleDistanceMap.providerName,
            .name = properties.obstacleDistanceMap.name,
            .timestamp = armarx::core::time::Clock::Now()};
 
        const auto result = services.costmapReaderPlugin->get().query(query);
 
        if (result.costmap.has_value())
        {
            obstacleDistanceMap.emplace(result.costmap.value());
            return true;
        }
 
        ARMARX_INFO << deactivateSpam(1) << "Obstacle distance map not available yet";
        return false;
    }
 
    void
    Component::queryHumans()
    {
        detectedHumans.clear();
 
 
        armem::human::client::PersonInstanceReader::QueryResolved query;
        query.resolveHumanPose = true;
        query.resolveFaceDetection = true;
        query.resolveProfile = true;
        query.maxAge = Duration::Seconds(2);
        query.providerName = properties.personInstanceProviderName;
        query.timestamp = armarx::core::time::Clock::Now();
 
 
        const auto result = services.personInstanceReaderPlugin->get().queryResolved(query);
 
        if (result.status != armem::human::client::PersonInstanceReader::Result::Status::Success)
        {
            ARMARX_WARNING << "Failed to query person instances: " << result.errorMessage;
            return;
        }
 
        ARMARX_VERBOSE << "Queried " << result.personInstances.size() << " person instances";
        detectedHumans = result.personInstances;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
 
} // namespace armarx::navigation::components::handover_costmap_provider