Component.cpp

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/>.
 *
 * @package    Navigation::ArmarXObjects::Navigator
 * @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
 */
 
#include "Component.h"
 
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <experimental/memory>
#include <iterator>
#include <memory>
#include <optional>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <Ice/Current.h>
 
#include <opencv2/core/eigen.hpp>
 
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
#include <RobotAPI/interface/ArViz/Elements.h>
#include <RobotAPI/interface/aron/Aron.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/GoalReachedConfig.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/events.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/factories/NavigationStackFactory.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/server/NavigationStack.h>
#include <armarx/navigation/server/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/execution/PlatformControllerExecutor.h>
#include <armarx/navigation/server/introspection/ArvizIntrospector.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/parameterization/MemoryParameterizationService.h>
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
 
namespace armarx::navigation::components::navigator
{
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
}
 
namespace armarx::navigation
{
 
    std::vector<core::Pose>
    convert(const std::vector<Eigen::Matrix4f>& wps)
    {
        std::vector<core::Pose> p;
        p.reserve(wps.size());
        std::transform(wps.begin(),
                       wps.end(),
                       std::back_inserter(p),
                       [](const auto& p) { return core::Pose(p); });
        return p;
    }
 
} // namespace armarx::navigation
 
namespace armarx::navigation::components::navigator
{
 
    Component::Component() : parameterizationService(nullptr, nullptr)
    // publisher(&resultsWriter, &eventsWriter)
    {
        // scene().timeServer = &timeServer;
 
        addPlugin(parameterizationReaderPlugin);
        addPlugin(parameterizationWriterPlugin);
        addPlugin(eventsWriterPlugin);
        addPlugin(resultsWriterPlugin);
        addPlugin(graphReaderPlugin);
        addPlugin(costmapReaderPlugin);
        addPlugin(costmap3dReaderPlugin);
        addPlugin(humanReaderPlugin);
        addPlugin(laserScannerFeaturesReaderPlugin);
 
        addPlugin(virtualRobotReaderPlugin);
 
        parameterizationService = server::MemoryParameterizationService(
            &parameterizationReaderPlugin->get(), &parameterizationWriterPlugin->get());
    }
 
 
    Component::~Component() = default;
 
    void
    Component::onInitComponent()
    {
        usingProxy("RobotStateMemory");
        usingProxy("ObjectMemory");
        usingProxy("navigation_memory");
    }
 
