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 <SimoxUtility/color/cmaps/colormaps.h>
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/ObjectIO.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/algorithms/orientation_aware/AStarPlanner.h>
#include <armarx/navigation/algorithms/orientation_aware/Costmap3D.h>
#include <armarx/navigation/algorithms/orientation_aware/io.h>
#include <armarx/navigation/algorithms/visualization.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/rooms/Reader.h>
#include <armarx/navigation/util/Visualization.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/filter.hpp>
#include <range/v3/view/transform.hpp>
 
namespace armarx::navigation::components::nav_3d_test
{
    Component::Component()
    {
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(costmapWriterPlugin);
        addPlugin(articulatedObjectReaderPlugin);
        addPlugin(roomReaderPlugin);
    }
 
    const std::string Component::defaultName = "nav_3d_test";
 
    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.orientations, "p.costmapParams.orientations");
        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.");
 
        def->optional(properties.robotModelXmlFilepath, "p.robotModelXmlFilepath", "");
 
        def->optional(properties.costmapBuilderParams.approximation2D,
                      "p.use2dApproximation",
                      "Calculate distances in 2d instead of 3d space");
        def->optional(properties.costmapBuilderParams.approx2DignoreVerticesOver,
                      "p.approx2DignoreVerticesOver",
                      "Ignore vertices that are higher than this in 2d approximation [mm]");
 
        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);
    }
 
    Eigen::Vector3f
    vecTo3D(const Eigen::Vector2f& pos)
    {
        return {pos.x(), pos.y(), 0.F};
    }
 
    Eigen::Matrix3f
    matTo3D(const Eigen::Matrix2f& mat)
    {
        Eigen::Matrix3f mat3D;
        mat3D.setZero();
        mat3D.topLeftCorner<2, 2>() = mat;
        mat3D(2, 2) = 1.F;
        return mat3D;
    }
 
    void
    Component::drawGlobalTrajectory(const std::vector<core::Pose2D>& trajectory, viz::Client& arviz)
    {
        auto layer = arviz.layer("global_planner");
 
        const auto traj3D = ranges::views::transform(
                                trajectory,
                                [](const core::Pose2D& pose) -> Eigen::Vector3f
                                { return {pose.translation().x(), pose.translation().y(), 0}; }) |
                            ranges::to_vector;
 
        layer.add(viz::Path("path").points(traj3D).color(simox::color::Color::blue()));
 
        const auto cmap = simox::color::cmaps::viridis();
 
        for (const auto& [idx, tp] : trajectory | ranges::views::enumerate)
        {
            const float scale = 150.F;
 
            const Eigen::Vector2f target = scale * tp.linear() * Eigen::Vector2f::UnitY();
 
            layer.add(viz::Arrow("velocity_" + std::to_string(idx))
                          .fromTo(vecTo3D(tp.translation()), vecTo3D(tp.translation() + target))
                          .color(cmap.at(1.F)));
        }
 
        arviz.commit(layer);
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_VERBOSE << "Creating initial creation of costmap";
        const bool success = createAndStoreCostmap();
 
        drawCostmap(arviz);
 
 
        if (not success)
        {
            ARMARX_ERROR << "Failed to create costmap.";
            return;
        }
 
        ARMARX_INFO << "Planning trajectory";
        const auto aStarWithOrientationParams =
            algorithms::orientation_aware::io::AStarWithOrientationParams{};
        algorithms::orientation_aware::AStarPlanner planner{state.costmap.value(),
                                                            aStarWithOrientationParams};
        const core::Pose2D start = Eigen::Isometry2f{Eigen::Translation2f{0, 0}};
        const core::Pose2D goal =
            Eigen::Isometry2f{Eigen::Translation2f{4000, 7000}} * Eigen::Rotation2Df{3.F};
        /*const core::Pose2D goal = Eigen::Isometry2f{Eigen::Translation2f{0, 1000}} *
                                  Eigen::Rotation2Df{3.F};*/
        const auto timestamp1 = armarx::core::time::Clock::Now();
        const auto trajectory = planner.plan(start, goal);
        const auto timestamp2 = armarx::core::time::Clock::Now();
        ARMARX_INFO << "Calculating trajectory took " << (timestamp2 - timestamp1).toMilliSeconds()
                    << "ms";
        for (const auto& pose : trajectory)
        {
            ARMARX_INFO << "-- (" << pose.translation().x() << ", " << pose.translation().y()
                        << ") " << Eigen::Rotation2Df{pose.linear()}.angle() * 180.F / M_PI
                        << " deg";
        }
        drawGlobalTrajectory(trajectory, arviz);
    }
 
    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 " << QUOTED(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::orientation_aware::Costmap3D
    extendOrCreateCostmap(armem::client::plugins::ReaderWriterPlugin<
                              memory::client::costmap::Reader>* costmapReaderPlugin,
                          algorithms::orientation_aware::Costmap3DBuilder& costmapBuilder,
                          std::string& costmapNameWithProvider)
    {
        // At the moment, only create the costmap from model data (do not extend given costmap)
        /*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;
 
        auto robot = virtualRobotReaderPlugin->get().getRobotWaiting(properties.robotName,
                                                                     armarx::DateTime::Invalid(),
                                                                     loadMode);
 
        if (properties.robotModelXmlFilepath != "")
        {
            robot = VirtualRobot::RobotIO::loadRobot(properties.robotModelXmlFilepath, loadMode);
            robot->setName("custom_robot");
        }
 
        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,
            VirtualRobot::ObjectIO::ObjectDescription::eCollisionModel);
 
        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";
        state.costmap.emplace(
            [&]() -> algorithms::orientation_aware::Costmap3D
            {
                algorithms::orientation_aware::Costmap3DBuilder costmapBuilder(
                    robot,
                    obstacles,
                    articulatedObjects,
                    rooms,
                    properties.costmapParams,
                    properties.colModel,
                    properties.costmapBuilderParams);
 
                return extendOrCreateCostmap(costmapReaderPlugin,
                                             costmapBuilder,
                                             properties.costmapToExtend);
            }());
 
        ARMARX_INFO << "Costmap created";
 
        // Note: Currently we do not store the costmap3d in memory
 
        return state.costmap.has_value();
    }
 
    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);
        }
    }
    */
 
 
    void
    Component::drawCostmap(viz::Client& arviz)
    {
        // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
        // See the ArVizExample in RobotAPI for more examples.
 
        // debug output
        for (const auto& grid : state.costmap->getGrid())
        {
            ARMARX_DEBUG << "costmap min: " << grid.minCoeff()
                         << ", costmap max: " << grid.maxCoeff();
        }
 
        std::vector<viz::Layer> layers;
        for (int i = 0; i < state.costmap->params().orientations; ++i)
        {
            std::stringstream ss;
            ss << "costmap3d_" << i;
            //viz::Layer layer = arviz.layer(ss.str());
            layers.emplace_back(arviz.layer(ss.str()));
        }
        algorithms::visualize(state.costmap.value(), layers, "costmap3d", 0.5);
        for (const auto& layer : layers)
        {
            arviz.commit(layer);
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::nav_3d_test