NavigateToAlternatives.cpp

Go to the documentation of this file.

#include "NavigateToAlternatives.h"
 
#include <chrono>
#include <future>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
 
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/SPFA.h>
#include <armarx/navigation/trajectory_control/local/TrajectoryFollowingController.h>
 
namespace armarx::navigation::skills
{
    NavigateToAlternatives::NavigateToAlternatives(
        const NavigatingSkillHelper::Properties& properties,
        const NavigatingSkillHelper::Services& srv) :
        Base(DefaultSkillDescription()), helper(properties, srv)
    {
    }
 
    ::armarx::skills::Skill::InitResult
    NavigateToAlternatives::init(const Base::SpecializedInitInput& in)
    {
        helper.init(in.parameters.navigatingSkillParams,
                    DefaultSkillDescription().skillId.skillName);
 
        return ::armarx::skills::Skill::InitResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    ::armarx::skills::Skill::MainResult
    NavigateToAlternatives::main(const Base::SpecializedMainInput& in)
    {
        std::vector<core::TargetAlternative> targetAlternatives;
        for (const auto& t : in.parameters.targetAlternatives)
        {
            core::TargetAlternative bo;
            core::fromAron(t, bo);
            targetAlternatives.emplace_back(bo);
        }
 
        ARMARX_INFO << "moving to " << targetAlternatives.size() << " alternatives";
 
        // execute
        ARMARX_INFO << "Sending navigation request";
        // Wait until goal is reached, re-planning while the retry budget lasts
        auto se = helper.moveAndWaitForStop(
            [&]
            {
                helper.getNavigator()->moveToAlternatives(targetAlternatives,
                                                          core::NavigationFrame::Absolute);
            },
            [this] { return shouldSkillTerminate(); });
        if (se)
        {
            ARMARX_INFO << "Goal reached.";
        }
        else
        {
            if (se.isSafetyStopTriggeredEvent())
            {
                ARMARX_ERROR << "Safety stop was triggered!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isUserAbortTriggeredEvent())
            {
                ARMARX_ERROR << "Aborted by user!";
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isInternalErrorEvent())
            {
                ARMARX_ERROR << "Unknown internal error occured! "
                             << se.toInternalErrorEvent().message;
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
            if (se.isGlobalPlanningFailedEvent())
            {
                ARMARX_ERROR << "Global planning failed! "
                             << se.toGlobalPlanningFailedEvent().message;
 
                return ::armarx::skills::Skill::MainResult{
                    .status = ::armarx::skills::TerminatedSkillStatus::Failed};
            }
 
            ARMARX_ERROR << "Unknown event!";
            return ::armarx::skills::Skill::MainResult{
                .status = ::armarx::skills::TerminatedSkillStatus::Failed};
        }
 
 
        return ::armarx::skills::Skill::MainResult{
            .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
    void
    NavigateToAlternatives::onStopRequested()
    {
        if (helper.getNavigator().has_value())
        {
            ARMARX_CHECK(helper.getNavigator().has_value());
            helper.getNavigator()->stop();
        }
    }
 
    armarx::skills::SkillDescription
    NavigateToAlternatives::DefaultSkillDescription()
    {
        ParamType defaultParameters;
        defaultParameters.targetAlternatives = {};
        defaultParameters.navigatingSkillParams = NavigatingSkillHelper::DefaultSkillDescription();
 
        return armarx::skills::SkillDescription{
            .skillId = {.skillName = constants::skill_names::NavigateToAlternatives},
            .description = "Navigate to a list of prioritized target alternatives.",
            .rootProfileDefaults = defaultParameters.toAron(),
            .timeout = armarx::core::time::Duration::Hours(1),
            .parametersType = Params::ToAronType()};
    }
 
} // namespace armarx::navigation::skills