NavigatingSkillHelper.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 )
 * @date       2023
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <chrono>
#include <experimental/memory>
#include <future>
#include <mutex>
#include <optional>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time.h>
 
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
 
#include <armarx/navigation/client.h>
#include <armarx/navigation/client/Navigator.h>
#include <armarx/navigation/client/services/IceNavigatorFactory.h>
#include <armarx/navigation/client/services/MemorySubscriber.h>
#include <armarx/navigation/core/NavigationStackGeneralConfig.h>
#include <armarx/navigation/safety_guard/LaserBasedProximity.h>
#include <armarx/navigation/skills/aron/NavigatingSkillParams.aron.generated.h>
 
namespace armarx::navigation::skills
{
    namespace detail
    {
        /// Time to wait between two attempts to plan a path to the goal.
        inline const armarx::core::time::Duration RetryInterval =
            armarx::core::time::Duration::Seconds(3);
 
        /// Granularity at which `shouldTerminate` is evaluated while waiting.
        inline const armarx::core::time::Duration PollInterval =
            armarx::core::time::Duration::MilliSeconds(20);
    } // namespace detail
 
    /// True if `f` holds a result (or has none to wait for). A deferred task is started here.
    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;
        }
    }
 
    class NavigatingSkillHelper
    {
    public:
        struct Services
        {
            client::IceNavigatorFactory& iceNavigatorFactory;
 
            armem::client::MemoryNameSystem& memoryNameSystem;
        };
 
        struct Properties
        {
            std::experimental::observer_ptr<std::mutex> safetyGuardParamsMutex;
            std::experimental::observer_ptr<safety_guard::LaserBasedProximityParams>
                safetyGuardParams;
            core::GeneralConfig defaultGeneralConfig;
        };
 
        NavigatingSkillHelper(const Properties& properties, const Services& srv);
 
        void init(const arondto::NavigatingSkillParams& params, const std::string& id);
 
        std::optional<client::Navigator>& getNavigator();
 
        /// Issues the navigation command and waits for the navigator to stop, polling
        /// `shouldTerminate` meanwhile. If the global planner fails and the `retryTimeout`
        /// budget (measured from the first attempt) is not exhausted yet, the command is
        /// re-issued after a short pause. Returns the stop event of the last attempt.
        ///
        /// Templated rather than taking `std::function`s: the navigation commands can throw
        /// (e.g. Ice exceptions), which `std::function` cannot express and `-Wnoexcept` rejects.
        template <typename IssueNavigationCommandT, typename ShouldTerminateT>
        client::StopEvent
        moveAndWaitForStop(IssueNavigationCommandT&& issueNavigationCommand,
                           ShouldTerminateT&& shouldTerminate)
        {
            ARMARX_CHECK(navigator.has_value());
 
            const auto deadline = armarx::Clock::Now() + retryTimeout_;
 
            while (true)
            {
                // Re-issuing is safe: every `client::Navigator` move command resets the stop
                // event before forwarding the request, and a planning failure leaves the robot
                // in place.
                issueNavigationCommand();
 
                auto future =
                    std::async(std::launch::async, [this]() { return navigator->waitForStop(); });
                while (not is_ready(future))
                {
                    if (shouldTerminate())
                    {
                        ARMARX_INFO << "Skill should terminate. Stopping the navigator.";
                        navigator->stop();
                        break;
                    }
                    armarx::Clock::WaitFor(detail::PollInterval);
                }
 
                client::StopEvent se = future.get();
 
                if (not se.isGlobalPlanningFailedEvent() or shouldTerminate())
                {
                    return se;
                }
 
                const auto now = armarx::Clock::Now();
                if (now >= deadline)
                {
                    ARMARX_WARNING << "Global planning failed and the retry budget of "
                                   << retryTimeout_.toSecondsDouble()
                                   << "s is exhausted. Giving up.";
                    return se;
                }
 
                ARMARX_INFO << "Global planning failed ("
                            << se.toGlobalPlanningFailedEvent().message << "). Retrying, "
                            << (deadline - now).toSecondsDouble() << "s left.";
 
                // Wait for the retry interval in small slices so a stop request does not have
                // to wait out the full interval.
                const auto remaining = deadline - now;
                const auto waitBudget =
                    remaining < detail::RetryInterval ? remaining : detail::RetryInterval;
                const auto waitStart = armarx::Clock::Now();
                while (armarx::Clock::Now() - waitStart < waitBudget)
                {
                    if (shouldTerminate())
                    {
                        return se;
                    }
                    const auto remainingWait = waitBudget - (armarx::Clock::Now() - waitStart);
                    armarx::Clock::WaitFor(remainingWait < detail::PollInterval
                                               ? remainingWait
                                               : detail::PollInterval);
                }
            }
        }
 
        static arondto::NavigatingSkillParams DefaultSkillDescription();
 
    private:
        Properties properties_;
        std::optional<Services> srv_;
 
        /// Time budget for re-planning after a failed global plan. Set in `init()`.
        armarx::core::time::Duration retryTimeout_{};
 
        // will be initialized in "init()"
        std::optional<client::MemorySubscriber> memorySubscriber;
        client::NavigatorHandlePtr iceNavigator;
        std::optional<client::Navigator> navigator;
    };
 
} // namespace armarx::navigation::skills