    void
    Component::onConnectComponent()
    {
        ARMARX_TRACE;
 
        // virtualRobotReaderPlugin->get().setSyncTimeout(Duration::MilliSeconds(20));
        // virtualRobotReaderPlugin->get().setSleepAfterSyncFailure(Duration::MilliSeconds(10));
 
        ObjectPoseClientPluginUser::getClient().objectFinder.setLogObjectDiscoveryError(false);
 
        // initialize scene provider
        ARMARX_TRACE;
        {
            const server::scene_provider::SceneProvider::InjectedServices srv{
                .graphReader = &graphReaderPlugin->get(),
                .costmapReader = &costmapReaderPlugin->get(),
                .costmap3dReader = &costmap3dReaderPlugin->get(),
                .virtualRobotReader = &virtualRobotReaderPlugin->get(),
                .humanReader = &humanReaderPlugin->get(),
                .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
                .objectPoseClient = ObjectPoseClientPluginUser::getClient()};
 
            // emplace (below) will be effectless if object is already initialized
            if (sceneProvider)
            {
                ARMARX_INFO << "Resetting scene provider";
                sceneProvider.reset();
            }
            sceneProvider.emplace(srv, params.sceneCfg);
        }
 
        initializeScene();
 
        ARMARX_TRACE;
 
        if (not params.disableExecutor)
        {
            if (executor)
            {
                ARMARX_INFO << "Executor will be reset.";
                executor.reset();
            }
 
            // Resolved here rather than declared with `usingProxy`: that would make the
            // navigator wait for the emergency stop master and would break every deployment and
            // test that does not run one. Its absence is tolerated and means "not configured";
            // once resolved, a failure to reach it counts as broken and blocks activation.
            emergencyStop.reset();
            if (not params.emergencyStopMasterName.empty())
            {
                try
                {
                    emergencyStop.emplace(
                        getProxy<EmergencyStopMasterInterfacePrx>(params.emergencyStopMasterName));
                    ARMARX_INFO << "Emergency stop master "
                                << QUOTED(params.emergencyStopMasterName)
                                << " resolved. Controllers will not be activated while it is "
                                   "engaged.";
                }
                catch (const std::exception& e)
                {
                    ARMARX_WARNING << "Emergency stop master "
                                   << QUOTED(params.emergencyStopMasterName)
                                   << " is not available: " << e.what()
                                   << ". Proceeding without an emergency stop check.";
                }
            }
 
            executor.emplace(
                getControlComponentPlugin(),
                server::PlatformControllerExecutor::Properties{
                    .robotName = params.sceneCfg.robotName,
                    .rtUnitDynamicallyLoadLibraries = params.rtUnitDynamicallyLoadLibraries},
                emergencyStop ? &emergencyStop.value() : nullptr);
        }
 
        ARMARX_TRACE;
 
        if(introspector)
        {
            ARMARX_INFO << "Resetting introspector";
            introspector.reset();
        }
        introspector = server::ArvizIntrospector(
            getArvizClient(), scene().robot, ObjectPoseClientPluginUser::getClient());
        // memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
 
        // initialized = true;
 
        // Setup the remote GUI.
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
 
        ARMARX_INFO << "Initialized. Will now respond to navigation requests.";
    }
 
    void
    Component::onDisconnectComponent()
    {
        ARMARX_TRACE;
 
        // "Running" in a broader sense. The navigator could also be paused but will be stopped subsequenty.
        // Scoped: the snapshot holds the entries alive, and holding it across the resets below
        // would destroy the navigators after the executor they call `stop()` on.
        bool isAnyNavigatorRunning = false;
        {
            const auto entries = allEntries();
            isAnyNavigatorRunning = std::any_of(
                entries.begin(),
                entries.end(),
                [](const auto& p) -> bool { return not p.second->navigator.isStopped(); });
        }
 
        if (isAnyNavigatorRunning)
        {
            ARMARX_WARNING
                << "A navigator was running when the component got disconnected. Will try to stop "
                   "as much as possible but expect something to be broken now.";
        }
 
        stopAllImpl(isAnyNavigatorRunning);
 
        // Drop the registrations before the services they point into. Every navigator holds
        // raw `srv.sceneProvider` / `srv.executor` / `srv.introspector` / `srv.drawer`
        // pointers, so entries left registered across the resets below stay callable with
        // dangling services and the next request faults inside the navigator. That it
        // currently survives a reconnect at all is an accident of `std::optional` reusing its
        // inline storage, which puts the re-`emplace`d services back at the same addresses.
        //
        // Clients must re-register with `createConfig` after a reconnect. Every client in the
        // tree already does: statecharts and `NavigatingSkillHelper` register per invocation,
        // and `DigitalRobotTwinComponentPlugin` registers from `postOnConnectComponent()`.
        // A client that registered only once would have to call `configureNavigator()` again.
        //
        // Swapped out under the lock and destroyed after it: `~Navigator` joins the periodic
        // task, and joining while holding `navigatorsMtx` would deadlock a waiting request.
        decltype(navigators) stale;
        {
            const std::scoped_lock lock{navigatorsMtx};
            stale.swap(navigators);
        }
        stale.clear();
 
        sceneProvider.reset();
        executor.reset();
        introspector.reset();
    }
 
    void
    Component::onExitComponent()
    {
    }
 
    bool
    Component::initializeScene()
    {
        ARMARX_TRACE;
        return sceneProvider->initialize(armarx::Clock::Now());
    }
 
