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 <cstddef>
#include <limits>
 
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/SceneObjectSet.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/util/CPPUtility/trace.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 Costmap::Parameters& parameters,
                                   const std::string& robotCollisonModelName) :
        robot(robot),
        obstacles(obstacles),
        articulatedObjects(articulatedObjects),
        parameters(parameters),
        robotCollisionModelName(robotCollisonModelName)
    {
        ARMARX_CHECK_NOT_NULL(robot) << "Robot must be set";
        ARMARX_CHECK_NOT_NULL(obstacles);
        ARMARX_CHECK(robot->hasRobotNode(robotCollisionModelName));
    }
 
    Costmap
    CostmapBuilder::create(const SceneBounds& init)
    {
 
        const auto sceneBounds = computeSceneBounds(obstacles, articulatedObjects, init, parameters.sceneBoundsMargin);
        const auto grid = createUniformGrid(sceneBounds, parameters);
 
        ARMARX_VERBOSE << "Creating grid";
        Costmap costmap(grid, parameters, sceneBounds);
 
        ARMARX_VERBOSE << "Filling grid with size (" << costmap.getGrid().rows() << "/"
                       << costmap.getGrid().cols() << ")";
        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();
    }
 
    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
        const float distanceNonArticulated =
            VirtualRobot::CollisionChecker::getGlobalCollisionChecker()->calculateDistance(
                robotCollisionModel, obstacles);
 
 
        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)
    {
 
        VirtualRobot::RobotPtr collisionRobot;
        VirtualRobot::CollisionModelPtr robotCollisionModel;
 
        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!";
            }
 
            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 Costmap::Position position = costmap.toPositionGlobal(index);
 
                costmap.grid(x, y) = computeCost(position, collisionRobot, robotCollisionModel);
            }
        }
    }
 
 
} // namespace armarx::navigation::algorithms