TimedElasticBands.cpp

Go to the documentation of this file.

#include "TimedElasticBands.h"
 
#include <filesystem>
#include <fstream>
#include <memory>
#include <optional>
#include <string>
#include <vector>
 
#include <boost/smart_ptr/make_shared_object.hpp>
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/CollisionDetection/CollisionModel.h> // IWYU pragma: keep
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/SceneObjectSet.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
#include <RobotAPI/components/ArViz/Client/Layer.h>
#include <RobotAPI/components/ArViz/Client/elements/Path.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/human/types.h>
#include <armarx/navigation/local_planning/LocalPlanner.h>
#include <armarx/navigation/local_planning/aron/TimedElasticBands.aron.generated.h>
#include <armarx/navigation/local_planning/aron_conversions.h>
#include <armarx/navigation/local_planning/aron_conversions_teb.h>
#include <armarx/navigation/local_planning/ros_conversions.h>
 
#include <geometry_msgs/Twist.h>
#include <nlohmann/json_fwd.hpp>
#include <ros/time.h>
#include <teb_local_planner/homotopy_class_planner.h>
#include <teb_local_planner/pose_se2.h>
#include <teb_local_planner/robot_footprint_model.h>
#include <teb_local_planner/timed_elastic_band.h>
#include <teb_local_planner/visualization.h>
 
namespace armarx::navigation::local_planning
{
 
    // TimedElasticBands
 
    TimedElasticBands::TimedElasticBands(const Params& i_params, const core::Scene& scene) :
        LocalPlanner(scene), params(i_params), scene(scene), obstManager(teb_obstacles)
    {
        ARMARX_CHECK_NOT_NULL(scene.robot);
        const auto robotName = scene.robot->getName();
 
        //TODO (SALt): find proper place to init with default config
        readConfig(params.cfg, robotName);
 
        toTebCfg(params.cfg, cfg_);
 
        auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
            params.cfg.robot_footprint_radius);
 
        ARMARX_VERBOSE << "Robot footprint is " << params.cfg.robot_footprint_radius << "m";
 
