YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
JoypadControlServer.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
7#include <map>
8#include <vector>
10#include <yarp/os/LogStream.h>
11
12#define DEFAULT_THREAD_PERIOD 0.010 //s
13
14using namespace yarp::dev;
15using namespace yarp::os;
16using namespace yarp::sig;
17
18namespace {
19YARP_LOG_COMPONENT(JOYPADCONTROLSERVER, "yarp.device.JoypadControlServer")
20}
21
22
24{
25 m_countGetters.insert(std::make_pair(VOCAB_BUTTON, &IJoypadController::getButtonCount));
26 m_countGetters.insert(std::make_pair(VOCAB_HAT, &IJoypadController::getHatCount));
27 m_countGetters.insert(std::make_pair(VOCAB_TRACKBALL, &IJoypadController::getTrackballCount));
28 m_countGetters.insert(std::make_pair(VOCAB_AXIS, &IJoypadController::getAxisCount));
29 m_countGetters.insert(std::make_pair(VOCAB_STICK, &IJoypadController::getStickCount));
30 m_countGetters.insert(std::make_pair(VOCAB_TOUCH, &IJoypadController::getTouchSurfaceCount));
31}
32
33inline void cat(Vector& a, const Vector& b)
34{
35 for (size_t i = 0; i < b.size(); i++)
36 {
37 a.push_back(b[i]);
38 }
39}
40
42{
43 bool ret;
44 if(interface)
45 {
46 m_device = interface;
47 ret = true;
48 }
49 else
50 {
51 m_device = nullptr;
52 ret = false;
53 }
54
55 return ret;
56}
57
59{
60 bool ret;
61
62 ret = false;
63 if(cmd.get(0).asVocab32() != VOCAB_IJOYPADCTRL || !cmd.get(1).isVocab32() || !cmd.get(2).isVocab32() || !cmd.get(3).isVocab32())
64 {
65 response.addVocab32(VOCAB_FAILED);
66 return ret;
67 }
68
69 if(cmd.get(1).asVocab32() == VOCAB_GET)
70 {
71 int toGet;
72
73 toGet = cmd.get(2).asVocab32();
74
75 if(cmd.get(3).asVocab32() == VOCAB_COUNT)
76 {
77 if(m_countGetters.find(toGet) != m_countGetters.end())
78 {
79 unsigned int count;
80 getcountmethod getter;
81 getter = m_countGetters[toGet];
82 if((m_device->*getter)(count))
83 {
84 response.addVocab32(VOCAB_OK);
85 response.addInt32(count);
86 ret = true;
87 }
88 }
89 else if (toGet == VOCAB_STICKDOF && cmd.get(4).isInt32())
90 {
91 unsigned int count;
92 if (m_device->getStickDoF(cmd.get(4).asInt32(), count))
93 {
94 response.addVocab32(VOCAB_OK);
95 response.addInt32(count);
96 ret = true;
97 }
98 else
99 {
100 response.addVocab32(VOCAB_FAILED);
101 ret = false;
102 }
103 }
104 else
105 {
106 response.addVocab32(VOCAB_FAILED);
107 ret = false;
108 }
109 }
110 else if(cmd.get(3).asVocab32() == VOCAB_VALUE)
111 {
112 switch (cmd.get(2).asVocab32()) {
113 case VOCAB_BUTTON:
114 {
115 float value;
116 if(cmd.get(4).isInt32() && m_device->getButton(cmd.get(4).asInt32(), value))
117 {
118 response.addVocab32(VOCAB_OK);
119 response.addFloat64(value);
120 ret = true;
121 }
122 break;
123 }
124 case VOCAB_AXIS:
125 {
126 double value;
127 if(cmd.get(4).isInt32() && m_device->getAxis(cmd.get(4).asInt32(), value))
128 {
129 response.addVocab32(VOCAB_OK);
130 response.addFloat64(value);
131 ret = true;
132 }
133 break;
134 }
135 case VOCAB_STICK:
136 {
137 if(cmd.get(4).isVocab32())
138 {
140
142 if(cmd.get(5).isInt32() && m_device->getStick(cmd.get(5).asInt32(), frame, mode))
143 {
144 response.addVocab32(VOCAB_OK);
145 for(size_t i = 0; i < frame.size(); ++i)
146 {
147 response.addFloat64(frame[i]);
148 }
149
150 ret = true;
151 }
152 }
153 break;
154 }
155 case VOCAB_STICKDOF:
156 {
157
158 unsigned int dofCount;
159
160 if(cmd.get(5).isInt32() && m_device->getStickDoF(cmd.get(5).asInt32(), dofCount))
161 {
162 response.addVocab32(VOCAB_OK);
163 response.addInt32(dofCount);
164 ret = true;
165 }
166
167 break;
168 }
169 case VOCAB_TOUCH:
170 {
172 unsigned int id;
173
174 id = cmd.get(4).asInt32();
175 if(cmd.get(4).isInt32() && m_device->getTouch(id, pos))
176 {
177 response.addVocab32(VOCAB_OK);
178 for(size_t i = 0; i < pos.size(); ++i)
179 {
180 response.addFloat64(pos[i]);
181 }
182 ret = true;
183 }
184 break;
185 }
186 case VOCAB_TRACKBALL:
187 {
189 unsigned int id;
190
191 id = cmd.get(4).asInt32();
192 if(cmd.get(4).isInt32() && m_device->getTrackball(id, axes))
193 {
194 response.addVocab32(VOCAB_OK);
195 for(size_t i = 0; i < axes.size(); ++i)
196 {
197 response.addFloat64(axes[i]);
198 }
199 ret = true;
200 }
201 break;
202 }
203 case VOCAB_HAT:
204 {
205 unsigned char value;
206 if(cmd.get(4).isInt32() && m_device->getHat(cmd.get(4).asInt32(), value))
207 {
208 response.addVocab32(VOCAB_OK);
209 response.addInt32(value);
210 ret = true;
211 }
212 break;
213 }
214 default:
215 break;
216 }
217 }
218 }
219 return ret;
220
221}
222
223
227
231
233{
234 if (!parseParams(config)) { return false; }
235
236 if(m_use_separate_ports == false)
237 {
238 yCError(JOYPADCONTROLSERVER) << "Single port mode not supported at the moment";
239 return false;
240 }
241
242 m_rpcPortName = m_name + "/rpc:i";
243 m_portButtons.name = m_name + "/buttons:o";
244 m_portAxis.name = m_name + "/axis:o";
245 m_portStick.name = m_name + "/stick:o";
246 m_portTouch.name = m_name + "/touch:o";
247 m_portTrackball.name = m_name + "/trackball:o";
248 m_portHats.name = m_name + "/hat:o";
249
250 yCInfo(JOYPADCONTROLSERVER) << "Running, waiting for attach...";
251 return true;
252}
253
255{
256 if (poly) {
257 poly->view(m_IJoypad);
258 }
259
260 if(m_IJoypad == nullptr)
261 {
262 yCError(JOYPADCONTROLSERVER) << "Attached device has no valid IJoypadController interface.";
263 return false;
264 }
265
267 if (!PeriodicThread::start()) {
268 return false;
269 }
270
271 openPorts();
272 if (!m_parser.configure(m_IJoypad))
273 {
274 yCError(JOYPADCONTROLSERVER) << "Error configuring interfaces for parsers";
275 return false;
276 }
277
278 return true;
279}
280
282{
285 }
286
287 m_IJoypad = nullptr;
288 return true;
289}
290
292{
293 // Get interface from attached device if any.
294 return true;
295}
296
298{
299 // Detach() calls stop() which in turns calls this functions, therefore no calls to detach here!
300}
301
302bool JoypadControlServer::openPorts()
303{
304 if(!m_IJoypad)
305 {
306 return false;
307 }
308
309 if(!m_rpcPort.open(m_rpcPortName))
310 {
311 yCError(JOYPADCONTROLSERVER) << "Unable to open rpc Port" << m_rpcPortName.c_str();
312 return false;
313 }
314 m_rpcPort.setReader(m_parser);
315// dumb(or K.I.S.S.) version of the method:
316// unsigned int count;
317// if(m_device->getAxisCount(count))
318// {
319// if(count == 0)
320// {
321// m_portAxis.valid = false;
322// }
323// else
324// {
325// m_portAxis.open();
326// m_portAxis.valid = true;
327// }
328// }
329// else
330// {
331// return false;
332// }
333//
334// if(m_device->getButtonCount(count))
335// {
336// if(count == 0)
337// {
338// m_portButton.valid = false;
339// }
340// else
341// {
342// m_portButton.open();
343// m_portButton.valid = true;
344// }
345// }
346// else
347// {
348// return false;
349// }
350//
351// if(m_device->getStickCount(count))
352// {
353// if(count == 0)
354// {
355// m_portStick.valid = false;
356// }
357// else
358// {
359// m_portStick.open();
360// m_portStick.valid = true;
361// }
362// }
363// else
364// {
365// return false;
366// }
367//
368// if(m_device->getTouchSurfaceCount(count))
369// {
370// if(count == 0)
371// {
372// m_portTouch.valid = false;
373// }
374// else
375// {
376// m_portTouch.open();
377// m_portTouch.valid = true;
378// }
379// }
380// else
381// {
382// return false;
383// }
384// if(m_device->getTrackballCount(count))
385// {
386// if(count == 0)
387// {
388// m_portTrackball.valid = false;
389// }
390// else
391// {
392// m_portTrackball.open();
393// m_portTrackball.valid = true;
394// }
395// }
396// else
397// {
398// return false;
399// }
400//
401// if(m_device->getHatCount(count))
402// {
403// if(count == 0)
404// {
405// m_portHats.valid = false;
406// }
407// else
408// {
409// m_portHats.open();
410// m_portHats.valid = true;
411// }
412// }
413// else
414// {
415// return false;
416// }
417// return true;
419 {
421
422 struct solver
423 {
426
428 {}
429 };
430
431 std::vector<solver> getters;
432
433 getters.emplace_back(&IJoypadController::getAxisCount, &m_portAxis );
434 getters.emplace_back(&IJoypadController::getButtonCount, &m_portButtons );
435 getters.emplace_back(&IJoypadController::getStickCount, &m_portStick );
436 getters.emplace_back(&IJoypadController::getTouchSurfaceCount, &m_portTouch );
437 getters.emplace_back(&IJoypadController::getTrackballCount, &m_portTrackball);
438 getters.emplace_back(&IJoypadController::getHatCount, &m_portHats );
439
440 for(auto& getter : getters)
441 {
442 if((m_IJoypad->*(getter.getter))(getter.port->count))
443 {
444 if(getter.port->count == 0)
445 {
446 getter.port->valid = false;
447 }
448 else
449 {
450 getter.port->contactable->open(getter.port->name);
451 getter.port->valid = true;
452 }
453 }
454 else
455 {
456 return false;
457 }
458 }
459
460 return true;
461 }
462 else
463 {
464 return false;
465 //m_godPort.open(m_name + "/joydata:o");
466 }
467}
468
469void JoypadControlServer::profile()
470{
471 std::string message;
472 unsigned int count;
473
474 message = "Axes: ";
475 m_IJoypad->getAxisCount(count);
476 for(unsigned int i = 0; i < count; ++i)
477 {
478 double data;
479 m_IJoypad->getAxis(i, data);
480 message += std::to_string(data) + " ";
481 }
482 yCInfo(JOYPADCONTROLSERVER) << message;
483
484 message = "Hats: ";
485 m_IJoypad->getHatCount(count);
486 for(unsigned int i = 0; i < count; ++i)
487 {
488 unsigned char data;
489 m_IJoypad->getHat(i, data);
490 message += std::to_string(data) + " ";
491 }
492 yCInfo(JOYPADCONTROLSERVER) << message;
493
494 message = "Buttons: ";
495 m_IJoypad->getButtonCount(count);
496 for(unsigned int i = 0; i < count; ++i)
497 {
498 float data;
499 m_IJoypad->getButton(i, data);
500 message += std::to_string(data) + " ";
501 }
502 yCInfo(JOYPADCONTROLSERVER) << message;
503
504 message = "Stick: ";
505 m_IJoypad->getStickCount(count);
506 for(unsigned int i = 0; i < count; ++i)
507 {
508 Vector data;
510 message += "n_" + std::to_string(i) + ": ";
511 for (size_t j = 0; j < data.size(); ++j)
512 {
513 message += std::to_string(data[j]) + " ";
514 }
515 message += "\n";
516
517 }
518 yCInfo(JOYPADCONTROLSERVER) << message;
519
520 message = "trackball: ";
521 m_IJoypad->getTrackballCount(count);
522 for(unsigned int i = 0; i < count; ++i)
523 {
524 Vector data;
525 m_IJoypad->getTrackball(i, data);
526 message += "n_" + std::to_string(i) + ": ";
527 for (size_t j = 0; j < data.size(); ++j)
528 {
529 message += std::to_string(data[j]) + " ";
530 }
531 message += "\n";
532 }
533
534 message = "touch Surface: ";
535 m_IJoypad->getTouchSurfaceCount(count);
536 for(unsigned int i = 0; i < count; ++i)
537 {
538 Vector data;
539 m_IJoypad->getTouch(i, data);
540 message += "n_" + std::to_string(i) + ": ";
541 for (size_t j = 0; j < data.size(); ++j)
542 {
543 message += std::to_string(data[j]) + " ";
544 }
545 message += "\n";
546 }
547 yCInfo(JOYPADCONTROLSERVER) << message;
548}
549
551{
553 {
554 if (m_portButtons.valid)
555 {
556 bool write;
557 write = true;
558 Vector& b = m_portButtons.prepare();
559 b.clear();
560 for(size_t i = 0; i < m_portButtons.count; ++i)
561 {
562 float v;
563 if(!m_IJoypad->getButton(i, v))
564 {
565 write = false;
566 break;
567 }
568 b.push_back(v);
569 }
570 if (write) {
571 m_portButtons.write();
572 }
573 }
574
575 if (m_portHats.valid)
576 {
577 bool write;
578
579 write = true;
580 VecOfChar& b = m_portHats.prepare();
581 b.clear();
582 for(size_t i = 0; i < m_portHats.count; ++i)
583 {
584 unsigned char v;
585 if(!m_IJoypad->getHat(i, v))
586 {
587 write = false;
588 break;
589 }
590 b.push_back(v);
591 }
592 if (write) {
593 m_portHats.write();
594 }
595 }
596
597 if (m_portAxis.valid)
598 {
599 bool write;
600
601 write = true;
602 Vector& b = m_portAxis.prepare();
603 b.clear();
604 for(size_t i = 0; i < m_portAxis.count; ++i)
605 {
606 double v;
607 if(!m_IJoypad->getAxis(i, v))
608 {
609 yCError(JOYPADCONTROLSERVER) << "Cannot get axis with id" << i;
610 write = false;
611 break;
612 }
613 b.push_back(v);
614 }
615 if (write) {
616 m_portAxis.write();
617 }
618 }
619
620 if (m_portTrackball.valid)
621 {
622 bool write;
623
624 write = true;
625 Vector& b = m_portTrackball.prepare();
626 b.clear();
627 for(size_t i = 0; i < m_portTrackball.count; ++i)
628 {
629 Vector v;
630 if(!m_IJoypad->getTrackball(i, v))
631 {
632 yCError(JOYPADCONTROLSERVER) << "Cannot get axis with id" << i;
633 write = false;
634 break;
635 }
636 cat(b, v);
637 }
638 if (write) {
639 m_portTrackball.write();
640 }
641 }
642
643 if (m_portStick.valid)
644 {
645 bool write;
646 write = true;
647 Vector& b = m_portStick.prepare();
648 b.clear();
649 for(size_t i = 0; i < m_portStick.count; ++i)
650 {
651 Vector v;
652 unsigned int dofCount;
653 if(!m_IJoypad->getStick(i, v, m_coordsMode) || !m_IJoypad->getStickDoF(i, dofCount) || v.size() != dofCount)
654 {
655 write = false;
656 break;
657 }
658 cat(b, v);
659 }
660 if (write) {
661 m_portStick.write();
662 }
663 }
664
665 if (m_portTouch.valid)
666 {
667 bool write;
668 write = true;
669 Vector& b = m_portTouch.prepare();
670 b.clear();
671 for(unsigned int i = 0; i < m_portTouch.count; ++i)
672 {
673 Vector v;
674 if(!m_IJoypad->getTouch(i, v))
675 {
676 write = false;
677 break;
678 }
679 cat(b, v);
680 }
681
682 if (write) {
683 m_portTouch.write();
684 }
685 }
686 }
687 else
688 {
689 return;
690 //JoyData& message = m_godPort.prepare();
691 //for(size_t i = 0; i < m_device->getAxisCount();)
692 //message.Axes
693 }
694
695 if(m_profile)
696 {
697 profile();
698 }
699}
700
702{
703 detachAll();
704
705 // Closing port
706 std::vector<JoypadControl::LoopablePort*> portv;
707 portv.push_back(&m_portButtons);
708 portv.push_back(&m_portAxis);
709 portv.push_back(&m_portStick);
710 portv.push_back(&m_portTouch);
711 portv.push_back(&m_portTrackball);
712 portv.push_back(&m_portHats);
713
714 for(auto p : portv)
715 {
716 //p->contactable->interrupt();
717 p->contactable->close();
718 }
719
720 m_rpcPort.close();
721 return true;
722}
constexpr yarp::conf::vocab32_t VOCAB_OK
constexpr yarp::conf::vocab32_t VOCAB_GET
constexpr yarp::conf::vocab32_t VOCAB_FAILED
constexpr yarp::conf::vocab32_t VOCAB_VALUE
constexpr yarp::conf::vocab32_t VOCAB_COUNT
constexpr yarp::conf::vocab32_t VOCAB_BUTTON
constexpr yarp::conf::vocab32_t VOCAB_IJOYPADCTRL
constexpr yarp::conf::vocab32_t VOCAB_TOUCH
constexpr yarp::conf::vocab32_t VOCAB_HAT
constexpr yarp::conf::vocab32_t VOCAB_TRACKBALL
constexpr yarp::conf::vocab32_t VOCAB_CARTESIAN
constexpr yarp::conf::vocab32_t VOCAB_STICKDOF
constexpr yarp::conf::vocab32_t VOCAB_AXIS
constexpr yarp::conf::vocab32_t VOCAB_STICK
bool ret
#define DEFAULT_THREAD_PERIOD
void cat(Vector &a, const Vector &b)
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
void threadRelease() override
Release method.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool close() override
Close the DeviceDriver.
bool detach() override
Detach the object (you must have first called attach).
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool configure(yarp::dev::IJoypadController *interface)
bool respond(const yarp::os::Bottle &cmd, yarp::os::Bottle &response) override
Respond to a message.
bool view(T *&x)
Get an interface to the device driver.
virtual bool getHatCount(unsigned int &Hat_count)=0
Get number of hats.
virtual bool getTrackballCount(unsigned int &Trackball_count)=0
Get number of trackballs.
virtual bool getTouchSurfaceCount(unsigned int &touch_count)=0
Get the number of touch surface.
virtual bool getButton(unsigned int button_id, float &value)=0
Get the value of a button.
virtual bool getAxisCount(unsigned int &axis_count)=0
Get number of axes.
virtual bool getAxis(unsigned int axis_id, double &value)=0
Get the value of an axis if present, return false otherwise.
virtual bool getStickCount(unsigned int &stick_count)=0
Get the number of the sticks.
virtual bool getTrackball(unsigned int trackball_id, yarp::sig::Vector &value)=0
Get the axes change of a Trackball.
virtual bool getStick(unsigned int stick_id, yarp::sig::Vector &value, JoypadCtrl_coordinateMode coordinate_mode)=0
Get the value of a stick if present, return false otherwise.
virtual bool getStickDoF(unsigned int stick_id, unsigned int &DoF)=0
Get the Degree Of Freedom count for desired stick.
virtual bool getHat(unsigned int hat_id, unsigned char &value)=0
Get the value of an Hat.
virtual bool getButtonCount(unsigned int &button_count)=0
Get number of buttons.
virtual bool getTouch(unsigned int touch_id, yarp::sig::Vector &value)=0
Get the value of a touch if present, return false otherwise.
A container for a device driver.
Definition PolyDriver.h:23
bool detachAll() final
Detach the object (you must have first called attach).
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
void addVocab32(yarp::conf::vocab32_t x)
Places a vocabulary item in the bottle, at the end of the list.
Definition Bottle.cpp:164
void addFloat64(yarp::conf::float64_t x)
Places a 64-bit floating point number in the bottle, at the end of the list.
Definition Bottle.cpp:158
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:246
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition Bottle.cpp:140
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
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 setReader(PortReader &reader) override
Set an external reader for port data.
Definition Port.cpp:511
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
virtual yarp::conf::vocab32_t asVocab32() const
Get 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 bool isInt32() const
Checks if value is a 32-bit integer.
Definition Value.cpp:132
virtual bool isVocab32() const
Checks if value is a vocabulary identifier.
Definition Value.cpp:174
size_t size() const
Definition Vector.h:322
void push_back(const T &elem)
Push a new element in the vector: size is changed.
Definition Vector.h:249
#define yCInfo(component,...)
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.