YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
StreamingMessagesParser.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
9#include <yarp/os/LogStream.h>
10
12#include <iostream>
13
14using namespace yarp::os;
15using namespace yarp::dev;
16using namespace yarp::dev::impl;
17using namespace yarp::sig;
18
19
33
35{
37 stream_IPosCtrl = nullptr;
38 stream_IPosDirect = nullptr;
39 stream_IVel = nullptr;
40 stream_IVelDirect = nullptr;
41 stream_ITorque = nullptr;
42 stream_IPWM = nullptr;
43 stream_ICurrent = nullptr;
44 stream_IAxis = nullptr;
45 stream_IPid = nullptr;
46}
47
49{
52 if (stream_IVel) { stream_IVel->getAxes(stream_nJoints); return true; }
54 if (stream_IAxis) { stream_IAxis->getAxes(stream_nJoints); return true; }
55
56 yCError(CONTROLBOARD, "Unable to get number of joints");
57 return false;
58}
59
60// streaming port callback
62{
63 Bottle& b = v.head;
65
66 //Use the following only for debug, since it can heavily slow down the system
67 yCTrace(CONTROLBOARD, "Received command %s, %s\n", b.toString().c_str(), cmdVector.toString().c_str());
68
69 // some consistency checks
70 if (static_cast<int>(cmdVector.size()) > stream_nJoints) {
71 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
72 yCError(CONTROLBOARD, "Received command vector with number of elements bigger than axis controlled by this wrapper (cmd: %s requested jnts: %d received jnts: %d)\n", str.c_str(), stream_nJoints, (int)cmdVector.size());
73 return;
74 }
75 if (cmdVector.data() == nullptr) {
76 yCError(CONTROLBOARD, "Received null command vector");
77 return;
78 }
79
80 switch (b.get(0).asVocab32()) {
81 // manage commands with interface name as first...
82 // PWM control
84 switch (b.get(1).asVocab32()) {
86 if (stream_IPWM) {
87 int joint = b.get(2).asInt32();
88 bool ok = stream_IPWM->setRefDutyCycle(joint, cmdVector[0]);
89 if (!ok) {
90 yCError(CONTROLBOARD, "Errors while trying to command an pwm message");
91 }
92 } else {
93 yCError(CONTROLBOARD, "PWM interface not valid");
94 }
95 } break;
97 if (stream_IPWM) {
98 bool ok = stream_IPWM->setRefDutyCycles(cmdVector.data());
99 if (!ok) {
100 yCError(CONTROLBOARD, "Errors while trying to command an pwm message");
101 }
102 } else {
103 yCError(CONTROLBOARD, "PWM interface not valid");
104 }
105 } break;
106 default:
107 {
108 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
109 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
110 } break;
111 }
112 } break;
113
114 // PID control
116 switch (b.get(1).asVocab32()) {
118 if (stream_IPid) {
120 int joint = b.get(3).asInt32();
121 double off = cmdVector[0];
122 bool ok = stream_IPid->setPidOffset(pidtype, joint, off);
123 if (!ok) {
124 yCError(CONTROLBOARD, "Errors while trying to execute streaming command setPidOffset()\n");
125 }
126 }
127 } break;
129 if (stream_IPid) {
131 int joint = b.get(3).asInt32();
132 double ffd = cmdVector[0];
133 bool ok = stream_IPid->setPidFeedforward(pidtype, joint, ffd);
134 if (!ok) {
135 yCError(CONTROLBOARD, "Errors while trying to execute streaming command setPidFeedforward()\n");
136 }
137 }
138 } break;
139 default:
140 {
141 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
142 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
143 } break;
144 }
145 } break;
146
147 // Current control
149 switch (b.get(1).asVocab32()) {
150 case VOCAB_CURRENT_REF: {
151 if (stream_ICurrent) {
152 int joint = b.get(2).asInt32();
153 bool ok = stream_ICurrent->setRefCurrent(joint, cmdVector[0]);
154 if (!ok) {
155 yCError(CONTROLBOARD, "Errors while trying to execute streaming command setRefCurrent()\n");
156 }
157 }
158 } break;
159 case VOCAB_CURRENT_REFS: {
160 if (stream_ICurrent) {
161 bool ok = stream_ICurrent->setRefCurrents(cmdVector.data());
162 if (!ok) {
163 yCError(CONTROLBOARD, "Errors while trying to execute streaming command setRefCurrents()\n");
164 }
165 }
166 } break;
168 if (stream_ICurrent) {
169 int n_joints = b.get(2).asInt32();
170 Bottle* jlut = b.get(3).asList();
171 if ((static_cast<int>(jlut->size()) != n_joints) && (static_cast<int>(cmdVector.size()) != n_joints)) {
172 yCError(CONTROLBOARD, "Received VOCAB_CURRENT_REF_GROUP size of joints vector or currents vector does not match the selected joint number\n");
173 }
174
175 int* joint_list = new int[n_joints];
176 for (int i = 0; i < n_joints; i++) {
177 joint_list[i] = jlut->get(i).asInt32();
178 }
179
180
181 bool ok = stream_ICurrent->setRefCurrents(n_joints, joint_list, cmdVector.data());
182 if (!ok) {
183 yCError(CONTROLBOARD, "Errors while trying to execute streaming command setRefCurrents() group\n");
184 }
185
186 delete[] joint_list;
187 }
188 } break;
189 default:
190 {
191 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
192 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
193 } break;
194 }
195 } break;
196
197 // Torque control
199 switch (b.get(1).asVocab32()) {
201 if (stream_ITorque) {
202 int joint = b.get(2).asInt32();
203 bool ok = stream_ITorque->setRefTorque(joint, cmdVector[0]);
204 if (!ok) {
205 yCError(CONTROLBOARD, "Errors while trying to command a streaming torque direct message on single joint\n");
206 }
207 }
208 } break;
210 if (stream_ITorque) {
211 bool ok = stream_ITorque->setRefTorques(cmdVector.data());
212 if (!ok) {
213 yCError(CONTROLBOARD, "Errors while trying to command a streaming torque direct message on all joints\n");
214 }
215 }
216 } break;
218 if (stream_ITorque) {
219 int n_joints = b.get(2).asInt32();
220 Bottle* jlut = b.get(3).asList();
221 if ((static_cast<int>(jlut->size()) != n_joints) && (static_cast<int>(cmdVector.size()) != n_joints)) {
222 yCError(CONTROLBOARD, "Received VOCAB_TORQUES_DIRECT_GROUP size of joints vector or torques vector does not match the selected joint number\n");
223 }
224
225 int* joint_list = new int[n_joints];
226 for (int i = 0; i < n_joints; i++) {
227 joint_list[i] = jlut->get(i).asInt32();
228 }
229 bool ok = stream_ITorque->setRefTorques(n_joints, joint_list, cmdVector.data());
230 if (!ok) {
231 yCError(CONTROLBOARD, "Error while trying to command a streaming toruqe direct message on joint group\n");
232 }
233 delete[] joint_list;
234 }
235 } break;
236 default:
237 {
238 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
239 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
240 } break;
241 }
242 } break;
243
244 // Position control
246 switch (b.get(1).asVocab32()) {
248 if (stream_IPosCtrl) {
249 bool ok = stream_IPosCtrl->positionMove(cmdVector.data());
250 if (!ok) {
251 yCError(CONTROLBOARD, "Errors while trying to start a position move");
252 }
253 }
254 } break;
255 default:
256 {
257 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
258 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
259 } break;
260 }
261 } break;
262
263 // Position direct control
265 switch (b.get(1).asVocab32()) {
267 if (stream_IPosDirect) {
268 int joint = b.get(2).asInt32();
269 bool ok = stream_IPosDirect->setPosition(joint, cmdVector[0]); // cmdVector.data());
270 if (!ok) {
271 yCError(CONTROLBOARD, "Errors while trying to command an streaming position direct message on joint %d\n", b.get(1).asInt32());
272 }
273 }
274 } break;
276 if (stream_IPosDirect) {
277 int n_joints = b.get(2).asInt32();
278 Bottle* jlut = b.get(3).asList();
279 if ((static_cast<int>(jlut->size()) != n_joints) && (static_cast<int>(cmdVector.size()) != n_joints)) {
280 yCError(CONTROLBOARD, "Received VOCAB_POSITION_DIRECT_GROUP size of joints vector or positions vector does not match the selected joint number\n");
281 }
282 int* joint_list = new int[n_joints];
283 for (int i = 0; i < n_joints; i++) {
284 joint_list[i] = jlut->get(i).asInt32();
285 }
286 bool ok = stream_IPosDirect->setPositions(n_joints, joint_list, cmdVector.data());
287 if (!ok) {
288 yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on joint group\n");
289 }
290 delete[] joint_list;
291 }
292 } break;
294 if (stream_IPosDirect) {
295 bool ok = stream_IPosDirect->setPositions(cmdVector.data());
296 if (!ok) {
297 yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on all joints\n");
298 }
299 }
300 } break;
301 default:
302 {
303 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
304 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
305 } break;
306 }
307 } break;
308
309 // Velocity control
311 switch (b.get(1).asVocab32()) {
312 case VOCAB_VELOCITY_MOVE: {
313 if (stream_IVel) {
314 int joint = b.get(2).asInt32();
315 bool ok = stream_IVel->velocityMove(joint, cmdVector[0]);
316 if (!ok) {
317 yCError(CONTROLBOARD, "Errors while trying to start a velocity move");
318 }
319 }
320 } break;
321
323 if (stream_IVel) {
324 bool ok = stream_IVel->velocityMove(cmdVector.data());
325 if (!ok) {
326 yCError(CONTROLBOARD, "Errors while trying to start a velocity move");
327 }
328 }
329 } break;
331 if (stream_IVel) {
332 int n_joints = b.get(2).asInt32();
333 Bottle* jlut = b.get(3).asList();
334 if ((static_cast<int>(jlut->size()) != n_joints) && (static_cast<int>(cmdVector.size()) != n_joints)) {
335 yCError(CONTROLBOARD, "Received VOCAB_VELOCITY_MOVE_GROUP size of joints vector or positions vector does not match the selected joint number\n");
336 }
337
338 int* joint_list = new int[n_joints];
339 for (int i = 0; i < n_joints; i++) {
340 joint_list[i] = jlut->get(i).asInt32();
341 }
342
343 bool ok = stream_IVel->velocityMove(n_joints, joint_list, cmdVector.data());
344 if (!ok) {
345 yCError(CONTROLBOARD, "Error while trying to command a velocity move on joint group\n");
346 }
347
348 delete[] joint_list;
349 }
350 } break;
351 default:
352 {
353 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
354 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
355 } break;
356 }
357 } break;
358
359 // Velocity direct control
361 switch (b.get(1).asVocab32()) {
363 if (stream_IVelDirect) {
364 int joint = b.get(2).asInt32();
365 bool ok = stream_IVelDirect->setRefVelocity(joint, cmdVector[0]); // cmdVector.data());
366 if (!ok) {
367 yCError(CONTROLBOARD, "Errors while trying to command an streaming velocity direct message on joint %d\n", joint);
368 }
369 }
370 } break;
372 if (stream_IVelDirect) {
373 int n_joints = b.get(2).asInt32();
374 Bottle* jlut = b.get(3).asList();
375 if ((static_cast<int>(jlut->size()) != n_joints) &&
376 (static_cast<int>(cmdVector.size()) != n_joints)) {
377 yCError(CONTROLBOARD, "Received VOCAB_VELOCITY_DIRECT_SET_GROUP size of joints vector or positions vector does not match the selected joint number\n");
378 }
379 std::vector<int> joint_list(n_joints);
380 for (int i = 0; i < n_joints; i++) {
381 joint_list[i] = jlut->get(i).asInt32();
382 }
383 std::vector<double> vel_list(n_joints);
384 for (int i = 0; i < n_joints; i++) {
385 vel_list[i] = cmdVector[i];
386 }
388 if (!ok) {
389 yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on joint group\n");
390 }
391 }
392 } break;
394 if (stream_IVelDirect) {
395 int n_joints = cmdVector.size();
396 std::vector<double> vel_list(n_joints);
397 for (int i = 0; i < n_joints; i++) {
398 vel_list[i] = cmdVector[i];
399 }
401 if (!ok) {
402 yCError(CONTROLBOARD, "Error while trying to command a streaming position direct message on all joints\n");
403 }
404 }
405 } break;
406 default:
407 {
408 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
409 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
410 } break;
411 }
412 } break;
413
414 // fallback error
415 default:
416 {
417 std::string str = yarp::os::Vocab32::decode(b.get(0).asVocab32());
418 yCError(CONTROLBOARD, "Unrecognized message while receiving on command port (%s)\n", str.c_str());
419 } break;
420 }
421}
const yarp::os::LogComponent & CONTROLBOARD()
constexpr yarp::conf::vocab32_t VOCAB_POSITION_MOVES
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF_GROUP
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REFS
constexpr yarp::conf::vocab32_t VOCAB_CURRENTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_CURRENT_REF
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWMS
constexpr yarp::conf::vocab32_t VOCAB_PWMCONTROL_REF_PWM
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_SET_FEEDFORWARD
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_PIDCONTROL_SET_OFFSET
constexpr yarp::conf::vocab32_t VOCAB_POSITIONCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_POSITIONDIRECTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_POSITION_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECTS
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT_GROUP
constexpr yarp::conf::vocab32_t VOCAB_TORQUES_DIRECT
constexpr yarp::conf::vocab32_t VOCAB_TORQUECONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITYCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVE_GROUP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_MOVES
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_ONE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITYDIRECTCONTROL_INTERFACE
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_GROUP
constexpr yarp::conf::vocab32_t VOCAB_VELOCITY_DIRECT_SET_ALL
yarp::dev::IPositionDirect * stream_IPosDirect
void init(yarp::dev::DeviceDriver *x)
Initialization.
yarp::dev::IVelocityControl * stream_IVel
yarp::dev::ICurrentControl * stream_ICurrent
yarp::dev::IPidControl * stream_IPid
yarp::dev::IPWMControl * stream_IPWM
yarp::dev::IVelocityDirect * stream_IVelDirect
yarp::dev::ITorqueControl * stream_ITorque
yarp::dev::IPositionControl * stream_IPosCtrl
void onRead(yarp::dev::CommandMessageData &v) override
Callback function.
yarp::dev::IAxisInfo * stream_IAxis
Interface implemented by all device drivers.
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setRefCurrent(int m, double curr)=0
Set the reference value of the current for a single motor.
virtual yarp::dev::ReturnValue setRefCurrents(const double *currs)=0
Set the reference value of the currents for all motors.
virtual yarp::dev::ReturnValue setRefDutyCycles(const double *refs)=0
Sets the reference dutycycle for all the motors.
virtual yarp::dev::ReturnValue setRefDutyCycle(int m, double ref)=0
Sets the reference dutycycle to a single motor.
virtual yarp::dev::ReturnValue setPidFeedforward(const PidControlTypeEnum &pidtype, int j, double v)=0
Set the feedforward value for the specified PID controller.
virtual yarp::dev::ReturnValue setPidOffset(const PidControlTypeEnum &pidtype, int j, double v)=0
Set an offset value for a given controller.
virtual yarp::dev::ReturnValue positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setPositions(const int n_joint, const int *joints, const double *refs)=0
Set new reference point for all axes.
virtual yarp::dev::ReturnValue setPosition(int j, double ref)=0
Set new position for a single axis.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setRefTorque(int j, double t)=0
Set the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue setRefTorques(const double *t)=0
Set the reference value of the torque for all joints.
virtual yarp::dev::ReturnValue velocityMove(int j, double sp)=0
Start motion at a given speed, single joint.
virtual yarp::dev::ReturnValue getAxes(size_t &axes)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setRefVelocity(int jnt, double vel)=0
Set the velocity of single joint.
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
std::string toString() const override
Gives a human-readable textual representation of the bottle.
Definition Bottle.cpp:212
A mini-server for performing network communication in the background.
virtual yarp::conf::vocab32_t asVocab32() const
Get 32 bit vocabulary identifier as an integer.
Definition Value.cpp:228
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition Value.cpp:204
virtual Bottle * asList() const
Get list value.
Definition Value.cpp:246
#define yCError(component,...)
#define yCTrace(component,...)
For streams capable of holding different kinds of content, check what they actually have.
PidControlTypeEnum
Definition PidEnums.h:15
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.