00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "MotoPlus.h"
00033 #include "ParameterExtraction.h"
00034 #include "CtrlGroup.h"
00035 #include "SimpleMessage.h"
00036 #include "Controller.h"
00037 #include "MotionServer.h"
00038 #include "StateServer.h"
00039
00040 extern STATUS setsockopt
00041 (
00042 int s,
00043 int level,
00044 int optname,
00045 char * optval,
00046 int optlen
00047 );
00048
00049
00050
00051
00052 BOOL Ros_Controller_Init(Controller* controller);
00053 BOOL Ros_Controller_WaitInitReady(Controller* controller);
00054 BOOL Ros_Controller_IsValidGroupNo(Controller* controller, int groupNo);
00055 int Ros_Controller_OpenSocket(int tcpPort);
00056 void Ros_Controller_ConnectionServer_Start(Controller* controller);
00057
00058 void Ros_Controller_StatusInit(Controller* controller);
00059 BOOL Ros_Controller_IsAlarm(Controller* controller);
00060 BOOL Ros_Controller_IsError(Controller* controller);
00061 BOOL Ros_Controller_IsPlay(Controller* controller);
00062 BOOL Ros_Controller_IsTeach(Controller* controller);
00063 BOOL Ros_Controller_IsRemote(Controller* controller);
00064 BOOL Ros_Controller_IsOperating(Controller* controller);
00065 BOOL Ros_Controller_IsHold(Controller* controller);
00066 BOOL Ros_Controller_IsServoOn(Controller* controller);
00067 BOOL Ros_Controller_IsEStop(Controller* controller);
00068 BOOL Ros_Controller_IsWaitingRos(Controller* controller);
00069 int Ros_Controller_GetNotReadySubcode(Controller* controller);
00070 int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg);
00071 BOOL Ros_Controller_StatusRead(Controller* controller, USHORT ioStatus[IOSTATUS_MAX]);
00072 BOOL Ros_Controller_StatusUpdate(Controller* controller);
00073
00074 BOOL Ros_Controller_GetIOState(ULONG signal);
00075 void Ros_Controller_SetIOState(ULONG signal, BOOL status);
00076 int Ros_Controller_GetAlarmCode();
00077 void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize);
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087 BOOL Ros_Controller_Init(Controller* controller)
00088 {
00089 int grpNo;
00090 int i;
00091 BOOL bInitOk;
00092 STATUS status;
00093
00094 printf("Initializing controller\r\n");
00095
00096
00097 Ros_Controller_SetIOState(IO_FEEDBACK_WAITING_MP_INCMOVE, FALSE);
00098 Ros_Controller_SetIOState(IO_FEEDBACK_MP_INCMOVE_DONE, FALSE);
00099 Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, FALSE);
00100 Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, FALSE);
00101 Ros_Controller_SetIOState(IO_FEEDBACK_MOTIONSERVERCONNECTED, FALSE);
00102 Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, FALSE);
00103 Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, FALSE);
00104
00105
00106 bInitOk = TRUE;
00107 controller->bRobotJobReady = FALSE;
00108 controller->bRobotJobReadyRaised = FALSE;
00109 controller->bStopMotion = FALSE;
00110 Ros_Controller_StatusInit(controller);
00111 Ros_Controller_StatusRead(controller, controller->ioStatus);
00112
00113
00114 Ros_Controller_WaitInitReady(controller);
00115
00116
00117 status = GP_getInterpolationPeriod(&controller->interpolPeriod);
00118 if(status!=OK)
00119 bInitOk = FALSE;
00120
00121
00122 controller->numGroup = GP_getNumberOfGroups();
00123 if(controller->numGroup < 1)
00124 bInitOk = FALSE;
00125
00126 controller->numRobot = 0;
00127
00128
00129 for(grpNo=0; grpNo < MP_GRP_NUM; grpNo++)
00130 {
00131 if(grpNo < controller->numGroup)
00132 {
00133
00134 controller->ctrlGroups[grpNo] = Ros_CtrlGroup_Create(grpNo, controller->interpolPeriod);
00135 if(controller->ctrlGroups[grpNo] != NULL)
00136 {
00137 if(Ros_CtrlGroup_IsRobot(controller->ctrlGroups[grpNo]))
00138 controller->numRobot++;
00139 }
00140 else
00141 bInitOk = FALSE;
00142 }
00143 else
00144 controller->ctrlGroups[grpNo] = NULL;
00145 }
00146
00147
00148 controller->tidConnectionSrv = INVALID_TASK;
00149
00150 controller->tidStateSendState = INVALID_TASK;
00151 for (i = 0; i < MAX_STATE_CONNECTIONS; i++)
00152 {
00153 controller->sdStateConnections[i] = INVALID_SOCKET;
00154 }
00155
00156 for (i = 0; i < MAX_MOTION_CONNECTIONS; i++)
00157 {
00158 controller->sdMotionConnections[i] = INVALID_SOCKET;
00159 controller->tidMotionConnections[i] = INVALID_TASK;
00160 }
00161 controller->tidIncMoveThread = INVALID_TASK;
00162
00163 #ifdef DX100
00164 controller->bSkillMotionReady[0] = FALSE;
00165 controller->bSkillMotionReady[1] = FALSE;
00166 #endif
00167
00168 if(bInitOk)
00169 {
00170
00171 Ros_Controller_SetIOState(IO_FEEDBACK_INITIALIZATION_DONE, TRUE);
00172 }
00173 else
00174 {
00175 Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE);
00176 printf("Failure to initialize controller\r\n");
00177 }
00178
00179 return bInitOk;
00180 }
00181
00182
00183
00184
00185
00186 BOOL Ros_Controller_WaitInitReady(Controller* controller)
00187 {
00188 while(Ros_Controller_IsAlarm(controller))
00189 {
00190 mpTaskDelay(1000);
00191 Ros_Controller_StatusRead(controller, controller->ioStatus);
00192 }
00193
00194 return TRUE;
00195 }
00196
00197
00198
00199
00200
00201 BOOL Ros_Controller_IsValidGroupNo(Controller* controller, int groupNo)
00202 {
00203 if((groupNo >= 0) && (groupNo < controller->numGroup))
00204 return TRUE;
00205 else
00206 {
00207 printf("ERROR: Attempt to access invalid Group No. (%d) \r\n", groupNo);
00208 return FALSE;
00209 }
00210 }
00211
00212
00213
00214
00215
00216
00217
00218 int Ros_Controller_OpenSocket(int tcpPort)
00219 {
00220 int sd;
00221 struct sockaddr_in serverSockAddr;
00222 int ret;
00223
00224
00225 sd = mpSocket(AF_INET, SOCK_STREAM, 0);
00226 if (sd < 0)
00227 return -1;
00228
00229
00230 memset(&serverSockAddr, 0, sizeof(struct sockaddr_in));
00231 serverSockAddr.sin_family = AF_INET;
00232 serverSockAddr.sin_addr.s_addr = INADDR_ANY;
00233 serverSockAddr.sin_port = mpHtons(tcpPort);
00234
00235
00236 ret = mpBind(sd, (struct sockaddr *)&serverSockAddr, sizeof(struct sockaddr_in));
00237 if (ret < 0)
00238 goto closeSockHandle;
00239
00240
00241 ret = mpListen(sd, SOMAXCONN);
00242 if (ret < 0)
00243 goto closeSockHandle;
00244
00245 return sd;
00246
00247 closeSockHandle:
00248 printf("Error in Ros_Controller_OpenSocket\r\n");
00249
00250 if(sd >= 0)
00251 mpClose(sd);
00252
00253 return -2;
00254 }
00255
00256
00257
00258
00259
00260 void Ros_Controller_ConnectionServer_Start(Controller* controller)
00261 {
00262 int sdMotionServer = INVALID_SOCKET;
00263 int sdStateServer = INVALID_SOCKET;
00264 struct fd_set fds;
00265 struct timeval tv;
00266 int sdAccepted = INVALID_SOCKET;
00267 struct sockaddr_in clientSockAddr;
00268 int sizeofSockAddr;
00269 int useNoDelay = 1;
00270 STATUS s;
00271
00272
00273 Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, TRUE);
00274
00275 printf("Controller connection server running\r\n");
00276
00277
00278 sdMotionServer = Ros_Controller_OpenSocket(TCP_PORT_MOTION);
00279 if(sdMotionServer < 0)
00280 goto closeSockHandle;
00281
00282 sdStateServer = Ros_Controller_OpenSocket(TCP_PORT_STATE);
00283 if(sdStateServer < 0)
00284 goto closeSockHandle;
00285
00286 tv.tv_sec = 0;
00287 tv.tv_usec = 0;
00288
00289 FOREVER
00290 {
00291 FD_ZERO(&fds);
00292 FD_SET(sdMotionServer, &fds);
00293 FD_SET(sdStateServer, &fds);
00294
00295 if(mpSelect(sdStateServer+1, &fds, NULL, NULL, NULL) > 0)
00296 {
00297 memset(&clientSockAddr, 0, sizeof(clientSockAddr));
00298 sizeofSockAddr = sizeof(clientSockAddr);
00299
00300
00301 if(FD_ISSET(sdMotionServer, &fds))
00302 sdAccepted = mpAccept(sdMotionServer, (struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
00303 else if(FD_ISSET(sdStateServer, &fds))
00304 sdAccepted = mpAccept(sdStateServer, (struct sockaddr *)&clientSockAddr, &sizeofSockAddr);
00305 else
00306 continue;
00307
00308 if (sdAccepted < 0)
00309 break;
00310
00311 printf("Accepted connection from client PC\r\n");
00312
00313 s = setsockopt(sdAccepted, IPPROTO_TCP, TCP_NODELAY, (char*)&useNoDelay, sizeof (int));
00314 if( OK != s )
00315 {
00316 printf("Failed to set TCP_NODELAY.\r\n");
00317 }
00318
00319 if(FD_ISSET(sdMotionServer, &fds))
00320 Ros_MotionServer_StartNewConnection(controller, sdAccepted);
00321 else if(FD_ISSET(sdStateServer, &fds))
00322 Ros_StateServer_StartNewConnection(controller, sdAccepted);
00323 else
00324 mpClose(sdAccepted);
00325 }
00326 }
00327
00328 closeSockHandle:
00329 printf("Error!?... Connection Server is aborting. Reboot the controller.\r\n");
00330
00331 if(sdMotionServer >= 0)
00332 mpClose(sdMotionServer);
00333 if(sdStateServer >= 0)
00334 mpClose(sdStateServer);
00335
00336
00337 Ros_Controller_SetIOState(IO_FEEDBACK_CONNECTSERVERRUNNING, FALSE);
00338 }
00339
00340
00341
00342
00343
00344
00345
00346
00347 void Ros_Controller_StatusInit(Controller* controller)
00348 {
00349 controller->ioStatusAddr[IOSTATUS_ALARM_MAJOR].ulAddr = 50010;
00350 controller->ioStatusAddr[IOSTATUS_ALARM_MINOR].ulAddr = 50011;
00351 controller->ioStatusAddr[IOSTATUS_ALARM_SYSTEM].ulAddr = 50012;
00352 controller->ioStatusAddr[IOSTATUS_ALARM_USER].ulAddr = 50013;
00353 controller->ioStatusAddr[IOSTATUS_ERROR].ulAddr = 50014;
00354 controller->ioStatusAddr[IOSTATUS_PLAY].ulAddr = 50054;
00355 controller->ioStatusAddr[IOSTATUS_TEACH].ulAddr = 50053;
00356 controller->ioStatusAddr[IOSTATUS_REMOTE].ulAddr = 80011;
00357 controller->ioStatusAddr[IOSTATUS_OPERATING].ulAddr = 50070;
00358 controller->ioStatusAddr[IOSTATUS_HOLD].ulAddr = 50071;
00359 controller->ioStatusAddr[IOSTATUS_SERVO].ulAddr = 50073;
00360 controller->ioStatusAddr[IOSTATUS_ESTOP_EX].ulAddr = 80025;
00361 controller->ioStatusAddr[IOSTATUS_ESTOP_PP].ulAddr = 80026;
00362 controller->ioStatusAddr[IOSTATUS_ESTOP_CTRL].ulAddr = 80027;
00363 controller->ioStatusAddr[IOSTATUS_WAITING_ROS].ulAddr = IO_FEEDBACK_WAITING_MP_INCMOVE;
00364 controller->alarmCode = 0;
00365 }
00366
00367
00368 BOOL Ros_Controller_IsAlarm(Controller* controller)
00369 {
00370 return ((controller->ioStatus[IOSTATUS_ALARM_MAJOR]!=0)
00371 || (controller->ioStatus[IOSTATUS_ALARM_MINOR]!=0)
00372 || (controller->ioStatus[IOSTATUS_ALARM_SYSTEM]!=0)
00373 || (controller->ioStatus[IOSTATUS_ALARM_USER]!=0) );
00374 }
00375
00376 BOOL Ros_Controller_IsError(Controller* controller)
00377 {
00378 return ((controller->ioStatus[IOSTATUS_ERROR]!=0));
00379 }
00380
00381 BOOL Ros_Controller_IsPlay(Controller* controller)
00382 {
00383 return ((controller->ioStatus[IOSTATUS_PLAY]!=0));
00384 }
00385
00386 BOOL Ros_Controller_IsTeach(Controller* controller)
00387 {
00388 return ((controller->ioStatus[IOSTATUS_TEACH]!=0));
00389 }
00390
00391 BOOL Ros_Controller_IsRemote(Controller* controller)
00392 {
00393 return ((controller->ioStatus[IOSTATUS_REMOTE]!=0));
00394 }
00395
00396 BOOL Ros_Controller_IsOperating(Controller* controller)
00397 {
00398 return ((controller->ioStatus[IOSTATUS_OPERATING]!=0));
00399 }
00400
00401 BOOL Ros_Controller_IsHold(Controller* controller)
00402 {
00403 return ((controller->ioStatus[IOSTATUS_HOLD]!=0));
00404 }
00405
00406 BOOL Ros_Controller_IsServoOn(Controller* controller)
00407 {
00408 return ((controller->ioStatus[IOSTATUS_SERVO]!=0));
00409 }
00410
00411 BOOL Ros_Controller_IsEStop(Controller* controller)
00412 {
00413 return ((controller->ioStatus[IOSTATUS_ESTOP_EX]==0)
00414 || (controller->ioStatus[IOSTATUS_ESTOP_PP]==0)
00415 || (controller->ioStatus[IOSTATUS_ESTOP_CTRL]==0) );
00416 }
00417
00418 BOOL Ros_Controller_IsWaitingRos(Controller* controller)
00419 {
00420 return ((controller->ioStatus[IOSTATUS_WAITING_ROS]!=0));
00421 }
00422
00423 BOOL Ros_Controller_IsMotionReady(Controller* controller)
00424 {
00425 #ifdef DX100
00426 if(controller->numRobot < 2)
00427 return (controller->bRobotJobReady && controller->bSkillMotionReady[0]);
00428 else
00429 return (controller->bRobotJobReady && controller->bSkillMotionReady[0] && controller->bSkillMotionReady[1]);
00430 #else
00431 return (controller->bRobotJobReady);
00432 #endif
00433 }
00434
00435 int Ros_Controller_GetNotReadySubcode(Controller* controller)
00436 {
00437
00438 if(Ros_Controller_IsAlarm(controller))
00439 return ROS_RESULT_NOT_READY_ALARM;
00440
00441
00442 if(Ros_Controller_IsError(controller))
00443 return ROS_RESULT_NOT_READY_ERROR;
00444
00445
00446 if(Ros_Controller_IsEStop(controller))
00447 return ROS_RESULT_NOT_READY_ESTOP;
00448
00449
00450 if(!Ros_Controller_IsPlay(controller))
00451 return ROS_RESULT_NOT_READY_NOT_PLAY;
00452
00453
00454 if(!Ros_Controller_IsRemote(controller))
00455 return ROS_RESULT_NOT_READY_NOT_REMOTE;
00456
00457
00458 if(!Ros_Controller_IsServoOn(controller))
00459 return ROS_RESULT_NOT_READY_SERVO_OFF;
00460
00461
00462 if(Ros_Controller_IsHold(controller))
00463 return ROS_RESULT_NOT_READY_HOLD;
00464
00465
00466 if(!Ros_Controller_IsOperating(controller))
00467 return ROS_RESULT_NOT_READY_NOT_STARTED;
00468
00469
00470 if(!Ros_Controller_IsWaitingRos(controller))
00471 return ROS_RESULT_NOT_READY_WAITING_ROS;
00472
00473
00474 if(!Ros_Controller_IsMotionReady(controller))
00475 return ROS_RESULT_NOT_READY_SKILLSEND;
00476
00477 return ROS_RESULT_NOT_READY_UNSPECIFIED;
00478 }
00479
00480
00481
00482
00483 int Ros_Controller_StatusToMsg(Controller* controller, SimpleMsg* sendMsg)
00484 {
00485
00486 memset(sendMsg, 0x00, sizeof(SimpleMsg));
00487
00488
00489 sendMsg->prefix.length = sizeof(SmHeader) + sizeof(SmBodyRobotStatus);
00490
00491
00492 sendMsg->header.msgType = ROS_MSG_ROBOT_STATUS;
00493 sendMsg->header.commType = ROS_COMM_TOPIC;
00494 sendMsg->header.replyType = ROS_REPLY_INVALID;
00495
00496
00497 sendMsg->body.robotStatus.drives_powered = (int)(Ros_Controller_IsServoOn(controller));
00498 sendMsg->body.robotStatus.e_stopped = (int)(Ros_Controller_IsEStop(controller));
00499 sendMsg->body.robotStatus.error_code = controller->alarmCode;
00500 sendMsg->body.robotStatus.in_error = (int)Ros_Controller_IsAlarm(controller);
00501 sendMsg->body.robotStatus.in_motion = (int)(Ros_MotionServer_HasDataInQueue(controller));
00502 if(Ros_Controller_IsPlay(controller))
00503 sendMsg->body.robotStatus.mode = 2;
00504 else
00505 sendMsg->body.robotStatus.mode = 1;
00506 sendMsg->body.robotStatus.motion_possible = (int)(Ros_Controller_IsMotionReady(controller));
00507
00508 return(sendMsg->prefix.length + sizeof(SmPrefix));
00509 }
00510
00511
00512
00513
00514 BOOL Ros_Controller_StatusRead(Controller* controller, USHORT ioStatus[IOSTATUS_MAX])
00515 {
00516 return (mpReadIO(controller->ioStatusAddr, ioStatus, IOSTATUS_MAX) == 0);
00517 }
00518
00519
00520
00521
00522 BOOL Ros_Controller_StatusUpdate(Controller* controller)
00523 {
00524 USHORT ioStatus[IOSTATUS_MAX];
00525 int i;
00526
00527 if(Ros_Controller_StatusRead(controller, ioStatus))
00528 {
00529
00530 for(i=0; i<IOSTATUS_MAX; i++)
00531 {
00532 if(controller->ioStatus[i] != ioStatus[i])
00533 {
00534
00535
00536 controller->ioStatus[i] = ioStatus[i];
00537 switch(i)
00538 {
00539 case IOSTATUS_ALARM_MAJOR:
00540 case IOSTATUS_ALARM_MINOR:
00541 case IOSTATUS_ALARM_SYSTEM:
00542 case IOSTATUS_ALARM_USER:
00543 {
00544 if(ioStatus[i] == 0)
00545 controller->alarmCode = 0;
00546 else
00547 controller->alarmCode = Ros_Controller_GetAlarmCode();
00548 }
00549
00550
00551
00552
00553
00554
00555 case IOSTATUS_REMOTE:
00556 case IOSTATUS_OPERATING:
00557 case IOSTATUS_WAITING_ROS:
00558 {
00559 if(ioStatus[i] == 0)
00560 {
00561
00562 controller->bRobotJobReady = FALSE;
00563 controller->bRobotJobReadyRaised = FALSE;
00564 Ros_MotionServer_ClearQ_All(controller);
00565 }
00566 else
00567 {
00568 if(i==IOSTATUS_WAITING_ROS)
00569 controller->bRobotJobReadyRaised = TRUE;
00570
00571 if(controller->bRobotJobReadyRaised
00572 && (Ros_Controller_IsOperating(controller))
00573 && (Ros_Controller_IsRemote(controller)) )
00574 {
00575 controller->bRobotJobReady = TRUE;
00576 if(Ros_Controller_IsMotionReady(controller))
00577 printf("Robot job is ready for ROS commands.\r\n");
00578 }
00579 }
00580 break;
00581 }
00582 }
00583 }
00584 }
00585 return TRUE;
00586 }
00587 return FALSE;
00588 }
00589
00590
00591
00592
00593
00594
00595
00596
00597 BOOL Ros_Controller_GetIOState(ULONG signal)
00598 {
00599 MP_IO_INFO ioInfo;
00600 USHORT ioState;
00601 int ret;
00602
00603
00604 ioInfo.ulAddr = signal;
00605 ret = mpReadIO(&ioInfo, &ioState, 1);
00606 if(ret != 0)
00607 printf("mpReadIO failure (%d)\r\n", ret);
00608
00609 return (ioState != 0);
00610 }
00611
00612
00613
00614
00615
00616 void Ros_Controller_SetIOState(ULONG signal, BOOL status)
00617 {
00618 MP_IO_DATA ioData;
00619 int ret;
00620
00621
00622 ioData.ulAddr = signal;
00623 ioData.ulValue = status;
00624 ret = mpWriteIO(&ioData, 1);
00625 if(ret != 0)
00626 printf("mpWriteIO failure (%d)\r\n", ret);
00627 }
00628
00629
00630
00631
00632
00633 int Ros_Controller_GetAlarmCode()
00634 {
00635 MP_ALARM_CODE_RSP_DATA alarmData;
00636 memset(&alarmData, 0x00, sizeof(alarmData));
00637 if(mpGetAlarmCode(&alarmData) == 0)
00638 {
00639 if(alarmData.usAlarmNum > 0)
00640 return(alarmData.AlarmData.usAlarmNo[0]);
00641 else
00642 return 0;
00643 }
00644 return -1;
00645 }
00646
00647
00648
00649
00650
00651 void Ros_Controller_ErrNo_ToString(int errNo, char errMsg[ERROR_MSG_MAX_SIZE], int errMsgSize)
00652 {
00653 switch(errNo)
00654 {
00655 case 0x2010: memcpy(errMsg, "Robot is in operation", errMsgSize); break;
00656 case 0x2030: memcpy(errMsg, "In HOLD status (PP)", errMsgSize); break;
00657 case 0x2040: memcpy(errMsg, "In HOLD status (External)", errMsgSize); break;
00658 case 0x2050: memcpy(errMsg, "In HOLD status (Command)", errMsgSize); break;
00659 case 0x2060: memcpy(errMsg, "In ERROR/ALARM status", errMsgSize); break;
00660 case 0x2070: memcpy(errMsg, "In SERVO OFF status", errMsgSize); break;
00661 case 0x2080: memcpy(errMsg, "Wrong operation mode", errMsgSize); break;
00662 case 0x3040: memcpy(errMsg, "The home position is not registered", errMsgSize); break;
00663 case 0x3050: memcpy(errMsg, "Out of range (ABSO data", errMsgSize); break;
00664 case 0x3400: memcpy(errMsg, "Cannot operate MASTER JOB", errMsgSize); break;
00665 case 0x3410: memcpy(errMsg, "The JOB name is already registered in another task", errMsgSize); break;
00666 case 0x4040: memcpy(errMsg, "Specified JOB not found", errMsgSize); break;
00667 case 0x5200: memcpy(errMsg, "Over data range", errMsgSize); break;
00668 default: memcpy(errMsg, "Unspecified reason", errMsgSize); break;
00669 }
00670 }
00671
00672
00673 #ifdef DX100
00674
00675 void Ros_Controller_ListenForSkill(Controller* controller, int sl)
00676 {
00677 SYS2MP_SENS_MSG skillMsg;
00678 STATUS apiRet;
00679
00680 controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
00681 memset(&skillMsg, 0x00, sizeof(SYS2MP_SENS_MSG));
00682
00683 FOREVER
00684 {
00685
00686
00687
00688
00689 mpEndSkillCommandProcess(sl, &skillMsg);
00690
00691 mpTaskDelay(4);
00692
00693
00694
00695 apiRet = mpReceiveSkillCommand(sl, &skillMsg);
00696
00697 if (skillMsg.main_comm != MP_SKILL_COMM)
00698 {
00699 printf("Ignoring command, because it's not a SKILL command\n");
00700 continue;
00701 }
00702
00703
00704 switch(skillMsg.sub_comm)
00705 {
00706 case MP_SKILL_SEND:
00707 if(strcmp(skillMsg.cmd, "ROS-START") == 0)
00708 {
00709 controller->bSkillMotionReady[sl - MP_SL_ID1] = TRUE;
00710 if(Ros_Controller_IsMotionReady(controller))
00711 printf("Robot job is ready for ROS commands.\r\n");
00712 }
00713 else if(strcmp(skillMsg.cmd, "ROS-STOP") == 0)
00714 {
00715 controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
00716 }
00717 else
00718 {
00719 printf ("MP_SKILL_SEND(SL_ID=%d) - %s \n", sl, skillMsg.cmd);
00720 }
00721 break;
00722
00723 case MP_SKILL_END:
00724
00725 controller->bSkillMotionReady[sl - MP_SL_ID1] = FALSE;
00726 break;
00727 }
00728 }
00729 }
00730 #endif
00731