pg70_rs232_control.h

Go to the documentation of this file.

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

#ifndef PG70_RS232_CONTROL_H
#define PG70_RS232_CONTROL_H

#include <ros/ros.h>
#include <serial/serial.h>
#include <tf/transform_broadcaster.h>
#include <sensor_msgs/JointState.h>

//Service headers
#include <schunk_pg70/reference.h>
#include <schunk_pg70/set_position.h>
#include <schunk_pg70/get_error.h>
#include <schunk_pg70/get_position.h>
#include <schunk_pg70/acknowledge_error.h>
#include <schunk_pg70/stop.h>

namespace schunk_pg70
{

class PG70_serial
{
public:
  
  PG70_serial(ros::NodeHandle *nh);
  
  ~PG70_serial();
  
  bool referenceCallback(schunk_pg70::reference::Request &req,
                         schunk_pg70::reference::Response &res);
  
  bool getErrorCallback(schunk_pg70::get_error::Request &req,
                        schunk_pg70::get_error::Response &res);
  
  bool getPositionCallback(schunk_pg70::get_position::Request &req,
                           schunk_pg70::get_position::Response &res);

  bool setPositionCallback(schunk_pg70::set_position::Request &req,
                           schunk_pg70::set_position::Response &res);
  
  bool acknowledgeErrorCallback(schunk_pg70::acknowledge_error::Request &req,
                                schunk_pg70::acknowledge_error::Response &res);
  
  bool stopCallback(schunk_pg70::stop::Request &req,
                    schunk_pg70::stop::Response &res);
  
  void timerCallback(const ros::TimerEvent &event);  
  
  ros::Publisher joint_pub;
  
  static const float TF_UPDATE_PERIOD = 0.1;   
   
private:

  void reference(serial::Serial *port);
   
  uint8_t getError(serial::Serial *port);
    
  float getPosition(serial::Serial *port);
  
  void setPosition(serial::Serial *port, int goal_position, int velocity, int acceleration);
   
  void acknowledgeError(serial::Serial *port);
    
  void stop(serial::Serial *port);
    
  void getPeriodicPositionUpdate(serial::Serial *port, float update_frequency);
  
  uint16_t CRC16(uint16_t crc, uint16_t data);   
     
  float IEEE_754_to_float(uint8_t *raw);
  
  void float_to_IEEE_754(float position, unsigned int *output_array);
   
  //Launch params
  int gripper_id_;
  std::string port_name_;
  int baudrate_;  

  //Gripper state variables
  float act_position_;
  uint8_t pg70_error_;
  sensor_msgs::JointState pg70_joint_state_; 

  //Serial variables
  serial::Serial *com_port_;
  
  //Consts
  static const double MIN_GRIPPER_POS_LIMIT = 0;
  static const double MAX_GRIPPER_POS_LIMIT = 69;
  static const double MIN_GRIPPER_VEL_LIMIT = 0;
  static const double MAX_GRIPPER_VEL_LIMIT = 83;
  static const double MIN_GRIPPER_ACC_LIMIT = 0;
  static const double MAX_GRIPPER_ACC_LIMIT = 320;
  static const double WAIT_FOR_RESPONSE_INTERVAL = 0.5;
  static const double INPUT_BUFFER_SIZE = 64;
  static const int    URDF_SCALE_FACTOR = 2000;
    
};  //PG70_serial
}   //schunk_pg70

#endif //PG70_RS232_CONTROL_H