IceNavigator.cpp

Go to the documentation of this file.

#include "IceNavigator.h"
 
#include <algorithm>
#include <iterator>
#include <string>
#include <vector>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/navigation/client/NavigationStackConfig.h>
#include <armarx/navigation/client/ice/NavigatorInterface.h>
#include <armarx/navigation/client/ice_conversions.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
 
namespace armarx::navigation::client
{
 
    std::vector<Eigen::Matrix4f>
    convert(const std::vector<armarx::navigation::core::Pose>& ps)
    {
        using namespace armarx::navigation;
        std::vector<Eigen::Matrix4f> ms;
        ms.reserve(ps.size());
        std::transform(
            ps.begin(), ps.end(), std::back_inserter(ms), [](const auto& p) { return p.matrix(); });
        return ms;
    }
 
    IceNavigator::IceNavigator()
    {
        // pass
    }
 
    IceNavigator::IceNavigator(const NavigatorInterfacePrx& navigator) : navigator(navigator)
    {
        // pass
    }
 
    IceNavigator::~IceNavigator()
    {
        // pass
    }
 
    void
    IceNavigator::setNavigatorComponent(const NavigatorInterfacePrx& navigator)
    {
        this->navigator = navigator;
    }
 
    void
    IceNavigator::createConfig(const client::NavigationStackConfig& config,
                               const std::string& configId)
    {
        ARMARX_TRACE;
        this->configId = configId;
        navigator->createConfig(config.toAron(), configId);
    }
 
    void
    IceNavigator::moveTo(const std::vector<core::Pose>& waypoints,
                         core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        navigator->begin_moveTo(
            convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId);
    }
 
    void
    IceNavigator::moveTo(const std::vector<client::WaypointTarget>& targets,
                         core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        navigator->moveTo2(
            freeze(targets), core::NavigationFrameNames.to_name(navigationFrame), configId);
    }
 
    void
    IceNavigator::moveToLocation(const std::string& location)
    {
        ARMARX_TRACE;
        navigator->moveToLocation(location, configId);
    }
 
    void
    IceNavigator::moveTowards(const core::Direction& direction,
                              core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        navigator->moveTowards(
            direction, core::NavigationFrameNames.to_name(navigationFrame), configId);
    }
 
    void
    IceNavigator::update(const std::vector<core::Pose>& waypoints,
                         core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        navigator->updateMoveTo(
            convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId);
    }
 
    void
    IceNavigator::pause()
    {
        navigator->pause(configId);
    }
 
    void
    IceNavigator::resume()
    {
        navigator->resume(configId);
    }
 
    void
    IceNavigator::stop()
    {
        navigator->stop(configId);
    }
 
    bool
    IceNavigator::isPaused() const noexcept
    {
        return navigator->isPaused(configId);
    }
 
    bool
    IceNavigator::isStopped() const noexcept
    {
        return navigator->isStopped(configId);
    }
 
 
} // namespace armarx::navigation::client