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);
}
}
void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int arg7, int arg8, int arg9, int arg10)
{
//#ifdef DX100
mpTaskDelay(10000); // 10 sec. delay to enable DX100 system to complete initialization
//#endif
//Creates and starts a new task in a seperate thread of execution.
//All arguments will be passed to the new task if the function
//prototype will accept them.
RosInitTaskID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)RosInitTask,
arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10);
//Ends the initialization task.
mpExitUsrRoot;
}
//-----------------------------------------------------------------------
// 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);
}
}
