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    armarx::navigation::components::costmap_merger
 * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "Component.h"
 
#include <algorithm>
#include <cstddef>
#include <limits>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
 
#include <SimoxUtility/algorithm/string/string_tools.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/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/armem/core/MemoryID.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/constants.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/filter.hpp>
 
namespace armarx::navigation::components::costmap_merger
{
    using algorithms::Costmap;
 
    Component::Component()
    {
        addPlugin(costmapReaderPlugin);
        addPlugin(costmapWriterPlugin);
    }
 
    const std::string Component::defaultName = "costmap_merger";
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->required(
            properties.costmapsToMerge,
            "p.costmapsToMerge",
            "The costmaps to merge, they are prioritized in the order they are specified, i.e. the "
            "first costmap will take priority over the second one etc.");
 
        def->optional(
            properties.outputCostmapName, "p.outputCostmapName", "The name of the merged costmap.");
 
        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{memory::constants::NavigationMemoryName,
                                    memory::constants::CostmapCoreSegmentName},
            this,
            &Component::processObjectInstance);
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_VERBOSE << "Creating initial merge of costmaps";
        mergeAndStoreCostmap();
    }
 
    void
    Component::processObjectInstance(const armem::MemoryID& /*id*/,
                                     const std::vector<armem::MemoryID>& snapshotIDs)
    {
        ARMARX_VERBOSE << "A costmap instance has changed!";
 
        const std::vector<armem::MemoryID> relevantCostmaps =
            snapshotIDs |
            ranges::views::filter(
                [&](const armem::MemoryID& id)
                {
                    std::string name = id.providerSegmentName + "/" + id.entityName;
                    return std::find(properties.costmapsToMerge.cbegin(),
                                     properties.costmapsToMerge.cend(),
                                     name) != properties.costmapsToMerge.cend();
                }) |
            ranges::to_vector;
 
        if (relevantCostmaps.empty())
        {
            return;
        }
 
        ARMARX_INFO << "Relevant costmaps changed: " << relevantCostmaps;
 
        // Re-merge the costmaps and store them
        mergeAndStoreCostmap();
    }
 
    void
    Component::mergeAndStoreCostmap()
    {
        std::lock_guard g{mergeCostmapMtx};
 
        ARMARX_VERBOSE << "Reading costmaps";
 
        const armarx::DateTime timestamp = Clock::Now();
 
        std::vector<Costmap> costmaps;
 
        for (const auto& costmap : properties.costmapsToMerge)
        {
            ARMARX_VERBOSE << "Reading costmap `" << costmap << "`.";
            const std::vector<std::string> splits = simox::alg::split(costmap, "/");
            ARMARX_CHECK_EQUAL(splits.size(), 2) << "Invalid costmap name: " << costmap;
 
            const memory::client::costmap::Reader::Query query{
                .providerName = splits.front(), .name = splits.back(), .timestamp = timestamp};
            auto result = costmapReaderPlugin->get().query(query);
 
            if (not result)
            {
                ARMARX_WARNING << "Attempting to merge costmap `" << costmap
                               << "` which doesn't exist (yet) (" << result.errorMessage << ")";
            }
            else
            {
                ARMARX_CHECK_NOT_NULL(result.costmap);
                costmaps.emplace_back(std::move(*result.costmap));
            }
        }
 
        if (costmaps.empty())
        {
            ARMARX_WARNING << "Nothing to merge";
            return;
        }
 
        ARMARX_TRACE;
        ARMARX_VERBOSE << "Merging costmaps";
 
        auto mergedCostmap = [&]()
        {
            // calculate the union of the two costmaps
            // and the cell size of the most detailed costmap
            Eigen::AlignedBox2f aabb;
            float minCellSize = std::numeric_limits<float>::max();
            for (const auto& c : costmaps)
            {
                // origin transformation might mix up where min/max lies, therefore not min/max anymore
                Eigen::Vector2f globalCornerA = c.origin() * c.getLocalSceneBounds().min;
                Eigen::Vector2f globalCornerB = c.origin() * c.getLocalSceneBounds().max;
 
                aabb.extend(globalCornerA);
                aabb.extend(globalCornerB);
 
                minCellSize = std::min(minCellSize, c.params().cellSize);
            }
            ARMARX_TRACE;
            ARMARX_VERBOSE << "Scene bounds intersections is " << aabb.min() << " and "
                           << aabb.max() << " with minCellSize as " << minCellSize;
 
 
            // create costmap with given cell size and scene bounds
 
            //+1 for explicit rounding up
            Eigen::Vector2f boundingSizes = aabb.sizes();
            const auto cX = static_cast<std::size_t>((boundingSizes.x() / minCellSize) + 1);
            const auto cY = static_cast<std::size_t>((boundingSizes.y() / minCellSize) + 1);
 
            Eigen::MatrixXf grid(cX, cY);
            grid.setZero();
 
            Costmap costmap(grid,
                            Costmap::Parameters{.cellSize = minCellSize, .sceneBoundsMargin = 0.F},
                            algorithms::SceneBounds{.min = aabb.min(), .max = aabb.max()});
 
            // to copy shape from grid to mask
            costmap.getMutableMask() = costmap.getGrid().array() == 0;
            costmap.getMutableMask()->setOnes();
 
            return costmap;
        }();
 
        ARMARX_CHECK(mergedCostmap.getMask().has_value());
 
        // check entry of first valid costmap in order and return whether any costmap was valid
        const auto& checkInOrder = [&](int x, int y, const Eigen::Vector2f& position)
        {
            for (const auto& other : costmaps)
            {
                Eigen::AlignedBox2f bounds(other.getLocalSceneBounds().min,
                                           other.getLocalSceneBounds().max);
                const auto otherVertex = other.toVertex(position);
                // only use this costmap if the current position lies inside it's bounds
                if (bounds.contains(position) && other.isValid(otherVertex.index))
                {
                    mergedCostmap.getMutableGrid()(x, y) = *other.value(otherVertex.index);
                    return true;
                }
            }
            return false;
        };
 
        for (int x = 0; x < mergedCostmap.getGrid().rows(); x++)
        {
            for (int y = 0; y < mergedCostmap.getGrid().cols(); y++)
            {
                ARMARX_TRACE;
                const Costmap::Position position = mergedCostmap.toPositionGlobal({x, y});
                mergedCostmap.getMutableMask().value()(x, y) = checkInOrder(x, y, position);
            }
        }
 
        costmapWriterPlugin->get().store(
            mergedCostmap, properties.outputCostmapName, getName(), timestamp);
    }
 
    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::costmap_merger