00001 /*############################################################################################################################################## 00002 Note:aubo_driver is a sample of TCP/IP interface for AUBO-i5. 00003 There ara some main API for application as follow,please see our_control_api.h for more information. 00004 ############################################################################################################################################## 00005 void init_move_profile(); //初始化move的属性 00006 void set_scurve(int val); //设置S曲线是否有效 00007 void set_tcp(double *tcp_pose,int count); //设置TCP参数 00008 void set_relative_offset(double *offset,int count); //设置MOVE的偏移量 00009 void set_wait_move_finish(int val); //设置MOVE的偏移量 设置是否等待到位信号 即 阻塞与非阻塞 00010 void set_feature(const char *feature_name); //设置坐标系 00011 void add_waypoint(const double *pos, int count); //用于MOVE 中增加路点 00012 00013 int movej(double *pos, int count, double acc, double velc); 00014 int movel(double *pos, int count, double acc, double velc); 00015 int movel_to(double x, double y, double z, double acc, double velc); 00016 int movep(double acc, double velc,double blend_radius,int track_mode); 00017 00018 int set_payload(double weight, double *cog, int count); //设置运行时负载 00019 00020 int is_exist_current_io ( our_contorl_io_type io_type, our_contorl_io_mode io_mode,int io_index); //判断对应IO是否存在 00021 int set_robot_one_io_status( our_contorl_io_type io_type, our_contorl_io_mode io_mode,int io_index, double io_value); //设置指定IO 的状态 00022 double get_robot_one_io_status( our_contorl_io_type io_type, our_contorl_io_mode io_mode,int io_index); //获取指定IO 的状态 00023 ##############################################################################################################################################*/ 00024 00025 00026 #ifndef AUBO_DRIVER_H_ 00027 #define AUBO_DRIVER_H_ 00028 00029 #include <ros/ros.h> 00030 #include <std_msgs/Float32MultiArray.h> 00031 #include <aubo_msgs/IOState.h> 00032 #include <aubo_msgs/JointPos.h> 00033 00034 00035 class AuboDriver { 00036 private: 00037 bool reverse_connected_; 00038 00039 ros::NodeHandle nh; 00040 ros::Publisher pos_pub; 00041 ros::Subscriber sub1; 00042 ros::Subscriber sub2; 00043 ros::Subscriber sub3; 00044 ros::Timer timer; 00045 public: 00046 00047 AuboDriver(std::string host,unsigned int reverse_port = 8887); 00048 00049 void chatterCallback1(const std_msgs::Float32MultiArray::ConstPtr &msg); 00050 void chatterCallback2(const std_msgs::Float32MultiArray::ConstPtr &msg); 00051 void chatterCallback3(const aubo_msgs::IOState::ConstPtr &msg); 00052 void timerCallback(const ros::TimerEvent& e); 00053 00054 00055 }; 00056 00057 #endif /* AUBO_DRIVER_H_ */