YARP
Yet Another Robot Platform
fakeNavigationDev.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: LGPL-2.1-or-later
4 */
5
6#include <yarp/os/Network.h>
7#include <yarp/os/RFModule.h>
8#include <yarp/os/Time.h>
9#include <yarp/os/Port.h>
11#include <yarp/os/LogStream.h>
13#include "fakeNavigationDev.h"
14#include <math.h>
15#include <cmath>
16
17using namespace yarp::dev;
18using namespace yarp::dev::Nav2D;
19
20namespace {
21YARP_LOG_COMPONENT(FAKENAVIGATION, "yarp.device.fakeNavigation")
22}
23
24
26{
27#if 1
28
29 yCDebug(FAKENAVIGATION) << "config configuration: \n" << config.toString().c_str();
30
31 std::string context_name = "robotGoto";
32 std::string file_name = "robotGoto_cer.ini";
33
34 if (config.check("context")) {
35 context_name = config.find("context").asString();
36 }
37 if (config.check("from")) {
38 file_name = config.find("from").asString();
39 }
40
42 rf.setDefaultContext(context_name.c_str());
43 rf.setDefaultConfigFile(file_name.c_str());
44
46 std::string configFile = rf.findFile("from");
47 if (configFile != "") {
48 p.fromConfigFile(configFile.c_str());
49 }
50 yCDebug(FAKENAVIGATION) << "robotGotoDev configuration: \n" << p.toString().c_str();
51
52#else
53 Property p;
54 p.fromString(config.toString());
55#endif
56 navThread = new fakeNavigationThread(0.010, p);
57
58 if (!navThread->start())
59 {
60 delete navThread;
61 return false;
62 }
63
64 return true;
65}
66
68{
69 navThread =NULL;
70}
71
72//module cleanup
74{
75 navThread->stop();
76 delete navThread;
77 navThread =NULL;
78
79 return true;
80}
81
83{
85 {
87 m_absgoal_loc = loc;
88 }
89 return true;
90}
91
92bool fakeNavigation::gotoTargetByRelativeLocation(double x, double y, double theta)
93{
94 yCInfo(FAKENAVIGATION) << "gotoTargetByRelativeLocation not yet implemented";
95 return true;
96}
97
99{
100 yCInfo(FAKENAVIGATION) << "gotoTargetByRelativeLocation not yet implemented";
101 return true;
102}
103
104bool fakeNavigation::applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout)
105{
106 yCInfo(FAKENAVIGATION) << "applyVelocityCommand not yet implemented";
107 return true;
108}
109
110bool fakeNavigation::getLastVelocityCommand(double& x_vel, double& y_vel, double& theta_vel)
111{
112 yCInfo(FAKENAVIGATION) << "getLastVelocityCommand not yet implemented";
113 return true;
114}
115
117{
119 m_absgoal_loc=Map2DLocation();
120 return true;
121}
122
124{
126 {
128 }
129 return true;
130}
131
133{
135 {
137 }
138 return true;
139}
140
142{
143 yCInfo(FAKENAVIGATION) << "getAllNavigationWaypoints not yet implemented";
144 return true;
145}
146
148{
149 yCInfo(FAKENAVIGATION) << "getCurrentNavigationWaypoint not yet implemented";
150 return true;
151}
152
154{
155 yCInfo(FAKENAVIGATION) << "getCurrentNavigationMap not yet implemented";
156 return true;
157}
158
160{
161 status = m_status;
162 return true;
163}
164
166{
167 target=m_absgoal_loc;
168 return true;
169}
170
172{
174 {
175 //do something
176 }
177 return true;
178}
179
180bool fakeNavigation::getRelativeLocationOfCurrentTarget(double& x, double& y, double& theta)
181{
182 yCInfo(FAKENAVIGATION) << "getRelativeLocationOfCurrentTarget not yet implemented";
183 return true;
184}
185
186fakeNavigationThread::fakeNavigationThread(double _period, yarp::os::Searchable& _cfg) : PeriodicThread(_period)
187{
188}
189
191{
192 return true;
193}
194
196{
197
198}
199
201{
202
203}
define control board standard interfaces
virtual void threadRelease() override
Release method.
fakeNavigationThread(double _period, yarp::os::Searchable &_cfg)
virtual void run() override
Loop function.
virtual bool threadInit() override
Initialization method.
bool resumeNavigation() override
Resume a previously suspended navigation task.
virtual bool close() override
Close the DeviceDriver.
bool getNavigationStatus(yarp::dev::Nav2D::NavigationStatusEnum &status) override
Gets the current status of the navigation task.
bool getCurrentNavigationWaypoint(yarp::dev::Nav2D::Map2DLocation &curr_waypoint) override
Returns the current waypoint pursued by the navigation algorithm.
bool getLastVelocityCommand(double &x_vel, double &y_vel, double &theta_vel) override
Returns the last applied velocity command.
bool getRelativeLocationOfCurrentTarget(double &x, double &y, double &theta) override
Gets the last navigation target in the robot reference frame.
bool stopNavigation() override
Terminates the current navigation task.
bool suspendNavigation(double time) override
Ask to the robot to suspend the current navigation task for a defined amount of time.
bool applyVelocityCommand(double x_vel, double y_vel, double theta_vel, double timeout=0.1) override
Apply a velocity command.
bool gotoTargetByRelativeLocation(double x, double y, double theta) override
Ask the robot to reach a position defined in the robot reference frame.
virtual bool open(yarp::os::Searchable &config) override
Open the DeviceDriver.
bool recomputeCurrentNavigationPath() override
Forces the navigation system to recompute the path from the current robot position to the current goa...
bool getAbsoluteLocationOfCurrentTarget(yarp::dev::Nav2D::Map2DLocation &target) override
Gets the last navigation target in the world reference frame.
fakeNavigationThread * navThread
bool gotoTargetByAbsoluteLocation(yarp::dev::Nav2D::Map2DLocation loc) override
Ask the robot to reach a position defined in the world reference frame.
bool getAllNavigationWaypoints(yarp::dev::Nav2D::TrajectoryTypeEnum trajectory_type, yarp::dev::Nav2D::Map2DPath &waypoints) override
Returns the list of waypoints generated by the navigation algorithm.
bool getCurrentNavigationMap(yarp::dev::Nav2D::NavigationMapTypeEnum map_type, yarp::dev::Nav2D::MapGrid2D &map) override
Returns the current navigation map processed by the navigation algorithm.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition: Property.h:33
std::string toString() const override
Return a standard text representation of the content of the object.
Definition: Property.cpp:1069
void fromString(const std::string &txt, bool wipe=true)
Interprets a string as a list of properties.
Definition: Property.cpp:1063
bool fromConfigFile(const std::string &fname, bool wipe=true)
Interprets a file as a list of properties.
Definition: Property.cpp:1098
Helper class for finding config files and other external resources.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
std::string findFile(const std::string &name)
Find the full path to a file.
bool setDefaultConfigFile(const std::string &fname)
Provide a default value for the configuration file (can be overridden from command line with the –fro...
A base class for nested structures that can be searched.
Definition: Searchable.h:56
virtual bool check(const std::string &key) const =0
Check if there exists a property of the given name.
virtual std::string toString() const =0
Return a standard text representation of the content of the object.
virtual Value & find(const std::string &key) const =0
Gets a value corresponding to a given keyword.
virtual std::string asString() const
Get string value.
Definition: Value.cpp:234
#define yCInfo(component,...)
Definition: LogComponent.h:171
#define yCDebug(component,...)
Definition: LogComponent.h:128
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:76
For streams capable of holding different kinds of content, check what they actually have.