28 #ifndef QB_DEVICE_COMMUNICATION_HANDLER_H 29 #define QB_DEVICE_COMMUNICATION_HANDLER_H 91 virtual int activate(
const int &
id,
const int &max_repeats);
100 bool activateCallback(qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response);
107 virtual int close(
const std::string &serial_port);
115 virtual int deactivate(
const int &
id,
const int &max_repeats);
124 bool deactivateCallback(qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response);
136 virtual int getCurrents(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts);
146 virtual int getInfo(
const int &
id,
const int &max_repeats, std::string &info);
155 bool getInfoCallback(qb_device_srvs::TriggerRequest &request, qb_device_srvs::TriggerResponse &response);
171 virtual int getMeasurements(
const int &
id,
const int &max_repeats, std::vector<short int> ¤ts, std::vector<short int> &positions);
180 bool getMeasurementsCallback(qb_device_srvs::GetMeasurementsRequest &request, qb_device_srvs::GetMeasurementsResponse &response);
194 virtual int getParameters(
const int &
id, std::vector<int> &limits, std::vector<int> &resolutions);
207 virtual int getPositions(
const int &
id,
const int &max_repeats, std::vector<short int> &positions);
229 virtual int isActive(
const int &
id,
const int &max_repeats,
bool &status);
238 virtual int isConnected(
const int &
id,
const int &max_repeats);
254 virtual bool isInOpenMap(
const std::string &serial_port);
265 virtual bool initializeCallback(qb_device_srvs::InitializeDeviceRequest &request, qb_device_srvs::InitializeDeviceResponse &response);
274 virtual int open(
const std::string &serial_port);
286 virtual int setCommandsAndWait(
const int &
id,
const int &max_repeats, std::vector<short int> &commands);
297 virtual int setCommandsAsync(
const int &
id, std::vector<short int> &commands);
306 bool setCommandsCallback(qb_device_srvs::SetCommandsRequest &request, qb_device_srvs::SetCommandsResponse &response);
316 int setPID(
const int &
id,
const int &max_repeats, std::vector<float> &pid);
326 bool setPIDCallback(qb_device_srvs::SetPIDRequest &request, qb_device_srvs::SetPIDResponse &response);
347 virtual int activate(
const int &
id,
const bool &command,
const int &max_repeats);
355 inline bool isReliable(
int const &failures,
int const &max_repeats) {
return failures >= 0 && failures <= max_repeats; }
359 #endif // QB_DEVICE_COMMUNICATION_HANDLER_H ros::ServiceServer deactivate_motors_
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.
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...
virtual int getCurrents(const int &id, const int &max_repeats, std::vector< short int > ¤ts)
Retrieve the motor currents of the given device.
virtual int getInfo(const int &id, const int &max_repeats, std::string &info)
Retrieve the printable configuration setup of the given device.
ros::ServiceServer get_measurements_
qb_device_driver::qbDeviceAPIPtr device_api_
virtual int getPositions(const int &id, const int &max_repeats, std::vector< short int > &positions)
Retrieve the motor positions of the given device.
ros::ServiceServer set_pid_
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.
ros::ServiceServer activate_motors_
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...
std::map< std::string, comm_settings > file_descriptors_
virtual ~qbDeviceCommunicationHandler()
Close all the still open serial ports.
virtual int getParameters(const int &id, std::vector< int > &limits, std::vector< int > &resolutions)
Retrieve some of the parameters from the given device.
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...
ros::NodeHandle node_handle_
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 physic...
std::map< int, std::string > connected_devices_
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.
virtual int activate(const int &id, const int &max_repeats)
Activate the motors of the given device.
virtual bool isInOpenMap(const std::string &serial_port)
Check whether the given serial port is managed by the Communication Handler, i.e. ...
bool setPIDCallback(qb_device_srvs::SetPIDRequest &request, qb_device_srvs::SetPIDResponse &response)
Set (temporarily, i.e.
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.
ros::ServiceServer set_commands_
ros::ServiceServer get_info_
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)...
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.
qbDeviceCommunicationHandler()
Wait until at least one device is connected and then initialize the Communication Handler...
bool isReliable(int const &failures, int const &max_repeats)
Check whether the reading failures are in the given range.
ros::AsyncSpinner spinner_
virtual int deactivate(const int &id, const int &max_repeats)
Deactivate the motors of the given device.
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...
The Communication Handler class is aimed to instantiate a ROS node which provides several ROS service...
virtual bool isInConnectedSet(const int &id)
Check whether the physical device specified by the given ID is connected to the Communication Handler...
std::map< std::string, std::unique_ptr< std::mutex > > serial_protectors_
std::shared_ptr< qbDeviceAPI > qbDeviceAPIPtr
virtual int close(const std::string &serial_port)
Close the communication with all the devices connected to the given serial port.
ros::ServiceServer initialize_device_
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.
virtual int open(const std::string &serial_port)
Open the serial communication on the given serial port.
virtual int getSerialPortsAndDevices(const int &max_repeats)
Scan for all the serial ports of type /dev/ttyUSB* detected in the system, initialize their mutex pro...