YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
JoypadControl_nws_yarp.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 <algorithm>
8#include <sstream>
9#include <limits>
10
12#include <yarp/os/LogStream.h>
13
14#define DEFAULT_THREAD_PERIOD 0.010 //s
15
16using namespace yarp::dev;
17using namespace yarp::os;
18using namespace yarp::sig;
19
20namespace {
21YARP_LOG_COMPONENT(JOYPADCONTROLSERVER, "yarp.device.JoypadControl_nws_yarp")
22}
23
24template <class T>
26{
27 for (size_t i = 0; i < b.size(); i++)
28 {
29 a.push_back(b[i]);
30 }
31}
32
34{
35 private:
36 mutable std::mutex m_mutex;
37 yarp::dev::IJoypadController* m_iJoy = nullptr;
38
39 public:
41 {
42 m_iJoy = interfaces;
43 }
44
45 //Methods
52 return_getButton getButton(const std::int32_t button_id) override;
53 return_getTrackball getTrackball(const std::int32_t trackball_id) override;
54 return_getHat getHat(const std::int32_t hat_id) override;
55 return_getAxis getAxis(const std::int32_t axis_id) override;
58 return_getTouch getTouch(const std::int32_t touch_id) override;
59};
60
62{
64
65 if (!m_iJoy) {
66 yCError(JOYPADCONTROLSERVER, "Invalid interface");
67 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
68 return ret;
69 }
70
71 size_t temp=0;
72 ret.ret = m_iJoy->getAxisCount(temp);
73 ret.axis_count = temp;
74 return ret;
75}
76
78{
80
81 if (!m_iJoy) {
82 yCError(JOYPADCONTROLSERVER, "Invalid interface");
83 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
84 return ret;
85 }
86
87 size_t temp=0;
88 ret.ret = m_iJoy->getButtonCount(temp);
89 ret.button_count = temp;
90 return ret;
91}
92
94{
96
97 if (!m_iJoy) {
98 yCError(JOYPADCONTROLSERVER, "Invalid interface");
99 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
100 return ret;
101 }
102
103 size_t temp=0;
104 ret.ret = m_iJoy->getTrackballCount(temp);
106 return ret;
107}
108
110{
112
113 if (!m_iJoy) {
114 yCError(JOYPADCONTROLSERVER, "Invalid interface");
115 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
116 return ret;
117 }
118
119 size_t temp=0;
120 ret.ret = m_iJoy->getHatCount(temp);
121 ret.Hat_count= temp;
122 return ret;
123}
124
126{
128
129 if (!m_iJoy) {
130 yCError(JOYPADCONTROLSERVER, "Invalid interface");
131 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
132 return ret;
133 }
134
135 size_t temp=0;
136 ret.ret = m_iJoy->getTouchSurfaceCount(temp);
137 ret.touch_count= temp;
138 return ret;
139}
140
142{
144
145 if (!m_iJoy) {
146 yCError(JOYPADCONTROLSERVER, "Invalid interface");
147 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
148 return ret;
149 }
150
151 size_t temp=0;
152 ret.ret = m_iJoy->getStickCount(temp);
153 ret.stick_count=temp;
154 return ret;
155}
156
158{
160
161 if (!m_iJoy) {
162 yCError(JOYPADCONTROLSERVER, "Invalid interface");
163 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
164 return ret;
165 }
166
167 ret.ret = m_iJoy->getButton(id, ret.value);
168 return ret;
169}
170
172{
174
175 if (!m_iJoy) {
176 yCError(JOYPADCONTROLSERVER, "Invalid interface");
177 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
178 return ret;
179 }
180
181 size_t temp=0;
182 ret.ret = m_iJoy->getTrackball(id, ret.value);
183 return ret;
184}
185
187{
188 return_getHat ret;
189
190 if (!m_iJoy) {
191 yCError(JOYPADCONTROLSERVER, "Invalid interface");
192 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
193 return ret;
194 }
195
196 unsigned char temp = 0;
197 ret.ret = m_iJoy->getHat(id, temp);
198 ret.value= temp;
199 return ret;
200}
201
203{
205
206 if (!m_iJoy) {
207 yCError(JOYPADCONTROLSERVER, "Invalid interface");
208 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
209 return ret;
210 }
211
212 std::vector<double> vals;
213 ret.ret = m_iJoy->getAllAxes(ret.value);
214 return ret;
215}
216
218{
219 return_getAxis ret;
220
221 if (!m_iJoy) {
222 yCError(JOYPADCONTROLSERVER, "Invalid interface");
223 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
224 return ret;
225 }
226
227
228 ret.ret = m_iJoy->getAxis(id, ret.value);
229 return ret;
230}
231
233{
234 return_getStick ret;
235
236 if (!m_iJoy) {
237 yCError(JOYPADCONTROLSERVER, "Invalid interface");
238 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
239 return ret;
240 }
241
242 ret.ret = m_iJoy->getStick(id, ret.value, mode);
243 return ret;
244}
245
247{
248 return_getTouch ret;
249
250 if (!m_iJoy) {
251 yCError(JOYPADCONTROLSERVER, "Invalid interface");
252 ret.ret = ReturnValue::return_code::return_value_error_not_ready;
253 return ret;
254 }
255
256 ret.ret = m_iJoy->getTouch(id, ret.value);
257 return ret;
258}
259
260//-------------------------------------------------------------
261
265
269
271{
272 if (!parseParams(config)) { return false; }
273
274 yCInfo(JOYPADCONTROLSERVER) << "Running, waiting for attach...";
275 return true;
276}
277
279{
280 if (poly) {
281 poly->view(m_IJoypad);
282 }
283
284 if(m_IJoypad == nullptr)
285 {
286 yCError(JOYPADCONTROLSERVER) << "Attached device has no valid IJoypadController interface.";
287 return false;
288 }
289
290 if (!openPorts())
291 {
292 yCError(JOYPADCONTROLSERVER) << "Unable to open ports";
293 return false;
294 }
295
296 m_RPCMsgs = std::make_unique<IJoypadControlRPCd>(this->m_IJoypad);
297
299 if (!PeriodicThread::start()) {
300 return false;
301 }
302
303 return true;
304}
305
307{
310 }
311
312 m_IJoypad = nullptr;
313 return true;
314}
315
317{
318 bool v= true;
319 v&=m_IJoypad->getAxisCount (m_numberOfAxes);
320 v&=m_IJoypad->getButtonCount (m_numberOfButtons);
321 v&=m_IJoypad->getTrackballCount (m_numberOfTrackballs);
322 v&=m_IJoypad->getHatCount (m_numberOfHats);
323 v&=m_IJoypad->getTouchSurfaceCount (m_numberOfTouchSurfaces);
324 v&=m_IJoypad->getStickCount (m_numberOfSticks);
325 if (!v) return false;
326
327 m_curr_axes.resize (m_numberOfAxes);
328 m_curr_buttons.resize (m_numberOfButtons);
329 m_curr_trackballs.resize (m_numberOfTrackballs);
330 m_curr_hats.resize (m_numberOfHats);
331 m_curr_sticks.resize (m_numberOfSticks);
332 m_curr_touches.value.resize (m_numberOfTouchSurfaces);
333
334 m_last_axes.resize (m_numberOfAxes);
335 m_last_buttons.resize (m_numberOfButtons);
336 m_last_trackballs.resize (m_numberOfTrackballs);
337 m_last_hats.resize (m_numberOfHats);
338 m_last_sticks.resize (m_numberOfSticks);
339 m_last_touches.value.resize (m_numberOfTouchSurfaces);
340
341 return true;
342}
343
347
348bool JoypadControl_nws_yarp::openPorts()
349{
350 if(!m_IJoypad)
351 {
352 return false;
353 }
354
355 if (!m_rpcPort.open(m_name + "/rpc:i"))
356 {
357 yCError(JOYPADCONTROLSERVER) << "Unable to open rpc Port" << (m_name+"/rpc:o");
358 return false;
359 }
360 m_rpcPort.setReader(*this);
361
362 bool b=true;
363 b &= m_portAxes.open(m_name+"/axis:o");
364 b &= m_portButtons.open(m_name+"/buttons:o");
365 b &= m_portSticks.open(m_name+"/stick:o");
366 b &= m_portTouches.open(m_name+"/touch:o");
367 b &= m_portTrackballs.open(m_name+"/trackball:o");
368 b &= m_portHats.open(m_name+"/hat:o");
369 b &= m_allPort.open(m_name+"/allJoyData:o");
370 if (!b)
371 {
372 yCError(JOYPADCONTROLSERVER) << "Unable to streaming ports";
373 return false;
374 }
375
376 return true;
377}
378
379template <class T>
380bool VectorsDifferent(const std::vector<T>& a, const std::vector<T>& b)
381{
382 return a.size() != b.size() ||
383 !std::equal(a.begin(), a.end(), b.begin());
384}
385
386
387template <class T, class Compare>
388bool VectorsDifferentC(const std::vector<T>& a, const std::vector<T>& b, Compare comp)
389{
390 return a.size() != b.size() ||
391 !std::equal(a.begin(), a.end(), b.begin(), comp);
392}
393
394inline std::string printVector(const std::vector<double>& v)
395{
396 std::ostringstream oss;
397 oss.precision(std::numeric_limits<double>::max_digits10);
398 oss << '(';
399 for (std::size_t i = 0; i < v.size(); ++i)
400 {
401 oss << ' ' << v[i];
402 }
403 oss << ')';
404 return oss.str();
405}
406
408{
409 std::string profile_message;
410
411 //1 Buttons
412 {
413 bool write;
414 write = m_portButtons.getOutputCount()>0;
415 auto& b = m_portButtons.prepare();
416 b.value.clear();
417 for(size_t i = 0; i < m_numberOfButtons; ++i)
418 {
419 if (!m_IJoypad->getButton(i, m_curr_buttons[i]))
420 {
421 write = false;
422 break;
423 }
424 }
425
426 if (write)
427 {
429 {
430 if (VectorsDifferent(m_curr_buttons,m_last_buttons))
431 {
432 b.value = m_curr_buttons;
433 m_portButtons.write();
434 m_last_buttons = m_curr_buttons;
435 }
436 }
437 else
438 {
439 b.value = m_curr_buttons;
440 m_portButtons.write();
441 m_last_buttons = m_curr_buttons;
442 }
443 }
444 }
445
446 //2 Hats
447 {
448 bool write;
449
450 write = m_portHats.getOutputCount()>0;
451 auto& b = m_portHats.prepare();
452 b.value.clear();
453 for(size_t i = 0; i < m_numberOfHats; ++i)
454 {
455 if (!m_IJoypad->getHat(i, m_curr_hats[i]))
456 {
457 write = false;
458 break;
459 }
460 }
461
462 if (write)
463 {
465 {
466 if (VectorsDifferent(m_curr_hats,m_last_hats))
467 {
468 b.value.resize(m_curr_hats.size());
469 std::transform(m_curr_hats.begin(), m_curr_hats.end(), b.value.begin(), [](unsigned char c) { return static_cast<char>(c); });
470 //b.value = m_curr_hats; //using std::transform instead
471 m_portHats.write();
472 m_last_hats = m_curr_hats;
473 }
474 }
475 else
476 {
477 b.value.resize(m_curr_hats.size());
478 std::transform(m_curr_hats.begin(), m_curr_hats.end(), b.value.begin(), [](unsigned char c) { return static_cast<char>(c); });
479 //b.value = m_curr_hats; //using std::transform instead
480 m_portHats.write();
481 m_last_hats = m_curr_hats;
482 }
483 }
484 }
485
486 //3 Axis
487 {
488 bool write;
489
490 write = m_portAxes.getOutputCount()>0;
491 auto& b = m_portAxes.prepare();
492 b.value.clear();
493 for(size_t i = 0; i < m_numberOfAxes; ++i)
494 {
495 if (!m_IJoypad->getAxis(i, m_curr_axes[i]))
496 {
497 write = false;
498 break;
499 }
500 }
501
502 if (write)
503 {
505 {
506 if (VectorsDifferent(m_curr_axes,m_last_axes))
507 {
508 b.value = m_curr_axes;
509 m_portAxes.write();
510 m_last_axes = m_curr_axes;
511 }
512 }
513 else
514 {
515 b.value = m_curr_axes;
516 m_portAxes.write();
517 m_last_axes = m_curr_axes;
518 }
519 }
520 }
521
522 //4 Trackball
523 {
524 bool write;
525
526 write = m_portTrackballs.getOutputCount()>0;
527 auto& b = m_portTrackballs.prepare();
528 b.value.clear();
529 for(size_t i = 0; i < m_numberOfTrackballs; ++i)
530 {
531 if (!m_IJoypad->getTrackball(i, m_curr_trackballs[i]))
532 {
533 write = false;
534 break;
535 }
536 }
537
538 if (write)
539 {
541 {
542 if (VectorsDifferentC(m_curr_trackballs,m_last_trackballs,
545 {
546 return lhs.x == rhs.x &&
547 lhs.y == rhs.y;
548 }
549 ))
550 {
551 b.value = m_curr_trackballs;
552 m_portTrackballs.write();
553 m_last_trackballs = m_curr_trackballs;
554 }
555 }
556 else
557 {
558 b.value = m_curr_trackballs;
559 m_portTrackballs.write();
560 m_last_trackballs = m_curr_trackballs;
561 }
562 }
563 }
564
565 //5 Stick
566 {
567 bool write;
568 write = m_portSticks.getOutputCount()>0;
569 auto& b = m_portSticks.prepare();
570 b.value.clear();
571 for(size_t i = 0; i < m_numberOfSticks; ++i)
572 {
573 if (!m_IJoypad->getStick(i, m_curr_sticks[i], m_coordsMode))
574 {
575 write = false;
576 break;
577 }
578 }
579
580 if (write)
581 {
583 {
584 if (VectorsDifferentC(m_curr_sticks,m_last_sticks,
585 [](const yarp::dev::StickData& lhs,
587 {
588 return lhs.s1 == rhs.s1 &&
589 lhs.s2 == rhs.s2;
590 }
591 ))
592 {
593 b.value = m_curr_sticks;
594 m_portSticks.write();
595 m_last_sticks = m_curr_sticks;
596 }
597 }
598 else
599 {
600 b.value = m_curr_sticks;
601 m_portSticks.write();
602 m_last_sticks = m_curr_sticks;
603 }
604 }
605 }
606
607 //6 Touch
608 {
609 bool write;
610 write = m_portTouches.getOutputCount()>0;
611 auto& b = m_portTouches.prepare();
612 b.value.clear();
613
614 m_curr_touches.value.resize(m_numberOfTouchSurfaces);
615 for(unsigned int i = 0; i < m_numberOfTouchSurfaces; ++i)
616 {
617 std::vector<yarp::dev::TouchData> temp;
618 if (!m_IJoypad->getTouch(i, temp))
619 {
620 write = false;
621 break;
622 }
623 m_curr_touches.value[i].touches = temp;
624 }
625
626 if (write)
627 {
629 {
630 if (1)
631 //if (VectorsDifferent(m_curr_touches.value,m_last_touches.value))
632 {
633 b.value = m_curr_touches.value;
634 m_portTouches.write();
635 m_last_touches = m_curr_touches;
636 }
637 }
638 else
639 {
640 b.value = m_curr_touches.value;
641 m_portTouches.write();
642 m_last_touches = m_curr_touches;
643 }
644 }
645 }
646
647
648 //AllJoyDataPort
649 if (m_allPort.getOutputCount()>0)
650 {
651 yarp::dev::AllJoyData& message = m_allPort.prepare();
652 message.AxisDataVal = m_curr_axes;
653 message.ButtonDataVal = m_curr_buttons;
654 message.HatsDataVal = std::vector<std::int8_t>(m_curr_hats.begin(), m_curr_hats.end());
655 message.StickDataVal = m_curr_sticks;
656 message.TrackballDataVal = m_curr_trackballs;
657 message.TouchDataVal = m_curr_touches.value;
658 m_allPort.write();
659 }
660
661 //print data on screen
662 if(m_profile)
663 {
664 profile_message = " Axes: ";
665 profile_message += printVector(m_curr_axes);
666 profile_message += " Buttons: ";
667 profile_message += printVector(m_curr_buttons);
668 //profile_message += " Trackballs: ";
669 //profile_message += printVector(m_curr_trackballs);
670 //profile_message += " Sticks: ";
671 //profile_message += printVector(m_curr_sticks);
672 //profile_message += " Hats: ";
673 //profile_message += printVector(m_curr_hats);
674 //profile_message += " Touch Surface: ";
675 //profile_message += printVector(m_curr_touches);
677 }
678}
679
681{
682 detachAll();
683
684 // Closing port
685 m_portAxes.close();
686 m_portButtons.close();
687 m_portSticks.close();
688 m_portTouches.close();
689 m_portTrackballs.close();
690 m_portHats.close();
691
692 m_rpcPort.close();
693
694 m_allPort.close();
695
696 return true;
697}
698
700{
701 if (!connection.isValid()) { return false;}
702 if (!m_RPCMsgs) { return false;}
703
704 if (m_RPCMsgs)
705 {
706 bool b = m_RPCMsgs->read(connection);
707 if (b) {
708 return true;
709 }
710 }
711 yCDebug(JOYPADCONTROLSERVER) << "read() Command failed";
712 return false;
713}
std::string printVector(const std::vector< double > &v)
bool VectorsDifferent(const std::vector< T > &a, const std::vector< T > &b)
#define DEFAULT_THREAD_PERIOD
bool VectorsDifferentC(const std::vector< T > &a, const std::vector< T > &b, Compare comp)
void cat(yarp::sig::VectorOf< T > &a, const yarp::sig::VectorOf< T > &b)
return_getAxis getAxis(const std::int32_t axis_id) override
return_getTouch getTouch(const std::int32_t touch_id) override
return_getButtonCount getButtonCount() override
return_getStickCount getStickCount() override
return_getButton getButton(const std::int32_t button_id) override
return_getTouchSurfaceCount getTouchSurfaceCount() override
return_getAxisCount getAxisCount() override
IJoypadControlRPCd(yarp::dev::IJoypadController *interfaces)
return_getHat getHat(const std::int32_t hat_id) override
return_getTrackball getTrackball(const std::int32_t trackball_id) override
return_getHatCount getHatCount() override
return_getTrackballCount getTrackballCount() override
return_getStick getStick(const std::int32_t stick_id, yarp::dev::IJoypadController::JoypadCtrl_coordinateMode mode) override
return_getAllAxes getAllAxes() override
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
bool detach() override
Detach the object (you must have first called attach).
void threadRelease() override
Release method.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
void run() override
Loop function.
bool threadInit() override
Initialization method.
bool open(yarp::os::Searchable &params) override
Open the DeviceDriver.
bool attach(yarp::dev::PolyDriver *poly) override
Attach to another object.
bool close() override
Close the DeviceDriver.
std::vector< double > ButtonDataVal
Definition AllJoyData.h:27
std::vector< yarp::dev::StickData > StickDataVal
Definition AllJoyData.h:31
std::vector< yarp::dev::TrackballData > TrackballDataVal
Definition AllJoyData.h:32
std::vector< double > AxisDataVal
Definition AllJoyData.h:28
std::vector< std::int8_t > HatsDataVal
Definition AllJoyData.h:29
std::vector< MultipleTouches > TouchDataVal
Definition AllJoyData.h:30
bool view(T *&x)
Get an interface to the device driver.
virtual yarp::dev::ReturnValue getTouchSurfaceCount(size_t &touch_count)=0
Get the number of touch surface.
virtual yarp::dev::ReturnValue getAxisCount(size_t &axis_count)=0
Get number of axes.
virtual yarp::dev::ReturnValue getTouch(size_t touch_id, std::vector< yarp::dev::TouchData > &value)=0
Get the value of a touch if present, return false otherwise.
virtual yarp::dev::ReturnValue getTrackball(size_t trackball_id, yarp::dev::TrackballData &value)=0
Get the axes change of a Trackball.
virtual yarp::dev::ReturnValue getAllAxes(std::vector< double > &values)
Get the values of all available joystick axes.
virtual yarp::dev::ReturnValue getButton(size_t button_id, double &value)=0
Get the value of a button.
virtual yarp::dev::ReturnValue getTrackballCount(size_t &Trackball_count)=0
Get number of trackballs.
virtual yarp::dev::ReturnValue getHatCount(size_t &Hat_count)=0
Get number of hats.
virtual yarp::dev::ReturnValue getHat(size_t hat_id, unsigned char &value)=0
Get the value of an Hat.
virtual yarp::dev::ReturnValue getButtonCount(size_t &button_count)=0
Get number of buttons.
virtual yarp::dev::ReturnValue getAxis(size_t axis_id, double &value)=0
Get the value of an axis if present, return false otherwise.
virtual yarp::dev::ReturnValue getStickCount(size_t &stick_count)=0
Get the number of the sticks.
virtual yarp::dev::ReturnValue getStick(size_t stick_id, yarp::dev::StickData &value, JoypadCtrl_coordinateMode coordinate_mode)=0
Get the value of a stick if present, return false otherwise.
A container for a device driver.
Definition PolyDriver.h:23
std::vector< MultipleTouches > value
bool detachAll() final
Detach the object (you must have first called attach).
std::vector< double > value
yarp::dev::ReturnValue ret
yarp::dev::ReturnValue ret
yarp::dev::ReturnValue ret
yarp::dev::ReturnValue ret
yarp::dev::ReturnValue ret
yarp::dev::StickData value
std::vector< yarp::dev::TouchData > value
yarp::dev::ReturnValue ret
yarp::dev::TrackballData value
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void setReader(PortReader &reader) override
Set an external reader for port data.
void close() override
Stop port activity.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
int getOutputCount() override
Determine how many output connections this port has.
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 interface for reading from a network connection.
virtual bool isValid() const =0
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...
A base class for nested structures that can be searched.
Definition Searchable.h:31
size_t size() const
Definition Vector.h:351
void push_back(const T &elem)
Push a new element in the vector: size is changed.
Definition Vector.h:278
#define yCInfo(component,...)
#define yCError(component,...)
#define yCDebug(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.
Definition OSTestMsg.cpp:13