NavigateToLocation.cpp

Go to the documentation of this file.

#include "NavigateToLocation.h"
 
#include <chrono>
#include <future>
#include <mutex>
#include <optional>
#include <sstream>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/services/IceNavigator.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/safety_guard/LaserBasedProximity.h>
#include <armarx/navigation/skills/skill_ids.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
 
namespace armarx::navigation::skills
{
 
    template <typename R>
    bool
    is_ready(std::future<R> const& f)
    {
        if (!f.valid())
        {
            return true;
        }
        std::future_status status = f.wait_for(std::chrono::seconds(0));
        if (status == std::future_status::ready)
        {
            return true;
        }
        else if (status == std::future_status::deferred)
        {
            // if the task has not been started --> start the task
            f.wait();
            return true;
        }
        else
        {
            return false;
        }
    }
 
    armarx::skills::SkillDescription
    NavigateToLocation::DefaultSkillDescription()
    {
        ParamType defaultParameters;
        defaultParameters.location = "";
        defaultParameters.enableSafetyGuard = false;
        defaultParameters.velocityLimitAngular = std::nullopt;
        defaultParameters.velocityLimitLinear = std::nullopt;
 
        std::stringstream description;
        description
            << "Retrieve the location `location` from the memory and navigate to it."
            << "\n\nThe `location` is specified by its provider segment name and entity name"
               " in the format `providerSegmentName/entityName` .";
 
        return armarx::skills::SkillDescription{
            .skillId = skill_ids::NavigateToLocation,
            .description = description.str(),
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Hours(1),
            .parametersType = Params::ToAronType(),
        };
    }
 
    NavigateToLocation::NavigateToLocation(const Properties& properties, const Services& srv) :
        Base(DefaultSkillDescription()), properties_(properties)
    {
        srv_.emplace(srv);
    }
 
    ::armarx::skills::Skill::InitResult
    NavigateToLocation::init(const Base::SpecializedInitInput& in)
    {
        armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams
            trajControllerParams{};
        armarx::navigation::global_planning::SPFAParams globalPlannerParams{};
 
        client::GeneralConfig generalParams{};
 
        if (in.parameters.velocityLimitLinear.has_value())
        {
            trajControllerParams.limits.linear = in.parameters.velocityLimitLinear.value();
            globalPlannerParams.linearVelocity = in.parameters.velocityLimitLinear.value();
            generalParams.maxVel.linear = in.parameters.velocityLimitLinear.value();
        }
 
        if (in.parameters.velocityLimitAngular.has_value())
        {
            trajControllerParams.limits.angular = in.parameters.velocityLimitAngular.value();
            globalPlannerParams.angularVelocity = in.parameters.velocityLimitAngular.value();
            generalParams.maxVel.angular = in.parameters.velocityLimitAngular.value();
        }
 
        ARMARX_INFO << "Configuring navigator to respect the velocity limits:";
        ARMARX_INFO << "linear: " << trajControllerParams.limits.linear;
        ARMARX_INFO << "angular: " << trajControllerParams.limits.angular;
 
        // FIXME set
        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;
            }
        }
 
        // parameterize the navigation stack
        client::NavigationStackConfig cfg;
        cfg.general(generalParams);
        cfg.globalPlanner(globalPlannerParams);
        cfg.trajectoryController(trajControllerParams);
        if (in.parameters.enableSafetyGuard)
        {
            cfg.safetyGuard(safetyGuardParams);
        }
 
        const std::string configId = DefaultSkillDescription().skillId.skillName;
 
        // configure the `navigator` which provides a simplified and typed interface to the navigation server
        ARMARX_TRACE;
        memorySubscriber.reset();
 
        ARMARX_TRACE;
        memorySubscriber.emplace(configId, srv_->memoryNameSystem);
 
        // register our config
        ARMARX_INFO << "Registering config";
        srv_->iceNavigator.createConfig(cfg, configId);
 
        navigator.emplace(client::Navigator::InjectedServices{
            .navigator = &srv_->iceNavigator, .subscriber = &memorySubscriber.value()});
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    NavigateToLocation::main(const Base::SpecializedMainInput& in)
    {
        const std::string& location = in.parameters.location;
 
        ARMARX_INFO << "moving to target " << VAROUT(location);
 
        // execute
        ARMARX_INFO << "Sending navigation request";
 
        // client::PathBuilder builder;
        // builder.add(location, client::GlobalPlanningStrategy::Free);
 
        navigator->moveToLocation(location);
 
        // Wait until goal is reached
        ARMARX_INFO << "Waiting until goal is reached.";
        auto future = std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
        while (not is_ready(future))
        {
            if (shouldSkillTerminate())
            {
                ARMARX_INFO << "Skill was aborted by user";
                navigator->stop();
                break;
            }
        }
 
        auto se = future.get();
        if (se)
        {
            ARMARX_INFO << "Goal `" << location << "`reached.";
            return ::armarx::skills::Skill::MainResult{
                .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
        }
        else
        {
            if (se.isSafetyStopTriggeredEvent())
            {
                ARMARX_INFO << "Safety stop was triggered!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isUserAbortTriggeredEvent())
            {
                ARMARX_INFO << "Aborted by user!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Aborted};
            }
            if (se.isInternalErrorEvent())
            {
                ARMARX_INFO << "Unknown internal error occured! "
                            << se.toInternalErrorEvent().message;
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
        }
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Failed};
    }
 
    void
    NavigateToLocation::onStopRequested()
    {
        if (navigator.has_value())
        {
            ARMARX_CHECK(navigator.has_value());
            navigator->stop();
        }
    }
 
} // namespace armarx::navigation::skills