void SetUp()
{
while (!IsNodeReady())
{
ros::spinOnce();
}
}
/*
* AddTask2Queue
*/
int AddTask2Queue(Queue ReadyQ, int size) {
int i = 0;
do {
if(IsFull(ReadyQ)){
//fprintf(stderr,"Q is Full \n");
break;
}
if (IsNodeReady(i) == NO || isTaskQed(i) == YES) {
// MoveDependentTask2TheFront(ReadyQ,MAX_QUEUE_TASKS,i);
i++;
continue;
}
setTaskMode(i, dfg1[i].mode);
switch (getTaskMode(i)) {
case HybSW:
case SWOnly:
Enqueue(i, ReadyQ);
taskQed(i);
break;
case HWOnly:
case HybHW:
Enqueue(i, ReadyQ);
taskQed(i);
/*TODO add some error checking here */
setTaskTypeCanRun(dfg1[i].TypeID,dfg1[i].CanRun);
#if DEBUG_PRINT
fprintf(stderr,"Enqueue %d \r\n",i);
#endif
break;
case CustomHW:
case CustomHWnSW:
default:
fprintf(stderr, "ERROR [SchedSimple] Unsupported mode[%d] "
"for task [%d] check you DFG file .. Exiting\n",
dfg1[i].mode, i);
return EXIT_FAILURE;
}
i++;
} while (i < size);
return EXIT_SUCCESS;
}
