YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Localization2D_nws_yarp.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#define _USE_MATH_DEFINES
7
9
10#include <yarp/os/Bottle.h>
12#include <yarp/os/LogStream.h>
13#include <yarp/os/Network.h>
14#include <yarp/os/Port.h>
15#include <yarp/os/RFModule.h>
16#include <yarp/os/Time.h>
17
18#include <yarp/sig/Vector.h>
19
22#include <yarp/dev/IMap2D.h>
23#include <yarp/dev/PolyDriver.h>
24
25#include <yarp/math/Math.h>
26
27#include <cmath>
28
31using namespace yarp::os;
32using namespace yarp::dev;
33using namespace yarp::dev::Nav2D;
34
35#define DEFAULT_THREAD_PERIOD 0.01
36
37namespace
38{
39 YARP_LOG_COMPONENT(LOCALIZATION2D_NWS_YARP, "yarp.device.localization2D_nws_yarp")
40}
41
42//------------------------------------------------------------------------------------------------------------------------------
43
48
50{
51 if (driver->isValid())
52 {
53 driver->view(iLoc);
55 }
56
57 if (nullptr == iLoc)
58 {
59 yCError(LOCALIZATION2D_NWS_YARP, "Subdevice passed to attach method is invalid");
60 return false;
61 }
62
63 //initialize m_current_position and m_current_status, if available
64 bool ret = true;
66 Map2DLocation loc;
67 ret &= iLoc->getLocalizationStatus(status);
69 if (ret)
70 {
71 m_RPC.m_current_status = status;
73 }
74 else
75 {
76 yCWarning(LOCALIZATION2D_NWS_YARP) << "Localization data not yet available during server initialization";
77 }
78
79 if (!m_rpcPort.open(m_rpcPortName.c_str()))
80 {
81 yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_rpcPortName.c_str());
82 return false;
83 }
84
86 return PeriodicThread::start();
87}
88
90{
92 {
94 }
97 iLoc = nullptr;
98 return true;
99}
100
102{
103 if (!parseParams(config)) { return false; }
104
105 //check some parameters consistency
107 {
109
111 {yCWarning(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically is true, but data is not published because publish_odometry is false. This configuration is strange";}
113 {yCWarning(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically is true, but data is not published because publish_location is false. This configuration is strange";}
114 }
115 else
116 {
118 }
119
120 //Some debug prints
122 {
123 yCInfo(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically requested, Period:" << m_GENERAL_period;
124 }
125 else
126 {
127 yCInfo(LOCALIZATION2D_NWS_YARP) << "retrieve_position_periodically NOT requested. Localization data obtained asynchronously.";
128 }
129
130
132 m_rpcPortName = m_local_name + "/rpc";
133 m_2DLocationPortName = m_local_name + "/streaming:o";
134 m_odometryPortName = m_local_name + "/odometry:o";
135
136 yCInfo(LOCALIZATION2D_NWS_YARP) << "Waiting for device to attach";
137
139
140 m_rpcPort.setReader(*this);
141
142 yCInfo(LOCALIZATION2D_NWS_YARP) << "Waiting for device to attach";
143
144 return true;
145}
146
148{
150 {
151 yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_2DLocationPortName.c_str());
152 return false;
153 }
154
156 {
157 yCError(LOCALIZATION2D_NWS_YARP, "Failed to open port %s", m_odometryPortName.c_str());
158 return false;
159 }
160 return true;
161}
162
164{
166// m_2DLocationPort.interrupt();
168// m_odometryPort.interrupt();
170}
171
173{
175
176 detach();
177
178 yCDebug(LOCALIZATION2D_NWS_YARP) << "Execution terminated";
179 return true;
180}
181
183{
184 bool b = m_RPC.read(connection);
185 if (b)
186 {
187 return true;
188 }
189 else
190 {
191 yCDebug(LOCALIZATION2D_NWS_YARP) << "read() Command failed";
192 return false;
193 }
194}
195
197{
200 {
201 yCInfo(LOCALIZATION2D_NWS_YARP) << "Running";
203 }
204
205 //enter the critical section
206 m_RPC.getMutex()->lock();
207 {
209 {
211 if (ret == false)
212 {
213 yCError(LOCALIZATION2D_NWS_YARP) << "getLocalizationStatus() failed";
214 }
215
216 if (m_RPC.m_current_status == LocalizationStatusEnum::localization_status_localized_ok)
217 {
219 if (ret2 == false)
220 {
221 yCError(LOCALIZATION2D_NWS_YARP) << "getCurrentPosition() failed";
222 }
223 else
224 {
226 }
228 if (ret3 == false)
229 {
230 //yCError(LOCALIZATION2D_NWS_YARP) << "getEstimatedOdometry() failed";
231 }
232 else
233 {
235 }
236 }
237 else
238 {
239 yCWarning(LOCALIZATION2D_NWS_YARP, "The system is not properly localized!");
240 }
241 }
242 }
243 m_RPC.getMutex()->unlock();
244
246 publish_odometry_on_yarp_port();
247 }
249 publish_2DLocation_on_yarp_port();
250 }
251}
252
253void Localization2D_nws_yarp::publish_odometry_on_yarp_port()
254{
256 {
259
260 //send data to port
263 }
264}
265
266void Localization2D_nws_yarp::publish_2DLocation_on_yarp_port()
267{
269 {
271 if (m_RPC.m_current_status == LocalizationStatusEnum::localization_status_localized_ok)
272 {
274 }
275 else
276 {
278 temp_loc.x = std::nan("");
279 temp_loc.y = std::nan("");
280 temp_loc.theta = std::nan("");
281 loc = temp_loc;
282 }
283
284 //send data to port
287 }
288}
define control board standard interfaces
bool ret
#define DEFAULT_THREAD_PERIOD
contains the definition of a Vector type
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::dev::OdometryData m_current_odometry
void setInterface(yarp::dev::Nav2D::ILocalization2D *_iloc)
yarp::dev::Nav2D::LocalizationStatusEnum m_current_status
yarp::dev::Nav2D::Map2DLocation m_current_position
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void run() override
Loop function.
yarp::os::BufferedPort< yarp::dev::OdometryData > m_odometryPort
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::dev::Nav2D::ILocalization2D * iLoc
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool threadInit() override
Initialization method.
void threadRelease() override
Release method.
bool detach() override
Detach the object (you must have first called attach).
yarp::os::BufferedPort< yarp::dev::Nav2D::Map2DLocation > m_2DLocationPort
bool view(T *&x)
Get an interface to the device driver.
virtual bool getEstimatedOdometry(yarp::dev::OdometryData &odom)=0
Gets the estimated odometry the robot, including its velocity expressed in the world and in the local...
virtual bool getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the current position of the robot w.r.t world reference frame.
virtual bool getLocalizationStatus(LocalizationStatusEnum &status)=0
Gets the current status of the localization task.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
int getOutputCount() override
Determine how many output connections this port has.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An interface for reading from a network connection.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
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...
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:511
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition Port.cpp:383
void close() override
Stop port activity.
Definition Port.cpp:363
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
void delay(double seconds)
Wait for a certain number of seconds.
Definition Time.cpp:111
An interface to the operating system, including Port based communication.