main.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/>.
 */
 
#include <chrono>
#include <cstdlib>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <optional>
#include <random>
#include <string>
#include <vector>
 
#include <boost/program_options.hpp>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
#include <armarx/navigation/global_planning/SPFA.h>
 
#include "../global_planning_analysis/Scene.h"
#include "Metrics.h"
#include "RoomScene.h"
#include "Sampling.h"
 
namespace po = boost::program_options;
 
using namespace armarx::navigation;
 
namespace
{
    armarx::MessageTypeT
    toLogLevel(const std::string& name)
    {
        if (name == "debug")
            return armarx::MessageTypeT::DEBUG;
        if (name == "verbose")
            return armarx::MessageTypeT::VERBOSE;
        if (name == "info")
            return armarx::MessageTypeT::INFO;
        if (name == "important")
            return armarx::MessageTypeT::IMPORTANT;
        if (name == "warn")
            return armarx::MessageTypeT::WARN;
        if (name == "error")
            return armarx::MessageTypeT::ERROR;
 
        throw std::invalid_argument{"Unknown log level `" + name + "`."};
    }
 
    std::filesystem::path
    resolvePriorKnowledge(const std::string& override)
    {
        if (not override.empty())
        {
            return override;
        }
 
        // The package layout puts the data one level below the package root.
        return armarx::PackagePath("PriorKnowledgeData", "").toSystemPath();
    }
 
    using SmoothingParams = decltype(algorithms::spfa::smoothing::io::loadSmoothingParams());
 
    /// Apply the `--corridor`, `--serpentine` and `--clutter` scene modifiers, in that order.
    void
    shapeScene(const po::variables_map& vm,
               const SmoothingParams& smoothingParams,
               analysis::Config& config,
               evaluation::RoomScene& roomScene,
               std::mt19937& rng)
    {
        const float robotRadius = config.costmapParams.robotRadius;
 
        if (const float corridorWidth = vm["corridor"].as<float>(); corridorWidth > 0.F)
        {
            const evaluation::CorridorParameters parameters{
                .width = corridorWidth, .length = vm["corridor-length"].as<float>()};
 
            roomScene.corridor = evaluation::makeCorridor(config.scene, parameters);
 
            const float clearance = 0.5F * corridorWidth - robotRadius;
 
            ARMARX_IMPORTANT << "Corridor: " << corridorWidth << " mm wide, " << parameters.length
                             << " mm long, leaving a " << robotRadius << " mm robot " << clearance
                             << " mm either side.";
 
            if (clearance <= static_cast<float>(smoothingParams.min_clearance))
            {
                ARMARX_WARNING << "That is at or below the " << smoothingParams.min_clearance
                               << " mm the trajectory checker enforces, so no path along the "
                                  "corridor can be accepted however well it is smoothed. Use a "
                                  "width above "
                               << 2.F * (robotRadius +
                                         static_cast<float>(smoothingParams.min_clearance))
                               << " mm.";
            }
        }
 
        if (const float passageWidth = vm["serpentine"].as<float>(); passageWidth > 0.F)
        {
            const evaluation::SerpentineParameters parameters{
                .passageWidth = passageWidth, .walls = vm["serpentine-walls"].as<int>()};
 
            const float clearance =
                evaluation::makeSerpentine(config.scene, parameters, robotRadius);
 
            ARMARX_IMPORTANT << "Serpentine: " << parameters.walls << " walls, " << passageWidth
                             << " mm openings, tightest clearance " << clearance << " mm.";
 
            if (clearance <= static_cast<float>(smoothingParams.clearance))
            {
                ARMARX_WARNING << "That is at or below the " << smoothingParams.clearance
                               << " mm the trajectory checker demands, so every path through "
                                  "an opening is rejected however well it is smoothed. This "
                                  "sweep would measure the scene, not the planner. Use openings "
                                  "wider "
                               << 2.F *
                                      (robotRadius + static_cast<float>(smoothingParams.clearance))
                               << " mm.";
            }
        }
 
        if (const int clutter = vm["clutter"].as<int>(); clutter > 0)
        {
            const evaluation::ClutterParameters parameters{.count = clutter,
                                                           .minGap = vm["clutter-gap"].as<float>()};
 
            // A gap of g leaves a robot of radius r only g/2 - r of clearance on the centre
            // line. Below the checker's clearance every path through a clutter passage is
            // rejected no matter how good the smoothing is, and the sweep measures the scene
            // rather than the planner.
            const float required =
                2.F * (robotRadius + static_cast<float>(smoothingParams.clearance));
 
            if (parameters.minGap <= required)
            {
                ARMARX_WARNING
                    << "clutter-gap is " << parameters.minGap << " mm, which leaves a "
                    << robotRadius << " mm robot only " << (0.5F * parameters.minGap - robotRadius)
                    << " mm of clearance -- below the " << smoothingParams.clearance
                    << " mm the trajectory checker demands. Every path through a clutter passage "
                       "will be rejected regardless of smoothing quality, so the rejection rate "
                       "from this sweep says nothing about the smoother. Use more than "
                    << required << " mm.";
            }
 
            const int placed = evaluation::addClutter(config.scene, parameters, rng);
 
            ARMARX_IMPORTANT << "Clutter: placed " << placed << " of " << clutter
                             << " boxes at a minimum gap of " << parameters.minGap << " mm."
                             << (placed < clutter ? " The room saturated before the rest fit."
                                                  : "");
        }
    }
 
