void
PrtEnqueueInOrder(
_In_ PRT_VALUE *source,
_In_ PRT_INT64 seqNum,
_Inout_ PRT_MACHINEINST_PRIV *context,
_In_ PRT_VALUE *event,
_In_ PRT_VALUE *payload
)
{
// Check if the enqueued event is in order
if (context->isHalted)
return;
PrtLockMutex(context->stateMachineLock);
if (PrtMapExists(context->recvMap, source) && PrtMapGet(context->recvMap, source)->valueUnion.nt >= seqNum)
{
PrtUnlockMutex(context->stateMachineLock);
// Drop the event
return;
}
else
{
PrtMapUpdate(context->recvMap, source, PrtMkIntValue((PRT_INT32)seqNum));
}
PrtUnlockMutex(context->stateMachineLock);
PrtSendPrivate(context, event, payload);
}
PRT_VALUE *P_FUN_GetSystemTime_IMPL(PRT_MACHINEINST *context)
{
PRT_MACHINEINST_PRIV *p_tmp_mach_priv = (PRT_MACHINEINST_PRIV *)context;
PRT_FUNSTACK_INFO p_tmp_frame;
PrtPopFrame(p_tmp_mach_priv, &p_tmp_frame);
int ms = GetTickCount();
PrtFreeLocals(p_tmp_mach_priv, &p_tmp_frame);
return PrtMkIntValue(ms);
}
int main(int argc, char *argv[])
{
#ifndef PRT_PLAT_WINUSER
init_ros("test_motion_planner", &argc, argv);
#endif
if (!ParseCommandLine(argc, argv))
{
PrintUsage();
return 1;
}
const char* motion_planner_delta_s = getenv("MOTION_PLANNER_DELTA");
if(motion_planner_delta_s) {
Delta = atoi(motion_planner_delta_s);
printf("Using MOTION_PLANNER_DELTA = %d\n", Delta);
}
printf("Press any key to start simulation\n");
getchar();
PRT_DBG_START_MEM_BALANCED_REGION
{
PRT_PROCESS *process;
PRT_GUID processGuid;
PRT_VALUE *payload;
//Initialize the workspace
WORKSPACE_INFO = ParseWorkspaceConfig(workspaceConfig);
#ifdef USE_DIJKSTRA_PRECOMPUTATION
WS_LocationsList ends;
ends.size = WORKSPACE_INFO->starts.size + WORKSPACE_INFO->ends.size;
ends.locations = malloc(sizeof(WS_Coord) * ends.size);
int count = 0;
for(int i=0; i < WORKSPACE_INFO->starts.size; i++) {
ends.locations[count++] = WORKSPACE_INFO->starts.locations[i];
}
for(int i=0; i < WORKSPACE_INFO->ends.size; i++) {
ends.locations[count++] = WORKSPACE_INFO->ends.locations[i];
}
PreComputeObstacleDistanceH(WORKSPACE_INFO->dimension, WORKSPACE_INFO->obstacles, ends);
#endif
processGuid.data1 = 1;
processGuid.data2 = 0;
processGuid.data3 = 0;
processGuid.data4 = 0;
process = PrtStartProcess(processGuid, &P_GEND_PROGRAM, ErrorHandler, Log);
if (cooperative)
{
PrtSetSchedulingPolicy(process, PRT_SCHEDULINGPOLICY_COOPERATIVE);
}
if (parg == NULL)
{
payload = PrtMkNullValue();
}
else
{
int i = atoi(parg);
payload = PrtMkIntValue(i);
}
PrtUpdateAssertFn(MyAssert);
PrtMkMachine(process, P_MACHINE_Main, payload);
if (cooperative)
{
// test some multithreading across state machines.
#if defined(PRT_PLAT_WINUSER)
HANDLE* threadsArr = (HANDLE*)PrtMalloc(threads*sizeof(HANDLE));
for (int i = 0; i < threads; i++)
{
DWORD threadId;
threadsArr[i] = CreateThread(NULL, 16000, (LPTHREAD_START_ROUTINE)RunToIdle, process, 0, &threadId);
}
WaitForMultipleObjects(threads, threadsArr, TRUE, INFINITE);
PrtFree(threadsArr);
#elif defined(PRT_PLAT_LINUXUSER)
typedef void *(*start_routine) (void *);
pthread_t tid[threads];
for (int i = 0; i < threads; i++)
{
pthread_create(&tid[i], NULL, (start_routine)RunToIdle, (void*)process);
}
for (int i = 0; i < threads; i++)
{
pthread_join(tid[i], NULL);
}
#else
#error Invalid Platform
#endif
}
PrtFreeValue(payload);
PrtStopProcess(process);
}
PRT_DBG_END_MEM_BALANCED_REGION
//_CrtSetReportMode(_CRT_ERROR, _CRTDBG_MODE_DEBUG);
//_CrtDumpMemoryLeaks();
}
