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.
        const bool isAnyNavigatorRunning = std::any_of(navigators.begin(),
                                                       navigators.end(),
                                                       [](const auto& p) -> bool
                                                       {
                                                           const server::Navigator& navigator =
                                                               p.second;
 
                                                           return not 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);
 
        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());
        }
 
        memoryIntrospectors.emplace_back(
            std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
 
        // keep track of all memory publishers
        memoryPublishers.emplace(
            callerId,
            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;
                    }
                }
            }
        }
 
        // if we emplace an existing key it is not replaced, so remove it first.
        navigators.erase(callerId);
        navigators.emplace(
            std::piecewise_construct,
            std::forward_as_tuple(callerId),
            std::forward_as_tuple(
                server::Navigator::Config{.stack = std::move(stack),
                                          .general = params.navigatorGeneralConfig,
                                          .goalReachedConfig = goalReachedCfg},
                server::Navigator::InjectedServices{
                    .executor = executorPtr,
                    .publisher = memoryPublishers.at(callerId).get(),
                    .introspector = introspector.has_value() ? &(introspector.value()) : nullptr,
                    .drawer = introspector.has_value() ? &(introspector.value()) : nullptr,
                    .debugObserverHelper = &getDebugObserverComponentPlugin(),
                    .sceneProvider =
                        sceneProvider.has_value() ? &sceneProvider.value() : nullptr}));
    }
 
    void
    Component::removeConfig(const std::string& callerId, const Ice::Current&)
    {
        navigators.erase(callerId);
        memoryPublishers.erase(callerId);
    }
 
    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 << "`.";
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        try
        {
            navigators.at(callerId).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);
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        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!";
 
        navigators.at(callerId).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);
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        navigators.at(callerId).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 << "`.";
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        const std::vector<core::TargetAlternative> alts = client::defrost(targets);
 
        try
        {
            navigators.at(callerId).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
                    << "'";
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        std::optional<std::string> providerName;
        if (providerNameIce)
        {
            providerName = providerNameIce.value();
        }
 
        navigators.at(callerId).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);
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
 
        navigators.at(callerId).moveTowards(direction,
                                            core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::setVelocityFactor(float velocityFactor,
                                 const std::string& callerId,
                                 const Ice::Current&)
    {
        ARMARX_TRACE;
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller " << QUOTED(callerId) << " not registered!";
        navigators.at(callerId).setVelocityFactor(velocityFactor);
    }
 
    void
    Component::pause(const std::string& configId, const Ice::Current&)
    {
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller " << QUOTED(configId) << " not registered!";
        navigators.at(configId).pause();
    }
 
    void
    Component::resume(const std::string& configId, const Ice::Current&)
    {
        ARMARX_TRACE;
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller " << QUOTED(configId) << " not registered!";
        navigators.at(configId).resume();
    }
 
    void
    Component::stop(const std::string& configId, const Ice::Current&)
    {
        // FIXME make sure that event is emitted
        ARMARX_TRACE;
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller " << QUOTED(configId) << " not registered!";
        navigators.at(configId).stop();
 
        // emit UserAbortTriggered event
        const core::UserAbortTriggeredEvent event{{armarx::Clock::Now()},
                                                  core::Pose(scene().robot->getGlobalPose())};
        memoryPublishers.at(configId)->userAbortTriggered(event);
    }
 
    void
    Component::stopAll(const Ice::Current&)
    {
        stopAllImpl(true);
    }
 
    void
    Component::stopAllImpl(const bool emitMemoryEvent)
    {
        ARMARX_IMPORTANT << "stopAll()";
 
        ARMARX_TRACE;
        for (auto& [_, navigator] : navigators)
        {
            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 (auto& [callerId, memoryPublisher] : memoryPublishers)
            {
                memoryPublisher->userAbortTriggered(event);
            }
        }
    }
 
    bool
    Component::isPaused(const std::string& configId, const Ice::Current&)
    {
        ARMARX_TRACE;
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller " << QUOTED(configId) << " not registered!";
        return navigators.at(configId).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;
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller " << QUOTED(configId) << " not registered!";
        return navigators.at(configId).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);
    }
 
    server::Navigator*
    Component::activeNavigator()
    {
 
        const auto navigatorId = activeNavigatorId();
        if (not navigatorId.has_value())
        {
            return nullptr;
        }
 
        return &navigators.at(navigatorId.value());
    }
 
    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
        {
            const auto& [name, navigator] = nameNavPair;
            return not navigator.isPaused();
        };
 
        const auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
 
        // no navigator active?
        if (it == navigators.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