yarp-devices
CanBusBroker.hpp
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 #ifndef __CAN_BUS_BROKER_HPP__
4 #define __CAN_BUS_BROKER_HPP__
5 
6 #include <vector>
7 
8 #include <yarp/dev/ControlBoardInterfaces.h>
9 #include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
10 
11 #include "DeviceMapper.hpp"
12 #include "SingleBusBroker.hpp"
13 #include "SyncPeriodicThread.hpp"
14 
15 #define CHECK_JOINT(j) do { int n = deviceMapper.getControlledAxes(); if ((j) < 0 || (j) > n - 1) return false; } while (0)
16 
17 namespace roboticslab
18 {
19 
40 class CanBusBroker : public yarp::dev::DeviceDriver,
41  // control board interfaces
42  public yarp::dev::IAmplifierControl,
43  public yarp::dev::IAxisInfo,
44  public yarp::dev::IControlCalibration,
45  public yarp::dev::IControlLimits,
46  public yarp::dev::IControlMode,
47  public yarp::dev::ICurrentControl,
48  public yarp::dev::IEncodersTimed,
49  public yarp::dev::IImpedanceControl,
50  public yarp::dev::IInteractionMode,
51  public yarp::dev::IJointFault,
52  public yarp::dev::IMotor,
53  public yarp::dev::IMotorEncoders,
54  public yarp::dev::IPidControl,
55  public yarp::dev::IPositionControl,
56  public yarp::dev::IPositionDirect,
57  public yarp::dev::IPWMControl,
58  public yarp::dev::IRemoteVariables,
59  public yarp::dev::ITorqueControl,
60  public yarp::dev::IVelocityControl,
61  // multiple analog sensors interfaces
62  public yarp::dev::IContactLoadCellArrays,
63  public yarp::dev::IEncoderArrays,
64  public yarp::dev::IOrientationSensors,
65  public yarp::dev::IPositionSensors,
66  public yarp::dev::ISixAxisForceTorqueSensors,
67  public yarp::dev::ISkinPatches,
68  public yarp::dev::ITemperatureSensors,
69  public yarp::dev::IThreeAxisGyroscopes,
70  public yarp::dev::IThreeAxisLinearAccelerometers,
71  public yarp::dev::IThreeAxisMagnetometers
72 {
73 public:
74  ~CanBusBroker() override
75  { close(); }
76 
77  // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
78 
79  bool open(yarp::os::Searchable & config) override;
80  bool close() override;
81 
82  // ---------- CONTROL BOARD INTERFACES ----------
83 
84  // --------- IAmplifierControl declarations. Implementation in IAmplifierControlImpl.cpp ---------
85 
86  bool enableAmp(int j) override;
87  bool disableAmp(int j) override;
88  bool getAmpStatus(int j, int * v) override;
89  bool getAmpStatus(int * st) override;
90  //bool getCurrent(int j, double * val) override;
91  //bool getCurrents(double * vals) override;
92  bool getMaxCurrent(int j, double * v) override;
93  bool setMaxCurrent(int j, double v) override;
94  bool getNominalCurrent(int m, double * val) override;
95  bool setNominalCurrent(int m, double val) override;
96  bool getPeakCurrent(int m, double * val) override;
97  bool setPeakCurrent(int m, double val) override;
98  bool getPWM(int j, double * val) override;
99  bool getPWMLimit(int j, double * val) override;
100  bool setPWMLimit(int j, double val) override;
101  bool getPowerSupplyVoltage(int j, double * val) override;
102 
103  // --------- IAxisInfo declarations. Implementation in IAxisInfoImpl.cpp ---------
104 
105  bool getAxisName(int axis, std::string & name) override;
106  bool getJointType(int axis, yarp::dev::JointTypeEnum & type) override;
107 
108  // --------- IControlCalibration declarations. Implementation in IControlCalibrationImpl.cpp ---------
109 
110  bool calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) override;
111  bool setCalibrationParameters(int axis, const yarp::dev::CalibrationParameters & params) override;
112  bool calibrationDone(int j) override;
113 
114  // --------- IControlLimits declarations. Implementation in IControlLimitsImpl.cpp ---------
115 
116  bool setLimits(int axis, double min, double max) override;
117  bool getLimits(int axis, double * min, double * max) override;
118  bool setVelLimits(int axis, double min, double max) override;
119  bool getVelLimits(int axis, double * min, double * max) override;
120 
121  // --------- IControlMode declarations. Implementation in IControlModeImpl.cpp ---------
122 
123  bool getControlMode(int j, int * mode) override;
124  bool getControlModes(int * modes) override;
125  bool getControlModes(int n_joint, const int * joints, int * modes) override;
126  bool setControlMode(int j, const int mode) override;
127  bool setControlModes(int n_joint, const int * joints, int * modes) override;
128  bool setControlModes(int * modes) override;
129 
130  // --------- ICurrentControl declarations. Implementation in ICurrentControlImpl.cpp ---------
131 
132  //bool getNumberOfMotors(int * ax) override;
133  bool getCurrent(int m, double * curr) override;
134  bool getCurrents(double * currs) override;
135  bool getCurrentRange(int m, double * min, double * max) override;
136  bool getCurrentRanges(double * mins, double * maxs) override;
137  bool setRefCurrent(int m, double curr) override;
138  bool setRefCurrents(const double * currs) override;
139  bool setRefCurrents(int n_motor, const int * motors, const double * currs) override;
140  bool getRefCurrent(int m, double * curr) override;
141  bool getRefCurrents(double * currs) override;
142 
143  // ---------- IEncoders declarations. Implementation in IEncodersImpl.cpp ----------
144 
145  bool getAxes(int * ax) override;
146  bool resetEncoder(int j) override;
147  bool resetEncoders() override;
148  bool setEncoder(int j, double val) override;
149  bool setEncoders(const double * vals) override;
150  bool getEncoder(int j, double * v) override;
151  bool getEncoders(double * encs) override;
152  bool getEncoderSpeed(int j, double * spd) override;
153  bool getEncoderSpeeds(double * spds) override;
154  bool getEncoderAcceleration(int j, double * spds) override;
155  bool getEncoderAccelerations(double * accs) override;
156 
157  // ---------- IEncodersTimed declarations. Implementation in IEncodersImpl.cpp ----------
158 
159  bool getEncoderTimed(int j, double * encs, double * time) override;
160  bool getEncodersTimed(double * encs, double * times) override;
161 
162  // --------- IImpedanceControl declarations. Implementation in IImpedanceControlImpl.cpp ---------
163 
164  //bool getAxes(int * ax) override;
165  bool getImpedance(int j, double * stiffness, double * damping) override;
166  bool setImpedance(int j, double stiffness, double damping) override;
167  bool setImpedanceOffset(int j, double offset) override;
168  bool getImpedanceOffset(int j, double * offset) override;
169  bool getCurrentImpedanceLimit(int j, double * min_stiff, double * max_stiff, double * min_damp, double * max_damp) override;
170 
171  // -----------IInteractionMode declarations. Implementation in IInteractionModeImpl.cpp --------------
172 
173  bool getInteractionMode(int axis, yarp::dev::InteractionModeEnum * mode) override;
174  bool getInteractionModes(yarp::dev::InteractionModeEnum * modes) override;
175  bool getInteractionModes(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override;
176  bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) override;
177  bool setInteractionModes(yarp::dev::InteractionModeEnum * modes) override;
178  bool setInteractionModes(int n_joints, int * joints, yarp::dev::InteractionModeEnum * modes) override;
179 
180  // --------- IJointFault declarations. Implementation in IJointFaultImpl.cpp ---------
181 
182  bool getLastJointFault(int j, int & fault, std::string & message) override;
183 
184  // --------- IMotor declarations. Implementation in IMotorImpl.cpp ---------
185 
186  bool getNumberOfMotors(int * num) override;
187  bool getTemperature(int m, double * val) override;
188  bool getTemperatures(double * vals) override;
189  bool getTemperatureLimit(int m, double * temp) override;
190  bool setTemperatureLimit(int m, double temp) override;
191  bool getGearboxRatio(int m, double * val) override;
192  bool setGearboxRatio(int m, double val) override;
193 
194  // --------- IMotorEncoders declarations. Implementation in IMotorEncodersImpl.cpp ---------
195 
196  bool getNumberOfMotorEncoders(int * num) override;
197  bool resetMotorEncoder(int m) override;
198  bool resetMotorEncoders() override;
199  bool setMotorEncoderCountsPerRevolution(int m, double cpr) override;
200  bool getMotorEncoderCountsPerRevolution(int m, double * cpr) override;
201  bool setMotorEncoder(int m, double val) override;
202  bool setMotorEncoders(const double * vals) override;
203  bool getMotorEncoder(int m, double * v) override;
204  bool getMotorEncoders(double * encs) override;
205  bool getMotorEncoderTimed(int m, double * enc, double * stamp) override;
206  bool getMotorEncodersTimed(double * encs, double * stamps) override;
207  bool getMotorEncoderSpeed(int m, double * sp) override;
208  bool getMotorEncoderSpeeds(double *spds) override;
209  bool getMotorEncoderAcceleration(int m, double * acc) override;
210  bool getMotorEncoderAccelerations(double * accs) override;
211 
212  // --------- IPidControl declarations. Implementation in IPidControlImpl.cpp ---------
213 
214  bool setPid(const yarp::dev::PidControlTypeEnum & pidtype, int j, const yarp::dev::Pid & pid) override;
215  bool setPids(const yarp::dev::PidControlTypeEnum & pidtype, const yarp::dev::Pid * pids) override;
216  bool setPidReference(const yarp::dev::PidControlTypeEnum & pidtype, int j, double ref) override;
217  bool setPidReferences(const yarp::dev::PidControlTypeEnum & pidtype, const double * refs) override;
218  bool setPidErrorLimit(const yarp::dev::PidControlTypeEnum & pidtype, int j, double limit) override;
219  bool setPidErrorLimits(const yarp::dev::PidControlTypeEnum & pidtype, const double * limits) override;
220  bool getPidError(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * err) override;
221  bool getPidErrors(const yarp::dev::PidControlTypeEnum & pidtype, double * errs) override;
222  bool getPidOutput(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * out) override;
223  bool getPidOutputs(const yarp::dev::PidControlTypeEnum & pidtype, double * outs) override;
224  bool getPid(const yarp::dev::PidControlTypeEnum & pidtype, int j, yarp::dev::Pid * pid) override;
225  bool getPids(const yarp::dev::PidControlTypeEnum & pidtype, yarp::dev::Pid * pids) override;
226  bool getPidReference(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * ref) override;
227  bool getPidReferences(const yarp::dev::PidControlTypeEnum & pidtype, double * refs) override;
228  bool getPidErrorLimit(const yarp::dev::PidControlTypeEnum & pidtype, int j, double * limit) override;
229  bool getPidErrorLimits(const yarp::dev::PidControlTypeEnum & pidtype, double * limits) override;
230  bool resetPid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
231  bool disablePid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
232  bool enablePid(const yarp::dev::PidControlTypeEnum & pidtype, int j) override;
233  bool setPidOffset(const yarp::dev::PidControlTypeEnum & pidtype, int j, double v) override;
234  bool isPidEnabled(const yarp::dev::PidControlTypeEnum & pidtype, int j, bool * enabled) override;
235 
236  // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp -------
237 
238  //bool getAxes(int * ax) override;
239  bool positionMove(int j, double ref) override;
240  bool positionMove(const double * refs) override;
241  bool positionMove(int n_joint, const int * joints, const double * refs) override;
242  bool relativeMove(int j, double delta) override;
243  bool relativeMove(const double * deltas) override;
244  bool relativeMove(int n_joint, const int * joints, const double * deltas) override;
245  bool checkMotionDone(int j, bool * flag) override;
246  bool checkMotionDone(bool * flag) override;
247  bool checkMotionDone(int n_joint, const int * joints, bool * flag) override;
248  bool setRefSpeed(int j, double sp) override;
249  bool setRefSpeeds(const double * spds) override;
250  bool setRefSpeeds(int n_joint, const int * joints, const double * spds) override;
251  bool setRefAcceleration(int j, double acc) override;
252  bool setRefAccelerations(const double * accs) override;
253  bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
254  bool getRefSpeed(int j, double * spd) override;
255  bool getRefSpeeds(double * spds) override;
256  bool getRefSpeeds(int n_joint, const int * joints, double * spds) override;
257  bool getRefAcceleration(int j, double * acc) override;
258  bool getRefAccelerations(double * accs) override;
259  bool getRefAccelerations(int n_joint, const int * joints, double * accs) override;
260  bool stop(int j) override;
261  bool stop() override;
262  bool stop(int n_joint, const int *joints) override;
263  bool getTargetPosition(int joint, double * ref) override;
264  bool getTargetPositions(double * refs) override;
265  bool getTargetPositions(int n_joint, const int * joints, double * refs) override;
266 
267  // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
268 
269  //bool getAxes(int * ax) override;
270  bool setPosition(int j, double ref) override;
271  bool setPositions(const double * refs) override;
272  bool setPositions(int n_joint, const int * joints, const double * refs) override;
273  bool getRefPosition(int joint, double * ref) override;
274  bool getRefPositions(double * refs) override;
275  bool getRefPositions(int n_joint, const int * joints, double * refs) override;
276 
277  // --------- IPWMControl declarations. Implementation in IPWMControlImpl.cpp ---------
278 
279  //bool getNumberOfMotors(int * number) override;
280  bool setRefDutyCycle(int m, double ref) override;
281  bool setRefDutyCycles(const double * refs) override;
282  bool getRefDutyCycle(int m, double * ref) override;
283  bool getRefDutyCycles(double * refs) override;
284  bool getDutyCycle(int m, double * val) override;
285  bool getDutyCycles(double * vals) override;
286 
287  // ----------- IRemoteVariables declarations. Implementation in IRemoteVariablesImpl.cpp --------------
288 
289  bool getRemoteVariable(std::string key, yarp::os::Bottle & val) override;
290  bool setRemoteVariable(std::string key, const yarp::os::Bottle & val) override;
291  bool getRemoteVariablesList(yarp::os::Bottle * listOfKeys) override;
292 
293  // -------- ITorqueControl declarations. Implementation in ITorqueControlImpl.cpp --------
294 
295  //bool getAxes(int * ax) override;
296  bool getRefTorque(int j, double * t) override;
297  bool getRefTorques(double * t) override;
298  bool setRefTorque(int j, double t) override;
299  bool setRefTorques(const double * t) override;
300  bool setRefTorques(int n_joint, const int * joints, const double * t) override;
301  bool getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters * params) override;
302  bool setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override;
303  bool getTorque(int j, double * t) override;
304  bool getTorques(double * t) override;
305  bool getTorqueRange(int j, double * min, double * max) override;
306  bool getTorqueRanges(double * min, double * max) override;
307 
308  // --------- IVelocityControl declarations. Implementation in IVelocityControlImpl.cpp ---------
309 
310  //bool getAxes(int * ax) override;
311  bool velocityMove(int j, double spd) override;
312  bool velocityMove(const double * spds) override;
313  bool velocityMove(int n_joint, const int * joints, const double * spds) override;
314  bool getRefVelocity(int joint, double * vel) override;
315  bool getRefVelocities(double * vels) override;
316  bool getRefVelocities(int n_joint, const int * joints, double * vels) override;
317  //bool setRefAcceleration(int j, double acc) override;
318  //bool setRefAccelerations(const double * accs) override;
319  //bool setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
320  //bool getRefAcceleration(int j, double * acc) override;
321  //bool getRefAccelerations(double * accs) override;
322  //bool getRefAccelerations(int n_joint, const int * joints, double * accs) override;
323  //bool stop(int j) override;
324  //bool stop() override;
325  //bool stop(int n_joint, const int *joints) override;
326 
327  // ---------- MULTIPLE ANALOG SENSORS INTERFACES ----------
328 
329  // --------- IContactLoadCellArrays declarations. Implementation in IContactLoadCellArraysImpl.cpp ---------
330 
331  std::size_t getNrOfContactLoadCellArrays() const override;
332  yarp::dev::MAS_status getContactLoadCellArrayStatus(std::size_t sens_index) const override;
333  bool getContactLoadCellArrayName(std::size_t sens_index, std::string & name) const override;
334  bool getContactLoadCellArrayMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
335  std::size_t getContactLoadCellArraySize(std::size_t sens_index) const override;
336 
337  // --------- IEncoderArrays declarations. Implementation in IEncoderArraysImpl.cpp ---------
338 
339  std::size_t getNrOfEncoderArrays() const override;
340  yarp::dev::MAS_status getEncoderArrayStatus(std::size_t sens_index) const override;
341  bool getEncoderArrayName(std::size_t sens_index, std::string & name) const override;
342  bool getEncoderArrayMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
343  std::size_t getEncoderArraySize(std::size_t sens_index) const override;
344 
345  // --------- IOrientationSensors declarations. Implementation in IOrientationSensorsImpl.cpp ---------
346 
347  std::size_t getNrOfOrientationSensors() const override;
348  yarp::dev::MAS_status getOrientationSensorStatus(std::size_t sens_index) const override;
349  bool getOrientationSensorName(std::size_t sens_index, std::string & name) const override;
350  bool getOrientationSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
351  bool getOrientationSensorMeasureAsRollPitchYaw(std::size_t sens_index, yarp::sig::Vector & rpy, double & timestamp) const override;
352 
353  // --------- IPositionSensors declarations. Implementation in IPositionSensorsImpl.cpp ---------
354 
355  std::size_t getNrOfPositionSensors() const override;
356  yarp::dev::MAS_status getPositionSensorStatus(std::size_t sens_index) const override;
357  bool getPositionSensorName(std::size_t sens_index, std::string & name) const override;
358  bool getPositionSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
359  bool getPositionSensorMeasure(std::size_t sens_index, yarp::sig::Vector & xyz, double & timestamp) const override;
360 
361  // --------- ISixAxisForceTorqueSensors declarations. Implementation in ISixAxisForceTorqueSensorsImpl.cpp ---------
362 
363  std::size_t getNrOfSixAxisForceTorqueSensors() const override;
364  yarp::dev::MAS_status getSixAxisForceTorqueSensorStatus(std::size_t sens_index) const override;
365  bool getSixAxisForceTorqueSensorName(std::size_t sens_index, std::string & name) const override;
366  bool getSixAxisForceTorqueSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
367  bool getSixAxisForceTorqueSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
368 
369  // --------- ISkinPatches declarations. Implementation in ISkinPatchesImpl.cpp ---------
370 
371  std::size_t getNrOfSkinPatches() const override;
372  yarp::dev::MAS_status getSkinPatchStatus(std::size_t sens_index) const override;
373  bool getSkinPatchName(std::size_t sens_index, std::string & name) const override;
374  bool getSkinPatchMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
375  std::size_t getSkinPatchSize(std::size_t sens_index) const override;
376 
377  // --------- ITemperatureSensors declarations. Implementation in ITemperatureSensorsImpl.cpp ---------
378 
379  std::size_t getNrOfTemperatureSensors() const override;
380  yarp::dev::MAS_status getTemperatureSensorStatus(std::size_t sens_index) const override;
381  bool getTemperatureSensorName(std::size_t sens_index, std::string & name) const override;
382  bool getTemperatureSensorFrameName(std::size_t sens_index, std::string & frameName) const override;
383  bool getTemperatureSensorMeasure(std::size_t sens_index, double & out, double & timestamp) const override;
384  bool getTemperatureSensorMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
385 
386  // --------- IThreeAxisGyroscopes declarations. Implementation in IThreeAxisGyroscopesImpl.cpp ---------
387 
388  std::size_t getNrOfThreeAxisGyroscopes() const override;
389  yarp::dev::MAS_status getThreeAxisGyroscopeStatus(std::size_t sens_index) const override;
390  bool getThreeAxisGyroscopeName(std::size_t sens_index, std::string & name) const override;
391  bool getThreeAxisGyroscopeFrameName(std::size_t sens_index, std::string & frameName) const override;
392  bool getThreeAxisGyroscopeMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
393 
394  // --------- IThreeAxisLinearAccelerometers declarations. Implementation in IThreeAxisLinearAccelerometersImpl.cpp ---------
395 
396  std::size_t getNrOfThreeAxisLinearAccelerometers() const override;
397  yarp::dev::MAS_status getThreeAxisLinearAccelerometerStatus(std::size_t sens_index) const override;
398  bool getThreeAxisLinearAccelerometerName(std::size_t sens_index, std::string & name) const override;
399  bool getThreeAxisLinearAccelerometerFrameName(std::size_t sens_index, std::string & frameName) const override;
400  bool getThreeAxisLinearAccelerometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
401 
402  // --------- IThreeAxisMagnetometers declarations. Implementation in IThreeAxisMagnetometersImpl.cpp ---------
403 
404  std::size_t getNrOfThreeAxisMagnetometers() const override;
405  yarp::dev::MAS_status getThreeAxisMagnetometerStatus(std::size_t sens_index) const override;
406  bool getThreeAxisMagnetometerName(std::size_t sens_index, std::string & name) const override;
407  bool getThreeAxisMagnetometerFrameName(std::size_t sens_index, std::string & frameName) const override;
408  bool getThreeAxisMagnetometerMeasure(std::size_t sens_index, yarp::sig::Vector & out, double & timestamp) const override;
409 
410 private:
411  DeviceMapper deviceMapper;
412 
413  std::vector<yarp::dev::PolyDriver *> busDevices;
414  std::vector<yarp::dev::PolyDriver *> nodeDevices;
415  std::vector<SingleBusBroker *> brokers;
416 
417  SyncPeriodicThread * syncThread {nullptr};
418 };
419 
420 } // namespace roboticslab
421 
422 #endif // __CAN_BUS_BROKER_HPP__
roboticslab::CanBusBroker
CAN-oriented control board that implements all YARP motor interfaces.
Definition: CanBusBroker.hpp:72
roboticslab::DeviceMapper
Exposes raw subdevice interface handles on a per-axis manner.
Definition: DeviceMapper.hpp:65
roboticslab::SyncPeriodicThread
Periodic SYNC signal emitter.
Definition: SyncPeriodicThread.hpp:29
roboticslab
The main, catch-all namespace for Robotics Lab UC3M.
Definition: groups.dox:6
Generated on Sun Jul 28 2024 20:13:25 for yarp-devices by doxygen 1.9.1