    std::string
    Component::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    Component::GetDefaultName()
    {
        return "navigator";
    }
 
    void
    Component::createConfig(const aron::data::dto::DictPtr& stackConfig,
                            const std::string& callerId,
                            const Ice::Current&)
    {
        // TODO: Not thread-safe.
        ARMARX_TRACE;
        ARMARX_INFO << "Creating config for caller " << QUOTED(callerId);
 
        parameterizationService.store(stackConfig, callerId, armarx::Clock::Now());
 
        server::NavigationStack stack =
            fac::NavigationStackFactory::create(stackConfig,
                                                sceneProvider->scene(),
                                                std::experimental::make_observer(&getArvizClient()),
                                                std::experimental::make_observer(this));
 
        // FIXME unify this. The local planner also needs the visu. Yet, it is passed to the navigation stack factory above now.
        // set visualization of LocalPlanner
        if (stack.localPlanner)
        {
            stack.localPlanner->setVisualization(getArvizClient());
        }
 
        auto memoryIntrospector =
            std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId);
 
        auto memoryPublisher = std::make_unique<server::MemoryPublisher>(
            &resultsWriterPlugin->get(), &eventsWriterPlugin->get(), callerId);
 
        server::ExecutorInterface* executorPtr = nullptr;
        if (executor.has_value())
        {
            executorPtr = &executor.value();
        }
 
        // Extract goal reached config from stack config if present
        server::GoalReachedMonitorConfig goalReachedCfg = params.navigatorGoalReachedConfig;
        {
            aron::data::Dict aronDict(stackConfig);
            const std::string goalReachedKey =
                core::StackLayerNames.to_name(core::StackLayer::GoalReachedConfig);
            if (aronDict.hasElement(goalReachedKey))
            {
                auto goalReachedElement =
                    aron::data::Dict::DynamicCast(aronDict.getElement(goalReachedKey));
                if (goalReachedElement && goalReachedElement->hasElement(core::PARAMS_KEY))
                {
                    auto paramsDict = aron::data::Dict::DynamicCast(
                        goalReachedElement->getElement(core::PARAMS_KEY));
                    if (paramsDict)
                    {
                        auto coreCfg = core::GoalReachedConfig::FromAron(paramsDict);
                        goalReachedCfg.posTh = coreCfg.posTh;
                        goalReachedCfg.oriTh = coreCfg.oriTh;
                        goalReachedCfg.linearVelTh = coreCfg.linearVelTh;
                        goalReachedCfg.angularVelTh = coreCfg.angularVelTh;
                        goalReachedCfg.filterCount = coreCfg.filterCount;
                    }
                }
            }
        }
 
        // Read before the moves below: within a braced-init-list the members are initialized
        // left to right, so `memoryPublisher` is already moved from by the time `.navigator`
        // is initialized.
        server::MemoryPublisher* const publisherPtr = memoryPublisher.get();
 
        // Built outside the lock: constructing the navigator is expensive, and the entry is
        // not reachable by other threads until it is inserted below.
        auto entry = std::make_shared<NavigatorEntry>(NavigatorEntry{
            .publisher = std::move(memoryPublisher),
            .introspector = std::move(memoryIntrospector),
            .navigator = server::Navigator{
                server::Navigator::Config{.stack = std::move(stack),
                                          .general = params.navigatorGeneralConfig,
                                          .goalReachedConfig = goalReachedCfg},
                server::Navigator::InjectedServices{
                    .executor = executorPtr,
                    .publisher = publisherPtr,
                    .introspector = introspector.has_value() ? &(introspector.value()) : nullptr,
                    .drawer = introspector.has_value() ? &(introspector.value()) : nullptr,
                    .debugObserverHelper = &getDebugObserverComponentPlugin(),
                    .sceneProvider =
                        sceneProvider.has_value() ? &sceneProvider.value() : nullptr}}});
 
