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 std::vector<double> tmp_axes;
494 if (!m_IJoypad->getAllAxes(tmp_axes) || tmp_axes.size() != m_numberOfAxes)
495 {
496 write = false;
497 }
498 else
499 {
500 m_curr_axes = tmp_axes;
501 }
502
503 if (write)
504 {
506 {
507 if (VectorsDifferent(m_curr_axes,m_last_axes))
508 {
509 b.value = m_curr_axes;
510 m_portAxes.write();
511 m_last_axes = m_curr_axes;
512 }
513 }
514 else
515 {
516 b.value = m_curr_axes;
517 m_portAxes.write();
518 m_last_axes = m_curr_axes;
519 }
520 }
521 }
522
523 //4 Trackball
524 {
525 bool write;
526
527 write = m_portTrackballs.getOutputCount()>0;
528 auto& b = m_portTrackballs.prepare();
529 b.value.clear();
530 for(size_t i = 0; i < m_numberOfTrackballs; ++i)
531 {
532 if (!m_IJoypad->getTrackball(i, m_curr_trackballs[i]))
533 {
534 write = false;
535 break;
536 }
537 }
538
539 if (write)
540 {
542 {
543 if (VectorsDifferentC(m_curr_trackballs,m_last_trackballs,
546 {
547 return lhs.x == rhs.x &&
548 lhs.y == rhs.y;
549 }
550 ))
551 {
552 b.value = m_curr_trackballs;
553 m_portTrackballs.write();
554 m_last_trackballs = m_curr_trackballs;
555 }
556 }
557 else
558 {
559 b.value = m_curr_trackballs;
560 m_portTrackballs.write();
561 m_last_trackballs = m_curr_trackballs;
562 }
563 }
564 }
565
566 //5 Stick
567 {
568 bool write;
569 write = m_portSticks.getOutputCount()>0;
570 auto& b = m_portSticks.prepare();
571 b.value.clear();
572 for(size_t i = 0; i < m_numberOfSticks; ++i)
573 {
574 if (!m_IJoypad->getStick(i, m_curr_sticks[i], m_coordsMode))
575 {
576 write = false;
577 break;
578 }
579 }
580
581 if (write)
582 {
584 {
585 if (VectorsDifferentC(m_curr_sticks,m_last_sticks,
586 [](const yarp::dev::StickData& lhs,
588 {
589 return lhs.s1 == rhs.s1 &&
590 lhs.s2 == rhs.s2;
591 }
592 ))
593 {
594 b.value = m_curr_sticks;
595 m_portSticks.write();
596 m_last_sticks = m_curr_sticks;
597 }
598 }
599 else
600 {
601 b.value = m_curr_sticks;
602 m_portSticks.write();
603 m_last_sticks = m_curr_sticks;
604 }
605 }
606 }
607
608 //6 Touch
609 {
610 bool write;
611 write = m_portTouches.getOutputCount()>0;
612 auto& b = m_portTouches.prepare();
613 b.value.clear();
614
615 m_curr_touches.value.resize(m_numberOfTouchSurfaces);
616 for(unsigned int i = 0; i < m_numberOfTouchSurfaces; ++i)
617 {
618 std::vector<yarp::dev::TouchData> temp;
619 if (!m_IJoypad->getTouch(i, temp))
620 {
621 write = false;
622 break;
623 }
624 m_curr_touches.value[i].touches = temp;
625 }
626
627 if (write)
628 {
630 {
631 if (1)
632 //if (VectorsDifferent(m_curr_touches.value,m_last_touches.value))
633 {
634 b.value = m_curr_touches.value;
635 m_portTouches.write();
636 m_last_touches = m_curr_touches;
637 }
638 }
639 else
640 {
641 b.value = m_curr_touches.value;
642 m_portTouches.write();
643 m_last_touches = m_curr_touches;
644 }
645 }
646 }
647
648
649 //AllJoyDataPort
650 if (m_allPort.getOutputCount()>0)
651 {
652 yarp::dev::AllJoyData& message = m_allPort.prepare();
653 message.AxisDataVal = m_curr_axes;
654 message.ButtonDataVal = m_curr_buttons;
655 message.HatsDataVal = std::vector<std::int8_t>(m_curr_hats.begin(), m_curr_hats.end());
656 message.StickDataVal = m_curr_sticks;
657 message.TrackballDataVal = m_curr_trackballs;
658 message.TouchDataVal = m_curr_touches.value;
659 m_allPort.write();
660 }
661
662 //print data on screen
663 if(m_profile)
664 {
665 profile_message = " Axes: ";
666 profile_message += printVector(m_curr_axes);
667 profile_message += " Buttons: ";
668 profile_message += printVector(m_curr_buttons);
669 //profile_message += " Trackballs: ";
670 //profile_message += printVector(m_curr_trackballs);
671 //profile_message += " Sticks: ";
672 //profile_message += printVector(m_curr_sticks);
673 //profile_message += " Hats: ";
674 //profile_message += printVector(m_curr_hats);
675 //profile_message += " Touch Surface: ";
676 //profile_message += printVector(m_curr_touches);
678 }
679}
680
682{
683 detachAll();
684
685 // Closing port
686 m_portAxes.close();
687 m_portButtons.close();
688 m_portSticks.close();
689 m_portTouches.close();
690 m_portTrackballs.close();
691 m_portHats.close();
692
693 m_rpcPort.close();
694
695 m_allPort.close();
696
697 return true;
698}
699
701{
702 if (!connection.isValid()) { return false;}
703 if (!m_RPCMsgs) { return false;}
704
705 if (m_RPCMsgs)
706 {
707 bool b = m_RPCMsgs->read(connection);
708 if (b) {
709 return true;
710 }
711 }
712 yCDebug(JOYPADCONTROLSERVER) << "read() Command failed";
713 return false;
714}
FeatureMode mode
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
#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.
typename vector_selector< T >::type VectorOf
Definition Vector.h:174