iCub-main
embObjMultipleFTsensors.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  * Author: Luca Tricerri
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 _EMBOBJMULTIPLEFTSENSORS_H_
10 #define _EMBOBJMULTIPLEFTSENSORS_H_
11 
12 #include <yarp/dev/DeviceDriver.h>
13 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
14 #include <yarp/sig/Vector.h>
15 
16 #include <map>
17 #include <shared_mutex>
18 #include <memory>
19 #include <string>
20 
23 
24 namespace yarp::dev
25 {
26 class embObjMultipleFTsensors;
27 }
28 
29 static constexpr int ftChannels_{6};
30 
31 class FtData
32 {
33  public:
34  yarp::sig::Vector data_{0, 0, 0, 0, 0, 0};
35  double timeStamp_;
36  std::string sensorName_;
37  std::string frameName_;
38 };
39 
41 {
42  public:
43  eOmeas_temperature_t data_;
44  double timeStamp_;
45 };
46 
47 class yarp::dev::embObjMultipleFTsensors : public yarp::dev::DeviceDriver, public eth::IethResource, public yarp::dev::ITemperatureSensors, public yarp::dev::ISixAxisForceTorqueSensors
48 {
49  public:
51  embObjMultipleFTsensors(std::shared_ptr<yarp::dev::embObjDevPrivData> device); // For Unittesting only
53 
54  bool open(yarp::os::Searchable& config);
55  bool close();
56 
57  // IethResource interface
58  virtual bool initialised();
59  virtual eth::iethresType_t type();
60  virtual bool update(eOprotID32_t id32, double timestamp, void* rxdata);
61 
62  // ITemperatureSensors
63  virtual size_t getNrOfTemperatureSensors() const override;
64  virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sensorindex) const override;
65  virtual bool getTemperatureSensorName(size_t sensorindex, std::string& name) const override;
66  virtual bool getTemperatureSensorFrameName(size_t sensorindex, std::string& frameName) const override;
67  virtual bool getTemperatureSensorMeasure(size_t sensorindex, double& out, double& timestamp) const override;
68  virtual bool getTemperatureSensorMeasure(size_t sensorindex, yarp::sig::Vector& out, double& timestamp) const override;
69 
70  // ISixAxisForceTorqueSensors
71  virtual size_t getNrOfSixAxisForceTorqueSensors() const override;
72  virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sensorindex) const override;
73  virtual bool getSixAxisForceTorqueSensorName(size_t sensorindex, std::string& name) const override;
74  virtual bool getSixAxisForceTorqueSensorFrameName(size_t sensorindex, std::string& frameName) const override;
75  virtual bool getSixAxisForceTorqueSensorMeasure(size_t sensorindex, yarp::sig::Vector& out, double& timestamp) const override;
76 
77  protected:
78  std::shared_ptr<yarp::dev::embObjDevPrivData> device_;
79  mutable std::shared_mutex mutex_;
80  std::map<eOprotID32_t, FtData> ftSensorsData_;
81  std::map<eOprotID32_t, TemperatureData> temperaturesensordata_;
82  std::map<eOprotID32_t, eOabstime_t> timeoutUpdate_;
83 
87  void cleanup(void);
88  double calculateBoardTime(eOabstime_t current);
89  bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current);
90  static constexpr eOabstime_t updateTimeout_{11000};
91  std::vector<yarp::dev::MAS_status> masStatus_{MAS_WAITING_FOR_FIRST_READ, MAS_WAITING_FOR_FIRST_READ, MAS_WAITING_FOR_FIRST_READ, MAS_WAITING_FOR_FIRST_READ};
92 
93  static constexpr bool checkUpdateTimeoutFlag_{false}; // Check timer disabled
94  static constexpr bool useBoardTimeFlag_{true}; // Calculate board time if true otherway use yarp time
95 
97  eOabstime_t firstCanTimestamp_{0};
98 };
99 
100 #endif
std::string sensorName_
std::string frameName_
yarp::sig::Vector data_
eOmeas_temperature_t data_
std::vector< yarp::dev::MAS_status > masStatus_
virtual bool getTemperatureSensorFrameName(size_t sensorindex, std::string &frameName) const override
bool checkUpdateTimeout(eOprotID32_t id32, eOabstime_t current)
virtual yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(size_t sensorindex) const override
bool initRegulars(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
bool open(yarp::os::Searchable &config)
std::map< eOprotID32_t, FtData > ftSensorsData_
bool sendStart2boards(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
virtual bool getTemperatureSensorMeasure(size_t sensorindex, double &out, double &timestamp) const override
virtual size_t getNrOfTemperatureSensors() const override
double calculateBoardTime(eOabstime_t current)
virtual bool getSixAxisForceTorqueSensorFrameName(size_t sensorindex, std::string &frameName) const override
virtual bool update(eOprotID32_t id32, double timestamp, void *rxdata)
std::map< eOprotID32_t, TemperatureData > temperaturesensordata_
virtual size_t getNrOfSixAxisForceTorqueSensors() const override
static constexpr eOabstime_t updateTimeout_
bool sendConfig2boards(ServiceParserMultipleFt &parser, eth::AbstractEthResource *deviceRes)
virtual bool getSixAxisForceTorqueSensorMeasure(size_t sensorindex, yarp::sig::Vector &out, double &timestamp) const override
std::shared_ptr< yarp::dev::embObjDevPrivData > device_
std::map< eOprotID32_t, eOabstime_t > timeoutUpdate_
virtual bool getSixAxisForceTorqueSensorName(size_t sensorindex, std::string &name) const override
virtual yarp::dev::MAS_status getTemperatureSensorStatus(size_t sensorindex) const override
virtual bool getTemperatureSensorName(size_t sensorindex, std::string &name) const override
static constexpr int ftChannels_
iethresType_t
Definition: IethResource.h:62
out
Definition: sine.m:8