Navigator.cpp

Go to the documentation of this file.

#include "Navigator.h"
 
#include <chrono>
#include <cstdint>
#include <functional>
#include <future>
#include <mutex>
#include <string>
#include <vector>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::client
{
 
    Navigator::Navigator(const InjectedServices& services) : srv{services}
    {
        ARMARX_TRACE;
        // these checks cannot be used when component is started in property-readout mode
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        ARMARX_CHECK_NOT_NULL(srv.subscriber) << "Subscriber service must not be null!";
 
        auto stopped_callback = [this](const auto& e) { stopped(StopEvent(e)); };
 
        srv.subscriber->onGoalReached(stopped_callback);
        srv.subscriber->onSafetyStopTriggered(stopped_callback);
        srv.subscriber->onUserAbortTriggered(stopped_callback);
        srv.subscriber->onInternalError(stopped_callback);
        srv.subscriber->onGlobalPlanningFailed(stopped_callback);
    }
 
    void
    Navigator::moveTo(const core::Pose& pose, core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        moveTo(std::vector<core::Pose>{pose}, frame);
    }
 
    void
    Navigator::moveTo(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        {
            // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
            //  after the event is reset
            std::scoped_lock const l{stoppedInfo.m};
            stoppedInfo.event.reset();
        }
        forward("moveTo", [&] { srv.navigator->moveTo(waypoints, frame); });
    }
 
    void
    Navigator::moveTo(const PathBuilder& builder, core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
 
        const std::vector<WaypointTarget>& path = builder.path();
        validate(path);
 
        {
            // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
            //  after the event is reset
            std::scoped_lock const l{stoppedInfo.m};
            stoppedInfo.event.reset();
        }
        forward("moveTo", [&] { srv.navigator->moveTo(path, frame); });
    }
 
    void
    Navigator::moveToAlternatives(const std::vector<core::TargetAlternative>& targets,
                                  core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
 
        {
            // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
            //  after the event is reset
            std::scoped_lock const l{stoppedInfo.m};
            stoppedInfo.event.reset();
        }
        forward("moveToAlternatives",
                [&] { srv.navigator->moveToAlternatives(targets, frame); });
    }
 
    void
    Navigator::update(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        ;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        forward("update", [&] { srv.navigator->update(waypoints, frame); });
    }
 
    void
    Navigator::moveTowards(const core::Direction& direction, core::NavigationFrame frame)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
 
        {
            // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
            //  after the event is reset
            std::scoped_lock const l{stoppedInfo.m};
            stoppedInfo.event.reset();
        }
        forward("moveTowards", [&] { srv.navigator->moveTowards(direction, frame); });
    }
 
    void
    Navigator::moveToLocation(const std::string& location,
                              const std::optional<std::string>& providerName)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
 
        {
            // TODO: This still leads to a race condition, if extern a stop event is generated before moveTo but arrives
            //  after the event is reset
            std::scoped_lock const l{stoppedInfo.m};
            stoppedInfo.event.reset();
        }
        forward("moveToLocation",
                [&] { srv.navigator->moveToLocation(location, providerName); });
    }
 
    void
    Navigator::setVelocityFactor(float velocityFactor)
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        srv.navigator->setVelocityFactor(velocityFactor);
    }
 
    void
    Navigator::pause()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        srv.navigator->pause();
    }
 
    void
    Navigator::resume()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        srv.navigator->resume();
    }
 
    void
    Navigator::stop()
    {
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(srv.navigator) << "Navigator service must not be null!";
        forward("stop", [&] { srv.navigator->stop(); });
    }
 
    void
    Navigator::onGoalReached(const std::function<void(void)>& callback)
    {
        ARMARX_TRACE;
        onGoalReached([&callback](const core::GoalReachedEvent&) { callback(); });
    }
 
    void
    Navigator::onGoalReached(const std::function<void(const core::GoalReachedEvent&)>& callback)
    {
    }
 
    void
    Navigator::onWaypointReached(const std::function<void(int)>& callback)
    {
    }
 
    StopEvent
    Navigator::waitForStop(const std::int64_t timeoutMs)
    {
        ARMARX_TRACE;
 
        std::future<void> future = std::async(
            std::launch::async,
            [&]()
            {
                std::unique_lock l{stoppedInfo.m};
                stoppedInfo.cv.wait(l, [&i = stoppedInfo] { return i.event.has_value(); });
            });
 
 
        if (timeoutMs > 0)
        {
            ARMARX_VERBOSE << "future.wait()";
            auto status = future.wait_for(std::chrono::milliseconds(timeoutMs));
            ARMARX_VERBOSE << "done";
 
            switch (status)
            {
                case std::future_status::ready:
                    ARMARX_INFO << "waitForStop: terminated on goal reached";
                    break;
                case std::future_status::timeout:
                    ARMARX_INFO << "waitForStop: terminated due to timeout";
                    ARMARX_INFO << "Stopping robot due to timeout";
                    stop();
 
                    throw LocalException("Navigator::waitForStop: timeout");
                    break;
                case std::future_status::deferred:
                    ARMARX_INFO << "waitForStop: deferred";
                    break;
            }
        }
        else
        {
            ARMARX_VERBOSE << "future.wait()";
            future.wait();
            ARMARX_VERBOSE << "done";
        }
 
        // only due to timeout, stoppedInfo.event should be nullopt
        ARMARX_CHECK(stoppedInfo.event.has_value());
 
        StopEvent e = stoppedInfo.event.value();
        stoppedInfo.event.reset();
 
        return e;
    }
 
    void
    Navigator::forward(const char* what, const std::function<void()>& call)
    {
        try
        {
            call();
        }
        catch (const std::exception& e)
        {
            // Without a server there will be no stop event; unblock waitForStop() locally.
            ARMARX_WARNING << "Navigator call `" << what << "` failed: " << e.what();
            stopped(StopEvent(core::InternalErrorEvent{
                {armarx::Clock::Now()}, core::Pose::Identity(), std::string(what) + ": " + e.what()}));
        }
    }
 
    void
    Navigator::stopped(const StopEvent& e)
    {
        ARMARX_TRACE;
        {
            std::scoped_lock l{stoppedInfo.m};
            stoppedInfo.event = e;
        }
        stoppedInfo.cv.notify_all();
    }
 
} // namespace armarx::navigation::client