YARP
Yet Another Robot Platform
stateExtendedReader.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #ifndef YARP_DEV_REMOTECONTROLBOARD_STATEEXTENDEDREADER_H
10 #define YARP_DEV_REMOTECONTROLBOARD_STATEEXTENDEDREADER_H
11 
12 
13 #include <yarp/os/PortablePair.h>
14 #include <yarp/os/BufferedPort.h>
15 #include <yarp/os/Time.h>
16 #include <yarp/os/Network.h>
17 #include <yarp/os/Thread.h>
18 #include <yarp/os/Vocab.h>
19 #include <yarp/os/Stamp.h>
20 #include <yarp/os/Log.h>
21 
22 #include <yarp/sig/Vector.h>
23 
25 #include <yarp/dev/PolyDriver.h>
29 
30 #include <yarp/dev/impl/jointData.h>
31 
32 #include <cstring>
33 #include <mutex>
34 
35 // encoders should arrive at least every 0.5s to be considered valide
36 // getEncoders will return false otherwise.
37 
38 using namespace yarp::os;
39 using namespace yarp::dev;
40 using namespace yarp::sig;
41 
42 class StateExtendedInputPort:public yarp::os::BufferedPort<yarp::dev::impl::jointData>
43 {
44  yarp::dev::impl::jointData last;
45  std::mutex mutex;
46  Stamp lastStamp;
47  double deltaT;
48  double deltaTMax;
49  double deltaTMin;
50  double now;
51  double prev;
52  double timeout;
53 
54  bool valid;
55  int count;
56 public:
57 
59 
60  void resetStat();
61  void init(int numberOfJoints);
62 
64  void onRead(yarp::dev::impl::jointData &v) override;
65 
70  void setTimeout(const double& timeout);
71 
72  // use vocab to identify the data to be read
73  // get a value for a single joint
74  bool getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime);
75  bool getLastSingle(int j, int field, int *data, Stamp &stamp, double &localArrivalTime);
76 
77  // get a value for all joints
78  bool getLastVector(int field, double *data, Stamp &stamp, double &localArrivalTime);
79  bool getLastVector(int field, int *data, Stamp &stamp, double &localArrivalTime);
80  int getIterations();
81 
82  // time is in ms
83  void getEstFrequency(int &ite, double &av, double &min, double &max);
84 };
85 
86 #endif // YARP_DEV_REMOTECONTROLBOARD_STATEEXTENDEDREADER_H
An interface for the device drivers.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition: Time.cpp:121
define control board standard interfaces
contains the definition of a Vector type
An abstraction for a time stamp and/or sequence number.
Definition: Stamp.h:24
A mini-server for performing network communication in the background.
Definition: BufferedPort.h:63
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:25