RoomNavigationTargetCreator.cpp

Go to the documentation of this file.

#include "RoomNavigationTargetCreator.h"
 
#include <cstddef>
 
#include <SimoxUtility/color/Color.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/components/ArViz/Client/Client.h"
#include "RobotAPI/components/ArViz/Client/Elements.h"
 
#include <armarx/navigation/algorithms/Room.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/visualization.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/reverse.hpp>
 
namespace armarx::navigation::rooms
{
    RoomNavigationTargetCreator::RoomNavigationTargetCreator(const Params& params)
    {
    }
 
    RoomNavigationTargetCreator::Result
    RoomNavigationTargetCreator::getClosestPositionOutsideOfRoom(
        const algorithms::Costmap& obstacleDistancesCostmap,
        const algorithms::Room& room,
        const core::Position& global_P_robot,
        viz::Client* arviz) const
    {
        using ShortestPathFasterAlgorithm = algorithms::spfa::ShortestPathFasterAlgorithm;
 
        const ShortestPathFasterAlgorithm spfa(obstacleDistancesCostmap,
                                               ShortestPathFasterAlgorithm::Parameters());
 
        ShortestPathFasterAlgorithm::Result result = spfa.spfa(conv::to2D(global_P_robot));
 
        const armarx::navigation::algorithms::Costmap navigationPlanningCostmap
            = armarx::navigation::algorithms::Costmap::WithSameDimsAs(
                obstacleDistancesCostmap, result.distances, result.reachable);
 
        armarx::navigation::algorithms::Costmap roomCostmap = navigationPlanningCostmap;
 
        const std::size_t cX = roomCostmap.getGrid().rows();
        const std::size_t cY = roomCostmap.getGrid().cols();
 
        // invalidate everything that is in collision
        // roomCostmap.getMutableMask().value() = roomCostmap.getGrid().array() >= 0.5;
 
#pragma omp parallel for schedule(static)
        for (unsigned int x = 0; x < cX; x++)
        {
            for (unsigned int y = 0; y < cY; y++)
            {
                const algorithms::Costmap::Index index{x, y};
                const algorithms::Costmap::Position position = roomCostmap.toPositionGlobal(index);
 
                auto pos3d = conv::to3D(position);
                pos3d.z() = room.zFloor + room.height / 2; // just at the center (height)
 
                // invalid outside of the room
                const bool isInside = room.isInside(pos3d);
 
                roomCostmap.getMutableMask().value()(x, y) &= isInside;
                roomCostmap.getMutableGrid()(x, y) *= static_cast<float>(isInside);
            }
        }
 
        ARMARX_INFO << "Closest position to room: " << roomCostmap.optimum().position;
 
        const core::Position closestReachablePositionOnRoomBoundary =
            conv::to3D(roomCostmap.optimum().position);
 
 
        // We don't stop here. We want to place the robot in front of the room at a
        const auto plan = spfa.plan(conv::to2D(global_P_robot),
                                    conv::to2D(closestReachablePositionOnRoomBoundary));
 
        if (arviz != nullptr)
        {
            auto layer = arviz->layer("RoomNavigationTargetCreator");
            algorithms::visualize(roomCostmap, layer, "room_costmap", 3); // FIXME magic number
 
            layer.add(viz::Sphere("closestReachablePositionOnRoomBoundary")
                          .position(closestReachablePositionOnRoomBoundary)
                          .radius(50)
                          .color(simox::Color::blue()));
 
            arviz->commit(layer);
        }
 
 
        ARMARX_CHECK(plan.success);
 
        const auto posInFrontOfRoom = [&]() -> core::Position
        {
            namespace r = ::ranges;
            namespace rv = ::ranges::views;
 
            const auto reversedPlan = rv::reverse(plan.path) | r::to_vector;
 
            const auto goalPos = conv::to2D(closestReachablePositionOnRoomBoundary);
 
            for (const auto& pos : reversedPlan)
            {
                if ((pos - goalPos).norm() > params.distanceToRoomEntry)
                {
                    return conv::to3D(pos);
                }
            }
 
            ARMARX_INFO << "There is something wrong with the path!";
            return conv::to3D(reversedPlan.back()); // the current robot pos
        }();
 
        if (arviz != nullptr)
        {
            auto layer = arviz->layer("RoomNavigationTargetCreator_target");
 
            layer.add(viz::Sphere("posInFrontOfRoom")
                          .position(posInFrontOfRoom)
                          .radius(50)
                          .color(simox::Color::green()));
 
            arviz->commit(layer);
        }
 
        return {.global_P_roomEntry = closestReachablePositionOnRoomBoundary,
                .global_P_robot = posInFrontOfRoom};
    }
} // namespace armarx::navigation::rooms