        // Replaces any previous registration for this caller. The old entry is released after
        // the lock, so its `~Navigator` (which joins the periodic task) cannot deadlock
        // against another request waiting for `navigatorsMtx`; and it survives until the last
        // in-flight request inside it has returned.
        NavigatorEntryPtr previous;
        {
            const std::scoped_lock lock{navigatorsMtx};
            previous = std::exchange(navigators[callerId], std::move(entry));
        }
        previous.reset();
    }
 
    void
    Component::removeConfig(const std::string& callerId, const Ice::Current&)
    {
        // Same contract as above: drop the map's reference under the lock, destroy outside it.
        NavigatorEntryPtr entry;
        {
            const std::scoped_lock lock{navigatorsMtx};
            if (const auto it = navigators.find(callerId); it != navigators.end())
            {
                entry = std::move(it->second);
                navigators.erase(it);
            }
        }
        entry.reset();
    }
 
    Component::NavigatorEntryPtr
    Component::entry(const std::string& callerId) const
    {
        const std::scoped_lock lock{navigatorsMtx};
 
        const auto it = navigators.find(callerId);
        ARMARX_CHECK(it != navigators.end())
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        return it->second;
    }
 
    std::vector<std::pair<std::string, Component::NavigatorEntryPtr>>
    Component::allEntries() const
    {
        const std::scoped_lock lock{navigatorsMtx};
        return {navigators.begin(), navigators.end()};
    }
 
    void
    Component::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
                      const std::string& navigationMode,
                      const std::string& callerId,
                      const Ice::Current&)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "moveTo() requested by caller " << QUOTED(callerId)
                    << " with navigation mode `" << navigationMode << "`.";
        try
        {
            entry(callerId)->navigator.moveTo(convert(waypoints),
                                              core::NavigationFrameNames.from_name(navigationMode));
        }
        catch (...)
        {
            ARMARX_WARNING << GetHandledExceptionString();
 
            // TODO: Error handling.
            ARMARX_WARNING << "Failed to execute moveTo()."
                           << "Movement will be paused.";
            stopAll(); // TODO pause movement must not throw
            throw;
        }
    }
 
    void
    Component::updateMoveTo(const std::vector<Eigen::Matrix4f>& waypoints,
                            const std::string& navigationMode,
                            const std::string& callerId,
                            const Ice::Current& /*c*/)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "updateMoveTo() requested by caller " << QUOTED(callerId);
        const auto checkIsActiveNavigator = [&](const std::string& callerId)
        {
            const std::optional<std::string> activeNavigatorCallerId = activeNavigatorId();
            if (not activeNavigatorCallerId.has_value())
            {
                ARMARX_VERBOSE << "No navigator is active.";
                return false;
            }
 
            return activeNavigatorCallerId == callerId;
        };
 
        ARMARX_CHECK(checkIsActiveNavigator(callerId))
            << "The navigator with id " << QUOTED(callerId) << " is not active!";
 
        entry(callerId)->navigator.update(convert(waypoints),
                                          core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::moveTo2(const client::detail::Waypoints& waypoints,
                       const std::string& navigationMode,
                       const std::string& callerId,
                       const Ice::Current& c)
    {
        ARMARX_TRACE;
 
        ARMARX_IMPORTANT << "moveTo2 requested by caller " << QUOTED(callerId);
 
        const std::vector<client::WaypointTarget> wps = client::defrost(waypoints);
 
        entry(callerId)->navigator.moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::moveToAlternatives(const client::detail::TargetAlternatives& targets,
                                  const std::string& navigationMode,
                                  const std::string& callerId,
                                  const Ice::Current& c)
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "moveToAlternatives() requested by caller " << QUOTED(callerId)
                    << " with navigation mode `" << navigationMode << "`.";
        const std::vector<core::TargetAlternative> alts = client::defrost(targets);
 
        try
        {
            entry(callerId)->navigator.moveToAlternatives(
                alts, core::NavigationFrameNames.from_name(navigationMode));
        }
        catch (...)
        {
            ARMARX_WARNING << GetHandledExceptionString();
 
            // TODO: Error handling.
            ARMARX_WARNING << "Failed to execute moveTo()."
                           << "Movement will be paused.";
            stopAll(); // TODO pause movement must not throw
            throw;
        }
    }
 
    void
    Component::moveToLocation(const std::string& location,
                              const IceUtil::Optional<std::string>& providerNameIce,
                              const std::string& callerId,
                              const Ice::Current& c)
    {
        ARMARX_TRACE;
        ARMARX_INFO << "MoveToLocation " << QUOTED(location) << " requested by caller '" << callerId
                    << "'";
 
        std::optional<std::string> providerName;
        if (providerNameIce)
        {
            providerName = providerNameIce.value();
        }
 
        entry(callerId)->navigator.moveToLocation(location, providerName);
    }
 
    void
    Component::moveTowards(const Eigen::Vector3f& direction,
                           const std::string& navigationMode,
                           const std::string& callerId,
                           const Ice::Current&)
    {
        // TODO: Error handling.
        ARMARX_TRACE;
        ARMARX_INFO << "MoveTowards requested by caller " << QUOTED(callerId);
 
        entry(callerId)->navigator.moveTowards(
            direction, core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::setVelocityFactor(float velocityFactor,
                                 const std::string& callerId,
                                 const Ice::Current&)
    {
        ARMARX_TRACE;
        entry(callerId)->navigator.setVelocityFactor(velocityFactor);
    }
 
    void
    Component::pause(const std::string& configId, const Ice::Current&)
    {
        entry(configId)->navigator.pause();
    }
 
    void
    Component::resume(const std::string& configId, const Ice::Current&)
    {
        ARMARX_TRACE;
        entry(configId)->navigator.resume();
    }
 
    void
    Component::stop(const std::string& configId, const Ice::Current&)
    {
        // FIXME make sure that event is emitted
        ARMARX_TRACE;
 
        // One lookup for both: the reference keeps the publisher alive across the stop.
        const NavigatorEntryPtr e = entry(configId);
        e->navigator.stop();
 
        // emit UserAbortTriggered event
        const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
                                                  core::Pose(scene().robot->getGlobalPose())};
        e->publisher->userAbortTriggered(event);
    }
 
    void
    Component::stopAll(const Ice::Current&)
    {
        stopAllImpl(true);
    }
 
    void
    Component::stopAllImpl(const bool emitMemoryEvent)
    {
        ARMARX_IMPORTANT << "stopAll()";
 
        ARMARX_TRACE;
        const auto entries = allEntries();
 
        for (const auto& [_, e] : entries)
        {
            e->navigator.stop();
        }
 
        if (emitMemoryEvent)
        {
            ARMARX_INFO << "Trying to emit memory event `UserAbortTriggered`";
            // emit UserAbortTriggered event
            const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
                                                      core::Pose(scene().robot->getGlobalPose())};
 
            // This call might fail if the component is already disconnected from the NavigationMemory in onDisconnectComponent.
            for (const auto& [callerId, e] : entries)
            {
                e->publisher->userAbortTriggered(event);
            }
        }
    }
 
    bool
    Component::isPaused(const std::string& configId, const Ice::Current&)
    {
        ARMARX_TRACE;
        return entry(configId)->navigator.isPaused();
    }
 
    const core::Scene&
    Component::scene() const
    {
        ARMARX_CHECK_NOT_NULL(sceneProvider);
        return sceneProvider->scene();
    }
 
    bool
    Component::isStopped(const std::string& configId, const Ice::Current&)
    {
        ARMARX_TRACE;
        return entry(configId)->navigator.isStopped();
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        ARMARX_TRACE;
 
        PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
 
        // Publish to a topic (passing the TopicListenerPrx).
        // def->topic(myTopicListener);
 
        // Subscribe to a topic (passing the topic name).
        // def->topic<PlatformUnitListener>("MyTopic");
 
        // Add a required property.
        // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
 
        // Add an optionalproperty.
        def->optional(params.occupiedGridThreshold,
                      "p.occupancy_grid.occopied_threshold",
                      "Threshold for each cell to be considered occupied. Increase this value to "
                      "reduce noise.");
 
        def->optional(params.disableExecutor,
                      "p.disableExecutor",
                      "If the executor is disabled, the navigator will only plan the trajectory "
                      "but won't execute it.");
 
        def->required(params.sceneCfg.robotName, "p.scene.robotName");
        def->optional(params.sceneCfg.staticCostmapProviderName,
                      "p.scene.staticCostmapProviderName");
        def->optional(params.sceneCfg.staticCostmapName, "p.scene.staticCostmapName");
        def->optional(params.sceneCfg.costmap3dProviderName, "p.scene.costmap3dProviderName");
        def->optional(params.sceneCfg.costmap3dName, "p.scene.costmap3dName");
        def->optional(params.sceneCfg.humanProviderName, "p.scene.humanProviderName");
        def->optional(params.sceneCfg.laserScannerFeaturesProviderName,
                      "p.scene.laserScannerFeaturesProviderName");
 
        def->optional(params.rtUnitDynamicallyLoadLibraries,
                      "p.rtUnitDynamicallyLoadLibraries",
                      "If enabled, the controller library will automatically be loaded in the "
                      "RobotUnit. If disabled (desired), you have to make sure that the controller "
                      "library is listed in the RobotUnit's property `LoadLibraries`");
 
        def->optional(params.emergencyStopMasterName,
                      "p.navigator.emergencyStopMasterName",
                      "Name of the emergency stop master. While it reports the emergency stop "
                      "engaged, the trajectory controllers are not (re-)activated. Empty disables "
                      "the check, as does an unresolvable name.");
 
 
        def->optional(params.navigatorGeneralConfig.tasks.replanningUpdatePeriod,
                      "p.navigator.general.replanningPeriod");
 
        def->optional(params.navigatorGeneralConfig.platform,
                      "p.navigator.general.platform",
                      "Which platform's drive parameters the TOPP-RA parametrization uses: the "
                      "suffix selecting config/platform/PlatformDynamics<Platform>.json in the "
                      "armarx_navigation package: Armar7, Armar6 or ArmarDE.");
        def->optional(params.navigatorGeneralConfig.platformDynamicsEnabled,
                      "p.navigator.general.platformDynamicsEnabled",
                      "Whether to read drive parameters at all. False makes TOPP-RA unavailable, "
                      "so the stack falls back to ramping.");
 
        def->optional(params.navigatorGeneralConfig.parametrizationDump,
                      "p.navigator.general.parametrizationDump",
                      "Write a record of every parametrized navigation request, for offline "
                      "analysis with `plot-navigation-request`.");
        def->optional(params.navigatorGeneralConfig.parametrizationDumpPath,
                      "p.navigator.general.parametrizationDumpPath",
                      "Where that record is written. A timestamped copy is written alongside it.");
 
        def->optional(
            params.navigatorGeneralConfig.subdivision.enable,
            "p.navigator.general.subdivision.enable",
            "Whether to enable global path subdivision when local planner is enabled as well.");
        def->optional(params.navigatorGeneralConfig.subdivision.localPlannerCostmapThreshold,
                      "p.navigator.general.subdivision.localPlannerCostmapThreshold",
                      "The minimum value in the costmap to enable local planning.");
        def->optional(
            params.navigatorGeneralConfig.subdivision.globalPlanExpansionDistance,
            "p.navigator.general.subdivision.globalPlanExpansionDistance",
            "The length by which global planner segments are expaned (in both directions).");
        def->optional(params.navigatorGeneralConfig.subdivision.minSegmentDistance,
                      "p.navigator.general.subdivision.minSegmentDistance",
                      "The minimum length for a local planner segment (otherwise it is deleted and "
                      "joined with the neighboring global planner segments).");
 
        def->optional(params.navigatorGeneralConfig.targetAlternativesFilterTimeSeconds,
                      "p.navigator.general.targetAlternativesFilterTimeSeconds",
                      "The number of seconds to wait before replanning alternatives after all "
                      "alternatives where impossible.");
 
        def->optional(params.navigatorGoalReachedConfig.posTh, "p.navigator.goalReached.posTh");
        def->optional(params.navigatorGoalReachedConfig.oriTh, "p.navigator.goalReached.oriTh");
        def->optional(params.navigatorGoalReachedConfig.linearVelTh,
                      "p.navigator.goalReached.linearVelTh");
        def->optional(params.navigatorGoalReachedConfig.angularVelTh,
                      "p.navigator.goalReached.angularVelTh");
        def->optional(params.navigatorGoalReachedConfig.filterCount,
                      "p.navigator.goalReached.filterCount");
 
        return def;
    }
 
    void
    visualize(const algorithms::Costmap& costmap, viz::Client& arviz, const std::string& name)
    {
        const auto cmap = simox::color::cmaps::viridis();
        const float vmax = costmap.getGrid().array().maxCoeff();
 
        const auto asColor = [&cmap, &vmax](const float distance) -> viz::data::Color
        {
            const auto color = cmap.at(distance, 0.F, vmax);
            return {color.a, color.r, color.g, color.b};
        };
 
        auto layer = arviz.layer("costmap_" + name);
 
        const std::int64_t cols = costmap.getGrid().cols();
        const std::int64_t rows = costmap.getGrid().rows();
 
        auto mesh = viz::Mesh("");
 
        std::vector<std::vector<Eigen::Vector3f>> vertices;
        std::vector<std::vector<viz::data::Color>> colors;
 
        for (int r = 0; r < rows; r++)
        {
            auto& verticesRow = vertices.emplace_back(cols);
            auto& colorsRow = colors.emplace_back(cols);
 
            for (int c = 0; c < cols; c++)
            {
                verticesRow.at(c) = conv::to3D(costmap.toPositionGlobal({r, c}));
                colorsRow.at(c) = asColor(costmap.getGrid()(r, c));
            }
        }
 
        mesh.grid2D(vertices, colors);
 
        layer.add(mesh);
 
        arviz.commit(layer);
    }
 
    Component::NavigatorEntryPtr
    Component::activeNavigator()
    {
        const auto isActive = [](const auto& nameNavPair) -> bool
        { return not nameNavPair.second->navigator.isPaused(); };
 
        // Found and returned from one snapshot: looking the id up again could race with a
        // `removeConfig` and throw where this contract is to return null.
        const auto entries = allEntries();
        const auto it = std::find_if(entries.begin(), entries.end(), isActive);
 
        return it == entries.end() ? nullptr : it->second;
    }
 
    std::optional<std::string>
    Component::activeNavigatorId() const
    {
        ARMARX_TRACE;
        // We define the active navigator to be the one whose movement is enabled.
        const auto isActive = [](const auto& nameNavPair) -> bool
        { return not nameNavPair.second->navigator.isPaused(); };
 
        const auto entries = allEntries();
        const auto it = std::find_if(entries.begin(), entries.end(), isActive);
 
        // no navigator active?
        if (it == entries.end())
        {
            return std::nullopt;
        }
 
        return it->first;
    }
 
    void
    Component::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root;
 
        tab.stopAll.setLabel("Stop All");
 
        root.addChild(tab.stopAll);
 
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    Component::RemoteGui_update()
    {
        if (tab.stopAll.wasClicked())
        {
            stopAll();
        }
    }
 
} // namespace armarx::navigation::components::navigator