    /// The near-obstacle clearance band, from the command line or derived from the smoother.
    evaluation::SamplingParameters
    samplingParameters(const po::variables_map& vm, const SmoothingParams& smoothingParams)
    {
        // By default the band starts just above the smoother's target clearance, which keeps
        // every endpoint above the (lower) limit the checker enforces, so the category
        // stresses the smoother rather than generating requests it cannot satisfy.
        evaluation::SamplingParameters sampling;
        const float bandMin = vm["endpoint-clearance-min"].as<float>();
        sampling.nearObstacleMinClearance =
            bandMin >= 0.F ? bandMin : static_cast<float>(smoothingParams.clearance) + 5.F;
 
        const float bandMax = vm["endpoint-clearance-max"].as<float>();
        sampling.nearObstacleMaxClearance =
            bandMax >= 0.F ? bandMax : sampling.nearObstacleMinClearance + 300.F;
 
        return sampling;
    }
 
    /// Scene geometry and the first few planned paths, for plotting.
    class PathDump
    {
    public:
        PathDump(const analysis::Scene& scene, const int maxPaths) : maxPaths_{maxPaths}
        {
            nlohmann::json obstacles = nlohmann::json::array();
            for (const analysis::Box& box : scene.obstacles)
            {
                obstacles.push_back({{"min", {box.min.x(), box.min.y()}},
                                     {"max", {box.max.x(), box.max.y()}},
                                     {"yaw", box.yaw}});
            }
 
            dump_["obstacles"] = obstacles;
            dump_["bounds"] = {{"min", {scene.bounds.min.x(), scene.bounds.min.y()}},
                               {"max", {scene.bounds.max.x(), scene.bounds.max.y()}}};
            dump_["paths"] = nlohmann::json::array();
        }
 
        /// Keep the path until `maxPaths` have been collected.
        void
        add(const evaluation::Request& request, const global_planning::GlobalPlannerResult& result)
        {
            if (static_cast<int>(dump_["paths"].size()) >= maxPaths_)
            {
                return;
            }
 
            nlohmann::json points = nlohmann::json::array();
            for (const auto& point : result.trajectory.points())
            {
                points.push_back(
                    {point.waypoint.pose.translation().x(), point.waypoint.pose.translation().y()});
            }
 
            dump_["paths"].push_back({{"kind", toString(request.kind)},
                                      {"index", request.index},
                                      {"smoothed", result.positionSmoothingApplied},
                                      {"points", points}});
        }
 
        void
        write(const std::string& path) const
        {
            std::ofstream file{path};
            file << dump_.dump() << std::endl;
            ARMARX_IMPORTANT << "Wrote scene and " << dump_["paths"].size() << " paths to `" << path
                             << "`.";
        }
 
    private:
        int maxPaths_;
 
        nlohmann::json dump_;
    };
} // namespace
 
