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