NavigatingSkillHelper.cpp

Go to the documentation of this file.

#include "NavigatingSkillHelper.h"
 
#include <memory>
#include <mutex>
#include <optional>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/core/UserException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/core/GoalReachedConfig.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/global_planning/AStarWithOrientation.h>
#include <armarx/navigation/global_planning/Point2Point.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/local_planning/TimedElasticBandsParams.h>
#include <armarx/navigation/safety_guard/LaserBasedProximity.h>
#include <armarx/navigation/skills/aron/NavigatingSkillParams.aron.generated.h>
#include <armarx/navigation/skills/skill_ids.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
 
namespace armarx::navigation::skills
{
 
    NavigatingSkillHelper::NavigatingSkillHelper(const Properties& properties,
                                                 const Services& srv) :
        properties_(properties)
    {
        srv_.emplace(srv);
    }
 
    namespace
    {
        void
        instantiateGlobalPlannerParams(
            std::unique_ptr<armarx::navigation::global_planning::GlobalPlannerParams>& params,
            const arondto::NavigatingSkillParams& skillParams)
        {
            switch (skillParams.globalPlanningAlgorithm.value)
            {
                case arondto::GlobalPlanningAlgorithm::ImplEnum::AStar:
                    ARMARX_INFO << "Using " << VAROUT("AStarParams")
                                << " global planning strategy.";
                    params = std::make_unique<armarx::navigation::global_planning::AStarParams>();
                    break;
                case arondto::GlobalPlanningAlgorithm::ImplEnum::AStarWithOrientation:
                    ARMARX_INFO << "Using " << VAROUT("AStarWithOrientationParams")
                                << " global planning strategy.";
                    params = std::make_unique<
                        armarx::navigation::global_planning::AStarWithOrientationParams>();
                    break;
                case arondto::GlobalPlanningAlgorithm::ImplEnum::Point2Point:
                {
                    ARMARX_INFO << "Using " << VAROUT("Point2Point")
                                << " global planning strategy.";
 
                    armarx::navigation::global_planning::Point2PointParams p;
                    p.checkCollisionsAlongTrajectory =
                        skillParams.p2pCheckCollisionsAlongTrajectory;
 
                    params =
                        std::make_unique<armarx::navigation::global_planning::Point2PointParams>(p);
                    break;
                }
                case arondto::GlobalPlanningAlgorithm::ImplEnum::SPFA:
                    ARMARX_INFO << "Using " << VAROUT("SPFA") << " global planning strategy.";
                    params = std::make_unique<armarx::navigation::global_planning::SPFAParams>();
                    break;
                default:
                    ARMARX_ERROR << "Algorithm selection incomplete";
                    throw armarx::InvalidArgumentException(
                        "Tried to instantiate global planner but parameter selection logic is "
                        "missing the planner type");
                    break;
            }
        }
    } // namespace
 
