The Communication Handler class is aimed to instantiate a ROS node which provides several ROS services to communicate with one - or many - qbrobotics devices connected to the ROS ecosystem. More...
#include <qb_device_communication_handler.h>
Public Member Functions | |
qbDeviceCommunicationHandler () | |
Wait until at least one device is connected and then initialize the Communication Handler. More... | |
qbDeviceCommunicationHandler (qb_device_driver::qbDeviceAPIPtr device_api) | |
Allow to mock the API for unit testing. More... | |
virtual | ~qbDeviceCommunicationHandler () |
Close all the still open serial ports. More... | |
Protected Member Functions | |
virtual int | activate (const int &id, const int &max_repeats) |
Activate the motors of the given device. More... | |
bool | activateCallback (qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response) |
Activate the motors of the device relative to the node requesting the service. More... | |
virtual int | close (const std::string &serial_port) |
Close the communication with all the devices connected to the given serial port. More... | |
virtual int | deactivate (const int &id, const int &max_repeats) |
Deactivate the motors of the given device. More... | |
bool | deactivateCallback (qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response) |
Deactivate the motors of the device relative to the node requesting the service. More... | |
virtual int | getCurrents (const int &id, const int &max_repeats, std::vector< short int > ¤ts) |
Retrieve the motor currents of the given device. More... | |
virtual int | getInfo (const int &id, const int &max_repeats, std::string &info) |
Retrieve the printable configuration setup of the given device. More... | |
bool | getInfoCallback (qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response) |
Retrieve the printable configuration setup of the device relative to the node requesting the service. More... | |
virtual int | getMeasurements (const int &id, const int &max_repeats, std::vector< short int > ¤ts, std::vector< short int > &positions) |
Retrieve the motor currents of the given device. More... | |
bool | getMeasurementsCallback (qb_device_srvs::GetMeasurementsRequest &request, qb_device_srvs::GetMeasurementsResponse &response) |
Retrieve the motor positions and currents of the device relative to the node requesting the service. More... | |
virtual int | getParameters (const int &id, std::vector< int > &limits, std::vector< int > &resolutions) |
Retrieve some of the parameters from the given device. More... | |
virtual int | getPositions (const int &id, const int &max_repeats, std::vector< short int > &positions) |
Retrieve the motor positions of the given device. More... | |
virtual int | getSerialPortsAndDevices (const int &max_repeats) |
Scan for all the serial ports of type /dev/ttyUSB* detected in the system, initialize their mutex protector (each serial port connected to the system has to be accessed in a mutually exclusive fashion), and retrieve all the qbrobotics devices connected to them. More... | |
virtual bool | initializeCallback (qb_device_srvs::InitializeDeviceRequest &request, qb_device_srvs::InitializeDeviceResponse &response) |
Initialize the device node requesting the service to the Communication Handler if the relative physical device is connected through any serial port to the system (can re-scan the serial resources if specified in the request). More... | |
virtual int | isActive (const int &id, const int &max_repeats, bool &status) |
Check whether the motors of the device specified by the given ID are active. More... | |
virtual int | isConnected (const int &id, const int &max_repeats) |
Check whether the the device specified by the given ID is connected through the serial port. More... | |
virtual bool | isInConnectedSet (const int &id) |
Check whether the physical device specified by the given ID is connected to the Communication Handler. More... | |
virtual bool | isInOpenMap (const std::string &serial_port) |
Check whether the given serial port is managed by the Communication Handler, i.e. More... | |
virtual int | open (const std::string &serial_port) |
Open the serial communication on the given serial port. More... | |
virtual int | setCommandsAndWait (const int &id, const int &max_repeats, std::vector< short int > &commands) |
Send the reference command to the motors of the given device and wait for acknowledge. More... | |
virtual int | setCommandsAsync (const int &id, std::vector< short int > &commands) |
Send the reference command to the motors of the given device in a non-blocking fashion. More... | |
bool | setCommandsCallback (qb_device_srvs::SetCommandsRequest &request, qb_device_srvs::SetCommandsResponse &response) |
Send the reference command to the motors of the device relative to the node requesting the service. More... | |
int | setPID (const int &id, const int &max_repeats, std::vector< float > &pid) |
Set the position control PID parameters of the given device, temporarily (until power off). More... | |
bool | setPIDCallback (qb_device_srvs::SetPIDRequest &request, qb_device_srvs::SetPIDResponse &response) |
Set (temporarily, i.e. More... | |
Protected Attributes | |
std::map< int, std::string > | connected_devices_ |
std::map< std::string, comm_settings > | file_descriptors_ |
ros::NodeHandle | node_handle_ |
std::map< std::string, std::unique_ptr< std::mutex > > | serial_protectors_ |
Private Member Functions | |
virtual int | activate (const int &id, const bool &command, const int &max_repeats) |
Activate (or deactivate, according to the given command) the motors of the given device. More... | |
bool | isReliable (int const &failures, int const &max_repeats) |
Check whether the reading failures are in the given range. More... | |
The Communication Handler class is aimed to instantiate a ROS node which provides several ROS services to communicate with one - or many - qbrobotics devices connected to the ROS ecosystem.
Each hardware interface which manage a single qbrobotics device must first request the initialization of its ID to the Communication Handler and then interact with it through specific ROS services - blocking by nature. The Communication Handler essentially manage the shared serial port resources and lets all the hardware interfaces to get access to them. This could seem a bottleneck in the architecture, but the real bottleneck is the shared resource itself; moreover this structure is also necessary since the multi-process nature of ROS which does not easily allow to share common resources among distinct processes. Actually the implementation of qbrobotics classes could be reshaped to exploit the multi-threading approach, e.g. using nodelet or similar plugins each device node could handle the communication process by itself, without the intermediary Communication Handler. The advantage of such a restyle in term of communication speed does not worth the effort though.
Definition at line 58 of file qb_device_communication_handler.h.
qbDeviceCommunicationHandler::qbDeviceCommunicationHandler | ( | ) |
Wait until at least one device is connected and then initialize the Communication Handler.
Definition at line 32 of file qb_device_communication_handler.cpp.
qbDeviceCommunicationHandler::qbDeviceCommunicationHandler | ( | qb_device_driver::qbDeviceAPIPtr | device_api | ) |
Allow to mock the API for unit testing.
It is called from the default constructor with the real API smart pointer, but it can be called with a pointer to the mock class inherited from the qbDeviceAPI
itself.
device_api | The shared pointer to the current API derived from qb_device_driver::qbDeviceAPI . |
Definition at line 42 of file qb_device_communication_handler.cpp.
|
virtual |
Close all the still open serial ports.
Definition at line 56 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Activate the motors of the given device.
Do nothing if the device is not connected in the Communication Handler.
id | The ID of the device to be activated, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
Definition at line 82 of file qb_device_communication_handler.cpp.
|
privatevirtual |
Activate (or deactivate, according to the given command) the motors of the given device.
Do nothing if the device is not connected in the Communication Handler.
id | The ID of the device to be activated (or deactivated), in range [1 , 128 ]. |
command | true to turn motors on, false to turn them off. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
Definition at line 62 of file qb_device_communication_handler.cpp.
|
protected |
Activate the motors of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::Trigger for details). |
response | The response of the given service (see qb_device_srvs::Trigger for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 86 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Close the communication with all the devices connected to the given serial port.
serial_port | The serial port which has to be closed, e.g. /dev/ttyUSB0 . |
Definition at line 101 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Deactivate the motors of the given device.
Do nothing if the device is not connected in the Communication Handler.
id | The ID of the device to be deactivated, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
Definition at line 119 of file qb_device_communication_handler.cpp.
|
protected |
Deactivate the motors of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::Trigger for details). |
response | The response of the given service (see qb_device_srvs::Trigger for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 123 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Retrieve the motor currents of the given device.
id | The ID of the device of interest, in range [1 , 128 ]. | |
[out] | currents | The two-element device motor current vector, expressed in mA: if the device is a qbhand only the first element is filled while the other remains always 0 ; in the case of a qbmove both the elements contain relevant data, i.e. the currents respectively of motor_1 and motor_2 . |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
0
and max_repeats
. Definition at line 138 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Retrieve the printable configuration setup of the given device.
id | The ID of the device of interest, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
info | The configuration setup formatted as a plain text string (empty string on communication error). |
0
and max_repeats
. Definition at line 152 of file qb_device_communication_handler.cpp.
|
protected |
Retrieve the printable configuration setup of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::Trigger for details). |
response | The response of the given service (see qb_device_srvs::Trigger for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 166 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Retrieve the motor currents of the given device.
id | The ID of the device of interest, in range [1 , 128 ]. | |
[out] | currents | The two-element device motor current vector, expressed in mA: if the device is a qbhand only the first element is filled while the other remains always 0 ; in the case of a qbmove both the elements contain relevant data, i.e. the currents respectively of motor_1 and motor_2 . |
[out] | positions | The device position vector, expressed in ticks: if the device is a qbhand only the first element is filled while the others remain always 0 ; in the case of a qbmove all the elements contain relevant data, i.e. the positions respectively of motor_1 , motor_2 and motor_shaft (which is not directly actuated). |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
0
and max_repeats
. Definition at line 180 of file qb_device_communication_handler.cpp.
|
protected |
Retrieve the motor positions and currents of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::GetMeasurements for details). |
response | The response of the given service (see qb_device_srvs::GetMeasurements for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 198 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Retrieve some of the parameters from the given device.
id | The ID of the device of interest, in range [1 , 128 ]. | |
[out] | limits | The vector of motor position limits expressed in ticks: two values for each motor, respectively [lower_limit , upper_limit ]. |
[out] | resolutions | The vector of encoder resolutions, each in range [0 , 8 ]: one value for each encoder (note: the qbmove has also the shaft encoder even if it is not actuated). The word "resolution" could be misunderstood: taken the resolution r , is the number of turns of the wire inside the device mechanics. It is used essentially to convert the measured position of the motors in ticks to radians or degrees. |
0
on success. Definition at line 228 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Retrieve the motor positions of the given device.
id | The ID of the device of interest, in range [1 , 128 ]. | |
[out] | positions | The device position vector, expressed in ticks: if the device is a qbhand only the first element is filled while the others remain always 0 ; in the case of a qbmove all the elements contain relevant data, i.e. the positions respectively of motor_1 , motor_2 and motor_shaft (which is not directly actuated). |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
0
and max_repeats
. Definition at line 238 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Scan for all the serial ports of type /dev/ttyUSB*
detected in the system, initialize their mutex protector (each serial port connected to the system has to be accessed in a mutually exclusive fashion), and retrieve all the qbrobotics devices connected to them.
For each device, store its ID in the private map connected_devices_
, i.e. insert a pair [device_id
, serial_port
]. The map connected_devices_
is constructed from scratch at each call.
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
Definition at line 252 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Initialize the device node requesting the service to the Communication Handler if the relative physical device is connected through any serial port to the system (can re-scan the serial resources if specified in the request).
If the device is found, retrieve some of its parameter and activate its motors, if requested.
request | The request of the given service (see qb_device_srvs::InitializeDevice for details). |
response | The response of the given service (see qb_device_srvs::InitializeDevice for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 350 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Check whether the motors of the device specified by the given ID are active.
id | The ID of the device of interest, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
status | true if the device motors are on. |
0
and max_repeats
. Definition at line 292 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Check whether the the device specified by the given ID is connected through the serial port.
id | The ID of the device of interest, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
0
and max_repeats
. Definition at line 306 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Check whether the physical device specified by the given ID is connected to the Communication Handler.
id | The ID of the device of interest, in range [1 , 128 ]. |
true
if the given device belongs to the connected device vector, i.e. connected_devices_
. Definition at line 319 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Check whether the given serial port is managed by the Communication Handler, i.e.
is open.
serial_port | The name of the serial port of interest, e.g. /dev/ttyUSB0 . |
true
if the given serial port belongs to the open file descriptor map, i.e. file_descriptors_
. Definition at line 323 of file qb_device_communication_handler.cpp.
|
inlineprivate |
Check whether the reading failures are in the given range.
failures | The current number of communication failures per serial resource reading. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
true
if the failures are less than the given threshold. Definition at line 355 of file qb_device_communication_handler.h.
|
protectedvirtual |
Open the serial communication on the given serial port.
On success, store the opened file descriptor in the private map file_descriptors_
, i.e. insert a pair [serial_port
, file_descriptor
].
serial_port | The serial port which has to be opened, e.g. /dev/ttyUSB0 . |
0
on success. Definition at line 327 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Send the reference command to the motors of the given device and wait for acknowledge.
id | The ID of the device of interest, in range [1 , 128 ]. |
max_repeats | The maximum number of consecutive repetitions to mark retrieved data as corrupted. |
commands | The reference command vector, expressed in ticks: if the device is a qbhand only the first element is meaningful while the other remains always 0 ; in the case of a qbmove both the elements contain relevant data, i.e. the commands respectively of motor_1 and motor_2 . |
0
on success. Definition at line 391 of file qb_device_communication_handler.cpp.
|
protectedvirtual |
Send the reference command to the motors of the given device in a non-blocking fashion.
id | The ID of the device of interest, in range [1 , 128 ]. |
commands | The reference command vector, expressed in ticks: if the device is a qbhand only the first element is meaningful while the other remains always 0 ; in the case of a qbmove both the elements contain relevant data, i.e. the commands respectively of motor_1 and motor_2 . |
0
(note that this is a non reliable method). Definition at line 405 of file qb_device_communication_handler.cpp.
|
protected |
Send the reference command to the motors of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::SetCommands for details). |
response | The response of the given service (see qb_device_srvs::SetCommands for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 412 of file qb_device_communication_handler.cpp.
|
protected |
Set the position control PID parameters of the given device, temporarily (until power off).
file_descriptor | The file descriptor of the serial port on which the device is connected. |
id | The ID of the device of interest, in range [1 , 128 ]. |
pid | The three-element [P , I , D ] vector of parameters to be set. |
0
(note that this is a non reliable method). Definition at line 432 of file qb_device_communication_handler.cpp.
|
protected |
Set (temporarily, i.e.
until power off) the position control PID parameters of the device relative to the node requesting the service.
request | The request of the given service (see qb_device_srvs::SetPID for details). |
response | The response of the given service (see qb_device_srvs::SetPID for details). |
true
if the call succeed (actually response.success
may be false). Definition at line 445 of file qb_device_communication_handler.cpp.
|
private |
Definition at line 330 of file qb_device_communication_handler.h.
|
protected |
Definition at line 83 of file qb_device_communication_handler.h.
|
private |
Definition at line 331 of file qb_device_communication_handler.h.
|
private |
Definition at line 337 of file qb_device_communication_handler.h.
|
protected |
Definition at line 82 of file qb_device_communication_handler.h.
|
private |
Definition at line 332 of file qb_device_communication_handler.h.
|
private |
Definition at line 333 of file qb_device_communication_handler.h.
|
private |
Definition at line 334 of file qb_device_communication_handler.h.
|
protected |
Definition at line 80 of file qb_device_communication_handler.h.
|
protected |
Definition at line 81 of file qb_device_communication_handler.h.
|
private |
Definition at line 335 of file qb_device_communication_handler.h.
|
private |
Definition at line 336 of file qb_device_communication_handler.h.
|
private |
Definition at line 329 of file qb_device_communication_handler.h.