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::dynamic_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 <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/algorithm/vector.hpp>
#include <SimoxUtility/color/cmaps/colormaps.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/util/geometry.h>
 
// Include headers you only need in function definitions in the .cpp.
 
// #include <Eigen/Core>
 
// #include <SimoxUtility/color/Color.h>
 
 
namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider
{
 
    const std::string Component::defaultName = "dynamic_distance_to_obstacle_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 optionalproperty.
        // def->optional(
        //     properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
        def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
        def->optional(properties.staticCostmap.name, "p.staticCostmap.name", "");
        def->required(properties.staticCostmap.providerName, "p.staticCostmap.providerName", "");
 
        def->optional(properties.laserScannerFeatures.name, "p.laserScannerFeatures.name", "");
        def->optional(properties.laserScannerFeatures.providerName,
                      "p.laserScannerFeatures.providerName",
                      "");
 
        def->optional(properties.robot.name, "p.robot.name", "");
 
 
        costmapReader.registerPropertyDefinitions(def);
        costmapWriter.registerPropertyDefinitions(def);
        laserScannerFeaturesReader.registerPropertyDefinitions(def);
 
        return def;
    }
 
    Component::Component() :
        robotReader(), costmapReader(), costmapWriter(), laserScannerFeaturesReader()
    {
    }
 
    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();
        }
        */
 
        // connect memory readers and writers
        ARMARX_INFO << "Connecting to robot state memory";
        robotReader.connect(memoryNameSystem());
 
        ARMARX_INFO << "Connecting to costmap segments";
        costmapReader.connect(memoryNameSystem());
        costmapWriter.connect(memoryNameSystem());
 
        ARMARX_INFO << "Connecting to laser scanner features segment";
        laserScannerFeaturesReader.connect(memoryNameSystem());
 
        ARMARX_CHECK(readStaticCostmap());
 
        updateCostmapTask =
            new PeriodicTask<Component>(this, &Component::updateCostmap, properties.updatePeriodMs);
 
        updateCostmapTask->start();
    }
 
    bool
    Component::readStaticCostmap()
    {
 
        while (true)
        {
            const memory::client::costmap::Reader::Query query{
                .providerName = properties.staticCostmap.providerName,
                .name = properties.staticCostmap.name,
                .timestamp = armarx::core::time::Clock::Now()};
 
            const auto result = costmapReader.query(query);
 
            if (result.costmap.has_value())
            {
                staticCostmap.emplace(result.costmap.value());
                return true;
            }
 
            ARMARX_INFO << deactivateSpam(1) << "Static costmap not available yet";
        }
 
        // return false;
    }
 
    void
    fillCostmap(const std::vector<memory::LaserScannerFeatures>& features,
                algorithms::Costmap& costmap)
    {
 
        std::vector<util::geometry::polygon_type> obstacles;
 
        for (const auto& f : features)
        {
            const auto& global_T_sens = f.frameGlobalPose;
            for (const auto& ff : f.features)
            {
 
                std::vector<Eigen::Vector2f> hull = ff.convexHull;
                for (auto& h : hull)
                {
                    h = conv::to2D(global_T_sens * conv::to3D(h));
                }
 
                obstacles.push_back(util::geometry::toPolygon(hull));
            }
        }
 
        ARMARX_VERBOSE << "Found " << obstacles.size() << " obstacles/polygons.";
 
 
        for (int r = 0; r < costmap.getGrid().rows(); r++)
        {
            for (int c = 0; c < costmap.getGrid().cols(); c++)
            {
                algorithms::Costmap::Index idx(r, c);
                const auto pos = costmap.toPositionGlobal(idx);
 
                // if(costmap.getMask().has_value())
                // {
                //     costmap.getMutableMask().value()(r,c) = true; // validate this cell
                // }
 
                if (costmap.isInCollision(pos))
                {
                    continue;
                }
 
                constexpr float robotRadius = 500;
 
                const float dist = util::geometry::computeDistance(obstacles, pos) - robotRadius;
 
                if (dist <= 0) // in collision?
                {
                    if (costmap.getMask().has_value())
                    {
                        costmap.getMutableMask().value()(r, c) = false; // invalidate this cell
                    }
                }
 
                // update the costmap. combine static and dynamic distances
                costmap.getMutableGrid()(r, c) =
                    std::min(costmap.getGrid()(r, c), std::max(dist, 0.F));
                // costmap.getMutableGrid()(r, c) = std::max(dist, 0.F);
            }
        }
    }
 
    void
    Component::drawCostmap(const armarx::navigation::algorithms::Costmap& costmap,
                           const std::string& name,
                           const float heightOffset)
    {
        const auto cmap = simox::color::cmaps::viridis();
        const float vmax = costmap.getGrid().maxCoeff();
 
        ARMARX_VERBOSE << "Grid `" << name << "` max value is " << vmax;
 
        const auto asColor = [&cmap, &vmax](const float distance) -> armarx::viz::data::Color
        {
            const auto color = cmap.at(distance, 0.F, vmax);
            return {color.a, color.r, color.g, color.b};
        };
 
        const armarx::viz::data::Color colorInvalid(0, 0, 0, 0);
 
        auto layer = arviz.layer("costmap_" + name);
 
        const std::int64_t cols = costmap.getGrid().cols();
        const std::int64_t rows = costmap.getGrid().rows();
 
        auto mesh = armarx::viz::Mesh("");
 
        const Eigen::Vector3f heightOffsetV = Eigen::Vector3f::UnitZ() * heightOffset;
 
        std::vector<std::vector<Eigen::Vector3f>> vertices;
        std::vector<std::vector<armarx::viz::data::Color>> colors;
 
        for (int r = 0; r < rows; r++)
        {
            auto& verticesRow = vertices.emplace_back(cols);
            auto& colorsRow = colors.emplace_back(cols);
            for (int c = 0; c < cols; c++)
            {
                verticesRow.at(c) =
                    armarx::navigation::conv::to3D(costmap.toPositionGlobal({r, c})) +
                    heightOffsetV;
 
                colorsRow.at(c) = [&]()
                {
                    if (costmap.isValid({r, c}))
                    {
                        return asColor(costmap.getGrid()(r, c));
                    }
 
                    return colorInvalid;
                }();
            }
        }
 
        mesh.grid2D(vertices, colors);
 
        layer.add(mesh);
 
        arviz.commit(layer);
    }
 
    void
    Component::updateCostmap()
    {
 
        ARMARX_CHECK(staticCostmap.has_value());
 
        const auto timestamp = armarx::core::time::Clock::Now();
 
        const memory::client::laser_scanner_features::Reader::Query query{
            .providerName = properties.laserScannerFeatures.providerName,
            .name = properties.laserScannerFeatures.name,
            .timestamp = timestamp};
 
        const auto result = laserScannerFeaturesReader.queryData(query);
 
        if (result.status != result.Success)
        {
            ARMARX_WARNING << "Failed to retrieve data from memory";
            return;
        }
 
        for (const auto& e : result.features)
        {
            ARMARX_VERBOSE << "Number of laser scanner clusters for sensor " << e.frame << ": "
                           << e.features.size();
        }
 
        // copy the static costmap. masked out regions don't need to be updated.
        auto dynamicCostmap = staticCostmap.value();
 
        // create costmap and store it in memory
        {
            const auto start = armarx::core::time::Clock::Now();
            fillCostmap(result.features, dynamicCostmap);
            const auto end = armarx::core::time::Clock::Now();
 
            ARMARX_INFO << deactivateSpam(1) << "Creation of dynamic costmap (filling) took "
                        << (end - start).toMilliSecondsDouble() << " milliseconds";
        }
 
 
        costmapWriter.store(dynamicCostmap, "dynamic_distance_to_obstacles", getName(), timestamp);
 
        const auto timestamp1 = armarx::core::time::Clock::Now();
        const auto navigationCostmap = computeNavigationCostmap(dynamicCostmap);
        const auto timestamp2 = armarx::core::time::Clock::Now();
 
        ARMARX_INFO << deactivateSpam(1) << "Navigation costmap. Computation took "
                    << (timestamp2 - timestamp1).toMilliSecondsDouble() << "ms";
 
 
        // drawing
        drawCostmap(dynamicCostmap, "dynamic_costmap", 10);
        drawCostmap(navigationCostmap, "navigation_costmap", 20);
    }
 
    algorithms::Costmap
    Component::computeNavigationCostmap(const algorithms::Costmap& obstacleDistancesCostmap)
    {
 
        const armem::robot::RobotDescription robotDescription{.name = properties.robot.name};
 
        const auto globalPose = robotReader.queryGlobalPose(robotDescription, armarx::Clock::Now());
        ARMARX_CHECK(globalPose.has_value());
 
        const Eigen::Vector2f globalRobotPosition = globalPose->translation().head<2>();
 
        // navigation costs
        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Parameters
            spfaParams;
        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm spfa(
            obstacleDistancesCostmap, spfaParams);
 
        const armarx::navigation::algorithms::spfa::ShortestPathFasterAlgorithm::Result result =
            spfa.spfa(globalRobotPosition);
 
        ARMARX_VERBOSE << "max distance " << result.distances.maxCoeff();
 
        // here, we again treat the result as a costmap
        const armarx::navigation::algorithms::Costmap navigationPlanningCostmap(
            result.distances,
            obstacleDistancesCostmap.params(),
            obstacleDistancesCostmap.getLocalSceneBounds(),
            result.reachable);
 
        return navigationPlanningCostmap;
    }
 
    void
    Component::onDisconnectComponent()
    {
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    std::string
    Component::getDefaultName() const
    {
        return Component::defaultName;
    }
 
    std::string
    Component::GetDefaultName()
    {
        return Component::defaultName;
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider