NavigateRelativeSkill.h

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <future>
#include <optional>
#include <string>
 
#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/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
 
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/services/IceNavigator.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/Point2Point.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;
        }
    }
 
    // abstract base class of a relative movement skill
    template <class AronT>
    class NavigateRelativeSkill : public armarx::skills::SimpleSpecializedSkill<AronT>
    {
 
    public:
        using Base = armarx::skills::SimpleSpecializedSkill<AronT>;
 
        struct Services
        {
            client::IceNavigator& iceNavigator;
            armem::client::MemoryNameSystem& memoryNameSystem;
        };
 
        NavigateRelativeSkill() = delete;
 
        NavigateRelativeSkill(const Services& srv, const armarx::skills::SkillDescription& desc) :
            Base(desc)
        {
            srv_.emplace(srv);
        }
 
        virtual ~NavigateRelativeSkill() = default;
 
    private:
        // implemented by children returning the relative movement to execute
        virtual Eigen::Isometry3f relativeTarget(const typename Base::SpecializedMainInput& in) = 0;
 
        armarx::skills::Skill::InitResult
        init(const typename Base::SpecializedInitInput& in) override
        {
            // parameterize the navigation stack
            client::NavigationStackConfig cfg;
            cfg.general(client::GeneralConfig{});
            cfg.globalPlanner(armarx::navigation::global_planning::Point2PointParams{});
            cfg.trajectoryController(
                armarx::navigation::traj_ctrl::local::TrajectoryFollowingControllerParams{});
 
            const std::string configId = this->getSkillId().skillName;
 
            // configure the `navigator` which provides a simplified and typed interface to the navigation server
            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
        main(const typename Base::SpecializedMainInput& in) override
        {
            const Eigen::Isometry3f relativeTarget = this->relativeTarget(in);
 
            ARMARX_INFO << "Moving to relative target " << VAROUT(relativeTarget.matrix());
 
            // excecute
            ARMARX_INFO << "Sending navigation request";
            navigator->moveTo(relativeTarget, core::NavigationFrame::Relative);
 
            // 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 (Base::shouldSkillTerminate())
                {
                    ARMARX_INFO << "Skill was aborted by user " << Base::stopped << ", "
                                << Base::timeoutReached;
                    navigator->stop();
                    break;
                }
            }
 
            auto se = future.get();
            if (se)
            {
                ARMARX_INFO << "Goal `" << relativeTarget.matrix() << "`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};
                }
            }
 
            return armarx::skills::Skill::MainResult{
                .status = armarx::skills::TerminatedSkillStatus::Succeeded};
        }
 
        void
        onStopRequested() override
        {
            if (navigator.has_value())
            {
                ARMARX_CHECK(navigator.has_value());
                navigator->stop();
            }
        }
 
 
    private:
        std::optional<Services> srv_;
 
        // will be initialized in "init()"
        std::optional<client::MemorySubscriber> memorySubscriber;
        std::optional<client::Navigator> navigator;
    };
 
} // namespace armarx::navigation::skills