YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
stateExtendedReader.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
8#include <cstring>
9
11#include <yarp/os/Time.h>
12#include <yarp/os/Network.h>
13#include <yarp/os/Thread.h>
14#include <yarp/os/Vocab.h>
15#include <yarp/os/Stamp.h>
16#include <yarp/os/LogStream.h>
17
18#include <yarp/sig/Vector.h>
19
21#include <yarp/dev/PolyDriver.h>
25
26
27using namespace yarp::os;
28using namespace yarp::dev;
29using namespace yarp::sig;
30
32{
33 mutex.lock();
34 count=0;
35 deltaT=0;
36 deltaTMax=0;
37 deltaTMin=1e22;
38 now=Time::now();
39 prev=now;
40 mutex.unlock();
41}
42
44 deltaTMax{0},
45 deltaTMin{1e22},
46 now{Time::now()},
47 prev{now},
48 timeout{0.5},
49 valid{false},
50 count{0}
51{
52}
53
54void StateExtendedInputPort::init(int numberOfJoints)
55{
56 last.jointPosition.resize(numberOfJoints);
57 last.jointVelocity.resize(numberOfJoints);
58 last.jointAcceleration.resize(numberOfJoints);
59 last.motorPosition.resize(numberOfJoints);
60 last.motorVelocity.resize(numberOfJoints);
61 last.motorAcceleration.resize(numberOfJoints);
62 last.torque.resize(numberOfJoints);
63 last.pwmDutycycle.resize(numberOfJoints);
64 last.temperature.resize(numberOfJoints);
65 last.current.resize(numberOfJoints);
66 last.controlMode.resize(numberOfJoints);
67 last.interactionMode.resize(numberOfJoints);
68}
69
71{
72 now=Time::now();
73 mutex.lock();
74
75 if (count>0)
76 {
77 double tmpDT=now-prev;
78 deltaT+=tmpDT;
79 if (tmpDT > deltaTMax) {
80 deltaTMax = tmpDT;
81 }
82 if (tmpDT < deltaTMin) {
83 deltaTMin = tmpDT;
84 }
85 }
86
87 prev=now;
88 count++;
89
90 valid=true;
91 last=v;
92 getEnvelope(lastStamp);
93 //check that timestamp are available
94 if (!lastStamp.isValid()) {
95 lastStamp.update(now);
96 }
97 mutex.unlock();
98}
99
100void StateExtendedInputPort::setTimeout(const double& timeout) {
101 this->timeout = timeout;
102}
103
104bool StateExtendedInputPort::getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime)
105{
106 mutex.lock();
107 bool ret = valid;
108 if (ret)
109 {
110 switch(field)
111 {
112 case VOCAB_ENCODER:
113 *data = last.jointPosition[j];
114 ret = last.jointPosition_isValid;
115 break;
116
118 ret = last.jointVelocity_isValid;
119 *data = last.jointVelocity[j];
120 break;
121
123 ret = last.jointAcceleration_isValid;
124 *data = last.jointAcceleration[j];
125 break;
126
128 ret = last.motorPosition_isValid;
129 *data = last.motorPosition[j];
130 break;
131
133 ret = last.motorVelocity_isValid;
134 *data = last.motorVelocity[j];
135 break;
136
138 ret = last.motorAcceleration_isValid;
139 *data = last.motorAcceleration[j];
140 break;
141
142 case VOCAB_TRQ:
143 ret = last.torque_isValid;
144 *data = last.torque[j];
145 break;
146
148 ret = last.pwmDutycycle_isValid;
149 *data = double(last.pwmDutycycle[j]);
150 break;
151
153 ret = last.current_isValid;
154 *data = last.current[j];
155 break;
156
158 ret = last.temperature_isValid;
159 *data = double(last.temperature[j]);
160 break;
161
162
163 default:
164 yCError(CONTROLBOARD_NWC_YARP) << "Internal error while reading data. Cannot get 'single' data of type " << yarp::os::Vocab32::decode(field);
165 break;
166 }
167
169 stamp = lastStamp;
170 double current_time = Time::now();
171 if (ret && ((current_time - localArrivalTime) > timeout)) {
172 ret = false;
173 }
174 }
175 mutex.unlock();
176
177 return ret;
178}
179
180bool StateExtendedInputPort::getLastSingle(int j, int field, int *data, Stamp &stamp, double &localArrivalTime)
181{
182 mutex.lock();
183 bool ret = valid;
184 if (ret)
185 {
186 switch(field)
187 {
189 ret = last.controlMode_isValid;
190 *data = last.controlMode[j];
191 break;
192
194 ret = last.interactionMode_isValid;
195 *data = last.interactionMode[j];
196 break;
197
198 default:
199 yCError(CONTROLBOARD_NWC_YARP) << "Internal error while reading data. Cannot get 'single' data of type " << yarp::os::Vocab32::decode(field);
200 break;
201 }
203 stamp = lastStamp;
204 double curr_time = Time::now();
205 if (ret && (( curr_time - localArrivalTime) > timeout)) {
206 ret = false;
207 }
208 }
209 mutex.unlock();
210 return ret;
211}
212
214{
215 mutex.lock();
216 bool ret = valid;
217 if (ret)
218 {
219 switch(field)
220 {
221 case VOCAB_ENCODERS:
222 ret = last.jointPosition_isValid;
224 break;
225
227 ret = last.jointVelocity_isValid;
229 break;
230
232 ret = last.jointAcceleration_isValid;
234 break;
235
237 ret = last.motorPosition_isValid;
239 break;
240
242 ret = last.motorVelocity_isValid;
244 break;
245
247 ret = last.motorAcceleration_isValid;
249 break;
250
251 case VOCAB_TRQS:
252 ret = last.torque_isValid;
253 memcpy(data, last.torque.data(), last.torque.size() * last.torque.getElementSize() );
254 break;
255
257 ret = last.pwmDutycycle_isValid;
258
259 // In the interface the pwmDutycycle is a double, but in the jointData it is a float so we need to copy it manually
260 for (size_t i = 0; i < last.pwmDutycycle.size(); i++) {
261 data[i] = last.pwmDutycycle[i];
262 }
263 break;
264
266 ret = last.current_isValid;
267 memcpy(data, last.current.data(), last.current.size() * last.current.getElementSize());
268 break;
269
271 ret = last.temperature_isValid;
272
273 // In the interface the temperature is a double, but in the jointData it is a float so we need to copy it manually
274 for (size_t i = 0; i < last.temperature.size(); i++) {
275 data[i] = last.temperature[i];
276 }
277 break;
278
279 default:
280 yCError(CONTROLBOARD_NWC_YARP) << "Internal error while reading data. Cannot get 'vector' data of type " << yarp::os::Vocab32::decode(field);
281 break;
282 }
283
285 stamp = lastStamp;
286 if (ret && ((Time::now() - localArrivalTime) > timeout)) {
287 ret = false;
288 }
289 }
290 mutex.unlock();
291
292 return ret;
293}
294
296{
297 mutex.lock();
298 bool ret = valid;
299 if (ret)
300 {
301 switch(field)
302 {
304 ret = last.controlMode_isValid;
305 memcpy(data, last.controlMode.data(), last.controlMode.size() * last.controlMode.getElementSize());
306 break;
307
309 ret = last.interactionMode_isValid;
311 break;
312
313 default:
314 yCError(CONTROLBOARD_NWC_YARP) << "Internal error while reading data. Cannot get 'vector' data of type " << yarp::os::Vocab32::decode(field);
315 break;
316 }
318 stamp = lastStamp;
319 if (ret && ((Time::now() - localArrivalTime) > timeout)) {
320 ret = false;
321 }
322 }
323 mutex.unlock();
324 return ret;
325}
326
328{
329 mutex.lock();
330 int ret=count;
331 mutex.unlock();
332 return ret;
333}
334
335// time is in ms
336void StateExtendedInputPort::getEstFrequency(int &ite, double &av, double &min, double &max)
337{
338 mutex.lock();
339 ite=count;
340 min=deltaTMin*1000;
341 max=deltaTMax*1000;
342 if (count<1)
343 {
344 av=0;
345 }
346 else
347 {
348 av=deltaT/count;
349 }
350 av=av*1000;
351 mutex.unlock();
352}
define control board standard interfaces
const yarp::os::LogComponent & CONTROLBOARD_NWC_YARP()
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENT
constexpr yarp::conf::vocab32_t VOCAB_AMP_CURRENTS
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODES
constexpr yarp::conf::vocab32_t VOCAB_CM_CONTROL_MODE
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATIONS
Definition IEncoders.h:214
constexpr yarp::conf::vocab32_t VOCAB_ENCODER
Definition IEncoders.h:207
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEEDS
Definition IEncoders.h:212
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_SPEED
Definition IEncoders.h:211
constexpr yarp::conf::vocab32_t VOCAB_ENCODER_ACCELERATION
Definition IEncoders.h:213
constexpr yarp::conf::vocab32_t VOCAB_ENCODERS
Definition IEncoders.h:208
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODES
constexpr yarp::conf::vocab32_t VOCAB_INTERACTION_MODE
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEED
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATIONS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_ACCELERATION
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODER_SPEEDS
constexpr yarp::conf::vocab32_t VOCAB_MOTOR_ENCODERS
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURES
Definition IMotor.h:163
constexpr yarp::conf::vocab32_t VOCAB_TEMPERATURE
Definition IMotor.h:161
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUTS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_PWM_OUTPUT
constexpr yarp::conf::vocab32_t VOCAB_TRQ
constexpr yarp::conf::vocab32_t VOCAB_TRQS
contains the definition of a Vector type
void onRead(yarp::dev::JointStateData &v) override
void setTimeout(const double &timeout)
setTimeout, set the timeout for retrieving data
bool getLastVector(int field, double *data, Stamp &stamp, double &localArrivalTime)
bool getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime)
void init(int numberOfJoints)
void getEstFrequency(int &ite, double &av, double &min, double &max)
yarp::sig::VectorOf< float > pwmDutycycle
yarp::sig::VectorOf< int > controlMode
yarp::sig::VectorOf< int > interactionMode
yarp::sig::VectorOf< double > motorAcceleration
yarp::sig::VectorOf< double > jointPosition
yarp::sig::VectorOf< double > torque
yarp::sig::VectorOf< double > motorVelocity
yarp::sig::VectorOf< double > jointVelocity
yarp::sig::VectorOf< double > current
yarp::sig::VectorOf< float > temperature
yarp::sig::VectorOf< double > jointAcceleration
yarp::sig::VectorOf< double > motorPosition
A mini-server for performing network communication in the background.
bool getEnvelope(PortReader &envelope) override
An abstraction for a time stamp and/or sequence number.
Definition Stamp.h:21
void update()
Set the timestamp to the current time, and increment the sequence number (wrapping to 0 if the sequen...
Definition Stamp.cpp:124
bool isValid() const
Check if this Stamp is valid.
Definition Stamp.cpp:39
void resize(size_t size) override
Resize the vector.
Definition Vector.h:231
size_t size() const
Definition Vector.h:351
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:216
size_t getElementSize() const override
Definition Vector.h:189
std::string current_time()
Definition utils.h:24
#define yCError(component,...)
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
std::string decode(NetInt32 code)
Convert a vocabulary identifier into a string.
Definition Vocab32.cpp:33
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13