iCub-main
FakeRawDataPublisherTester.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2024 iCub Facility - Istituto Italiano di Tecnologia
3  * Author: Jacopo Losi
4  * email: jacopo.losi@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
19 
20 #include <yarp/os/Network.h>
21 #include <yarp/os/Log.h>
22 #include <yarp/os/LogStream.h>
23 
24 using namespace std;
25 using namespace yarp::os;
26 
27 
28 bool FakeRawDataPublisherTester::configure(yarp::os::ResourceFinder& rf)
29 {
30  // Read configuration file
31  Bottle &conf_group = rf.findGroup("GENERAL");
32  if (conf_group.isNull())
33  {
34  yWarning() << "Missing GENERAL group! The module uses the default values";
35  }
36  else
37  {
38  if(conf_group.check("portprefix")) { m_portPrefix = conf_group.find("portprefix").asString(); }
39  if(conf_group.check("period")) { _updatePeriod = conf_group.find("period").asFloat64(); }
40  if(conf_group.check("robotname")) { _robotName = conf_group.find("robotname").asString(); }
41 
42  }
43 
44  // Create remote motion control device
45  Property options;
46  options.put("device", "rawValuesPublisherClient");
47  options.put("remote", "/" + _robotName + m_portPrefix);
48  options.put("local", "/" + _robotName + m_portPrefix);
49 
50 
51  yDebug() << "++++ config:\n"
52  << "\t portprefix: " << m_portPrefix << "\n"
53  << "\t period: " << _updatePeriod << "\n"
54  << "\t robotname: " << _robotName << "\n";
55 
56  _rawDataPublisherDevice.open(options);
57 
58  if (!_rawDataPublisherDevice.isValid())
59  {
60  yError() << "Unable to open device driver. Aborting...";
61  return false;
62  }
63 
64  if (!_rawDataPublisherDevice.view(_iravap) || _iravap==nullptr)
65  {
66  yError() << "Unable to open motor raw interface. Aborting...";
67  return false;
68  }
69 
70  // open the communication port for streaming data
71  if(!_outputPort.open(m_portPrefix + "/rawdata_publisher:o"))
72  {
73  yError() << "Error opening output port for rawdata publisher";
74  return false;
75  }
76 
77  // Initialize the local map as an empty map --> will be filled later
78  _rawDataValuesMap = {};
79 
80  return true;
81 }
82 
84 {
85  // Closing port explicitely
86  yInfo() << "Calling close functionality\n";
87 
88  return true;
89 }
90 
92 {
93  return _updatePeriod;
94 }
95 
97 {
98  Bottle &b = _outputPort.prepare();
99  b.clear();
100 
101  if(!_iravap->getRawDataMap(_rawDataValuesMap))
102  {
103  yError() << "Map of raw values to be streamed cannot be retrieved. Unpreparing...\n";
104  _outputPort.unprepare();
105  return true;
106  }
107  else
108  {
109  for (auto [key, value] : _rawDataValuesMap)
110  {
111  b.addString(key);
112  Bottle &bKeyContent = b.addList();
113  for (auto el : value)
114  {
115  bKeyContent.addInt32(el);
116  }
117  }
118 
119  _stamp.update();
120  _outputPort.setEnvelope(_stamp);
121  _outputPort.write();
122  }
123 
124  return true;
125 }
126 
127 
129 {;}
130 
132 {;}
bool configure(yarp::os::ResourceFinder &rf)