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/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2026
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <optional>
#include <string>
 
#include <boost/program_options.hpp>
 
#include <SimoxUtility/json/json.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/simulation/TrajectoryFollowingSimulation.h>
 
#include "Scene.h"
#include "io.h"
#include "reparametrization.h"
 
namespace po = boost::program_options;
 
using namespace armarx::navigation;
 
int
main(int argc, char** argv)
try
{
    po::options_description desc("Allowed options");
 
    // clang-format off
    desc.add_options()
        ("help,h", "produce help message")
        ("scene", po::value<std::string>()->required(), "Scene description (JSON).")
        ("out", po::value<std::string>()->default_value("result.json"), "Output file (JSON).")
        ;
    // 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);
 
    const std::filesystem::path sceneFilename = vm["scene"].as<std::string>();
    const std::filesystem::path outFilename = vm["out"].as<std::string>();
 
    ARMARX_INFO << "Reading scene from `" << sceneFilename.string() << "`.";
    const analysis::Config config = analysis::readConfig(sceneFilename);
 
    const algorithms::Costmap costmap = analysis::buildCostmap(config);
 
    // SPFAImpl is the actual pipeline used by the navigation stack (grid search, resampling,
    // smoothing, orientation optimization, obstacle-aware velocities). Unlike the SPFA wrapper
    // it needs no core::Scene, so it can be driven from a synthetic costmap.
    global_planning::SPFAImpl planner(config.plannerParams, config.generalConfig, costmap);
 
    ARMARX_INFO << "Planning ...";
    // Not const: the reparametrization below replaces the trajectory's velocities.
    std::optional<global_planning::GlobalPlannerResult> result =
        planner.plan(config.start, config.goal);
 
    if (result.has_value())
    {
        ARMARX_IMPORTANT << "Planning succeeded: " << result->trajectory.points().size()
                         << " points, " << result->trajectory.length() << " mm.";
    }
    else
    {
        ARMARX_WARNING << "Planning failed. Writing costmap only.";
    }
 
    // TOPPRA's time-parametrized reference, kept so the plots can compare it against what the
    // simulated controller executed. Null for the other modes.
    nlohmann::json reference = nullptr;
 
    // Re-assign the velocities along the path before simulating. This is what the three modes
    // compare: the planner's raw obstacle-aware velocities, the robot's ramping, or TOPPRA.
    // Wall-clock cost of the parametrization. The planner's own stage timings stop at plan(),
    // so without this the seconds TOPP-RA spends are invisible next to its 15 ms of planning.
    double parametrizationSeconds = 0.0;
 
    if (result.has_value() and
        config.parametrization != core::TrajectoryParametrization::None)
    {
        ARMARX_IMPORTANT << "Parametrization: " << analysis::toString(config.parametrization);
 
        analysis::ReparametrizationResult reparametrized =
            analysis::reparametrize(result->trajectory, config);
 
        result->trajectory = std::move(reparametrized.trajectory);
        reference = std::move(reparametrized.reference);
        parametrizationSeconds = reparametrized.seconds;
    }
 
    // Whatever assigned the velocities -- the planner, applyRamping or TOPPRA -- none of them
    // is aware of the device command ramp, so check the result against it before simulating.
    analysis::CommandRampCheck rampCheck;
    if (result.has_value())
    {
        rampCheck = analysis::checkAgainstCommandRamp(result->trajectory,
                                                      config.simulationParams.rateLimit,
                                                      config.generalConfig.boundaryVelocity);
    }
 
    // Simulate the low-level controller following the planned trajectory. The planned velocity
    // profile says nothing about the accelerations the base is asked for; the simulation does.
    std::optional<simulation::TrajectoryFollowingSimulation::Result> simulated;
    if (result.has_value() and config.simulate)
    {
        ARMARX_INFO << "Simulating trajectory following ...";
        const simulation::TrajectoryFollowingSimulation sim(config.simulationParams);
        simulated = sim.run(result->trajectory, config.start);
    }
 
    // Verify against the planner's own limit, not a reimplementation of it.
    analysis::writeResult(
        outFilename, config, costmap, result, planner.velocityLimit(), simulated, reference,
        rampCheck, parametrizationSeconds);
    ARMARX_IMPORTANT << "Wrote `" << outFilename.string() << "`.";
 
    return EXIT_SUCCESS;
}
catch (const std::exception& e)
{
    // Without this the process aborts and the reason is buried under the runtime's
    // `terminate called after throwing an instance of ...`, which reaches the caller as a
    // bare exit code.
    ARMARX_ERROR << e.what();
 
    return EXIT_FAILURE;
}