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);
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)
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