aubo_driver.cpp
Go to the documentation of this file.
00001 #include "aubo_driver/aubo_driver.h"
00002 #include "our_control_api.h"
00003 
00004 
00005 double last_road_point[6];
00006 
00007 int road_point_compare(double *goal)
00008 {
00009   int ret = 0;
00010   for(int i=0;i<6;i++)
00011   {
00012     if(fabs(goal[i]-last_road_point[i])>=0.000001)
00013     {
00014        ret = 1;
00015        break;
00016     }
00017   }
00018 
00019   if(ret == 1)
00020   {
00021     last_road_point[0]= goal[0];
00022     last_road_point[1]= goal[1];
00023     last_road_point[2]= goal[2];
00024     last_road_point[3]= goal[3];
00025     last_road_point[4]= goal[4];
00026     last_road_point[5]= goal[5];
00027   }
00028 
00029   return ret;
00030 }
00031 
00032 
00033 int connect_server(const char * server_ip, int server_port)
00034 {
00035     int ret = -1;
00036         
00037     ret= login(server_ip, server_port,"123","123");
00038     if(ret == 0)
00039     {
00040        init_move_profile_for_script();
00041        set_enable_block(false);
00042     }
00043     return ret;
00044 }
00045 
00046 int disconnect_server()
00047 {
00048     int ret = -1;
00049     ret =logout_system();
00050     return ret;
00051 }
00052 
00053 
00054 int get_current_position(double *pos)
00055 {
00056     int ret = -1;
00057     our_robot_road_point road_point;
00058     ret = get_robot_position(&road_point);
00059     if(ret == 0)
00060     {
00061         pos[0] = road_point.joint_pos[0];
00062         pos[1] = road_point.joint_pos[1];
00063         pos[2] = road_point.joint_pos[2];
00064         pos[3] = road_point.joint_pos[3];
00065         pos[4] = road_point.joint_pos[4];
00066         pos[5] = road_point.joint_pos[5];
00067     }
00068 
00069     return ret;
00070 }
00071 
00072 /*see enum type define: io_type,io_mode in ourcontrol.h*/
00073 int  set_robot_io(int io_type,int io_mode, int io_index, double io_value)
00074 {
00075    int ret = -1;
00076    ret = set_single_io_status((our_contorl_io_type)io_type,(our_contorl_io_mode)io_mode, io_index, io_value);
00077    return ret;
00078 }
00079 
00080 /*see enum type define: io_type,io_mode in ourcontrol.h*/
00081 double get_robot_io(int  io_type,int io_mode, int io_index)
00082 {
00083    double io_value;
00084    get_single_io_status((our_contorl_io_type)io_type,(our_contorl_io_mode)io_mode, io_index,&io_value);
00085    return io_value;    
00086 }
00087 
00088 
00089 AuboDriver::AuboDriver(std::string host,unsigned int reverse_port)
00090 {
00091 
00092     int ret = -1;
00093         
00094     ret= connect_server(host.c_str(), reverse_port);
00095 
00096     if(ret == 0)
00097     {
00098         ROS_INFO("Connect robot server %s:%d success!",host.c_str(),reverse_port);
00099         reverse_connected_ = true;
00100         get_current_position(last_road_point);
00101     }
00102     else
00103     {
00104         ROS_WARN("Connect robot server %s:%d failure!",host.c_str(),reverse_port);
00105         reverse_connected_ = false;
00106     }
00107 
00108 
00109     timer = nh.createTimer(ros::Duration(0.100),&AuboDriver::timerCallback,this);
00110     timer.stop();
00111 
00112     pos_pub = nh.advertise<aubo_msgs::JointPos>("current_pos", 1);
00113     sub1 = nh.subscribe("movej_cmd", 1000, &AuboDriver::chatterCallback1,this);
00114     sub2 = nh.subscribe("servoj_cmd", 1, &AuboDriver::chatterCallback2,this);
00115     sub3 = nh.subscribe("io_state", 1, &AuboDriver::chatterCallback3,this);
00116 
00117     timer.start();
00118 }
00119 
00120 
00121 void AuboDriver::chatterCallback1(const std_msgs::Float32MultiArray::ConstPtr &msg)
00122 {
00123     ROS_INFO("goal=[%f,%f,%f,%f,%f,%f]",msg->data[0],msg->data[1],msg->data[2],msg->data[3],msg->data[4],msg->data[5]);
00124 
00125     double pos[6];   
00126     pos[0] = msg->data[0];
00127     pos[1] = msg->data[1];
00128     pos[2] = msg->data[2];
00129     pos[3] = msg->data[3];
00130     pos[4] = msg->data[4];
00131     pos[5] = msg->data[5];
00132 
00133 
00134     if(road_point_compare(pos))
00135     {
00136         pos[0] = msg->data[0];
00137         pos[1] = msg->data[1];
00138         pos[2] = msg->data[2];
00139         pos[3] = msg->data[3];
00140         pos[4] = msg->data[4];
00141         pos[5] = msg->data[5];
00142 
00143         if(reverse_connected_)
00144         {
00145            robot_moveJ_for_script(pos,6,1000,1000);
00146         }
00147     }
00148 
00149 }
00150 
00151 
00152 void AuboDriver::chatterCallback2(const std_msgs::Float32MultiArray::ConstPtr &msg)
00153 {
00154     //ROS_INFO("goal=[%f,%f,%f,%f,%f,%f]",msg->data[0],msg->data[1],msg->data[2],msg->data[3],msg->data[4],msg->data[5]);
00155 
00156     double pos[6];
00157     pos[0] = msg->data[0];
00158     pos[1] = msg->data[1];
00159     pos[2] = msg->data[2];
00160     pos[3] = msg->data[3];
00161     pos[4] = msg->data[4];
00162     pos[5] = msg->data[5];
00163 
00164 
00165     if(road_point_compare(pos))
00166     {
00167         pos[0] = msg->data[0];
00168         pos[1] = msg->data[1];
00169         pos[2] = msg->data[2];
00170         pos[3] = msg->data[3];
00171         pos[4] = msg->data[4];
00172         pos[5] = msg->data[5];
00173 
00174         if(reverse_connected_)
00175         {
00176            robot_servoj(pos,6);
00177         }
00178     }
00179 }
00180 
00181 void AuboDriver::chatterCallback3(const aubo_msgs::IOState::ConstPtr &msg)
00182 {
00183     ROS_INFO("IO[%d,%d,%d,%f]", msg->type,msg->mode,msg->index,msg->state);
00184 
00185     if(reverse_connected_)
00186     {
00187         int io_type = msg->type ;
00188         int io_mode = msg->mode;
00189         int io_index = msg->index;
00190         double io_state = msg->state;
00191         set_robot_io((our_contorl_io_type)io_type, (our_contorl_io_mode)io_mode ,io_index,io_state);
00192     }
00193 
00194 }
00195 
00196 
00197 void AuboDriver::timerCallback(const ros::TimerEvent& e)
00198 {
00199     aubo_msgs::JointPos cur_pos;
00200     double pos[6];
00201     if(reverse_connected_ == true)
00202     {
00203         get_current_position(pos);
00204         cur_pos.joint1 = pos[0];
00205         cur_pos.joint2 = pos[1];
00206         cur_pos.joint3 = pos[2];
00207         cur_pos.joint4 = pos[3];
00208         cur_pos.joint5 = pos[4];
00209         cur_pos.joint6 = pos[5];
00210 
00211         pos_pub.publish(cur_pos);
00212     }
00213 }
00214 
00215 
00216 int main(int argc, char **argv) {
00217         std::string host;
00218         int reverse_port;
00219 
00220         ros::init(argc, argv, "aubo_driver");
00221 
00222         if (!(ros::param::get("~robot_ip", host))) {
00223                 if (argc > 1) {
00224                         host = argv[1];
00225                 } else 
00226             {
00227             ROS_WARN("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
00228                         exit(1);
00229                 }
00230 
00231         }
00232 
00233         if (!(ros::param::get("~reverse_port", reverse_port))) {
00234                 if (argc > 2) {
00235                         reverse_port = atoi(argv[2]);
00236                 }
00237         else
00238                 {
00239             reverse_port = 8887;
00240                 }
00241 
00242                 if((reverse_port <= 0) or (reverse_port >= 65535)) {
00243             ROS_WARN("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 8887");
00244             reverse_port = 8887;
00245                 }
00246         } 
00247 
00248         AuboDriver auboDriver(host, reverse_port);
00249 
00250     ros::AsyncSpinner spinner(6);
00251         spinner.start();
00252 
00253         ros::waitForShutdown();
00254 
00255         return(0);
00256 }


aubo_driver
Author(s): liuxin
autogenerated on Sat Jun 8 2019 19:06:01