IceNavigatorHandle.cpp

Go to the documentation of this file.

#include "IceNavigatorHandle.h"
 
#include <algorithm>
#include <iterator>
#include <optional>
#include <string>
#include <vector>
 
#include <Ice/Exception.h>
#include <IceUtil/Optional.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#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
{
    namespace
    {
        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;
        }
    } // namespace
 
    IceNavigatorHandle::IceNavigatorHandle(const NavigatorInterfacePrx& navigator,
                                           const std::string& configId_) :
        navigator_(navigator), configId_(configId_)
    {
        // pass
    }
 
    IceNavigatorHandle::~IceNavigatorHandle()
    {
        ARMARX_TRACE;
        try
        {
            navigator_->removeConfig(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << "Failed to remove navigation config '" << configId_
                           << "' on navigator: " << e.what();
        }
    }
 
    void
    IceNavigatorHandle::moveTo(const std::vector<core::Pose>& waypoints,
                               core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->begin_moveTo(
                convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call moveTo on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::moveTo(const std::vector<client::WaypointTarget>& targets,
                               core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->moveTo2(
                freeze(targets), core::NavigationFrameNames.to_name(navigationFrame), configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call moveTo on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::moveToAlternatives(const std::vector<core::TargetAlternative>& targets,
                                           core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->moveToAlternatives(
                freeze(targets), core::NavigationFrameNames.to_name(navigationFrame), configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call moveToAlternatives on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::moveToLocation(const std::string& location,
                                       const std::optional<std::string>& providerName)
    {
        ARMARX_TRACE;
        IceUtil::Optional<std::string> providerNameIce = IceUtil::None;
        if (providerName.has_value())
        {
            providerNameIce = providerName.value();
        }
 
        try
        {
            navigator_->moveToLocation(location, providerNameIce, configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call moveToLocation on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::moveTowards(const core::Direction& direction,
                                    core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->moveTowards(
                direction, core::NavigationFrameNames.to_name(navigationFrame), configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call moveTowards on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::update(const std::vector<core::Pose>& waypoints,
                               core::NavigationFrame navigationFrame)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->updateMoveTo(
                convert(waypoints), core::NavigationFrameNames.to_name(navigationFrame), configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call update on navigator: " << e.what();
            throw;
        }
    }
 
    void
    IceNavigatorHandle::setVelocityFactor(float velocityFactor)
    {
        ARMARX_TRACE;
 
        try
        {
            navigator_->setVelocityFactor(velocityFactor, configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call setVelocityFactor on navigator: " << e.what();
        }
    }
 
    void
    IceNavigatorHandle::pause()
    {
        try
        {
            navigator_->pause(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call pause on navigator: " << e.what();
        }
    }
 
    void
    IceNavigatorHandle::resume()
    {
        try
        {
            navigator_->resume(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call resume on navigator: " << e.what();
        }
    }
 
    void
    IceNavigatorHandle::stop()
    {
        try
        {
            navigator_->stop(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call stop on navigator: " << e.what();
            throw;
        }
    }
 
    bool
    IceNavigatorHandle::isPaused() const noexcept
    {
        try
        {
            return navigator_->isPaused(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call isPaused on navigator: " << e.what();
            return false;
        }
    }
 
    bool
    IceNavigatorHandle::isStopped() const noexcept
    {
        try
        {
            return navigator_->isStopped(configId_);
        }
        catch (const Ice::LocalException& e)
        {
            ARMARX_WARNING << deactivateSpam(1) << "Failed to call isStopped on navigator: " << e.what();
            return false;
        }
    }
 
 
} // namespace armarx::navigation::client