aubopanel.cpp
Go to the documentation of this file.
00001 #include "aubo_panel/aubopanel.h"
00002 #include "ui_aubopanel.h"
00003 #include <math.h>
00004 
00005 
00006 namespace aubo_panel
00007 {
00008 
00009 AuboPanel::AuboPanel(QWidget *parent) :
00010     rviz::Panel(parent),
00011     ui(new Ui::AuboPanel),
00012     m_step(0.16),
00013     m_speed(50.0),
00014     m_jointStateSync(0),
00015     m_controMode(3),
00016     m_busInterface(1)
00017 
00018 {
00019     ui->setupUi(this);
00020 
00021     ui->rbx_pcan->setChecked(false);
00022     ui->rbx_tcp->setChecked(true);
00023 
00024 
00025     ui->rbx_continuous->setChecked(false);
00026     ui->rbx_goal->setChecked(false);
00027     ui->rbx_moveit->setChecked(false);
00028     ui->rbx_sync->setChecked(true);
00029 
00030 
00031     ui->pbn_setIO->setEnabled(false);
00032     ui->pbn_sendGoal->setEnabled(false);
00033 
00034     ui->cbx_ioType->setMaxCount(4);
00035     ui->cbx_ioType->addItem("PLC_IO");
00036     ui->cbx_ioType->addItem("TOOL_IO");
00037     ui->cbx_ioType->addItem("Board_IO");
00038     ui->cbx_ioType->addItem("Modbus_IO");
00039 
00040     ui->cbx_ioMode->setMaxCount(4);
00041     ui->cbx_ioMode->addItem("DO");
00042     ui->cbx_ioMode->addItem("DI");
00043     ui->cbx_ioMode->addItem("AO");
00044     ui->cbx_ioMode->addItem("AI");
00045 
00046     ui->cbx_ioIndex->setMaxCount(8);
00047     ui->cbx_ioIndex->addItem("1");
00048     ui->cbx_ioIndex->addItem("2");
00049     ui->cbx_ioIndex->addItem("3");
00050     ui->cbx_ioIndex->addItem("4");
00051     ui->cbx_ioIndex->addItem("5");
00052     ui->cbx_ioIndex->addItem("6");
00053     ui->cbx_ioIndex->addItem("7");
00054     ui->cbx_ioIndex->addItem("8");
00055 
00056 
00057     m_timer = new QTimer(this);
00058     connect(m_timer, SIGNAL(timeout()), this, SLOT(sendCommand()));
00059 
00060     initROS();
00061 
00062     m_timer->start(10);     // Start the timer.
00063 }
00064 
00065 AuboPanel::~AuboPanel()
00066 {
00067     delete ui;
00068 }
00069 
00070 void AuboPanel::initROS()
00071 {
00072     joints.data.resize(6);
00073     cmd1_publisher_ = nh_.advertise<std_msgs::Float32MultiArray>("pcan_cmd", 1000);
00074     cmd2_publisher_ = nh_.advertise<std_msgs::Float32MultiArray>("movej_cmd", 1000);
00075     cmd3_publisher_ = nh_.advertise<std_msgs::Float32MultiArray>("servoj_cmd", 1000);
00076     goal_publisher_ = nh_.advertise<aubo_msgs::GoalPoint>("send_goal", 1);
00077     io_publisher_ = nh_.advertise<aubo_msgs::IOState> ("io_state",1);
00078     subJointState_ = nh_.subscribe("joint_states", 1000, &AuboPanel::jointStateCallback,this);
00079 }
00080 
00081 
00082 int AuboPanel::pointCompare(void)
00083 {
00084   int ret = 0;
00085   for(int i=0;i<6;i++)
00086   {
00087     if(fabs(joints.data[i]-lastJointPosition[i])>=0.000001)
00088     {
00089        ret = 1;
00090        break;
00091     }
00092   }
00093   return ret;
00094 }
00095 
00096 void AuboPanel::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
00097 {
00098     lastJointPosition[0] = msg->position[0];
00099     lastJointPosition[1] = msg->position[1];
00100     lastJointPosition[2] = msg->position[2];
00101     lastJointPosition[3] = msg->position[3];
00102     lastJointPosition[4] = msg->position[4];
00103     lastJointPosition[5] = msg->position[5];
00104 
00105     if(m_controMode == 3)//sync with real robot
00106     {
00107         if(pointCompare())
00108         {
00109             joints.data[0] = lastJointPosition[0];
00110             joints.data[1] = lastJointPosition[1];
00111             joints.data[2] = lastJointPosition[2];
00112             joints.data[3] = lastJointPosition[3];
00113             joints.data[4] = lastJointPosition[4];
00114             joints.data[5] = lastJointPosition[5];
00115 
00116             ui->lb_joint1->setText(QString::number((joints.data[0]*180/M_PI),'f',6));
00117             ui->lb_joint2->setText(QString::number((joints.data[1]*180/M_PI),'f',6 ));
00118             ui->lb_joint3->setText(QString::number((joints.data[2]*180/M_PI),'f',6 ));
00119             ui->lb_joint4->setText(QString::number((joints.data[3]*180/M_PI),'f',6 ));
00120             ui->lb_joint5->setText(QString::number((joints.data[4]*180/M_PI),'f',6 ));
00121             ui->lb_joint6->setText(QString::number((joints.data[5]*180/M_PI),'f',6 ));
00122         }
00123     }
00124 }
00125 
00126 void AuboPanel::on_pbn_joint1Left_pressed()
00127 {
00128     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00129     else m_step = 0.035;
00130 
00131     joints.data[0] = joints.data[0] - m_step * m_speed/100 < -M_PI ?
00132                      joints.data[0] :
00133                      joints.data[0] - m_step  * m_speed/100;
00134     if(joints.data[0]< -3.05) joints.data[0] = -3.05;
00135 
00136     ui->lb_joint1->setText(QString::number(joints.data[0]*180.0/M_PI, 'f', 6));
00137 }
00138 
00139 
00140 void AuboPanel::on_pbn_joint1Right_pressed()
00141 {
00142     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00143     else m_step = 0.035;
00144 
00145     joints.data[0] = joints.data[0] + m_step * m_speed/100 > M_PI ?
00146                      joints.data[0] :
00147                      joints.data[0] + m_step * m_speed/100;
00148     if(joints.data[0]> 3.05) joints.data[0] = 3.05;
00149 
00150     ui->lb_joint1->setText(QString::number(joints.data[0]*180.0/M_PI, 'f', 6));
00151 }
00152 
00153 void AuboPanel::on_pbn_joint2Left_pressed()
00154 {
00155     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00156     else m_step = 0.035;
00157 
00158     joints.data[1] = joints.data[1] - m_step * m_speed/100 < -M_PI ?
00159                      joints.data[1] :
00160                      joints.data[1] - m_step * m_speed/100;
00161     if(joints.data[1]< -3.05) joints.data[1] = -3.05;
00162 
00163     ui->lb_joint2->setText(QString::number(joints.data[1]*180.0/M_PI, 'f', 6));
00164 }
00165 
00166 void AuboPanel::on_pbn_joint2Right_pressed()
00167 {    
00168     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00169     else m_step = 0.035;
00170 
00171     joints.data[1] = joints.data[1] + m_step * m_speed/100 > M_PI ?
00172                      joints.data[1] :
00173                      joints.data[1] + m_step * m_speed/100;
00174     if(joints.data[1]> 3.05) joints.data[1] = 3.05;
00175 
00176     ui->lb_joint2->setText(QString::number(joints.data[1]*180.0/M_PI, 'f', 6));
00177 }
00178 
00179 void AuboPanel::on_pbn_joint3Left_pressed()
00180 {
00181     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00182     else m_step = 0.035;
00183 
00184     joints.data[2] = joints.data[2] - m_step * m_speed/100 < -M_PI ?
00185                      joints.data[2] :
00186                      joints.data[2] - m_step * m_speed/100;
00187 
00188     if(joints.data[2]< -3.05) joints.data[2] = -3.05;
00189     ui->lb_joint3->setText(QString::number(joints.data[2]*180.0/M_PI, 'f', 6));
00190 }
00191 
00192 void AuboPanel::on_pbn_joint3Right_pressed()
00193 {
00194     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00195     else m_step = 0.035;
00196 
00197     joints.data[2] = joints.data[2] + m_step * m_speed/100 > M_PI ?
00198                      joints.data[2] :
00199                      joints.data[2] + m_step * m_speed/100;
00200     if(joints.data[2]> 3.05) joints.data[2] = 3.05;
00201 
00202     ui->lb_joint3->setText(QString::number(joints.data[2]*180.0/M_PI, 'f', 6));
00203 }
00204 
00205 void AuboPanel::on_pbn_joint4Left_pressed()
00206 {
00207     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00208     else m_step = 0.035;
00209 
00210     joints.data[3] = joints.data[3] - m_step * m_speed/100 < -M_PI ?
00211                      joints.data[3] :
00212                      joints.data[3] - m_step * m_speed/100;
00213     if(joints.data[3]< -3.05) joints.data[3] = -3.05;
00214 
00215     ui->lb_joint4->setText(QString::number(joints.data[3]*180.0/M_PI, 'f', 6));
00216 }
00217 
00218 void AuboPanel::on_pbn_joint4Right_pressed()
00219 {
00220     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00221     else m_step = 0.035;
00222 
00223     joints.data[3] = joints.data[3] + m_step * m_speed/100 > M_PI ?
00224                      joints.data[3] :
00225                      joints.data[3] + m_step * m_speed/100;
00226     if(joints.data[3]> 3.05) joints.data[3] = 3.05;
00227 
00228     ui->lb_joint4->setText(QString::number(joints.data[3]*180.0/M_PI, 'f', 6));
00229 }
00230 
00231 void AuboPanel::on_pbn_joint5Left_pressed()
00232 {
00233     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00234     else m_step = 0.035;
00235 
00236     joints.data[4] = joints.data[4] - m_step * m_speed/100 < -M_PI ?
00237                      joints.data[4] :
00238                      joints.data[4] - m_step * m_speed/100;
00239     if(joints.data[4]< -3.05) joints.data[4] = -3.05;
00240 
00241     ui->lb_joint5->setText(QString::number(joints.data[4]*180.0/M_PI, 'f', 6));
00242 }
00243 
00244 void AuboPanel::on_pbn_joint5Right_pressed()
00245 {
00246     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00247     else m_step = 0.035;
00248 
00249     joints.data[4] = joints.data[4] + m_step * m_speed/100 > M_PI ?
00250                      joints.data[4] :
00251                      joints.data[4] + m_step * m_speed/100;
00252     if(joints.data[4]> 3.05) joints.data[4] = 3.05;
00253 
00254     ui->lb_joint5->setText(QString::number(joints.data[4]*180.0/M_PI, 'f', 6));
00255 }
00256 
00257 void AuboPanel::on_pbn_joint6Left_pressed()
00258 {
00259     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00260     else m_step = 0.035;
00261 
00262     joints.data[5] = joints.data[5] - m_step * m_speed/100 < -M_PI ?
00263                      joints.data[5] :
00264                      joints.data[5] - m_step * m_speed/100;
00265     if(joints.data[5]< -3.05) joints.data[5] = -3.05;
00266 
00267     ui->lb_joint6->setText(QString::number(joints.data[5]*180.0/M_PI, 'f', 6));
00268 }
00269 
00270 void AuboPanel::on_pbn_joint6Right_pressed()
00271 {
00272     if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
00273     else m_step = 0.035;
00274 
00275     joints.data[5] = joints.data[5] + m_step * m_speed/100 > M_PI ?
00276                      joints.data[5] :
00277                      joints.data[5] + m_step * m_speed/100;
00278     if(joints.data[5]> 3.05) joints.data[5] = 3.05;
00279 
00280     ui->lb_joint6->setText(QString::number(joints.data[5]*180.0/M_PI, 'f', 6));
00281 }
00282 
00283 
00284 void AuboPanel::on_pbn_zero_clicked()
00285 {
00286     joints.data[0] = 0;
00287     joints.data[1] = 0;
00288     joints.data[2] = 0;
00289     joints.data[3] = 0;
00290     joints.data[4] = 0;
00291     joints.data[5] = 0;
00292     ui->lb_joint1->setText(QString::number(0.0, 'f', 6));
00293     ui->lb_joint2->setText(QString::number(0.0, 'f', 6));
00294     ui->lb_joint3->setText(QString::number(0.0, 'f', 6));
00295     ui->lb_joint4->setText(QString::number(0.0, 'f', 6));
00296     ui->lb_joint5->setText(QString::number(0.0, 'f', 6));
00297     ui->lb_joint6->setText(QString::number(0.0, 'f', 6));
00298 }
00299 
00300 void AuboPanel::on_pbn_classicPos1_clicked()
00301 {
00302     if((m_controMode == 1)||(m_controMode == 2))
00303     {
00304         joints.data[0] = 91*M_PI/180;
00305         joints.data[1] = 52.3*M_PI/180;
00306         joints.data[2] = -95*M_PI/180;
00307         joints.data[3] = 40*M_PI/180;
00308         joints.data[4] = 92.5*M_PI/180;
00309         joints.data[5] = 123.8*M_PI/180;
00310         ui->lb_joint1->setText(QString::number(91.0, 'f', 6));
00311         ui->lb_joint2->setText(QString::number(52.3, 'f', 6));
00312         ui->lb_joint3->setText(QString::number(-95.0, 'f', 6));
00313         ui->lb_joint4->setText(QString::number(40.0, 'f', 6));
00314         ui->lb_joint5->setText(QString::number(92.5, 'f', 6));
00315         ui->lb_joint6->setText(QString::number(123.8, 'f', 6));
00316     }
00317 }
00318 
00319 void AuboPanel::on_pbn_classicPos2_clicked()
00320 {
00321     if((m_controMode == 1)||(m_controMode == 2))
00322     {
00323         joints.data[0] = -92*M_PI/180;
00324         joints.data[1] = -61.7*M_PI/180;
00325         joints.data[2] = 102*M_PI/180;
00326         joints.data[3] = -24*M_PI/180;
00327         joints.data[4] = -83.6*M_PI/180;
00328         joints.data[5] = -101.1*M_PI/180;
00329         ui->lb_joint1->setText(QString::number(-92.0, 'f', 6));
00330         ui->lb_joint2->setText(QString::number(-61.7, 'f', 6));
00331         ui->lb_joint3->setText(QString::number(102.0, 'f', 6));
00332         ui->lb_joint4->setText(QString::number(-24.0, 'f', 6));
00333         ui->lb_joint5->setText(QString::number(-83.6, 'f', 6));
00334         ui->lb_joint6->setText(QString::number(-101.1, 'f', 6));
00335     }
00336 }
00337 
00338 void AuboPanel::on_pbn_sendGoal_clicked()
00339 {   
00340     if(m_controMode == 1)
00341     {
00342         cmd2_publisher_.publish(joints);
00343     }
00344     else if(m_controMode == 2)
00345     {
00346         goalPoint.bus = m_busInterface;
00347         goalPoint.joint1 = joints.data[0];
00348         goalPoint.joint2 = joints.data[1];
00349         goalPoint.joint3 = joints.data[2];
00350         goalPoint.joint4 = joints.data[3];
00351         goalPoint.joint5 = joints.data[4];
00352         goalPoint.joint6 = joints.data[5];
00353         goal_publisher_.publish(goalPoint);
00354     }
00355 }
00356 
00357 void AuboPanel::on_rbx_pcan_clicked()
00358 {
00359     ui->rbx_pcan->setChecked(true);
00360     ui->rbx_tcp->setChecked(false);
00361     ui->pbn_setIO->setEnabled(false);
00362     m_busInterface = 0;
00363 }
00364 
00365 
00366 void AuboPanel::on_rbx_tcp_clicked()
00367 {
00368     ui->rbx_pcan->setChecked(false);
00369     ui->rbx_tcp->setChecked(true);
00370     ui->pbn_setIO->setEnabled(true);
00371     m_busInterface = 1;
00372 }
00373 
00374 void AuboPanel::on_rbx_continuous_clicked()
00375 {
00376     ui->rbx_continuous->setChecked(true);
00377     ui->rbx_goal->setChecked(false);
00378     ui->rbx_moveit->setChecked(false);
00379     ui->rbx_sync->setChecked(false);
00380 
00381     ui->pbn_sendGoal->setEnabled(false);
00382     m_controMode = 0;
00383 }
00384 
00385 void AuboPanel::on_rbx_goal_clicked()
00386 {
00387     ui->rbx_continuous->setChecked(false);
00388     ui->rbx_goal->setChecked(true);
00389     ui->rbx_moveit->setChecked(false);
00390     ui->rbx_sync->setChecked(false);
00391 
00392     ui->pbn_sendGoal->setEnabled(true);
00393     m_controMode = 1;
00394 
00395 }
00396 
00397 void AuboPanel::on_rbx_moveit_clicked()
00398 {
00399 
00400     ui->rbx_continuous->setChecked(false);
00401     ui->rbx_goal->setChecked(false);
00402     ui->rbx_moveit->setChecked(true);
00403     ui->rbx_sync->setChecked(false);
00404 
00405     ui->pbn_sendGoal->setEnabled(true);
00406     m_controMode = 2;
00407 }
00408 
00409 void AuboPanel::on_rbx_sync_clicked()
00410 {
00411 
00412     ui->rbx_continuous->setChecked(false);
00413     ui->rbx_goal->setChecked(false);
00414     ui->rbx_moveit->setChecked(false);
00415     ui->rbx_sync->setChecked(true);
00416 
00417     ui->pbn_sendGoal->setEnabled(false);
00418     m_controMode = 3;
00419 }
00420 
00421 
00422 
00423 void AuboPanel::on_pbn_setIO_clicked()
00424 {
00425     iostate.type = ui->cbx_ioType->currentIndex()+1;
00426     iostate.mode = ui->cbx_ioMode->currentIndex()+1;
00427     iostate.index = ui->cbx_ioMode->currentIndex()+1;
00428 
00429     iostate.state = ui->le_ioState->text().toDouble();
00430 
00431     if((m_controMode == 1)&&(m_busInterface == 1))
00432     {
00433         io_publisher_.publish(iostate);
00434     }
00435 }
00436 
00437 
00438 void AuboPanel::sendCommand()
00439 {
00440     if( ros::ok()&&(cmd1_publisher_||cmd3_publisher_))
00441     {
00442         if(m_controMode == 0)
00443         {
00444             ROS_WARN("Send");
00445             if(m_busInterface == 0)
00446             {
00447                 cmd1_publisher_.publish(joints);
00448             }
00449             else if(m_busInterface == 1)
00450             {
00451                 cmd3_publisher_.publish(joints);
00452             }
00453         }
00454     }
00455 }
00456 
00457 }
00458 
00459 #include <pluginlib/class_list_macros.h>
00460 PLUGINLIB_EXPORT_CLASS(aubo_panel::AuboPanel,rviz::Panel)
00461 
00462 
00463 
00464 
00465 


aubo_panel
Author(s): liuxin
autogenerated on Sat Jun 8 2019 19:06:05