iCub-main
Loading...
Searching...
No Matches
RawValuesPublisherServer.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2024 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7
8// yarp includes
9#include <yarp/os/BufferedPort.h>
10#include <yarp/os/Bottle.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
13#include <yarp/os/LogComponent.h>
14
15namespace {
16 YARP_LOG_COMPONENT(RAWVALUESPUBLISHERSERVER, "iCub.debugLibrary.rawvaluespublisherserver")
17}
18
19#undef DEBUG_RAW_VALUES_MACRO
20
21using namespace yarp::os;
22
23
27
29
30bool RawValuesPublisherServer::open(yarp::os::Searchable& config)
31{
32 if (!parseParams(config))
33 {
34 return false;
35 }
36 // set the period. m_period var is defined in the <device_name>_ParamsParser.h file
37 // and updates at config file parsing as per m_name var.
38 m_threadPeriodInS = m_period / 1000.0;
39
40 if(m_threadPeriodInS <= 0)
41 {
42 yCError(RAWVALUESPUBLISHERSERVER)
43 << "Period parameter is present with value:"
44 << m_threadPeriodInS << "but it is not a positive integer. Closing...";
45 return false;
46 }
47
48 // set streaming port
49 m_streamingPortName = m_name + "/rawdata:o";
50 // Open port
51 if(!m_streamingRawDataPort.open(m_streamingPortName))
52 {
53 yCError(RAWVALUESPUBLISHERSERVER)
54 << "Error opening streaming raw data port:" << m_streamingPortName;
55 close();
56 return false;
57 }
58
59 m_rpcPortName = m_name + "/rpc:o";
60
61 // Attach to port
62 if(!this->yarp().attachAsServer(m_rpcRawDataPort))
63 {
64 yCError(RAWVALUESPUBLISHERSERVER)
65 << "Failure in attaching RPC port to thrift RPC interface";
66 close();
67 return false;
68 }
69 // Open port
70 if(!m_rpcRawDataPort.open(m_rpcPortName))
71 {
72 yCError(RAWVALUESPUBLISHERSERVER)
73 << "Failure in opening rpc port:" << m_rpcPortName;
74 close();
75 return false;
76 }
77
78 yCInfo(RAWVALUESPUBLISHERSERVER) << "Open ports completes";
79 return true;
80}
81
83{
84 return(this->detachAll());
85}
86
91
92bool RawValuesPublisherServer::attachAll(const yarp::dev::PolyDriverList &p)
93{
94 if (p.size() != 1 )
95 {
96 yCError(RAWVALUESPUBLISHERSERVER)
97 << "Trying to expose" << p.size() << "device(s). Expected only one device to be exposed to YARP ports. Closing...";
98 close();
99 return false;
100 }
101
102 yarp::dev::PolyDriver* poly = p[0]->poly;
103
104 if(!poly)
105 {
106 yCError(RAWVALUESPUBLISHERSERVER)
107 << "NullPointerException when getting the polyDriver at attachAll.";
108 close();
109 return false;
110 }
111
112 // View all the interfaces
113 if(!poly->view(m_iRawValuesPublisher))
114 {
115 yCError(RAWVALUESPUBLISHERSERVER)
116 << "Failure in viewing raw values publisher interface";
117 close();
118 return false;
119 }
120
121 // Set rate period
122 if(!this->setPeriod(m_threadPeriodInS))
123 {
124 yCError(RAWVALUESPUBLISHERSERVER)
125 << "Failure in setting periodic thread period";
126 close();
127 return false;
128 }
129
130 // Populate the RPC data to be served on the RPC port
131 if(!populateMetadata(m_mapOfMetadata))
132 {
133 yCError(RAWVALUESPUBLISHERSERVER, "Failure in getMetadata()");
134 close();
135 return false;
136 }
137
138 // Start periodic thread
139 if(!this->start())
140 {
141 yCError(RAWVALUESPUBLISHERSERVER)
142 << "Failure in starting periodic thread";
143 close();
144 return false;
145 }
146
147 yCInfo(RAWVALUESPUBLISHERSERVER) << "Attach completes";
148
149 return true;
150}
151
153{
154 // Stop periodicThread if running
155 if (this->isRunning())
156 {
157 this->stop();
158 }
159
160 m_rpcRawDataPort.close();
161 m_streamingRawDataPort.close();
162
163 yCInfo(RAWVALUESPUBLISHERSERVER) << "Detach complete";
164
165 return true;
166}
167
172
174{
175 rawValuesDataVectorsMap &rdm = m_streamingRawDataPort.prepare();
176 rdm.vectorsMap.clear();
177
178 if(!m_iRawValuesPublisher->getRawDataMap(rdm.vectorsMap))
179 {
180 m_streamingRawDataPort.unprepare();
181 return;
182 }
183
184 m_stamp.update();
185 m_streamingRawDataPort.setEnvelope(m_stamp);
186 m_streamingRawDataPort.write();
187}
188
189// Private nmethods
190bool RawValuesPublisherServer::populateMetadata(rawValuesKeyMetadataMap &metamap)
191{
192 if(!m_iRawValuesPublisher->getMetadataMap(metamap))
193 {
194 return false;
195 }
196
197 #ifdef DEBUG_RAW_VALUES_MACRO
198 for (auto [k,v] : metamap.metadataMap)
199 {
200 yCDebug(RAWVALUESPUBLISHERSERVER) << "Metadata at key:" << k << "with size:" << v.size;
201 for (size_t i = 0; i < v.size; i++)
202 {
203 yCDebug(RAWVALUESPUBLISHERSERVER) << "Metadata element:" << v.rawValueNames[i];
204 }
205
206 }
207 #endif
208
209
210 return true;
211}
bool parseParams(const yarp::os::Searchable &config) override
bool open(yarp::os::Searchable &params) override
rawValuesKeyMetadataMap getMetadata() override
Read the rawvalues metadata necessary to configure the RawValuesPublisherClient device.
bool attachAll(const yarp::dev::PolyDriverList &p) override
virtual bool getMetadataMap(rawValuesKeyMetadataMap &metamap)=0
virtual bool getRawDataMap(std::map< std::string, std::vector< std::int32_t > > &map)=0
rawValuesDataVectorsMap IDL struct of a map of vectors to store the raw value data sent by the device
std::map< std::string, std::vector< std::int32_t > > vectorsMap
contain a map of vectors of the raw data as <string, vector> the user wanna send from low to higher l...
Copyright (C) 2008 RobotCub Consortium.