CostmapBuilder.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/>.
 *
 * @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 "CostmapBuilder.h"
 
#include <algorithm>
#include <cstddef>
#include <functional>
#include <limits>
#include <optional>
#include <string>
#include <vector>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Nodes/RobotNode.h> // IWYU pragma: keep
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/ScopedStopWatch.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::algorithms
{
 
 
    CostmapBuilder::CostmapBuilder(const VirtualRobot::RobotPtr& robot,
                                   const VirtualRobot::SceneObjectSetPtr& obstacles,
                                   const std::vector<VirtualRobot::RobotPtr>& articulatedObjects,
                                   const std::vector<Room>& rooms,
                                   const Costmap::Parameters& parameters,
                                   const std::string& robotCollisonModelName,
                                   const CostmapBuilderParams& builderParameters) :
        robot(robot),
        obstacles(obstacles),
        articulatedObjects(articulatedObjects),
        rooms(rooms),
        parameters(parameters),
        robotCollisionModelName(robotCollisonModelName),
        builderParameters(builderParameters)
    {
        ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
        ARMARX_CHECK_NOT_NULL(obstacles);
        ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName));
    }
 
    Costmap
    CostmapBuilder::extend(Costmap costmap)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Creating costmap took " << duration; });
 
        // We do not restrict the costmap to rooms here. Instead, we only update the costmap
        // inside the rooms below (if this is requested).
 
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                { ARMARX_INFO << "Filling costmap took " << duration; });
 
            ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                           << costmap.getGrid().cols() << ") and resolution "
                           << costmap.params().cellSize;
 
            if (builderParameters.restrictToRooms)
            {
                ARMARX_INFO << "Restricting to rooms";
                extendGridCosts(costmap, rooms);
            }
            else
            {
                extendGridCosts(costmap, {});
            }
 
            ARMARX_VERBOSE << "Filled grid";
        }
 
        initializeMask(costmap);
        ARMARX_VERBOSE << "Initialized mask";
 
        return costmap;
    }
 
    Costmap
    CostmapBuilder::create(const SceneBounds& init)
    {
        armarx::core::time::ScopedStopWatch sw(
            [](const armarx::core::time::Duration& duration)
            { ARMARX_INFO << "Creating costmap took " << duration; });
 
        ARMARX_INFO << articulatedObjects.size() << " articulated objects";
        ARMARX_INFO << obstacles->getSize() << " objects";
 
        const auto sceneBounds = computeSceneBounds(obstacles,
                                                    articulatedObjects,
                                                    init,
                                                    rooms,
                                                    parameters.sceneBoundsMargin,
                                                    builderParameters.restrictToRooms);
        const auto grid = createUniformGrid(sceneBounds, parameters);
 
        ARMARX_VERBOSE << "Creating grid";
        Costmap costmap(grid, parameters, sceneBounds);
 
        if (builderParameters.restrictToRooms)
        {
            initializeEmptyMask(costmap);
 
            ARMARX_CHECK_NOT_EMPTY(rooms);
            ARMARX_VERBOSE << "Restricting to rooms:";
            for (const auto& room : rooms)
            {
                ARMARX_VERBOSE << " - " << room.name;
            }
 
            invalidateOutsideRooms(rooms, costmap);
 
            ARMARX_VERBOSE << "Restricted to rooms. Fraction of valid elements: "
                           << costmap.mask->cast<float>().sum() / costmap.mask->size();
        }
 
        {
            armarx::core::time::ScopedStopWatch sw(
                [](const armarx::core::time::Duration& duration)
                { ARMARX_INFO << "Filling costmap took " << duration; });
 
            ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                           << costmap.getGrid().cols() << ") and resolution "
                           << costmap.params().cellSize;
            fillGridCosts(costmap);
            ARMARX_VERBOSE << "Filled grid";
        }
 
        initializeMask(costmap);
        ARMARX_VERBOSE << "Initialized mask";
 
        return costmap;
    }
 
    void
    CostmapBuilder::initializeMask(Costmap& costmap)
    {
        costmap.mask = costmap.grid.array() > 0.F;
 
        ARMARX_VERBOSE << "Initializing mask: Fraction of valid elements: "
                       << costmap.mask->cast<float>().sum() / costmap.mask->size();
    }
 
    void
    CostmapBuilder::initializeEmptyMask(Costmap& costmap)
    {
        costmap.mask = costmap.grid.array() >= -42.F; // FIXME: magic number, just same size
        costmap.mask->setOnes();
 
        ARMARX_VERBOSE << "Initializing empty mask: Fraction of valid elements: "
                       << costmap.mask->cast<float>().sum() / costmap.mask->size();
    }
 
    Eigen::MatrixXf
    CostmapBuilder::createUniformGrid(const SceneBounds& sceneBounds,
                                      const Costmap::Parameters& parameters)
    {
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << "Scene bounds are " << sceneBounds.min << " and " << sceneBounds.max;
 
        //+1 for explicit rounding up
        size_t c_x = (sceneBounds.max.x() - sceneBounds.min.x()) / parameters.cellSize + 1;
        size_t c_y = (sceneBounds.max.y() - sceneBounds.min.y()) / parameters.cellSize + 1;
 
        ARMARX_VERBOSE << "Grid size: " << c_x << ", " << c_y;
 
        ARMARX_VERBOSE << "Resetting grid";
        Eigen::MatrixXf grid(c_x, c_y);
        grid.setZero();
 
        return grid;
    }
 
    float
    CostmapBuilder::computeCost(const Costmap::Position& position,
                                VirtualRobot::RobotPtr& collisionRobot,
                                const VirtualRobot::CollisionModelPtr& robotCollisionModel)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(collisionRobot);
        ARMARX_CHECK_NOT_NULL(robotCollisionModel);
        ARMARX_CHECK_NOT_NULL(VirtualRobot::CollisionChecker::getGlobalCollisionChecker());
        ARMARX_CHECK_NOT_NULL(obstacles);
        // ARMARX_CHECK_GREATER(obstacles->getSize(), 0);
 
        const core::Pose globalPose(Eigen::Translation3f(conv::to3D(position)));
        collisionRobot->setGlobalPose(globalPose.matrix());
 
        // distance to non-articulated objects
        float distanceNonArticulated = std::numeric_limits<float>::max();
        if (obstacles->getSize() > 0)
        {
            distanceNonArticulated =
                VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                    robotCollisionModel, obstacles);
        }
 
        // distance to articulated objects
        float distanceArticulatedMin = std::numeric_limits<float>::max();
        for (const auto& articulatedObject : articulatedObjects)
        {
            for (const auto& colModel : articulatedObject->getCollisionModels())
            {
                const float distanceArticulated =
                    VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                        robotCollisionModel, colModel);
                distanceArticulatedMin = std::min(distanceArticulated, distanceArticulatedMin);
            }
        }
 
        return std::min(distanceNonArticulated, distanceArticulatedMin);
 
 
        // Eigen::Vector3f P1;
        // Eigen::Vector3f P2;
        // int id1, id2;
 
        // float minDistance = std::numeric_limits<float>::max();
 
        // // TODO omp...
        // for (size_t i = 0; i < obstacles->getSize(); i++)
        // {
        //     // cheap collision check
        //     // VirtualRobot::BoundingBox obstacleBbox =
        //     //     obstacles->getSceneObject(i)->getCollisionModel()->getBoundingBox(true);
        //     // if (not intersects(robotBbox, obstacleBbox))
        //     // {
        //     //     continue;
        //     // }
 
        //     // precise collision check
        //     const float dist =
        //         VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
        //             robotCollisionModel,
        //             obstacles->getSceneObject(i)->getCollisionModel(),
        //             P1,
        //             P2,
        //             &id1,
        //             &id2);
 
        //     // check if objects collide
        //     if ((dist <= parameters.cellSize / 2) or
        //         VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->checkCollision(
        //             robotCollisionModel, obstacles->getSceneObject(i)->getCollisionModel()))
        //     {
        //         minDistance = 0;
        //         break;
        //     }
 
        //     minDistance = std::min(minDistance, dist);
        // }
        // // return n->position.x() >= sceneBounds.min.x() && n->position.x() <= sceneBounds.max.x() &&
        // //        n->position.y() >= sceneBounds.min.y() && n->position.y() <= sceneBounds.max.y();
 
        // return minDistance;
    }
 
    void
    CostmapBuilder::fillGridCosts(Costmap& costmap)
    {
        applyFnToCostmap(costmap,
                         [&](VirtualRobot::RobotPtr& collisionRobot,
                             VirtualRobot::CollisionModelPtr& robotCollisionModel,
                             const std::optional<float> maskOpt,
                             const float cost,
                             const Costmap::Index& index) -> float
                         {
                             const auto position = costmap.toPositionGlobal(index);
 
                             // consider mask if available and skip if not valid
                             if (maskOpt.has_value())
                             {
                                 if (not maskOpt.value())
                                 {
                                     return 0.F;
                                 }
                             }
 
                             return computeCost(position, collisionRobot, robotCollisionModel);
                         });
    }
 
    void
    CostmapBuilder::extendGridCosts(Costmap& costmap, const std::vector<Room>& rooms)
    {
        applyFnToCostmap(costmap,
                         [&](VirtualRobot::RobotPtr& collisionRobot,
                             VirtualRobot::CollisionModelPtr& robotCollisionModel,
                             const std::optional<float> maskOpt,
                             const float cost,
                             const Costmap::Index& index) -> float
                         {
                             const auto position = costmap.toPositionGlobal(index);
 
                             // consider mask if available and skip if not valid
                             if (maskOpt.has_value())
                             {
                                 if (not maskOpt.value())
                                 {
                                     return std::min(0.F, costmap.grid(index.x(), index.y()));
                                 }
                             }
 
                             if (cost < 0.F)
                             {
                                 return cost;
                             }
 
                             if (not rooms.empty())
                             {
                                 if (std::none_of(rooms.begin(),
                                                  rooms.end(),
                                                  [&position](const Room& room) -> bool
                                                  { return room.isInside(position); }))
                                 {
                                     return cost;
                                 }
                             }
 
                             return std::min(
                                 cost, computeCost(position, collisionRobot, robotCollisionModel));
                         });
    }
 
    void
    CostmapBuilder::applyFnToCostmap(Costmap& costmap,
                                     const std::function<float(VirtualRobot::RobotPtr&,
                                                               VirtualRobot::CollisionModelPtr&,
                                                               const std::optional<float>,
                                                               const float,
                                                               const Costmap::Index&)>& fn)
    {
        VirtualRobot::RobotPtr collisionRobot;
        VirtualRobot::CollisionModelPtr robotCollisionModel;
 
        if (costmap.mask.has_value())
        {
            ARMARX_VERBOSE << "Costmap provides mask.";
        }
 
        const std::size_t c_x = costmap.grid.rows();
        const std::size_t c_y = costmap.grid.cols();
 
        robot->setUpdateVisualization(false);
 
#pragma omp parallel for schedule(static) private(collisionRobot,                                  \
                                                      robotCollisionModel) default(shared)
        for (unsigned int x = 0; x < c_x; x++)
        {
            // initialize if needed
            if (collisionRobot == nullptr)
            {
#pragma omp critical
                {
                    ARMARX_DEBUG << "Copying robot";
                    ARMARX_CHECK_NOT_NULL(robot);
                    collisionRobot =
                        robot->clone("collision_robot_" + std::to_string(omp_get_thread_num()));
 
                    // replace the standard collision model by our custom one
                    collisionRobot->setPrimitiveApproximationModel({"navigation"}, false);
 
                    collisionRobot->setUpdateVisualization(false);
                    ARMARX_DEBUG << "Copying done";
                }
 
                ARMARX_CHECK_NOT_NULL(collisionRobot);
                ARMARX_CHECK(collisionRobot->hasRobotNode(robotCollisionModelName));
 
                ARMARX_CHECK(collisionRobot);
                const auto collisionRobotNode =
                    collisionRobot->getRobotNode(robotCollisionModelName);
                ARMARX_CHECK_NOT_NULL(collisionRobotNode);
 
                robotCollisionModel = collisionRobotNode->getCollisionModel();
                ARMARX_CHECK(robotCollisionModel) << "Collision model not available. "
                                                     "Make sure that you load the robot correctly!";
 
                // scale collision model
                robotCollisionModel->scale(builderParameters.collisionModelScaleFactor);
            }
 
            ARMARX_CHECK_NOT_NULL(collisionRobot);
            ARMARX_CHECK_NOT_NULL(robotCollisionModel);
 
            for (unsigned int y = 0; y < c_y; y++)
            {
                const Costmap::Index index{x, y};
 
                const auto maskVal = costmap.mask.has_value()
                                         ? std::make_optional(costmap.mask.value()(x, y))
                                         : std::nullopt;
                costmap.grid(x, y) =
                    fn(collisionRobot, robotCollisionModel, maskVal, costmap.grid(x, y), index);
            }
        }
    }
 
} // namespace armarx::navigation::algorithms