The qbrobotics Device HardWare interface extends the hardware_interface::RobotHW
by providing all the common structures to manage the communication with both the qbhand and qbmove devices.
More...
#include <qb_device_hardware_interface.h>
Public Member Functions | |
int | getDeviceId () |
std::string | getDeviceNamespace () |
virtual std::vector< std::string > | getJoints ()=0 |
This pure virtual method has to be redefined by derived classes to return the controlled joint names vector. More... | |
bool | init (ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh) override |
The init function is called to initialize the RobotHW from a non-realtime thread. More... | |
qbDeviceHW (qb_device_transmission_interface::TransmissionPtr transmission, const std::vector< std::string > &actuators, const std::vector< std::string > &joints) | |
Initialize HW resources and interfaces, and wait until it is initialized from the Communication Handler. More... | |
void | read (const ros::Time &time, const ros::Duration &period) override |
Read actuator state from the hardware, propagate it to joint states and publish the whole device state to a namespaced "~state" topic (each instance of qbDeviceHW should publish on its own topic). More... | |
void | write (const ros::Time &time, const ros::Duration &period) override |
Enforce joint limits for all registered joint limit interfaces, propagate joint commands to actuators, and send actuator commands to the hardware. More... | |
~qbDeviceHW () override | |
Deactivate motors and stop the async spinner. More... | |
Public Member Functions inherited from hardware_interface::RobotHW | |
virtual bool | checkForConflict (const std::list< ControllerInfo > &info) const |
virtual bool | checkForConflict (const std::list< ControllerInfo > &info) const |
virtual void | doSwitch (const std::list< ControllerInfo > &, const std::list< ControllerInfo > &) |
virtual void | doSwitch (const std::list< ControllerInfo > &, const std::list< ControllerInfo > &) |
virtual bool | prepareSwitch (const std::list< ControllerInfo > &start_list, const std::list< ControllerInfo > &stop_list) |
virtual bool | prepareSwitch (const std::list< ControllerInfo > &start_list, const std::list< ControllerInfo > &stop_list) |
RobotHW () | |
virtual | ~RobotHW () |
Public Member Functions inherited from hardware_interface::InterfaceManager | |
T * | get () |
std::vector< std::string > | getInterfaceResources (std::string iface_type) const |
std::vector< std::string > | getNames () const |
void | registerInterface (T *iface) |
void | registerInterfaceManager (InterfaceManager *iface_man) |
Protected Member Functions | |
virtual int | activateMotors () |
Call the service to activate the device motors and wait for the response. More... | |
virtual int | deactivateMotors () |
Call the service to deactivate the device motors and wait for the response. More... | |
virtual std::string | getInfo () |
Call the service to retrieve the printable configuration setup of the device and wait for the response. More... | |
virtual int | getMeasurements (std::vector< double > &positions, std::vector< double > ¤ts, ros::Time &stamp) |
Call the service to retrieve device measurements (both positions and currents) and wait for the response. More... | |
virtual int | initializeDevice () |
Call the service to initialize the device with parameters from the Communication Handler and wait for the response. More... | |
virtual int | setCommands (const std::vector< double > &commands) |
Call the service to send reference commands to the device and wait for the response. More... | |
Private Member Functions | |
std::vector< std::string > | addNamespacePrefix (const std::vector< std::string > &vector) |
Add the namespace prefix stored in the private namespace_ to all the elements of the given vector. More... | |
void | initializeServicesAndWait () |
Subscribe to all the services advertised by the Communication Handler and wait until all the services are properly advertised. More... | |
void | publish () |
Construct a qb_device_msgs::StateStamped message of the whole device state with the data retrieved during the read() and publish it to a namespaced "~state" topic. More... | |
void | resetServicesAndWait (const bool &reinitialize_device=true) |
Re-subscribe to all the services advertised by the Communication Handler and wait until all the services are properly advertised. More... | |
void | waitForInitialization () |
Wait until the device is initialized. More... | |
void | waitForServices () |
Wait until all the services advertised by the Communication Handler are active, then reinitialize the device to avoid disconnection problems. More... | |
Additional Inherited Members | |
Protected Types inherited from hardware_interface::InterfaceManager | |
typedef std::vector< InterfaceManager * > | InterfaceManagerVector |
typedef std::map< std::string, void * > | InterfaceMap |
typedef std::map< std::string, std::vector< std::string > > | ResourceMap |
typedef std::map< std::string, size_t > | SizeMap |
The qbrobotics Device HardWare interface extends the hardware_interface::RobotHW
by providing all the common structures to manage the communication with both the qbhand and qbmove devices.
The few differences among them are implemented in the two device-specific derived classes, qbHandHW
and qbMoveHW
. In detail, this class initialize all the ROS service clients needed to talk to the Communication Handler which is the intermediary for the serial communication with the physical devices. The core part is given by the two overridden function of the hardware_interface::RobotHW
base class, i.e. read() and write(), which are meant to be used in the control loop.
Definition at line 64 of file qb_device_hardware_interface.h.
qbDeviceHW::qbDeviceHW | ( | qb_device_transmission_interface::TransmissionPtr | transmission, |
const std::vector< std::string > & | actuators, | ||
const std::vector< std::string > & | joints | ||
) |
Initialize HW resources and interfaces, and wait until it is initialized from the Communication Handler.
If the Communication Handler is not running, wait forever. The three given parameters are strictly device-dependent, i.e. the qbhand specific transmission
differs from the qbmove one, and the actuator and joint names depend not only on the device type, but also on the whole robot_description
extracted from the URDF model. For example each qbmove of a given chain has its own namespace to avoid name clashes.
transmission | The shared pointer to the current transmission derived from transmission_interface::Transmission . |
actuators | The actuator names in the robot_description . |
joints | The joint names in the robot_description . |
Definition at line 32 of file qb_device_hardware_interface.cpp.
|
override |
Deactivate motors and stop the async spinner.
Definition at line 41 of file qb_device_hardware_interface.cpp.
|
protectedvirtual |
Call the service to activate the device motors and wait for the response.
0
on success. Definition at line 46 of file qb_device_hardware_interface.cpp.
|
private |
Add the namespace prefix stored in the private namespace_
to all the elements of the given vector.
vector | Any vector of strings. |
Definition at line 64 of file qb_device_hardware_interface.cpp.
|
protectedvirtual |
Call the service to deactivate the device motors and wait for the response.
0
on success. Definition at line 75 of file qb_device_hardware_interface.cpp.
|
inline |
1
, 128
]. Definition at line 87 of file qb_device_hardware_interface.h.
|
inline |
Definition at line 92 of file qb_device_hardware_interface.h.
|
protectedvirtual |
Call the service to retrieve the printable configuration setup of the device and wait for the response.
Definition at line 93 of file qb_device_hardware_interface.cpp.
|
pure virtual |
This pure virtual method has to be redefined by derived classes to return the controlled joint names vector.
|
protectedvirtual |
Call the service to retrieve device measurements (both positions and currents) and wait for the response.
When data is received, correct the positions direction with the motor_axis_direction
.
[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). |
[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] | stamp | The time stamp of the retrieved measurements (it is set by the process which does the read). |
0
on success. Definition at line 110 of file qb_device_hardware_interface.cpp.
|
overridevirtual |
The init function is called to initialize the RobotHW from a non-realtime thread.
In particular it build the HW resources, retrieve the robot_description
from the Parameter Server, and initialize the hardware_interface
, the joint_limit_interface
, and the transmission_interface
. If everything goes as expected it also initialize the current device with the parameters retrieve from the Communication Handler.
root_nh | A NodeHandle in the root of the caller namespace. |
robot_hw_nh | A NodeHandle in the namespace from which the RobotHW should read its configuration. |
true
on success. Reimplemented from hardware_interface::RobotHW.
Definition at line 139 of file qb_device_hardware_interface.cpp.
|
protectedvirtual |
Call the service to initialize the device with parameters from the Communication Handler and wait for the response.
If the initializaation succeed, store the device parameters received, e.g. position_limits
. It can additionally activate motors during the initialization process, if specified in the Parameter Server.
0
on success. Definition at line 171 of file qb_device_hardware_interface.cpp.
|
private |
Subscribe to all the services advertised by the Communication Handler and wait until all the services are properly advertised.
Definition at line 213 of file qb_device_hardware_interface.cpp.
|
private |
Construct a qb_device_msgs::StateStamped
message of the whole device state with the data retrieved during the read()
and publish it to a namespaced "~state"
topic.
Definition at line 223 of file qb_device_hardware_interface.cpp.
|
overridevirtual |
Read actuator state from the hardware, propagate it to joint states and publish the whole device state to a namespaced "~state"
topic (each instance of qbDeviceHW should publish on its own topic).
time | The current time. |
period | The time passed since the last call to this method, i.e. the control period. |
Reimplemented from hardware_interface::RobotHW.
Definition at line 255 of file qb_device_hardware_interface.cpp.
|
private |
Re-subscribe to all the services advertised by the Communication Handler and wait until all the services are properly advertised.
Then re-initialize the device parameters (it is assumed that the this method can be called only if the device was previously initialized), unless otherwise specified.
reinitialize_device | If true , i.e. by default, reinitialize the device. |
Definition at line 280 of file qb_device_hardware_interface.cpp.
|
protectedvirtual |
Call the service to send reference commands to the device and wait for the response.
Before sending the references, correct their direction with the motor_axis_direction
.
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 289 of file qb_device_hardware_interface.cpp.
|
private |
Wait until the device is initialized.
Definition at line 313 of file qb_device_hardware_interface.cpp.
|
private |
Wait until all the services advertised by the Communication Handler are active, then reinitialize the device to avoid disconnection problems.
Definition at line 319 of file qb_device_hardware_interface.cpp.
|
overridevirtual |
Enforce joint limits for all registered joint limit interfaces, propagate joint commands to actuators, and send actuator commands to the hardware.
time | The current time. |
period | The time passed since the last call to this method, i.e. the control period. |
Reimplemented from hardware_interface::RobotHW.
Definition at line 326 of file qb_device_hardware_interface.cpp.
|
protected |
Definition at line 136 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 135 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 134 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 138 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 139 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 137 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 131 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 133 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 130 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 132 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 140 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 141 of file qb_device_hardware_interface.h.
|
protected |
Definition at line 142 of file qb_device_hardware_interface.h.