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::GeneralConfig& generalConfig,
                                         const core::Scene& scene) :
        LocalPlanner(scene, generalConfig), params(i_params), 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
        // read config from .json file into dto
        readConfig(params.cfg, robotName);
        // write from dto to teb config file
        toTebCfg(params.cfg, cfg_);
 
        // overwrite specific config values
        {
            // cfg_.trajectory.allow_init_with_backwards_motion = true;
            // FIXME cfg_.trajectory.global_plan_overwrite_orientation = true;
            // cfg_.robot.min_turning_radius = 0;
            cfg_.robot.cmd_angle_instead_rotvel = false;
            // FIXME check cfg_.robot.use_proportional_saturation = false;
            cfg_.optim.weight_kinematics_nh = 0;
            cfg_.optim.weight_kinematics_forward_drive = 0;
            cfg_.optim.weight_kinematics_turning_radius = 0;
 
            // cfg_.goal_tolerance.xy_goal_tolerance = 0.001;
            // cfg_.goal_tolerance.yaw_goal_tolerance = 0.001;
            cfg_.goal_tolerance.free_goal_vel = false;
 
            // cfg_.obstacles.min_obstacle_dist = 0.5;
        }
 
        // apply max velocity from generalConfig
        {
            cfg_.robot.max_vel_x = generalConfig.maxVel.linear / 1000; // [mm/s] to [m/s]
            cfg_.robot.max_vel_x_backwards = generalConfig.maxVel.linear / 1000; // [mm/s] to [m/s]
            cfg_.robot.max_vel_y = generalConfig.maxVel.linear / 1000; // [mm/s] to [m/s]
            cfg_.robot.max_vel_trans = generalConfig.maxVel.linear / 1000; // [mm/s] to [m/s]
            cfg_.robot.max_vel_theta = generalConfig.maxVel.angular;
        }
 
        auto robot_model = boost::make_shared<teb_local_planner::CircularRobotFootprint>(
            params.cfg.robot_footprint_radius);
 
        ARMARX_INFO << "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 " << QUOTED(robotSpecificConfigPath)
                            << ".";
                return robotSpecificConfigPath;
            }
 
            const armarx::PackagePath defaultConfigPath(
                "armarx_navigation", "local_planner_config/TimedElasticBands/default.json");
            ARMARX_INFO << "Using default config " << QUOTED(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 " << QUOTED(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()};
        ARMARX_DEBUG << VAROUT(currentPose.translation().head<2>());
 
        // prune global trajectory
        const auto [prunedGoal, planToDest] = goal.getSubTrajectory(
            currentPose.translation(), params.cfg.planning_distance * 1000); // [m] to [mm]
 
        ARMARX_DEBUG << prunedGoal.points().size() << " waypoints in pruned goal";
 
        for (const auto& pt : prunedGoal.points())
        {
            ARMARX_DEBUG << pt.waypoint.pose.translation().head<2>();
        }
 
        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);
        // velocity_start.linear.x = 0.1; // FIXME remove
 
 
        fillObstacles();
 
        // TODO differentiate between exception and failure (e.g. no trajectory could be generated due to obstacles)
        const auto runTebOptimization =
            [&hpc = this->hcp_, &start, &end, &velocity_start, &planToDest]() -> bool
        {
            bool success = false;
            try
            {
                success = hpc->plan(start, end, &velocity_start, !planToDest);
            }
            catch (...)
            {
                ARMARX_ERROR << "Caught exception while planning: " << GetHandledExceptionString();
            }
 
            return success;
        };
 
        if (not runTebOptimization())
        {
            // try to recover
            ARMARX_VERBOSE
                << "Failed to plan trajectory. Trying to recover by replanning from scratch.";
            hcp_->clearPlanner();
 
            if (not runTebOptimization())
            {
                ARMARX_WARNING << "Failed to replan trajectory.";
                return std::nullopt;
            }
        }
 
 
        ARMARX_INFO << VAROUT(hcp_->hasDiverged());
 
        // if (not 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_INFO << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
                    << " Trajectories)";
 
        const auto timestamp = armarx::Clock::Now(); // FIXME
 
        const ::teb_local_planner::TebOptimalPlannerPtr tebResult = hcp_->findBestTeb();
        ARMARX_CHECK_NOT_NULL(tebResult);
 
        core::LocalTrajectory best = conv::fromRos(tebResult->teb(), timestamp);
 
        // a lot of false positives,
        // e.g. if the scene doesn't change in a meaningful way the trajectory won't either
        // if (lastTrajectory.has_value())
        // {
        //     if (not best.isApproximatelyEqual(lastTrajectory.value(), 1))
        //     {
 
        //         ARMARX_WARNING << "Trajectory did not change";
        //         hcp_->clearPlanner();
 
        //         lastTrajectory = std::nullopt;
 
        //         // try to recover from diverged state
        //         {
        //             ARMARX_INFO << "Trying to recover by replanning from scratch.";
        //             hcp_->clearPlanner();
 
        //             if (not runTebOptimization())
        //             {
        //                 // Recovery failed. Can't do anything more.
        //                 ARMARX_WARNING << "Failed to replan trajectory.";
        //                 return std::nullopt;
        //             }
 
        //             ARMARX_INFO << "Successfully recovered by replanned trajectory.";
 
        //             const ::teb_local_planner::TebOptimalPlannerPtr tebResult = hcp_->findBestTeb();
        //             ARMARX_CHECK_NOT_NULL(tebResult);
 
        //             best = conv::fromRos(tebResult->teb(), timestamp);
        //             lastTrajectory = best;
        //         }
 
        //         // check again whether the trajectory as changed
        //     }
        //     else
        //     {
        //         lastTrajectory = best;
        //     }
        // }
        // else
        // {
        //     // initialize lastTrajectory
        //     lastTrajectory = best;
        // }
 
        ARMARX_INFO << VAROUT(hcp_->bestTebIdx());
 
        ARMARX_INFO << VAROUT((best.points().front().pose.translation().head<2>() -
                               currentPose.translation().head<2>())
                                  .norm());
 
        // 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(), timestamp);
                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