Client class for EZN64 USB control. More...
#include <ezn64_usb_control.h>
Public Member Functions | |
bool | acknowledgeErrorCallback (schunk_ezn64::acknowledge_error::Request &req, schunk_ezn64::acknowledge_error::Response &res) |
AcknowledgeError service callback. | |
EZN64_usb (ros::NodeHandle *nh) | |
Construct a client for EZN64 USB control. | |
bool | getErrorCallback (schunk_ezn64::get_error::Request &req, schunk_ezn64::get_error::Response &res) |
GetError service callback. | |
bool | getPositionCallback (schunk_ezn64::get_position::Request &req, schunk_ezn64::get_position::Response &res) |
GetPosition service callback. | |
bool | referenceCallback (schunk_ezn64::reference::Request &req, schunk_ezn64::reference::Response &res) |
Reference service callback. | |
bool | setPositionCallback (schunk_ezn64::set_position::Request &req, schunk_ezn64::set_position::Response &res) |
SetPosition service callback. | |
bool | stopCallback (schunk_ezn64::stop::Request &req, schunk_ezn64::stop::Response &res) |
Stop service callback. | |
void | timerCallback (const ros::TimerEvent &event) |
Timer callback to read USB input buffer periodically. | |
~EZN64_usb () | |
Public Attributes | |
ros::Publisher | joint_pub |
Gripper joint state publisher. | |
Static Public Attributes | |
static const float | TF_UPDATE_PERIOD = 0.1 |
TF update period in seconds. | |
Private Member Functions | |
void | acknowledgeError (libusb_device_handle *handle) |
Send CMD_ACK(0x8b) command to the gripper. | |
int | close_ezn64_dev (libusb_device_handle *handle, libusb_context *usb_context) |
Function closing communication with USB device. | |
libusb_device * | find_ezn64_dev (int VendorID, int ProductID) |
Function looking for gripper controller in list of available USB devices. In case of success, pointer to device is returned. | |
void | float_to_IEEE_754 (float position, unsigned int *output_array) |
Conversion from float to 4bytes. | |
uint8_t | getError (libusb_device_handle *handle) |
Read actual error by GET STATE(0x95) command. | |
void | getPeriodicPositionUpdate (libusb_device_handle *handle, float period) |
Set periodic position reading by GET_STATE(0x95) command. | |
float | getPosition (libusb_device_handle *handle) |
Read actual position by GET_STATE(0x95) command. | |
float | IEEE_754_to_float (uint8_t *raw) |
Conversion from 4 bytes to float. | |
libusb_device_handle * | open_ezn64_dev (libusb_device *dev) |
Function openning communication with USB device. | |
void | print_libusb_dev (libusb_device *dev) |
Funtion printing information about active gripper USB interface. | |
void | reference (libusb_device_handle *handle) |
Send CMD REFERENCE(0x92) command to the gripper. | |
void | setPosition (libusb_device_handle *handle, float goal_position) |
Send MOV_POS(0x80) command to the gripper. | |
void | stop (libusb_device_handle *handle) |
Send CMD_STOP(0x91) to stop moving gripper. | |
std::vector< uint8_t > | usb_read (libusb_device_handle *handle) |
Function reading data from USB inpput buffer. | |
void | usb_write (libusb_device_handle *handle, std::vector< uint8_t > output) |
Function writing data to USB output buffer. | |
Private Attributes | |
float | act_position_ |
libusb_device * | ezn64_dev_ |
uint8_t | ezn64_error_ |
libusb_device_handle * | ezn64_handle_ |
sensor_msgs::JointState | ezn64_joint_state_ |
int | gripper_id_ |
int | product_id_ |
double | update_frequency |
libusb_context * | usb_context_ |
int | vendor_id_ |
Static Private Attributes | |
static const int | INPUT_BUFFER_SIZE = 512 |
static const int | LIBUSB_ENDPOINT = 129 |
static const int | LIBUSB_TIMEOUT = 1000 |
static const int | LIBUSB_VERBOSITY_LEVEL = 3 |
static const double | MAX_GRIPPER_POS_LIMIT = 12 |
static const double | MIN_GRIPPER_POS_LIMIT = 0 |
static const int | URDF_SCALE_FACTOR = 1000 |
static const double | WAIT_FOR_RESPONSE_INTERVAL = 0.5 |
Client class for EZN64 USB control.
Definition at line 54 of file ezn64_usb_control.h.
Construct a client for EZN64 USB control.
Definition at line 40 of file ezn64_usb_control_lib.cpp.
Definition at line 77 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::acknowledgeError | ( | libusb_device_handle * | handle | ) | [private] |
Send CMD_ACK(0x8b) command to the gripper.
Definition at line 174 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::acknowledgeErrorCallback | ( | schunk_ezn64::acknowledge_error::Request & | req, |
schunk_ezn64::acknowledge_error::Response & | res | ||
) |
AcknowledgeError service callback.
Definition at line 327 of file ezn64_usb_control_lib.cpp.
int schunk_ezn64::EZN64_usb::close_ezn64_dev | ( | libusb_device_handle * | handle, |
libusb_context * | usb_context | ||
) | [private] |
Function closing communication with USB device.
Definition at line 461 of file ezn64_usb_control_lib.cpp.
libusb_device * schunk_ezn64::EZN64_usb::find_ezn64_dev | ( | int | VendorID, |
int | ProductID | ||
) | [private] |
Function looking for gripper controller in list of available USB devices. In case of success, pointer to device is returned.
Definition at line 399 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::float_to_IEEE_754 | ( | float | position, |
unsigned int * | output_array | ||
) | [private] |
Conversion from float to 4bytes.
Definition at line 625 of file ezn64_usb_control_lib.cpp.
uint8_t schunk_ezn64::EZN64_usb::getError | ( | libusb_device_handle * | handle | ) | [private] |
Read actual error by GET STATE(0x95) command.
Definition at line 95 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::getErrorCallback | ( | schunk_ezn64::get_error::Request & | req, |
schunk_ezn64::get_error::Response & | res | ||
) |
GetError service callback.
Definition at line 311 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::getPeriodicPositionUpdate | ( | libusb_device_handle * | handle, |
float | period | ||
) | [private] |
Set periodic position reading by GET_STATE(0x95) command.
Definition at line 250 of file ezn64_usb_control_lib.cpp.
float schunk_ezn64::EZN64_usb::getPosition | ( | libusb_device_handle * | handle | ) | [private] |
Read actual position by GET_STATE(0x95) command.
Definition at line 206 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::getPositionCallback | ( | schunk_ezn64::get_position::Request & | req, |
schunk_ezn64::get_position::Response & | res | ||
) |
GetPosition service callback.
Definition at line 319 of file ezn64_usb_control_lib.cpp.
float schunk_ezn64::EZN64_usb::IEEE_754_to_float | ( | uint8_t * | raw | ) | [private] |
Conversion from 4 bytes to float.
Definition at line 608 of file ezn64_usb_control_lib.cpp.
libusb_device_handle * schunk_ezn64::EZN64_usb::open_ezn64_dev | ( | libusb_device * | dev | ) | [private] |
Function openning communication with USB device.
Definition at line 443 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::print_libusb_dev | ( | libusb_device * | dev | ) | [private] |
Funtion printing information about active gripper USB interface.
Definition at line 555 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::reference | ( | libusb_device_handle * | handle | ) | [private] |
Send CMD REFERENCE(0x92) command to the gripper.
Definition at line 83 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::referenceCallback | ( | schunk_ezn64::reference::Request & | req, |
schunk_ezn64::reference::Response & | res | ||
) |
Reference service callback.
Definition at line 285 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::setPosition | ( | libusb_device_handle * | handle, |
float | goal_position | ||
) | [private] |
Send MOV_POS(0x80) command to the gripper.
Definition at line 185 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::setPositionCallback | ( | schunk_ezn64::set_position::Request & | req, |
schunk_ezn64::set_position::Response & | res | ||
) |
SetPosition service callback.
Definition at line 293 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::stop | ( | libusb_device_handle * | handle | ) | [private] |
Send CMD_STOP(0x91) to stop moving gripper.
Definition at line 270 of file ezn64_usb_control_lib.cpp.
bool schunk_ezn64::EZN64_usb::stopCallback | ( | schunk_ezn64::stop::Request & | req, |
schunk_ezn64::stop::Response & | res | ||
) |
Stop service callback.
Definition at line 341 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::timerCallback | ( | const ros::TimerEvent & | event | ) |
Timer callback to read USB input buffer periodically.
Definition at line 351 of file ezn64_usb_control_lib.cpp.
std::vector< uint8_t > schunk_ezn64::EZN64_usb::usb_read | ( | libusb_device_handle * | handle | ) | [private] |
Function reading data from USB inpput buffer.
Definition at line 512 of file ezn64_usb_control_lib.cpp.
void schunk_ezn64::EZN64_usb::usb_write | ( | libusb_device_handle * | handle, |
std::vector< uint8_t > | output | ||
) | [private] |
Function writing data to USB output buffer.
Definition at line 470 of file ezn64_usb_control_lib.cpp.
float schunk_ezn64::EZN64_usb::act_position_ [private] |
Definition at line 151 of file ezn64_usb_control.h.
libusb_device* schunk_ezn64::EZN64_usb::ezn64_dev_ [private] |
Definition at line 156 of file ezn64_usb_control.h.
uint8_t schunk_ezn64::EZN64_usb::ezn64_error_ [private] |
Definition at line 152 of file ezn64_usb_control.h.
libusb_device_handle* schunk_ezn64::EZN64_usb::ezn64_handle_ [private] |
Definition at line 158 of file ezn64_usb_control.h.
sensor_msgs::JointState schunk_ezn64::EZN64_usb::ezn64_joint_state_ [private] |
Definition at line 153 of file ezn64_usb_control.h.
int schunk_ezn64::EZN64_usb::gripper_id_ [private] |
Definition at line 145 of file ezn64_usb_control.h.
const int schunk_ezn64::EZN64_usb::INPUT_BUFFER_SIZE = 512 [static, private] |
Definition at line 164 of file ezn64_usb_control.h.
Gripper joint state publisher.
Definition at line 91 of file ezn64_usb_control.h.
const int schunk_ezn64::EZN64_usb::LIBUSB_ENDPOINT = 129 [static, private] |
Definition at line 165 of file ezn64_usb_control.h.
const int schunk_ezn64::EZN64_usb::LIBUSB_TIMEOUT = 1000 [static, private] |
Definition at line 166 of file ezn64_usb_control.h.
const int schunk_ezn64::EZN64_usb::LIBUSB_VERBOSITY_LEVEL = 3 [static, private] |
Definition at line 167 of file ezn64_usb_control.h.
const double schunk_ezn64::EZN64_usb::MAX_GRIPPER_POS_LIMIT = 12 [static, private] |
Definition at line 162 of file ezn64_usb_control.h.
const double schunk_ezn64::EZN64_usb::MIN_GRIPPER_POS_LIMIT = 0 [static, private] |
Definition at line 161 of file ezn64_usb_control.h.
int schunk_ezn64::EZN64_usb::product_id_ [private] |
Definition at line 147 of file ezn64_usb_control.h.
const float schunk_ezn64::EZN64_usb::TF_UPDATE_PERIOD = 0.1 [static] |
TF update period in seconds.
Definition at line 94 of file ezn64_usb_control.h.
double schunk_ezn64::EZN64_usb::update_frequency [private] |
Definition at line 148 of file ezn64_usb_control.h.
const int schunk_ezn64::EZN64_usb::URDF_SCALE_FACTOR = 1000 [static, private] |
Definition at line 168 of file ezn64_usb_control.h.
libusb_context* schunk_ezn64::EZN64_usb::usb_context_ [private] |
Definition at line 157 of file ezn64_usb_control.h.
int schunk_ezn64::EZN64_usb::vendor_id_ [private] |
Definition at line 146 of file ezn64_usb_control.h.
const double schunk_ezn64::EZN64_usb::WAIT_FOR_RESPONSE_INTERVAL = 0.5 [static, private] |
Definition at line 163 of file ezn64_usb_control.h.