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::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 <optional>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
 
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/util/util.h>
 
#include <range/v3/view/for_each.hpp>
 
namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
{
    Component::Component()
    {
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapWriterPlugin);
        addPlugin(articulatedObjectReaderPlugin);
    }
 
    const std::string Component::defaultName = "distance_to_obstacle_costmap_provider";
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->required(properties.robotName, "p.robotName", "Robot name.");
 
        def->optional(properties.costmapBuilderParams.binaryGrid, "p.costmapBuilderParams.binaryGrid");
        def->optional(properties.costmapBuilderParams.cellSize, "p.costmapBuilderParams.cellSize");
        def->optional(properties.costmapBuilderParams.sceneBoundsMargin, "p.costmapBuilderParams.sceneBoundsMargin");
 
        def->optional(properties.colModel, "p.colModel");
 
        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();
        }
        */
 
        runningTask = new RunningTask<Component>(this, &Component::run, "createAndStoreCostmap");
        runningTask->start();
    }
 
    void
    Component::run()
    {
        // FIXME: whenever the static scene changes, update the costmap
 
        createAndStoreCostmap();
    }
 
    bool
    Component::createAndStoreCostmap()
    {
        const std::vector<std::string> primitiveModelIds = [&]() -> std::vector<std::string>
        {
            if (not properties.primitiveModelIds.empty())
            {
                const std::vector<std::string> primitiveModelIds =
                    simox::alg::split(properties.primitiveModelIds, ",", true, false);
                ARMARX_INFO << VAROUT(primitiveModelIds);
                return primitiveModelIds;
            }
 
            return {};
        }();
 
        const auto loadMode = primitiveModelIds.empty()
                                  ? VirtualRobot::RobotIO::RobotDescription::eCollisionModel
                                  : VirtualRobot::RobotIO::RobotDescription::eStructure;
 
        const auto robot = virtualRobotReaderPlugin->get().getRobotWaiting(
            properties.robotName, armarx::DateTime::Invalid(), loadMode);
 
        ARMARX_CHECK_NOT_NULL(robot);
 
        if (not primitiveModelIds.empty())
        {
            ARMARX_INFO << "Using primitive approximation model";
            robot->setPrimitiveApproximationModel(primitiveModelIds);
        }
 
        const auto objectPoseClient = ObjectPoseClientPluginUser::getClient();
 
        const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
 
        // remove those objects that belong to an object dataset. the manipulation object / distance computation is broken
        const objpose::ObjectPoseSeq objectPosesStatic =
            armarx::navigation::util::staticObjects(objectPoses);
 
        const objpose::ObjectPoseSeq objectPosesStaticNonArticulated =
            armarx::navigation::util::nonArticulatedObjects(objectPosesStatic);
        const objpose::ObjectPoseSeq objectPosesStaticArticulated =
            armarx::navigation::util::articulatedObjects(objectPosesStatic);
 
        const VirtualRobot::SceneObjectSetPtr objects =
            armarx::navigation::util::asSceneObjects(objectPosesStaticNonArticulated);
 
        std::vector<VirtualRobot::RobotPtr> articulatedObjects;
        for (const auto& objectPoseStaticArticulated : objectPosesStaticArticulated)
        {
            const auto articulatedObject =
                articulatedObjectReaderPlugin->get().getArticulatedObject(
                    objectPoseStaticArticulated.objectID.getClassID().str(),
                    Clock::Now(),
                    std::nullopt,
                    objectPoseStaticArticulated.objectID.instanceName());
 
            ARMARX_CHECK_NOT_NULL(articulatedObject)
                << objectPoseStaticArticulated.objectID.getClassID().str();
 
            ARMARX_CHECK(articulatedObjectReaderPlugin->get().synchronizeArticulatedObject(
                *articulatedObject, Clock::Now(), std::nullopt));
 
            std::set<std::string> primitiveIDs;
            articulatedObject->getPrimitiveApproximationIDs(primitiveIDs);
 
            if (articulatedObject->getCollisionModels().empty())
            {
                ARMARX_INFO << "The articualted object `" << articulatedObject->getType() << "/"
                            << articulatedObject->getName()
                            << "` does not have a collision model. Using primitive model instead.";
 
                // use all primitive ids
                // TODO: refine selection?
                const std::vector<std::string> usedPrimitiveModelIDs(primitiveIDs.begin(),
                                                                     primitiveIDs.end());
                articulatedObject->setPrimitiveApproximationModel(usedPrimitiveModelIDs);
            }
 
            articulatedObjects.push_back(articulatedObject);
        }
 
 
        ARMARX_CHECK_NOT_NULL(objects);
        ARMARX_INFO << objects->getSize() << " objects in the scene";
 
        ARMARX_INFO << "Static objects (non-articulated)";
        for (const objpose::ObjectPose& objectPose : objectPosesStaticNonArticulated)
        {
            ARMARX_INFO << " - " << objectPose.objectID;
        }
 
        ARMARX_INFO << "Static objects (articulated)";
        for (const auto& object : articulatedObjects)
        {
            ARMARX_INFO << " - " << object->getType() << "/" << object->getName();
        }
 
        ARMARX_INFO << "Creating costmap";
 
        // FIXME: move costmap creation out of this component
        // FIXME create costmap writer enum: type of costmaps
        algorithms::CostmapBuilder costmapBuilder(
            robot,
            objects,
            articulatedObjects,
            properties.costmapBuilderParams,
            properties.colModel);
 
        const auto costmap = costmapBuilder.create();
 
        ARMARX_INFO << "Storing costmap in the memory.";
        return costmapWriterPlugin->get().store(
            costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
    }
 
    void
    Component::onDisconnectComponent()
    {
        runningTask->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 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::distance_to_obstacle_costmap_provider