    void
    NavigatingSkillHelper::init(const arondto::NavigatingSkillParams& params, const std::string& id)
    {
        //armarx::navigation::global_planning::AStarWithOrientationParams globalPlannerParams{};
        std::unique_ptr<armarx::navigation::global_planning::GlobalPlannerParams>
            globalPlannerParams;
        instantiateGlobalPlannerParams(globalPlannerParams, params);
 
        armarx::navigation::local_planning::TimedElasticBandsParams localPlannerParams{};
        armarx::navigation::safety_guard::LaserBasedProximityParams safetyGuardParams{};
        {
            ARMARX_CHECK_NOT_NULL(properties_.safetyGuardParamsMutex);
            std::lock_guard g{*properties_.safetyGuardParamsMutex};
            if (properties_.safetyGuardParams != nullptr)
            {
                safetyGuardParams = *properties_.safetyGuardParams;
            }
        }
        safetyGuardParams.ignoredRegions =
            params.safetyGuardIgnoredRegions |
            ranges::views::transform(
                [](const core::arondto::BoundingBox2D& bbDto) -> Eigen::AlignedBox2f
                {
                    Eigen::AlignedBox2f bbBo;
                    core::fromAron(bbDto, bbBo);
                    return bbBo;
                }) |
            ranges::to_vector;
 
 
        core::GeneralConfig generalParams{};
        // load defaults from skill provider
        generalParams = properties_.defaultGeneralConfig;
 
        // ramping
        if (params.enableRampingStart.has_value())
        {
            generalParams.enableRampingStart = params.enableRampingStart.value();
        }
        if (params.enableRampingEnd.has_value())
        {
            generalParams.enableRampingEnd = params.enableRampingEnd.value();
        }
        if (params.enableRampingCorners.has_value())
        {
            generalParams.enableRampingCorners = params.enableRampingCorners.value();
        }
        if (params.rampLength.has_value())
        {
            generalParams.rampLength = params.rampLength.value();
        }
        if (params.cornerVelocity.has_value())
        {
            generalParams.cornerVelocity = params.cornerVelocity.value();
        }
        if (params.boundaryVelocity.has_value())
        {
            generalParams.boundaryVelocity = params.boundaryVelocity.value();
        }
        if (params.cornerLimit.has_value())
        {
            generalParams.cornerLimit = params.cornerLimit.value();
        }
 
        if (params.inCollisionDistanceThresholdForRecovery.has_value())
        {
            generalParams.inCollisionDistanceThresholdForRecovery =
                params.inCollisionDistanceThresholdForRecovery.value();
        }
        ARMARX_INFO << "Collision recovery threshold: "
                    << generalParams.inCollisionDistanceThresholdForRecovery << " mm";
 
        generalParams.navigateCloseAsPossible = params.navigateCloseAsPossible;
        ARMARX_INFO << "Navigate close as possible: " 
                   << (generalParams.navigateCloseAsPossible ? "enabled" : "disabled");
 
        retryTimeout_ = armarx::core::time::Duration::SecondsDouble(params.retryTimeout);
        ARMARX_INFO << "Re-planning after a failed global plan: "
                    << (retryTimeout_.toSecondsDouble() > 0
                            ? "enabled (budget " + std::to_string(retryTimeout_.toSecondsDouble()) +
                                  "s)"
                            : std::string("disabled"));
 
        if (generalParams.enableRampingStart or generalParams.enableRampingEnd or
            generalParams.enableRampingCorners)
        {
            ARMARX_INFO << "Velocity ramping is enabled (start=" << generalParams.enableRampingStart
                        << "; end=" << generalParams.enableRampingEnd
                        << "; corners=" << generalParams.enableRampingCorners
                        << "; rampLength=" << generalParams.rampLength
                        << "; cornerVelocity=" << generalParams.cornerVelocity
                        << "; boundaryVelocity=" << generalParams.boundaryVelocity
                        << "; cornerLimit=" << generalParams.cornerLimit << ").";
        }
 
        if (params.velocityLimitLinear.has_value())
        {
            generalParams.maxVel.linear = params.velocityLimitLinear.value();
        }
        if (params.velocityLimitAngular.has_value())
        {
            generalParams.maxVel.angular = params.velocityLimitAngular.value();
        }
        if (params.trajectoryParametrization.has_value())
        {
            core::fromAron(params.trajectoryParametrization.value(),
                           generalParams.parametrization);
        }
 
        ARMARX_INFO << "Configuring navigator to respect the velocity limits:";
        ARMARX_INFO << "linear: " << generalParams.maxVel.linear;
        ARMARX_INFO << "angular: " << generalParams.maxVel.angular;
 
        // parameterize the navigation stack
        client::NavigationStackConfig cfg;
        cfg.general(generalParams);
        cfg.globalPlanner(*globalPlannerParams);
 
        if (params.enableLocalPlanning)
        {
            cfg.localPlanner(localPlannerParams);
        }
 
        if (params.enableSafetyGuard)
        {
            cfg.safetyGuard(safetyGuardParams);
        }
 
        // goal reached config - optional override from skill params
        if (params.goalReachedThresholds.has_value())
        {
            core::GoalReachedConfig goalReachedCfg;
            goalReachedCfg.posTh = params.goalReachedThresholds->posTh;
            goalReachedCfg.oriTh = params.goalReachedThresholds->oriTh;
            goalReachedCfg.linearVelTh = params.goalReachedThresholds->linearVelTh;
            goalReachedCfg.angularVelTh = params.goalReachedThresholds->angularVelTh;
            goalReachedCfg.filterCount = 5; // Use default filter count
 
            ARMARX_INFO << "Goal reached config override: posTh=" << goalReachedCfg.posTh
                        << "mm, oriTh=" << goalReachedCfg.oriTh << "rad, linearVelTh="
                        << goalReachedCfg.linearVelTh << "mm/s, angularVelTh="
                        << goalReachedCfg.angularVelTh << "rad/s";
 
            cfg.goalReached(goalReachedCfg);
        }
 
        // configure the `navigator` which provides a simplified and typed interface to the navigation server
        ARMARX_TRACE;
        memorySubscriber.reset();
 
        ARMARX_TRACE;
        memorySubscriber.emplace(id, srv_->memoryNameSystem);
 
        // register our config
        ARMARX_INFO << "Registering config";
        iceNavigator = srv_->iceNavigatorFactory.createConfig(cfg, id);
 
        navigator.emplace(client::Navigator::InjectedServices{
            .navigator = iceNavigator.get(), .subscriber = &memorySubscriber.value()});
    }
 
    std::optional<client::Navigator>&
    NavigatingSkillHelper::getNavigator()
    {
        return navigator;
    }
 
    arondto::NavigatingSkillParams
    NavigatingSkillHelper::DefaultSkillDescription()
    {
        arondto::NavigatingSkillParams defaultParameters;
 
        defaultParameters.enableRampingStart = std::nullopt;
        defaultParameters.enableRampingEnd = std::nullopt;
        defaultParameters.enableRampingCorners = std::nullopt;
        defaultParameters.rampLength = std::nullopt;
        defaultParameters.cornerVelocity = std::nullopt;
        defaultParameters.boundaryVelocity = std::nullopt;
        defaultParameters.cornerLimit = std::nullopt;
 
        defaultParameters.inCollisionDistanceThresholdForRecovery = std::nullopt;
 
        defaultParameters.navigateCloseAsPossible = false;
 
        // No retrying by default: a failed global plan fails the skill right away.
        defaultParameters.retryTimeout = 0.0F;
 
        defaultParameters.enableLocalPlanning = false;
        defaultParameters.enableSafetyGuard = true;
        defaultParameters.safetyGuardIgnoredRegions =
            std::vector<armarx::navigation::core::arondto::BoundingBox2D>();
        defaultParameters.velocityLimitAngular = std::nullopt;
        defaultParameters.velocityLimitLinear = std::nullopt;
 
        // Unset: the skill provider's own default applies unless the caller overrides it.
        defaultParameters.trajectoryParametrization = std::nullopt;
        defaultParameters.globalPlanningAlgorithm = arondto::GlobalPlanningAlgorithm::SPFA;
        defaultParameters.p2pCheckCollisionsAlongTrajectory = false;
 
        // Goal reached thresholds - optional, defaults to nullopt (use component defaults)
        defaultParameters.goalReachedThresholds = std::nullopt;
 
        return defaultParameters;
    }
} // namespace armarx::navigation::skills