RoomScene.h

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/>.
 */
 
#pragma once
 
#include <cstddef>
#include <filesystem>
#include <optional>
#include <random>
#include <string>
#include <vector>
 
#include <Eigen/Core>
 
#include <armarx/navigation/core/basic_types.h>
 
#include "../global_planning_analysis/Scene.h"
 
namespace armarx::navigation::evaluation
{
 
    /// A pose from the room's navigation graph, kept with its name so a failing request can be
    /// reported as something a reader recognises rather than a pair of coordinates.
    struct NamedPose
    {
        std::string name;
 
        core::Pose pose;
    };
 
    /// Whether a point lies within the bounds, edges included.
    bool contains(const algorithms::SceneBounds& bounds, const Eigen::Vector2f& point);
 
    /// The room as the planner sees it, assembled from PriorKnowledgeData.
    struct RoomScene
    {
        /// The first `roomWallCount` obstacles are always the room walls, which is what lets
        /// the synthetic scenes below strip the furniture and keep the room.
        analysis::Scene scene;
 
        /// Poses from `navigation-graphs/<room>/locations.json`.
        std::vector<NamedPose> locations;
 
        /// Import problems: objects and locations dropped, and objects kept with an
        /// approximated footprint. Reported so a reader knows what is actually in the costmap.
        std::vector<std::string> importWarnings;
 
        /// Set when the scene is a corridor, so requests can be drawn as exits from it.
        std::optional<algorithms::SceneBounds> corridor;
    };
 
    /// Number of wall boxes `buildRoomScene` puts at the front of the obstacle list.
    inline constexpr std::size_t roomWallCount = 4;
 
    /**
     * @brief Build the scene for a room from PriorKnowledgeData.
     *
     * Each object's oriented bounding box is transformed by the object's pose and projected to
     * the ground plane. Room walls come from the room polygon's axis-aligned bounds.
     *
     * @param priorKnowledge  Root of the PriorKnowledgeData `data/PriorKnowledgeData` directory.
     * @param room            Room path within the scene collections, e.g. `50_19/R007`.
     * @param sceneFiles      Scene descriptions to merge, e.g. `static_scene`, `mobile_kitchen`.
     * @param wallThickness   Thickness [mm] of the boxes standing in for the room walls.
     */
    RoomScene buildRoomScene(const std::filesystem::path& priorKnowledge,
                             const std::string& room,
                             const std::vector<std::string>& sceneFiles,
                             float wallThickness = 200.F);
 
    struct ClutterParameters
    {
        /// Boxes to add. R007 is 6 x 14 m, so a few dozen already makes it hard.
        int count{40};
 
        float minExtent{300.F};
 
        float maxExtent{900.F};
 
        /**
         * Gap [mm] a new box must leave to every obstacle already placed.
         *
         * This is the knob that makes the scene difficult, and it is deliberately expressed
         * as a gap rather than as a density. Set a little above the robot's diameter it
         * produces passages the robot only just fits through, which is the regime where the
         * smoother has to thread a path rather than merely shorten one. Set below the
         * diameter it produces dead ends, which tests the planner rather than the smoother.
         */
        float minGap{750.F};
 
        /// Attempts per box before giving up, so a saturated room terminates.
        int maxAttempts{400};
    };
 
    /**
     * @brief Scatter boxes through the room to create tight passages.
     *
     * Seeded from the caller's generator, so a cluttered sweep is as reproducible as a plain
     * one. Returns the number actually placed, which falls short of `count` once the room
     * saturates.
     */
    int addClutter(analysis::Scene& scene, const ClutterParameters& parameters, std::mt19937& rng);
 
    struct CorridorParameters
    {
        /// Clear width [mm] between the corridor walls.
        float width{900.F};
 
        /// How far the corridor runs into the room [mm].
        float length{5000.F};
 
        float wallThickness{200.F};
    };
 
    /**
     * @brief A dead-end corridor opening into the rest of the room.
     *
     * Models the case the serpentine does not: the robot starts *inside* tight space and has to
     * get out. The difference matters because the optimizer holds the start fixed, so a start
     * deep in a corridor constrains the path in a way a start in open space never does -- the
     * first segments have one free endpoint each and no room either side.
     *
     * Removes the furniture, keeping only the room walls.
     *
     * @return The region the corridor occupies, so requests can be drawn as exits from it.
     */
    algorithms::SceneBounds makeCorridor(analysis::Scene& scene,
                                         const CorridorParameters& parameters);
 
    struct SerpentineParameters
    {
        /// Width [mm] of every opening. Exact by construction, which is the point: random
        /// clutter cannot guarantee a passage is traversable, and a scene whose passages are
        /// narrower than the checker's clearance measures its own geometry rather than the
        /// planner.
        float passageWidth{1000.F};
 
        /// Number of transverse walls. Each one adds an opening the path must thread and a
        /// direction change, which is what actually loads the smoother.
        int walls{6};
 
        float wallThickness{200.F};
    };
 
    /**
     * @brief Replace the room's contents with transverse walls whose openings alternate sides.
     *
     * Forces a serpentine path: every wall is a tight passage followed by a reversal, so a
     * single request exercises threading and turning repeatedly. Furniture is removed rather
     * than added to, so the passage width is the only thing constraining the path and the
     * scene stays interpretable.
     *
     * @return The narrowest clearance [mm] the scene offers a robot of the given radius, so
     *         the caller can refuse to run a sweep that is unwinnable by construction.
     */
    float makeSerpentine(analysis::Scene& scene,
                         const SerpentineParameters& parameters,
                         float robotRadius);
 
} // namespace armarx::navigation::evaluation