void RosInitTask() { Controller ros_controller; if(!Ros_Controller_Init(&ros_controller)) { //set feedback signal to notify failure Ros_Controller_SetIOState(IO_FEEDBACK_FAILURE, TRUE); mpDeleteSelf; return; } ros_controller.tidConnectionSrv = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_Controller_ConnectionServer_Start, (int)&ros_controller, 0, 0, 0, 0, 0, 0, 0, 0, 0); #ifdef DX100 // DX100 need to execute a SKILLSEND command prior to the WAIT in order for the // incremental motion function to work. These tasks monitor for those commands // This supports a maximum of two robots which should be assigned to slave id // MP_SL_ID1 and MP_SL_ID2 respectively. // first robot ros_controller.RosListenForSkillID[0] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_Controller_ListenForSkill, (int)&ros_controller, MP_SL_ID1, 0, 0, 0, 0, 0, 0, 0, 0); // if second robot if(ros_controller.numRobot > 1) { ros_controller.RosListenForSkillID[1] = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_Controller_ListenForSkill, (int)&ros_controller, MP_SL_ID2, 0, 0, 0, 0, 0, 0, 0, 0); } else { ros_controller.RosListenForSkillID[1] = INVALID_TASK; } #endif // start loop to monitor controller state FOREVER { // Check controller status if (!Ros_Controller_StatusUpdate(&ros_controller)) puts("Failed to update controller status. Check robot parameters."); mpTaskDelay(CONTROLLER_STATUS_UPDATE_PERIOD); } }
//----------------------------------------------------------------------- // Send state (robot position) as long as there is an active connection //----------------------------------------------------------------------- void Ros_StateServer_SendState(Controller* controller) { BOOL bHasConnections = TRUE; int groupNo; SimpleMsg sendMsg; int msgSize; printf("Starting State Server Send State task\r\n"); while(bHasConnections) { // Send position for each control group for(groupNo=0; groupNo < controller->numGroup; groupNo++) { msgSize = Ros_SimpleMsg_JointFeedback(controller->ctrlGroups[groupNo], &sendMsg); if(msgSize > 0) { bHasConnections = Ros_StateServer_SendMsgToAllClient(controller, &sendMsg, msgSize); } } // Send controller/robot status if(bHasConnections) { msgSize = Ros_Controller_StatusToMsg(controller, &sendMsg); if(msgSize > 0) { bHasConnections = Ros_StateServer_SendMsgToAllClient(controller, &sendMsg, msgSize); } } mpTaskDelay(STATE_UPDATE_MIN_PERIOD); } // Terminate this task controller->tidStateSendState = INVALID_TASK; Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, FALSE); printf("State Server Send State task was terminated\r\n"); mpDeleteSelf; }
//----------------------------------------------------------------------- // Start the tasks for a new state server connection: // - WaitForSimpleMsg: Task that waits to receive new SimpleMessage // - AddToIncQueueProcess: Task that take data from a message and generate Incmove //----------------------------------------------------------------------- void Ros_StateServer_StartNewConnection(Controller* controller, int sd) { int connectionIndex; printf("Starting new connection to the State Server\r\n"); //look for next available connection slot for (connectionIndex = 0; connectionIndex < MAX_STATE_CONNECTIONS; connectionIndex++) { if (controller->sdStateConnections[connectionIndex] == INVALID_SOCKET) { //Start the new connection in a different task. //Each task's memory will be unique IFF the data is on the stack. //Any global or heap stuff will not be unique. controller->sdStateConnections[connectionIndex] = sd; // If not started if(controller->tidStateSendState == INVALID_TASK) { //start task that will send the controller state controller->tidStateSendState = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)Ros_StateServer_SendState, (int)controller, 0, 0, 0, 0, 0, 0, 0, 0, 0); //set feedback signal if(controller->tidStateSendState != INVALID_TASK) Ros_Controller_SetIOState(IO_FEEDBACK_STATESERVERCONNECTED, TRUE); } break; } } if (connectionIndex == MAX_STATE_CONNECTIONS) { printf("Too many State server connections... not accepting last attempt.\r\n"); mpClose(sd); } }