ezn64_usb_control.h

Go to the documentation of this file.

/*********************************************************************************************/
/* Author: Frantisek Durovsky */

#ifndef EZN64_USB_CONTROL_H
#define EZN64_USB_CONTROL_H

#include <ros/ros.h>
#include <libusb-1.0/libusb.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/JointState.h>

#include <schunk_ezn64/reference.h>
#include <schunk_ezn64/set_position.h>
#include <schunk_ezn64/get_error.h>
#include <schunk_ezn64/get_position.h>
#include <schunk_ezn64/acknowledge_error.h>
#include <schunk_ezn64/stop.h>

namespace schunk_ezn64
{

class EZN64_usb
{
public:
  
  EZN64_usb(ros::NodeHandle *nh);
  
  ~EZN64_usb();
  
  bool referenceCallback(schunk_ezn64::reference::Request &req,
                         schunk_ezn64::reference::Response &res);
     
  bool getErrorCallback(schunk_ezn64::get_error::Request &req,
                        schunk_ezn64::get_error::Response &res);
  
  bool acknowledgeErrorCallback(schunk_ezn64::acknowledge_error::Request &req,
                                schunk_ezn64::acknowledge_error::Response &res);
  
  bool setPositionCallback(schunk_ezn64::set_position::Request &req,
                           schunk_ezn64::set_position::Response &res);
  
  bool getPositionCallback(schunk_ezn64::get_position::Request &req,
                           schunk_ezn64::get_position::Response &res);
  
  bool stopCallback(schunk_ezn64::stop::Request &req,
                    schunk_ezn64::stop::Response &res);
  
  void timerCallback(const ros::TimerEvent &event);  
  
  ros::Publisher joint_pub;
  
  static const float TF_UPDATE_PERIOD = 0.1;   
  
private:
    
  void reference(libusb_device_handle *handle);
   
  uint8_t getError(libusb_device_handle *handle);
    
  void acknowledgeError(libusb_device_handle *handle);
  
  float getPosition(libusb_device_handle *handle);
  
  void setPosition(libusb_device_handle *handle, float goal_position);
    
  void stop(libusb_device_handle *handle);
   
  void getPeriodicPositionUpdate(libusb_device_handle *handle, float period);
  
  libusb_device* find_ezn64_dev(int VendorID, int ProductID);
  
  void print_libusb_dev(libusb_device *dev);

  libusb_device_handle* open_ezn64_dev(libusb_device *dev);
  
  int close_ezn64_dev(libusb_device_handle *handle, libusb_context *usb_context);

  void usb_write(libusb_device_handle *handle, std::vector<uint8_t> output);
  
  std::vector<uint8_t> usb_read(libusb_device_handle *handle);
  
  float IEEE_754_to_float(uint8_t *raw);
  
  void float_to_IEEE_754(float position, unsigned int *output_array);
  
  //Launch params
  int gripper_id_;
  int vendor_id_;
  int product_id_;
  double update_frequency;

  //Gripper state variales
  float act_position_;
  uint8_t ezn64_error_;
  sensor_msgs::JointState ezn64_joint_state_; 
    
  //Libusb Variables
  libusb_device *ezn64_dev_;
  libusb_context *usb_context_;
  libusb_device_handle *ezn64_handle_;
  
  //Consts 
  static const double MIN_GRIPPER_POS_LIMIT = 0;
  static const double MAX_GRIPPER_POS_LIMIT = 12;
  static const double WAIT_FOR_RESPONSE_INTERVAL = 0.5;
  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 int    URDF_SCALE_FACTOR = 1000;
  
};  //EZN64_usb
}   //schunk_ezn64

#endif //EZN64_USB_CONTROL_H