Navigator.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <condition_variable>
#include <cstdint>
#include <functional>
#include <mutex>
#include <optional>
#include <string>
#include <variant>
#include <vector>
 
#include <armarx/navigation/client/services/EventSubscriptionInterface.h>
#include <armarx/navigation/core/NavigatorInterface.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::client
{
 
    class StopEvent
    {
 
    public:
        template <class T>
        StopEvent(T t) : event{t}
        {
        }
 
        bool
        isGoalReachedEvent() const
        {
            return std::holds_alternative<core::GoalReachedEvent>(event);
        }
 
        core::GoalReachedEvent&
        toGoalReachedEvent()
        {
            return std::get<core::GoalReachedEvent>(event);
        }
 
        bool
        isSafetyStopTriggeredEvent() const
        {
            return std::holds_alternative<core::SafetyStopTriggeredEvent>(event);
        }
 
        core::SafetyStopTriggeredEvent&
        toSafetyStopTriggeredEvent()
        {
            return std::get<core::SafetyStopTriggeredEvent>(event);
        }
 
        bool
        isUserAbortTriggeredEvent() const
        {
            return std::holds_alternative<core::UserAbortTriggeredEvent>(event);
        }
 
        core::UserAbortTriggeredEvent&
        toUserAbortTriggeredEvent()
        {
            return std::get<core::UserAbortTriggeredEvent>(event);
        }
 
        bool
        isInternalErrorEvent() const
        {
            return std::holds_alternative<core::InternalErrorEvent>(event);
        }
 
        core::InternalErrorEvent&
        toInternalErrorEvent()
        {
            return std::get<core::InternalErrorEvent>(event);
        }
 
        bool
        isGlobalPlanningFailedEvent() const
        {
            return std::holds_alternative<core::GlobalPlanningFailedEvent>(event);
        }
 
        core::GlobalPlanningFailedEvent&
        toGlobalPlanningFailedEvent()
        {
            return std::get<core::GlobalPlanningFailedEvent>(event);
        }
 
        operator bool() const
        {
            return isGoalReachedEvent();
        }
 
    private:
        using StopEventVar = std::variant<core::GoalReachedEvent,
                                          core::SafetyStopTriggeredEvent,
                                          core::UserAbortTriggeredEvent,
                                          core::InternalErrorEvent,
                                          core::GlobalPlanningFailedEvent>;
        StopEventVar event;
    };
 
    class PathBuilder;
 
    class Navigator
    {
 
    public:
        struct InjectedServices
        {
            core::NavigatorInterface* navigator;
            EventSubscriptionInterface* subscriber;
        };
 
        Navigator(const InjectedServices& services);
 
        void moveTo(const core::Pose& pose, core::NavigationFrame frame);
 
        void moveTo(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame);
 
        void moveTo(const PathBuilder& builder, core::NavigationFrame frame);
 
        void moveToAlternatives(const std::vector<core::TargetAlternative>& targets,
                                core::NavigationFrame frame);
 
        void moveTowards(const core::Direction& direction, core::NavigationFrame frame);
 
        void moveToLocation(const std::string& location,
                            const std::optional<std::string>& providerName);
 
        void update(const std::vector<core::Pose>& waypoints, core::NavigationFrame frame);
 
        void setVelocityFactor(float velocityFactor);
 
        void pause();
 
        void resume();
 
        void stop();
 
        void onGoalReached(const std::function<void(void)>& callback);
 
        void onGoalReached(const std::function<void(const core::GoalReachedEvent&)>& callback);
 
        void onWaypointReached(const std::function<void(int)>& callback);
 
        StopEvent waitForStop(std::int64_t timeoutMs = -1);
 
    protected:
    private:
        void stopped(const StopEvent&);
 
        struct
        {
            std::mutex m;
            std::condition_variable cv;
            std::optional<StopEvent> event;
        } stoppedInfo;
 
        InjectedServices srv;
    };
 
} // namespace armarx::navigation::client