YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
Map2D_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#include "Map2D_nws_yarp.h"
7
8#include <yarp/os/Log.h>
10#include <yarp/os/LogStream.h>
11
13#include <yarp/dev/IMap2D.h>
15
16#include <yarp/math/Math.h>
17
18#include <cstdlib>
19#include <fstream>
20#include <limits>
21#include <mutex>
22#include <sstream>
23
24using namespace yarp::sig;
25using namespace yarp::dev;
26using namespace yarp::dev::Nav2D;
27using namespace yarp::os;
28
29namespace {
30YARP_LOG_COMPONENT(MAP2D_NWS_YARP, "yarp.device.map2D_nws_yarp")
31}
32
40
42
43bool Map2D_nws_yarp::read(yarp::os::ConnectionReader& connection)
44{
45 if (!connection.isValid()) { return false;}
46 if (!m_RPC) { return false;}
47
48 std::lock_guard<std::mutex> lock (m_mutex);
49
50 bool b = m_RPC->read(connection);
51 if (b)
52 {
53 return true;
54 }
55 yCDebug(MAP2D_NWS_YARP) << "read() Command failed";
56 return false;
57}
58
60{
61 if (!parseParams(config)) { return false; }
62
63 //open rpc port
64 if (!m_rpcPort.open(m_name))
65 {
66 yCError(MAP2D_NWS_YARP, "Failed to open port %s", m_name.c_str());
67 return false;
68 }
69 m_rpcPort.setReader(*this);
70
71 yCInfo(MAP2D_NWS_YARP) << "Waiting for device to attach";
72
73 return true;
74}
75
77{
78 yCTrace(MAP2D_NWS_YARP, "Close");
79
80 m_rpcPort.interrupt();
81 m_rpcPort.close();
82
83 detach();
84
85 return true;
86}
87
89{
90 std::lock_guard lock (m_mutex);
91
92 m_iMap2D = nullptr;
93
94 if (m_RPC)
95 {
96 delete m_RPC;
97 m_RPC = nullptr;
98 }
99
100 return true;
101}
102
104{
105 std::lock_guard lock(m_mutex);
106
107 if (driver->isValid())
108 {
109 driver->view(m_iMap2D);
110 m_RPC = new IMap2DRPCd(m_iMap2D);
111 }
112
113 if (nullptr == m_iMap2D)
114 {
115 yCError(MAP2D_NWS_YARP, "Subdevice passed to attach method is invalid");
116 return false;
117 }
118
119 yCInfo(MAP2D_NWS_YARP, "Attach done");
120 return true;
121}
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
Map2D_nws_yarp()
Map2D_nws_yarp.
bool detach() override
Detach the object (you must have first called attach).
bool attach(yarp::dev::PolyDriver *driver) override
Attach to another object.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool close() override
Close the DeviceDriver.
bool view(T *&x)
Get an interface to the device driver.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void setReader(PortReader &reader) override
Set an external reader for port data.
void close() override
Stop port activity.
void interrupt() override
Interrupt any current reads or writes attached to the port.
A mini-server for performing network communication in the background.
An interface for reading from a network connection.
A base class for nested structures that can be searched.
Definition Searchable.h:31
#define yCInfo(component,...)
#define yCError(component,...)
#define yCTrace(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.