static int 
init_rosrt_test_task(void)
{

  int j, i;
  int ans;
  static int firsttime = TRUE;
ros::init(a,null,"ros_rt_test_subscriber");
init_ros();


  if (firsttime){
    firsttime = FALSE;
    freq = 0.1; // frequency
    amp  = 0.5; // amplitude
  }

  // prepare going to the default posture
  bzero((char *)&(target[1]),N_DOFS*sizeof(target[1]));
  for (i=1; i<=N_DOFS; i++)
    target[i] = joint_default_state[i];

  // go to the target using inverse dynamics (ID)
  if (!go_target_wait_ID(target)) 
    return FALSE;

  // ready to go
  ans = 999;
  while (ans == 999) {
    if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans))
      return FALSE;
  }
  
  // only go when user really types the right thing
  if (ans != 1) 
    return FALSE;

  start_time = task_servo_time;
  printf("start time = %.3f, task_servo_time = %.3f\n", 
	 start_time, task_servo_time);

  return TRUE;
}
Ejemplo n.º 2
0
void setup(){
	init_ros();
	init_imu();
	init_controller();
	init_encoder();
}
Ejemplo n.º 3
0
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();
}