int
main(int argc, char** argv)
try
{
    po::options_description desc("Offline sweep of planning requests against a real room.");
 
    // clang-format off
    desc.add_options()
        ("help,h", "produce help message")
        ("prior-knowledge", po::value<std::string>()->default_value(""),
         "Path to PriorKnowledgeData's data directory. Resolved from the package if omitted.")
        ("room", po::value<std::string>()->default_value("50_19/R007"),
         "Room within the scene collections.")
        ("scenes", po::value<std::vector<std::string>>()->multitoken()
             ->default_value({"static_scene", "mobile_kitchen"}, "static_scene mobile_kitchen"),
         "Scene descriptions to merge.")
        ("uniform", po::value<int>()->default_value(100), "Uniform free-space requests.")
        ("near-obstacle", po::value<int>()->default_value(200), "Near-obstacle requests.")
        ("locations", po::value<int>()->default_value(100), "Named-location requests.")
        ("seed", po::value<unsigned>()->default_value(1), "RNG seed; a sweep is reproducible.")
        ("corridor", po::value<float>()->default_value(0.F),
         "Replace the room's contents with a dead-end corridor of this clear width [mm] "
         "opening into the rest of the room. Requests then start inside it and finish outside, "
         "which is the case a start pinned in tight space produces and no other scene here "
         "does. 0 leaves the room as furnished.")
        ("corridor-length", po::value<float>()->default_value(5000.F),
         "How far the corridor runs into the room [mm].")
        ("corridor-exits", po::value<int>()->default_value(0),
         "Corridor-exit requests to draw.")
        ("serpentine", po::value<float>()->default_value(0.F),
         "Replace the room's contents with transverse walls whose openings alternate sides, "
         "each opening this wide [mm]. Forces a path to thread tight passages and reverse "
         "direction repeatedly. 0 leaves the room as furnished.")
        ("serpentine-walls", po::value<int>()->default_value(6),
         "Number of transverse walls in the serpentine scene.")
        ("endpoint-clearance-min", po::value<float>()->default_value(-1.F),
         "Lower bound [mm] of the clearance band near-obstacle endpoints are drawn from. "
         "Negative ties it to the smoother's target, which excludes endpoints the planner "
         "cannot satisfy; set it lower to include them and see how the rejection rate depends "
         "on how close to furniture the robot is asked to start and finish.")
        ("endpoint-clearance-max", po::value<float>()->default_value(-1.F),
         "Upper bound [mm] of that band. Negative uses the lower bound plus 300.")
        ("no-smoothing", po::bool_switch(),
         "Bypass the smoothing stage entirely, so the sweep measures the raw resampled path.")
        ("dump-scene", po::value<std::string>()->default_value(""),
         "Write the scene geometry and the first few planned paths here, for plotting.")
        ("dump-count", po::value<int>()->default_value(6), "How many paths to dump.")
        ("resample", po::value<float>()->default_value(-1.F),
         "Waypoint spacing [mm] before smoothing. Negative uses the planner's default.")
        ("clutter", po::value<int>()->default_value(0),
         "Extra boxes scattered through the room to create tight passages. 0 leaves the room "
         "as furnished.")
        ("clutter-gap", po::value<float>()->default_value(1000.F),
         "Gap [mm] clutter must leave to every other obstacle. Must exceed "
         "2 * (robot radius + smoothing clearance) or the passages it creates cannot be "
         "traversed at all; a warning says so.")
        ("cell-size", po::value<float>()->default_value(100.F), "Costmap cell size [mm].")
        ("robot-radius", po::value<float>()->default_value(350.F),
         "Robot radius [mm]. ARMAR-7 is circular; the static costmap inflates the 3D model "
         "geometry by this.")
        ("log-level", po::value<std::string>()->default_value("important"),
         "debug|verbose|info|important|warn|error. A sweep provokes planning failures by "
         "design, and each one logs a backtrace at error level.")
        ("out", po::value<std::string>()->default_value("evaluation.jsonl"),
         "Per-request records (JSON lines).")
        ;
    // clang-format on
 
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
 
    if (vm.count("help") != 0u)
    {
        std::cout << desc << "\n";
        return EXIT_SUCCESS;
    }
 
    po::notify(vm);
 
    armarx::LogSender::SetGlobalMinimumLoggingLevel(toLogLevel(vm["log-level"].as<std::string>()));
 
    const std::filesystem::path priorKnowledge =
        resolvePriorKnowledge(vm["prior-knowledge"].as<std::string>());
    const std::string room = vm["room"].as<std::string>();
 
    ARMARX_INFO << "Reading `" << room << "` from `" << priorKnowledge.string() << "`.";
 
    evaluation::RoomScene roomScene = evaluation::buildRoomScene(
        priorKnowledge, room, vm["scenes"].as<std::vector<std::string>>());
 
    ARMARX_IMPORTANT << "Scene: " << roomScene.scene.obstacles.size() << " obstacles (walls "
                     << "included), " << roomScene.locations.size() << " named locations.";
 
    for (const std::string& warning : roomScene.importWarnings)
    {
        ARMARX_WARNING << "Scene import -- " << warning;
    }
 
    analysis::Config config;
    config.scene = roomScene.scene;
    config.costmapParams.cellSize = vm["cell-size"].as<float>();
    config.costmapParams.robotRadius = vm["robot-radius"].as<float>();
 
    // Seeded before the scene is finished, so the clutter and the requests come from one
    // reproducible stream.
    std::mt19937 rng{vm["seed"].as<unsigned>()};
 
    const SmoothingParams smoothingParams = algorithms::spfa::smoothing::io::loadSmoothingParams();
 
    shapeScene(vm, smoothingParams, config, roomScene, rng);
 
    const algorithms::Costmap costmap = analysis::buildCostmap(config);
 
    // The pipeline under test: the same SPFAImpl the navigation stack runs.
    config.plannerParams.resampleDistance = vm["resample"].as<float>();
    config.plannerParams.enablePositionSmoothing = not vm["no-smoothing"].as<bool>();
 
    if (not config.plannerParams.enablePositionSmoothing)
    {
        ARMARX_IMPORTANT << "Smoothing disabled: measuring the raw resampled path.";
    }
 
    global_planning::SPFAImpl planner(config.plannerParams, config.generalConfig, costmap);
 
    const evaluation::SamplingParameters sampling = samplingParameters(vm, smoothingParams);
 
    ARMARX_IMPORTANT << "Near-obstacle endpoints drawn from clearance ["
                     << sampling.nearObstacleMinClearance << ", "
                     << sampling.nearObstacleMaxClearance << "] mm.";
 
    std::vector<evaluation::Request> requests;
    for (const auto& [kind, count] :
         {std::pair{evaluation::RequestKind::Uniform, vm["uniform"].as<int>()},
          std::pair{evaluation::RequestKind::NearObstacle, vm["near-obstacle"].as<int>()},
          std::pair{evaluation::RequestKind::Location, vm["locations"].as<int>()},
          std::pair{evaluation::RequestKind::CorridorExit, vm["corridor-exits"].as<int>()}})
    {
        if (count <= 0)
        {
            continue;
        }
 
        const std::vector<evaluation::Request> sampled =
            evaluation::sampleRequests(kind, count, costmap, roomScene, sampling, rng);
        requests.insert(requests.end(), sampled.begin(), sampled.end());
    }
 
    ARMARX_IMPORTANT << "Planning " << requests.size() << " requests ...";
 
    const std::filesystem::path outFilename = vm["out"].as<std::string>();
    std::ofstream out{outFilename};
    if (not out.good())
    {
        throw std::runtime_error{"Cannot write to `" + outFilename.string() + "`."};
    }
 
    const std::string dumpPath = vm["dump-scene"].as<std::string>();
    std::optional<PathDump> dump;
    if (not dumpPath.empty())
    {
        dump.emplace(config.scene, vm["dump-count"].as<int>());
    }
 
    std::vector<evaluation::Record> records;
    records.reserve(requests.size());
 
    for (const evaluation::Request& request : requests)
    {
        const auto before = std::chrono::steady_clock::now();
 
        std::optional<global_planning::GlobalPlannerResult> result;
        try
        {
            result = planner.plan(request.start, request.goal);
        }
        catch (const std::exception& e)
        {
            // One unplannable request must not end the sweep -- the interesting sweeps are
            // exactly the ones that provoke failures.
            ARMARX_WARNING << "Request " << request.index << " (" << toString(request.kind)
                           << ") threw: " << e.what();
        }
 
        const float seconds =
            std::chrono::duration<float>(std::chrono::steady_clock::now() - before).count();
 
        if (dump.has_value() and result.has_value())
        {
            dump->add(request, result.value());
        }
 
        evaluation::Record record = evaluation::measure(request, result, costmap, seconds);
        evaluation::writeRecord(out, record);
        records.push_back(std::move(record));
    }
 
    out.flush();
 
    if (dump.has_value())
    {
        dump->write(dumpPath);
    }
 
    ARMARX_IMPORTANT << "Wrote " << records.size() << " records to `" << outFilename.string()
                     << "`.";
 
    // Against the limit the gate enforces (`min_clearance`), not the target the smoother's
    // obstacle barrier aims for (`clearance`).
    std::cout << evaluation::summarize(records, static_cast<float>(smoothingParams.min_clearance))
              << std::endl;
 
    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    ARMARX_ERROR << e.what();
    return EXIT_FAILURE;
}