YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ControlBoard_nws_yarp.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
8
10#include "RPCMessagesParser.h"
12
13#include <yarp/os/LogStream.h>
15
16#include <numeric>
17#include <cmath>
18
19using namespace yarp::os;
20using namespace yarp::dev;
21using namespace yarp::sig;
23
24const double DEFAULT_PERIOD = 0.02;
25
30
31void ControlBoard_nws_yarp::closePorts()
32{
33 inputRPCPort.interrupt();
34 inputRPCPort.removeCallbackLock();
35 inputRPCPort.close();
36
37 inputStreamingPort.interrupt();
38 inputStreamingPort.close();
39
40 outputPositionStatePort.interrupt();
41 outputPositionStatePort.close();
42
43 extendedOutputStatePort.interrupt();
44 extendedOutputStatePort.close();
45}
46
48{
49 // Ensure that the device is not running
50 if (isRunning()) {
51 stop();
52 }
53
54 closeDevice();
55 closePorts();
56
57 return true;
58}
59
61{
62 if (!parseParams(config)) { return false; }
63
64 std::string rootName = m_name;
65
66 // Open ports, then attach the readers or callbacks
67 if (!inputRPCPort.open((rootName + "/rpc:i"))) {
68 yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i";
69 closePorts();
70 return false;
71 }
72 inputRPCPort.setReader(RPC_parser);
73 inputRPC_buffer.attach(inputRPCPort);
74 RPC_parser.attach(inputRPC_buffer);
75
76 if (!inputStreamingPort.open(rootName + "/command:i")) {
77 yCError(CONTROLBOARD) << "Error opening port " << rootName + "/rpc:i";
78 closePorts();
79 return false;
80 }
81 inputStreamingPort.setStrict();
82 inputStreamingPort.useCallback(streaming_parser);
83
84 if (!outputPositionStatePort.open(rootName + "/state:o")) {
85 yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o";
86 closePorts();
87 return false;
88 }
89
90 // Extended output state port
91 if (!extendedOutputStatePort.open(rootName + "/stateExt:o")) {
92 yCError(CONTROLBOARD) << "Error opening port " << rootName + "/state:o";
93 closePorts();
94 return false;
95 }
96 extendedOutputState_buffer.attach(extendedOutputStatePort);
97
98 // In case attach is not deferred and the controlboard already owns a valid device
99 // we can start the thread. Otherwise this will happen when attachAll is called
100 if (subdevice_ready) {
102 if (!start()) {
103 yCError(CONTROLBOARD) << "Error starting thread";
104 return false;
105 }
106 }
107
108 return true;
109}
110
111bool ControlBoard_nws_yarp::setDevice(yarp::dev::DeviceDriver* driver, bool owned)
112{
113 // Save the pointer and subDeviceOwned
115
116 // yarp::dev::IJointFault* iJointFault{nullptr};
117 subdevice_ptr->view(iJointFault);
118 if (!iJointFault) {
119 yCWarning(CONTROLBOARD, "Part <%s>: iJointFault interface was not found in subdevice.", partName.c_str());
120 }
121
122 // yarp::dev::IPidControl* iPidControl{nullptr};
123 subdevice_ptr->view(iPidControl);
124 if (!iPidControl) {
125 yCWarning(CONTROLBOARD, "Part <%s>: IPidControl interface was not found in subdevice.", partName.c_str());
126 }
127
128 // yarp::dev::IPositionControl* iPositionControl{nullptr};
129 subdevice_ptr->view(iPositionControl);
130 if (!iPositionControl) {
131 yCWarning(CONTROLBOARD, "Part <%s>: IPositionControl interface was not found in subdevice.", partName.c_str());
132 }
133
134 // yarp::dev::IPositionDirect* iPositionDirect{nullptr};
135 subdevice_ptr->view(iPositionDirect);
136 if (!iPositionDirect) {
137 yCWarning(CONTROLBOARD, "Part <%s>: IPositionDirect interface was not found in subdevice.", partName.c_str());
138 }
139
140 // yarp::dev::IVelocityControl* iVelocityControl{nullptr};
141 subdevice_ptr->view(iVelocityControl);
142 if (!iVelocityControl) {
143 yCWarning(CONTROLBOARD, "Part <%s>: IVelocityControl interface was not found in subdevice.", partName.c_str());
144 }
145
146 // yarp::dev::IEncodersTimed* iEncodersTimed{nullptr};
147 subdevice_ptr->view(iEncodersTimed);
148 if (!iEncodersTimed) {
149 yCWarning(CONTROLBOARD, "Part <%s>: IEncodersTimed interface was not found in subdevice.", partName.c_str());
150 }
151
152 // yarp::dev::IMotor* iMotor{nullptr};
153 subdevice_ptr->view(iMotor);
154 if (!iMotor) {
155 yCWarning(CONTROLBOARD, "Part <%s>: IMotor interface was not found in subdevice.", partName.c_str());
156 }
157
158 // yarp::dev::IMotorEncoders* iMotorEncoders{nullptr};
159 subdevice_ptr->view(iMotorEncoders);
160 if (!iMotorEncoders) {
161 yCWarning(CONTROLBOARD, "Part <%s>: IMotorEncoders interface was not found in subdevice.", partName.c_str());
162 }
163
164 // yarp::dev::IAmplifierControl* iAmplifierControl{nullptr};
165 subdevice_ptr->view(iAmplifierControl);
166 if (!iAmplifierControl) {
167 yCWarning(CONTROLBOARD, "Part <%s>: IAmplifierControl interface was not found in subdevice.", partName.c_str());
168 }
169
170 // yarp::dev::IControlLimits* iControlLimits{nullptr};
171 subdevice_ptr->view(iControlLimits);
172 if (!iControlLimits) {
173 yCWarning(CONTROLBOARD, "Part <%s>: IControlLimits interface was not found in subdevice.", partName.c_str());
174 }
175
176 // yarp::dev::IControlCalibration* iControlCalibration{nullptr};
177 subdevice_ptr->view(iControlCalibration);
178 if (!iControlCalibration) {
179 yCWarning(CONTROLBOARD, "Part <%s>: IControlCalibration interface was not found in subdevice.", partName.c_str());
180 }
181
182 // yarp::dev::ITorqueControl* iTorqueControl{nullptr};
183 subdevice_ptr->view(iTorqueControl);
184 if (!iTorqueControl) {
185 yCWarning(CONTROLBOARD, "Part <%s>: ITorqueControl interface was not found in subdevice.", partName.c_str());
186 }
187
188 // yarp::dev::IImpedanceControl* iImpedanceControl{nullptr};
189 subdevice_ptr->view(iImpedanceControl);
190 if (!iImpedanceControl) {
191 yCWarning(CONTROLBOARD, "Part <%s>: IImpedanceControl interface was not found in subdevice.", partName.c_str());
192 }
193
194 // yarp::dev::IControlMode* iControlMode{nullptr};
195 subdevice_ptr->view(iControlMode);
196 if (!iControlMode) {
197 yCWarning(CONTROLBOARD, "Part <%s>: IControlMode interface was not found in subdevice.", partName.c_str());
198 }
199
200 // yarp::dev::IAxisInfo* iAxisInfo{nullptr};
201 subdevice_ptr->view(iAxisInfo);
202 if (!iAxisInfo) {
203 yCWarning(CONTROLBOARD, "Part <%s>: IAxisInfo interface was not found in subdevice.", partName.c_str());
204 }
205
206 // yarp::dev::IPreciselyTimed* iPreciselyTimed{nullptr};
207 subdevice_ptr->view(iPreciselyTimed);
208 if (!iPreciselyTimed) {
209 yCWarning(CONTROLBOARD, "Part <%s>: IPreciselyTimed interface was not found in subdevice.", partName.c_str());
210 }
211
212 // yarp::dev::IInteractionMode* iInteractionMode{nullptr};
213 subdevice_ptr->view(iInteractionMode);
214 if (!iInteractionMode) {
215 yCWarning(CONTROLBOARD, "Part <%s>: IInteractionMode interface was not found in subdevice.", partName.c_str());
216 }
217
218 // yarp::dev::IRemoteVariables* iRemoteVariables{nullptr};
219 subdevice_ptr->view(iRemoteVariables);
220 if (!iRemoteVariables) {
221 yCWarning(CONTROLBOARD, "Part <%s>: IRemoteVariables interface was not found in subdevice.", partName.c_str());
222 }
223
224 // yarp::dev::IPWMControl* iPWMControl{nullptr};
225 subdevice_ptr->view(iPWMControl);
226 if (!iPWMControl) {
227 yCWarning(CONTROLBOARD, "Part <%s>: IPWMControl interface was not found in subdevice.", partName.c_str());
228 }
229
230 // yarp::dev::ICurrentControl* iCurrentControl{nullptr};
231 subdevice_ptr->view(iCurrentControl);
232 if (!iCurrentControl) {
233 yCWarning(CONTROLBOARD, "Part <%s>: ICurrentControl interface was not found in subdevice.", partName.c_str());
234 }
235
236 // Get the number of controlled joints
237 int tmp_axes = 0;
238 if (iAxisInfo)
239 {
240 if (!iAxisInfo->getAxes(&tmp_axes)) {
241 yCError(CONTROLBOARD) << "Part <%s>: iAxisInfo->getAxes() failed for subdevice " << partName.c_str();
242 return false;
243 }
244 }
245 else if (iEncodersTimed)
246 {
247 if (!iEncodersTimed->getAxes(&tmp_axes)) {
248 yCError(CONTROLBOARD) << "Part <%s>: iEncodersTimed->getAxes() failed for subdevice " << partName.c_str();
249 return false;
250 }
251 }
252 else if (iPositionControl)
253 {
254 if (!iPositionControl->getAxes(&tmp_axes)) {
255 yCError(CONTROLBOARD) << "Part <%s>: iPositionControl->getAxes() failed for subdevice " << partName.c_str();
256 return false;
257 }
258 }
259 else if (iVelocityControl)
260 {
261 if (!iVelocityControl->getAxes(&tmp_axes)) {
262 yCError(CONTROLBOARD) << "Part <%s>: iVelocityControl->getAxes() failed for subdevice " << partName.c_str();
263 return false;
264 }
265 }
266
267 if (tmp_axes <= 0) {
268 yCError(CONTROLBOARD, "Part <%s>: attached device has an invalid number of joints (%d)", partName.c_str(), tmp_axes);
269 return false;
270 }
271 subdevice_joints = static_cast<size_t>(tmp_axes);
272 times.resize(subdevice_joints);
273
274 // Initialization
275 streaming_parser.init(subdevice_ptr);
276 streaming_parser.initialize();
277
278 RPC_parser.init(subdevice_ptr);
279 RPC_parser.initialize();
280
281 return true;
282}
283
284void ControlBoard_nws_yarp::closeDevice()
285{
286 // Reset callbacks
287 streaming_parser.reset();
288 RPC_parser.reset();
289
290 subdevice_joints = 0;
291 subdevice_ready = false;
292
293 times.clear();
294
295 // Clear all interfaces
296 iPidControl = nullptr;
297 iPositionControl = nullptr;
298 iPositionDirect = nullptr;
299 iVelocityControl = nullptr;
300 iEncodersTimed = nullptr;
301 iMotor = nullptr;
302 iMotorEncoders = nullptr;
303 iAmplifierControl = nullptr;
304 iControlLimits = nullptr;
305 iControlCalibration = nullptr;
306 iTorqueControl = nullptr;
307 iImpedanceControl = nullptr;
308 iControlMode = nullptr;
309 iAxisInfo = nullptr;
310 iPreciselyTimed = nullptr;
311 iInteractionMode = nullptr;
312 iRemoteVariables = nullptr;
313 iPWMControl = nullptr;
314 iCurrentControl = nullptr;
315 iJointFault = nullptr;
316}
317
319{
320 // Check if we already instantiated a subdevice previously
321 if (subdevice_ready) {
322 return false;
323 }
324
325 if (!setDevice(poly, false)) {
326 return false;
327 }
328
330 if (!start()) {
331 yCError(CONTROLBOARD) << "Error starting thread";
332 return false;
333 }
334
335 return true;
336}
337
339{
340 // Ensure that the device is not running
341 if (isRunning()) {
342 stop();
343 }
344
345 closeDevice();
346
347 return true;
348}
349
351{
352 // check we are not overflowing with input messages
353 constexpr int reads_for_warning = 20;
354 if (inputStreamingPort.getPendingReads() >= reads_for_warning) {
355 yCIWarning(CONTROLBOARD, id()) << "Number of streaming input messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow";
356 }
357 // handle stateExt first
358 jointData& data = extendedOutputState_buffer.get();
359
360 data.jointPosition.resize(subdevice_joints);
361 data.jointVelocity.resize(subdevice_joints);
362 data.jointAcceleration.resize(subdevice_joints);
363 data.motorPosition.resize(subdevice_joints);
364 data.motorVelocity.resize(subdevice_joints);
365 data.motorAcceleration.resize(subdevice_joints);
366 data.torque.resize(subdevice_joints);
367 data.pwmDutycycle.resize(subdevice_joints);
368 data.current.resize(subdevice_joints);
369 data.controlMode.resize(subdevice_joints);
370 data.interactionMode.resize(subdevice_joints);
371
372 // Get data from HW
373 if (iEncodersTimed) {
374 data.jointPosition_isValid = iEncodersTimed->getEncodersTimed(data.jointPosition.data(), times.data());
375 data.jointVelocity_isValid = iEncodersTimed->getEncoderSpeeds(data.jointVelocity.data());
377 } else {
378 data.jointPosition_isValid = false;
379 data.jointVelocity_isValid = false;
380 data.jointAcceleration_isValid = false;
381 }
382
383 if (iMotorEncoders) {
384 data.motorPosition_isValid = iMotorEncoders->getMotorEncoders(data.motorPosition.data());
385 data.motorVelocity_isValid = iMotorEncoders->getMotorEncoderSpeeds(data.motorVelocity.data());
387 } else {
388 data.motorPosition_isValid = false;
389 data.motorVelocity_isValid = false;
390 data.motorAcceleration_isValid = false;
391 }
392
393 if (iTorqueControl) {
394 data.torque_isValid = iTorqueControl->getTorques(data.torque.data());
395 } else {
396 data.torque_isValid = false;
397 }
398
399 if (iPWMControl) {
400 data.pwmDutycycle_isValid = iPWMControl->getDutyCycles(data.pwmDutycycle.data());
401 } else {
402 data.pwmDutycycle_isValid = false;
403 }
404
405 if (iCurrentControl) {
406 data.current_isValid = iCurrentControl->getCurrents(data.current.data());
407 } else if (iAmplifierControl) {
408 data.current_isValid = iAmplifierControl->getCurrents(data.current.data());
409 } else {
410 data.current_isValid = false;
411 }
412
413 if (iControlMode) {
414 data.controlMode_isValid = iControlMode->getControlModes(data.controlMode.data());
415 } else {
416 data.controlMode_isValid = false;
417 }
418
419 if (iInteractionMode) {
420 data.interactionMode_isValid = iInteractionMode->getInteractionModes(reinterpret_cast<yarp::dev::InteractionModeEnum*>(data.interactionMode.data()));
421 } else {
422 data.interactionMode_isValid = false;
423 }
424
425 // Check if the encoders timestamps are consistent.
426 for (double tt : times)
427 {
428 if (std::abs(times[0] - tt) > 1.0)
429 {
430 yCIErrorThrottle(CONTROLBOARD, id(), 1.0) << "Encoder timestamps are not consistent! Data will not be published.";
431 return;
432 }
433 }
434
435 // Update the port envelope time by averaging all timestamps
436 time.update(std::accumulate(times.begin(), times.end(), 0.0) / subdevice_joints);
438
439 extendedOutputStatePort.setEnvelope(averageTime);
440 extendedOutputState_buffer.write();
441
442 // handle state:o
443 yarp::sig::Vector& v = outputPositionStatePort.prepare();
444 v.resize(subdevice_joints);
445 std::copy(data.jointPosition.begin(), data.jointPosition.end(), v.begin());
446
447 outputPositionStatePort.setEnvelope(averageTime);
448 outputPositionStatePort.write();
449}
const yarp::os::LogComponent & CONTROLBOARD()
const double DEFAULT_PERIOD
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
void run() override
Loop function.
bool open(yarp::os::Searchable &prop) override
Open the DeviceDriver.
bool close() override
Close the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool detach() override
Detach the object (you must have first called attach).
void init(yarp::dev::DeviceDriver *x)
Initialization.
virtual bool initialize()
Initialize the internal data.
void init(yarp::dev::DeviceDriver *x)
Initialization.
Interface implemented by all device drivers.
void attach(yarp::os::TypedReader< yarp::os::Bottle > &source)
Attach this object to a source of messages.
virtual bool getCurrents(double *vals)=0
virtual bool getAxes(int *ax)=0
Get the number of controlled axes.
virtual bool getControlModes(int *modes)=0
Get the current control mode (multiple joints).
virtual bool getCurrents(double *currs)=0
Get the instantaneous current measurement for all motors.
virtual bool getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual bool getEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all axes.
virtual bool getAxes(int *ax)=0
Get the number of controlled axes.
virtual bool getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum *modes)=0
Get the current interaction mode of the robot for a set of joints, values can be stiff or compliant.
virtual bool getMotorEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all motor encoders.
virtual bool getMotorEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all motor encoders.
virtual bool getMotorEncoders(double *encs)=0
Read the position of all motor encoders.
virtual bool getDutyCycles(double *vals)=0
Gets the current dutycycle of the output of the amplifier (i.e.
virtual bool getAxes(int *ax)=0
Get the number of controlled axes.
virtual bool getTorques(double *t)=0
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
virtual bool getAxes(int *axes)=0
Get the number of controlled axes.
A container for a device driver.
Definition PolyDriver.h:23
yarp::sig::VectorOf< double > motorVelocity
Definition jointData.h:35
yarp::sig::VectorOf< double > jointAcceleration
Definition jointData.h:31
yarp::sig::VectorOf< int > controlMode
Definition jointData.h:45
yarp::sig::VectorOf< int > interactionMode
Definition jointData.h:47
yarp::sig::VectorOf< double > current
Definition jointData.h:43
yarp::sig::VectorOf< double > motorAcceleration
Definition jointData.h:37
yarp::sig::VectorOf< double > motorPosition
Definition jointData.h:33
yarp::sig::VectorOf< double > pwmDutycycle
Definition jointData.h:41
yarp::sig::VectorOf< double > torque
Definition jointData.h:39
yarp::sig::VectorOf< double > jointPosition
Definition jointData.h:27
yarp::sig::VectorOf< double > jointVelocity
Definition jointData.h:29
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
int getPendingReads() override
Get the number of objects ready to be read.
void interrupt() override
Interrupt any current reads or writes attached to the port.
void useCallback(TypedReaderCallback< T > &callback) override
Set an object whose onRead method will be called when data is available.
void setStrict(bool strict=true) override
Call this to strictly keep all messages, or allow old ones to be quietly dropped.
void write(bool forceStrict=false)
Write the current object being returned by BufferedPort::prepare.
T & prepare()
Access the object which will be transmitted by the next call to yarp::os::BufferedPort::write.
An abstraction for a periodic thread.
bool setPeriod(double period)
Set the (new) period of the thread.
bool isRunning() const
Returns true when the thread is started, false otherwise.
bool start()
Call this to start the thread.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
void attach(Port &port)
Attach this buffer to a particular port.
T & get()
A synonym of PortWriterBuffer::prepare.
void write(bool forceStrict=false)
Try to write the last buffer returned by PortWriterBuffer::get.
void attach(Port &port)
Set the Port to which objects will be written.
void setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:511
bool removeCallbackLock() override
Remove a lock on callbacks added with setCallbackLock()
Definition Port.cpp:690
void interrupt() override
Interrupt any current reads or writes attached to the port.
Definition Port.cpp:383
bool setEnvelope(PortWriter &envelope) override
Set an envelope (e.g., a timestamp) to the next message which will be sent.
Definition Port.cpp:547
void close() override
Stop port activity.
Definition Port.cpp:363
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
Definition Port.cpp:79
A base class for nested structures that can be searched.
Definition Searchable.h:31
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
void resize(size_t size) override
Resize the vector.
Definition Vector.h:221
iterator begin() noexcept
Returns an iterator to the beginning of the VectorOf.
Definition Vector.h:454
T * data()
Return a pointer to the first element of the vector.
Definition Vector.h:206
iterator end() noexcept
Returns an iterator to the end of the VectorOf.
Definition Vector.h:461
#define yCError(component,...)
#define yCWarning(component,...)
#define yCIErrorThrottle(component, id, period,...)
#define yCIWarning(component, id,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
The main, catch-all namespace for YARP.
Definition dirs.h:16