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 <mutex>
#include <optional>
#include <set>
#include <string>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#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/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/util.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/rooms/Reader.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::components::distance_to_obstacle_costmap_provider
{
    Component::Component()
    {
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(costmapWriterPlugin);
        addPlugin(articulatedObjectReaderPlugin);
        addPlugin(roomReaderPlugin);
    }
 
    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.costmapParams.binaryGrid, "p.costmapParams.binaryGrid");
        def->optional(properties.costmapParams.cellSize, "p.costmapParams.cellSize");
        def->optional(properties.costmapParams.sceneBoundsMargin,
                      "p.costmapParams.sceneBoundsMargin");
        def->optional(properties.costmapBuilderParams.restrictToRooms,
                      "p.costmapBuilderParams.restrictToRooms");
 
        def->optional(properties.colModel, "p.colModel");
        def->optional(properties.buildSecondCostmapWithSmallerMargins,
                      "p.buildSecondCostmapWithSmallerMargins");
 
        def->optional(properties.costmapToExtend,
                      "p.costmapToExtend",
                      "Name the initial costmap that should be extended. Format is "
                      "`ProviderName`/`EntityName` to reference a costmap in the memory.");
        def->optional(
            properties.costmapSmallerMarginsToExtend,
            "p.costmapSmallerMarginsToExtend",
            "Name the initial costmap that should be extended with smaller margins. Format is "
            "`ProviderName`/`EntityName` to reference a costmap in the memory.");
 
        return def;
    }
 
    void
    Component::onInitComponent()
    {
        // This should not be necessary but seems to be. ToDo: Look into this.
        memoryNameSystem().setComponent(this);
 
        memoryNameSystem().subscribe(
            armarx::armem::MemoryID{"Object", "Instance"}, this, &Component::processObjectInstance);
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_VERBOSE << "Creating initial creation of costmap";
        createAndStoreCostmap();
    }
 
    void
    Component::processObjectInstance(const armem::MemoryID& /*id*/,
                                     const std::vector<armem::MemoryID>& snapshotIDs)
    {
 
        ARMARX_VERBOSE << "Object instance changed!";
 
        const auto toObjectID = [](const armem::MemoryID& id) -> armarx::ObjectID
        { return armarx::ObjectID{id.entityName}; };
 
        // Get the object IDs of the changed objects.
        const std::vector<armarx::ObjectID> changedObjectClasses =
            snapshotIDs | ranges::views::transform(toObjectID) | ranges::to_vector;
 
        // Filter out object classes that are not relevant for the costmap.
        // FIXME: include Kitchen/mobile-fridge, ...
        const std::set<std::string> nonRelevantObjectClasses = {"KIT",
                                                                "HOPE",
                                                                "MDB",
                                                                "YCB",
                                                                "Kitchen",
                                                                "Elevator",
                                                                "Maintenance",
                                                                "KIT_Coopetition",
                                                                "Stanford_SRC"};
 
        const auto isRelevantClass =
            [&nonRelevantObjectClasses](const armarx::ObjectID& objectID) -> bool
        { return nonRelevantObjectClasses.count(objectID.dataset()) == 0; };
 
        const std::vector<armarx::ObjectID> relevantObjectClasses =
            changedObjectClasses | ranges::views::filter(isRelevantClass) | ranges::to_vector;
 
        if (relevantObjectClasses.empty())
        {
            return;
        }
 
        ARMARX_INFO << "Relevant object classes changed: " << relevantObjectClasses;
 
        // Create and store the costmap.
        createAndStoreCostmap();
    }
 
    inline algorithms::Costmap
    loadCostmap(armem::client::plugins::ReaderWriterPlugin<memory::client::costmap::Reader>*
                    costmapReaderPlugin,
                std::string& costmapNameWithProvider)
    {
        ARMARX_INFO << "Extending costmap `" << costmapNameWithProvider << "`.";
        const std::vector<std::string> splits = simox::alg::split(costmapNameWithProvider, "/");
        ARMARX_CHECK_EQUAL(splits.size(), 2);
 
 
        const auto initialCostmap =
            [costmapReaderPlugin, &costmapNameWithProvider, &splits]() -> algorithms::Costmap
        {
            while (true)
            {
                const memory::client::costmap::Reader::Query query{.providerName = splits.front(),
                                                                   .name = splits.back(),
                                                                   .timestamp = Clock::Now()};
 
                const auto result = costmapReaderPlugin->get().query(query);
 
                if (not result)
                {
                    ARMARX_INFO << deactivateSpam(60) << "Costmap `" << costmapNameWithProvider
                                << "` is not available yet. Retrying ...";
                    armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(300));
                    continue;
                }
 
                ARMARX_CHECK_NOT_NULL(result.costmap);
                return result.costmap.value();
            }
        }();
        return initialCostmap;
    }
 
    inline algorithms::Costmap
    extendOrCreateCostmap(armem::client::plugins::ReaderWriterPlugin<
                              memory::client::costmap::Reader>* costmapReaderPlugin,
                          algorithms::CostmapBuilder& costmapBuilder,
                          std::string& costmapNameWithProvider)
    {
        if (not costmapNameWithProvider.empty())
        {
            const auto initialCostmap = loadCostmap(costmapReaderPlugin, costmapNameWithProvider);
            const auto costmap = costmapBuilder.extend(initialCostmap);
            return costmap;
        }
 
        // If no initial costmap is available, create it from scratch.
        {
            const auto costmap = costmapBuilder.create();
            return costmap;
        }
    }
 
    bool
    Component::createAndStoreCostmap()
    {
        std::lock_guard g{createCostmapMtx};
 
        ARMARX_VERBOSE << "Creating costmap";
 
        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_VERBOSE << 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_VERBOSE << "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::objpose::util::staticObjects(objectPoses);
 
        const objpose::ObjectPoseSeq objectPosesStaticNonArticulated =
            armarx::objpose::util::nonArticulatedObjects(objectPosesStatic);
        const objpose::ObjectPoseSeq objectPosesStaticArticulated =
            armarx::objpose::util::articulatedObjects(objectPosesStatic);
 
        const VirtualRobot::SceneObjectSetPtr obstacles =
            armarx::objpose::util::asSceneObjects(objectPosesStaticNonArticulated);
 
        std::vector<VirtualRobot::RobotPtr> articulatedObjects;
        for (const auto& objectPoseStaticArticulated : objectPosesStaticArticulated)
        {
            ARMARX_VERBOSE << "Loading articulated object: "
                           << objectPoseStaticArticulated.objectID;
 
            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_VERBOSE
                    << "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(obstacles);
        ARMARX_INFO << obstacles->getSize() << " objects in the scene";
 
        ARMARX_VERBOSE << "Static objects (non-articulated)";
        for (const objpose::ObjectPose& objectPose : objectPosesStaticNonArticulated)
        {
            ARMARX_VERBOSE << " - " << objectPose.objectID;
        }
 
        ARMARX_VERBOSE << "Static objects (articulated)";
        for (const auto& object : articulatedObjects)
        {
            ARMARX_VERBOSE << " - " << object->getType() << "/" << object->getName();
        }
 
        ARMARX_VERBOSE << "Rooms:";
 
        ARMARX_CHECK_NOT_NULL(roomReaderPlugin);
 
        // query all rooms
        const memory::client::rooms::Reader::Query query{.timestamp = Clock::Now()};
        const auto result = roomReaderPlugin->get().query(query);
 
        const auto& rooms = result.rooms;
 
        for (const auto& room : rooms)
        {
            ARMARX_VERBOSE << " - " << room.name;
        }
 
        ARMARX_VERBOSE << "Creating costmap";
        const auto costmap = [&]() -> algorithms::Costmap
        {
            algorithms::CostmapBuilder costmapBuilder(robot,
                                                      obstacles,
                                                      articulatedObjects,
                                                      rooms,
                                                      properties.costmapParams,
                                                      properties.colModel,
                                                      properties.costmapBuilderParams);
 
            return extendOrCreateCostmap(
                costmapReaderPlugin, costmapBuilder, properties.costmapToExtend);
        }();
 
        ARMARX_VERBOSE << "Storing costmap in the memory.";
        bool successful = costmapWriterPlugin->get().store(
            costmap, "distance_to_obstacles", getName(), armarx::Clock::Now());
 
        if (properties.buildSecondCostmapWithSmallerMargins)
        {
            auto costmapBuilderParams = properties.costmapBuilderParams;
            costmapBuilderParams.collisionModelScaleFactor = Eigen::Vector3f{0.01, 1, 0.01};
            algorithms::CostmapBuilder costmapBuilder(robot,
                                                      obstacles,
                                                      articulatedObjects,
                                                      rooms,
                                                      properties.costmapParams,
                                                      properties.colModel,
                                                      costmapBuilderParams);
 
            const auto costmap = extendOrCreateCostmap(
                costmapReaderPlugin, costmapBuilder, properties.costmapSmallerMarginsToExtend);
 
            ARMARX_VERBOSE << "Storing costmap with smaller margins in the memory.";
            successful =
                successful &&
                costmapWriterPlugin->get().store(
                    costmap, "distance_to_obstacles_small_margin", getName(), armarx::Clock::Now());
        }
 
        return successful;
    }
 
    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::distance_to_obstacle_costmap_provider