fakeNavigationDev.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
 
#include <yarp/os/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/Time.h>
#include <yarp/os/Port.h>
#include <yarp/os/LogComponent.h>
#include <yarp/os/LogStream.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include "fakeNavigationDev.h"
#include <math.h>
#include <cmath>
 
using namespace yarp::dev;
using namespace yarp::dev::Nav2D;
 
namespace {
YARP_LOG_COMPONENT(FAKENAVIGATION, "yarp.device.fakeNavigation")
}
 
 
bool fakeNavigation :: open(yarp::os::Searchable& config)
{
#if 1
 
    yCDebug(FAKENAVIGATION) << "config configuration: \n" << config.toString().c_str();
 
    std::string context_name = "robotGoto";
    std::string file_name = "robotGoto_cer.ini";
 
    if (config.check("context"))   context_name = config.find("context").asString();
    if (config.check("from")) file_name    = config.find("from").asString();
 
    yarp::os::ResourceFinder rf;
    rf.setDefaultContext(context_name.c_str());
    rf.setDefaultConfigFile(file_name.c_str());
 
    yarp::os::Property p;
    std::string configFile = rf.findFile("from");
    if (configFile != "") p.fromConfigFile(configFile.c_str());
    yCDebug(FAKENAVIGATION) << "robotGotoDev configuration: \n" << p.toString().c_str();
 
#else
    Property p;
    p.fromString(config.toString());
#endif
    navThread = new fakeNavigationThread(0.010, p);
 
    if (!navThread->start())
    {
        delete navThread;
        return false;
    }
 
    return true;
}
 
fakeNavigation::fakeNavigation()
{
    navThread =NULL;
}
 
//module cleanup
bool fakeNavigation:: close()
{
    navThread->stop();
    delete navThread;
    navThread =NULL;
 
    return true;
}
 
bool fakeNavigation::gotoTargetByAbsoluteLocation(Map2DLocation loc)
{
    yCInfo(FAKENAVIGATION) << "gotoTargetByAbsoluteLocation not yet implemented";
    return true;
}
 
bool fakeNavigation::gotoTargetByRelativeLocation(double x, double y, double theta)
{
    yCInfo(FAKENAVIGATION) << "gotoTargetByRelativeLocation not yet implemented";
    return true;
}
 
bool fakeNavigation::gotoTargetByRelativeLocation(double x, double y)
{
    yCInfo(FAKENAVIGATION) << "gotoTargetByRelativeLocation not yet implemented";
    return true;
}
 
bool fakeNavigation::applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout)
{
    yCInfo(FAKENAVIGATION) << "applyVelocityCommand not yet implemented";
    return true;
}
 
bool fakeNavigation::stopNavigation()
{
    yCInfo(FAKENAVIGATION) << "stopNavigation not yet implemented";
    return true;
}
 
bool fakeNavigation::suspendNavigation(double time)
{
    yCInfo(FAKENAVIGATION) << "suspendNavigation not yet implemented";
    return true;
}
 
bool fakeNavigation::resumeNavigation()
{
    yCInfo(FAKENAVIGATION) << "resumeNavigation not yet implemented";
    return true;
}
 
bool fakeNavigation::getAllNavigationWaypoints(yarp::dev::Nav2D::TrajectoryTypeEnum trajectory_type, yarp::dev::Nav2D::Map2DPath& waypoints)
{
    yCInfo(FAKENAVIGATION) << "getAllNavigationWaypoints not yet implemented";
    return true;
}
 
bool fakeNavigation::getCurrentNavigationWaypoint(Map2DLocation& curr_waypoint)
{
    yCInfo(FAKENAVIGATION) << "getCurrentNavigationWaypoint not yet implemented";
    return true;
}
 
bool fakeNavigation::getCurrentNavigationMap(yarp::dev::Nav2D::NavigationMapTypeEnum map_type, MapGrid2D& map)
{
    yCInfo(FAKENAVIGATION) << "getCurrentNavigationMap not yet implemented";
    return true;
}
 
bool fakeNavigation::getNavigationStatus(yarp::dev::Nav2D::NavigationStatusEnum& status)
{
    yCInfo(FAKENAVIGATION) << "getNavigationStatus not yet implemented";
    return true;
}
 
bool fakeNavigation::getAbsoluteLocationOfCurrentTarget(Map2DLocation& target)
{
    yCInfo(FAKENAVIGATION) << "getAbsoluteLocationOfCurrentTarget not yet implemented";
    return true;
}
 
bool fakeNavigation::recomputeCurrentNavigationPath()
{
    yCInfo(FAKENAVIGATION) << "recomputeCurrentNavigationPath not yet implemented";
    return true;
}
 
bool fakeNavigation::getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta)
{
    yCInfo(FAKENAVIGATION) << "getRelativeLocationOfCurrentTarget not yet implemented";
    return true;
}
 
fakeNavigationThread::fakeNavigationThread(double _period, yarp::os::Searchable& _cfg) : PeriodicThread(_period)
{
}
 
bool fakeNavigationThread::threadInit()
{
    return true;
}
 
void fakeNavigationThread::threadRelease()
{
 
}
 
void fakeNavigationThread::run()
{
 
}