        hcp_ = std::make_unique<teb_local_planner::HomotopyClassPlanner>();
        hcp_->initialize(
            cfg_, &teb_obstacles, robot_model, teb_local_planner::TebVisualizationPtr(), nullptr);
        //set member teb_costmap
        setTebCostmap();
        if (teb_costmap)
        {
            ARMARX_INFO << "Costmap available.";
            // TODO: where to put all the parameters
            const human::ExponentialPenaltyModel penalty{
                .minDistance = 0.5, .epsilon = 0, .exponent = 1.2};
 
            teb_costmap->setPenaltyModel(conv::toRos(penalty));
            hcp_->setCostmap(&teb_costmap.value());
        }
        ros::Time::init(); // we have to init time before we can use the planner
    }
 
    void
    TimedElasticBands::readConfig(arondto::TimedElasticBandsParams& target,
                                  const std::string& robotName)
    {
 
        const armarx::PackagePath configPath = [&]() -> armarx::PackagePath
        {
            const armarx::PackagePath robotSpecificConfigPath(
                "armarx_navigation",
                "local_planner_config/TimedElasticBands/" + robotName + ".json");
 
            if (std::filesystem::exists(robotSpecificConfigPath.toSystemPath()))
            {
                ARMARX_INFO << "Using robot-specific config `" << robotSpecificConfigPath << "`.";
                return robotSpecificConfigPath;
            }
 
            const armarx::PackagePath defaultConfigPath(
                "armarx_navigation", "local_planner_config/TimedElasticBands/default.json");
            ARMARX_INFO << "Using default config `" << robotSpecificConfigPath << "`";
 
            return defaultConfigPath;
        }();
 
        const std::filesystem::path file = configPath.toSystemPath();
 
        ARMARX_CHECK(std::filesystem::is_regular_file(file)) << file;
 
        ARMARX_INFO << "Loading config from file `" << file << "`.";
        std::ifstream ifs{file};
 
        nlohmann::json jsonConfig;
        ifs >> jsonConfig;
 
        ARMARX_INFO << "Reading config";
 
        armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
        target.read(reader, jsonConfig);
    }
 
    std::optional<LocalPlannerResult>
    TimedElasticBands::plan(const core::GlobalTrajectory& goal)
    {
        const core::Pose currentPose{scene.robot->getGlobalPose()};
 
        // prune global trajectory
        const auto& [prunedGoal, planToDest] = goal.getSubTrajectory(
            currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
 
        const teb_local_planner::TimedElasticBand globalPath = conv::toRos(prunedGoal);
        teb_globalPath = globalPath.poses();
        hcp_->setGlobalPath(&teb_globalPath);
 
        const teb_local_planner::PoseSE2 start = conv::toRos(currentPose);
        const teb_local_planner::PoseSE2 end =
            conv::toRos(prunedGoal.points().back().waypoint.pose);
 
        geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
 
 
        fillObstacles();
 
        bool success;
        try
        {
            success = hcp_->plan(start, end, &velocity_start, !planToDest);
        }
        catch (...)
        {
            hcp_->clearPlanner();
            ARMARX_ERROR << "Caugth exception while planning: " << GetHandledExceptionString();
            return std::nullopt;
        }
 
        ARMARX_INFO << VAROUT(success);
        ARMARX_INFO << VAROUT(hcp_->hasDiverged());
 
        if (!success)
        {
            ARMARX_INFO << "Found trajectory is not feasible!";
            hcp_->clearPlanner();
            ARMARX_WARNING << deactivateSpam(5) << "Found trajectory is not feasible!";
            return std::nullopt;
        }
 
        if (false && hcp_->hasDiverged())
        //if (hcp_->hasDiverged())
        {
            hcp_->clearPlanner();
            ARMARX_WARNING << /*deactivateSpam(5) << */ "Planner has diverged!";
            return std::nullopt;
        }
 
        if (hcp_->getTrajectoryContainer().empty())
        {
            hcp_->clearPlanner();
            ARMARX_INFO << "Did not find any trajectory!";
            return std::nullopt;
        }
 
        ARMARX_VERBOSE << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
                       << " Trajectories)";
 
        core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
 
        // visualize path alternatives
        if (arviz)
        {
            auto layer = arviz.value().layer("local_planner_path_alternatives");
 
            int i = 0;
            int bestTebIdx = hcp_->bestTebIdx();
            for (const auto& teb : hcp_->getTrajectoryContainer())
            {
                if (i == bestTebIdx)
                {
                    continue;
                }
                const core::LocalTrajectory trajectory = conv::fromRos(teb->teb());
                const std::vector<Eigen::Vector3f> points =
                    simox::alg::apply(trajectory.points(),
                                      [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
                                      { return pt.pose.translation(); });
                layer.add(viz::Path("path_alternative_" + std::to_string(i))
                              .points(points)
                              .color(simox::Color::gray()));
                i++;
            }
 
            arviz.value().commit(layer);
        }
 
        return {{.trajectory = best}};
    }
 
    void
    TimedElasticBands::fillObstacles()
    {
        obstManager.clear();
 
        viz::Layer* visPtr = nullptr;
        viz::Layer visLayer;
        if (arviz)
        {
            visLayer = arviz.value().layer("local_planner_obstacles");
            visPtr = &visLayer;
        }
 
 
        if (scene.staticScene)
        {
            ARMARX_CHECK(scene.staticScene.has_value());
            for (const auto& obst : scene.staticScene.value().objects->getCollisionModels())
            {
                obstManager.addBoxObstacle(obst->getGlobalBoundingBox(), visPtr);
            }
        }
        if (scene.dynamicScene)
        {
            ARMARX_CHECK(scene.dynamicScene.has_value());
            for (const auto& obst : scene.dynamicScene.value().humans)
            {
                obstManager.addHumanObstacle(obst, visPtr);
            }
 
            for (const auto& obst : scene.dynamicScene->laserScannerFeatures)
            {
                obstManager.addLaserScannerFeaturesObstacle(obst, visPtr);
            }
        }
 
        if (arviz)
        {
            arviz.value().commit(visLayer);
        }
 
        ARMARX_VERBOSE << "TEB: added " << obstManager.size() << " obstacles";
    }
 
    //export algorithms::Costmap to costmap type of teb local planner and provide costmap to planner
    void
    TimedElasticBands::setTebCostmap()
    {
        if (not scene.staticScene.has_value())
        {
            return;
        }
 
        if (not scene.staticScene->distanceToObstaclesCostmap.has_value())
        {
            return;
        }
 
        return;
 
        const algorithms::Costmap& navigationCostmap =
            scene.staticScene->distanceToObstaclesCostmap.value();
        teb_costmap.emplace(conv::toRos(navigationCostmap));
    }
 
} // namespace armarx::navigation::local_planning