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 <cstddef>
#include <cstdint>
#include <iterator>
#include <memory>
#include <optional>
#include <utility>
 
#include <Eigen/Geometry>
 
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgcodecs.hpp>
 
#include <Ice/Current.h>
 
#include <SimoxUtility/color/ColorMap.h>
#include <SimoxUtility/color/cmaps/colormaps.h>
#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/Component.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/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
 
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/CostmapBuilder.h>
#include <armarx/navigation/algorithms/astar/util.h>
#include <armarx/navigation/algorithms/persistence.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/util.h>
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/PathBuilder.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/components/navigator/RemoteGui.h>
#include <armarx/navigation/conversions/eigen.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/Navigator.h>
#include <armarx/navigation/server/event_publishing/MemoryPublisher.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/server/scene_provider/SceneProvider.h>
#include <armarx/navigation/util/util.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(humanReaderPlugin);
        addPlugin(laserScannerFeaturesReaderPlugin);
 
        addPlugin(virtualRobotReaderPlugin);
 
        parameterizationService = server::MemoryParameterizationService(
            &parameterizationReaderPlugin->get(), &parameterizationWriterPlugin->get());
    }
 
 
    Component::~Component() = default;
 
    void
    Component::onInitComponent()
    {
    }
 
    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(),
                .virtualRobotReader = &virtualRobotReaderPlugin->get(),
                .humanReader = &humanReaderPlugin->get(),
                .laserScannerFeaturesReader = &laserScannerFeaturesReaderPlugin->get(),
                .objectPoseClient = ObjectPoseClientPluginUser::getClient()};
 
            // emplace (below) will be effectless if object is already initialized
            ARMARX_CHECK_NULL(sceneProvider);
            sceneProvider.emplace(srv, params.sceneCfg);
        }
 
        initializeScene();
 
        ARMARX_TRACE;
 
        if (not params.disableExecutor)
        {
            ARMARX_CHECK_NULL(executor);
 
            executor.emplace(
                getControlComponentPlugin(),
                server::PlatformControllerExecutor::Properties{
                    .robotName = params.sceneCfg.robotName,
                    .rtUnitDynamicallyLoadLibraries = params.rtUnitDynamicallyLoadLibraries});
        }
 
        ARMARX_TRACE;
 
        ARMARX_CHECK_NULL(introspector);
        introspector = server::ArvizIntrospector(
            getArvizClient(), scene().robot, ObjectPoseClientPluginUser::getClient());
        // memoryIntrospector = server::MemoryIntrospector(resultsWriterPlugin->get(), );
 
        ARMARX_CHECK_NULL(navRemoteGui);
        navRemoteGui.emplace(remoteGui, *this);
        navRemoteGui->enable();
 
        // initialized = true;
 
        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);
 
        navRemoteGui->disable();
        navRemoteGui.reset();
 
        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 '" << callerId << "'";
 
        parameterizationService.store(stackConfig, callerId, armarx::Clock::Now());
 
        server::NavigationStack stack =
            fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
 
        // 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();
        }
 
        // 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 = server::Navigator::Config::General{}},
                server::Navigator::InjectedServices{
                    .executor = executorPtr,
                    .publisher = memoryPublishers.at(callerId).get(),
                    .introspector = introspector.has_value() ? &(introspector.value()) : nullptr,
                    .debugObserverHelper = &getDebugObserverComponentPlugin(),
                    .sceneProvider =
                        sceneProvider.has_value() ? &sceneProvider.value() : nullptr}));
    }
 
    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 '" << callerId << "' with navigation mode `"
                    << navigationMode << "`.";
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller `" << 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 '" << callerId << "'";
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller `" << 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 `" << 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 '" << callerId << "'";
 
        const std::vector<client::WaypointTarget> wps = client::defrost(waypoints);
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller `" << callerId << "` not registered!";
 
        navigators.at(callerId).moveTo(wps, core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::moveToLocation(const std::string& location,
                              const std::string& callerId,
                              const Ice::Current& c)
    {
        ARMARX_TRACE;
        ARMARX_INFO << "MoveToLocation `" << location << "` requested by caller '" << callerId
                    << "'";
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller `" << callerId << "` not registered!";
 
        navigators.at(callerId).moveToLocation(location);
    }
 
    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 '" << callerId << "'";
 
        ARMARX_CHECK(navigators.count(callerId) > 0)
            << "Navigator config for caller `" << callerId << "` not registered!";
 
        navigators.at(callerId).moveTowards(direction,
                                            core::NavigationFrameNames.from_name(navigationMode));
    }
 
    void
    Component::pause(const std::string& configId, const Ice::Current&)
    {
        ARMARX_CHECK(navigators.count(configId) > 0)
            << "Navigator config for caller `" << 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 `" << 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 `" << 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 `" << 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 `" << 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");
 
        // Use (and depend on) another component (passing the ComponentInterfacePrx).
        def->component(remoteGui, "RemoteGuiProvider");
        // 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.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`");
 
        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;
    }
 
} // namespace armarx::navigation::components::navigator