コード例 #1
0
int
main (int argc, char **argv)
{
  ServoInf *servo;		// servo level interface
  UsarsimInf *usarsim;		// usarsim interface
  void *rosTask = NULL;
  // init ros
  ros::init (argc, argv, "usarsim");
  //  ros::Rate r(60);

  servo = new ServoInf ();
  usarsim = new UsarsimInf ();

  // this code uses the ULAPI library to provide portability
  // between different operating systems and architectures
  if (ULAPI_OK != ulapi_init (UL_USE_DEFAULT))
    {
      ROS_FATAL ("can't initialize ulapi");
      return 1;
    }

  // initialize the ROS interface wrapper
  servo->init (usarsim);

  // initialize the USARSim interface wrapper
  usarsim->init (servo);

  rosTask = ulapi_task_new ();

  ulapi_task_start (rosTask, rosThread, (void *) servo, ulapi_prio_lowest (),
		    1);

  // main loop
  while ((usarsim->getNH ())->ok ())
    {
      if (usarsim->msgIn () != 1)
	{
	  ROS_ERROR ("Error from usarsimInf, exiting");
	  break;
	}
    }
  ulapi_exit ();
}
コード例 #2
0
int main(int argc, char *argv[])
{
  int option;
  ulapi_integer port = SAMPLE_APP_DEFAULT_PORT;
  enum {BUFFERLEN = 256};
  char host[BUFFERLEN] = "localhost";
  ulapi_integer debug = 0;
  ulapi_integer client_id;
  ulapi_task_struct *client_task;
  client_args *client_args_ptr;
  client_db_struct client_db;
  char buffer[BUFFERLEN];
  char *ptr;
  int number;

  ulapi_opterr = 0;

  for (;;) {
    option = ulapi_getopt(argc, argv, ":p:h:d");
    if (option == -1)
      break;

    switch (option) {
    case 'p':
      port = atoi(ulapi_optarg);
      break;

    case 'h':
      strncpy(host, ulapi_optarg, sizeof(host));
      host[sizeof(host) - 1] = 0;
      break;

    case 'd':
      debug = 1;
      break;

    case ':':
      fprintf(stderr, "missing value for -%c\n", ulapi_optopt);
      return 1;
      break;

    default:			/* '?' */
      fprintf(stderr, "unrecognized option -%c\n", ulapi_optopt);
      return 1;
      break;
    }
  }
  if (ulapi_optind < argc) {
    fprintf(stderr, "extra non-option characters: %s\n", argv[ulapi_optind]);
    return 1;
  }

  if (ULAPI_OK != ulapi_init()) {
    fprintf(stderr, "ulapi_init error\n");
    return 1;
  }

  if (debug) ulapi_set_debug(ULAPI_DEBUG_ALL);

  if (0 != sample_app_init()) {
    fprintf(stderr, "can't init the sample app\n");
    return 1;
  }

  client_id = ulapi_socket_get_client_id(port, host);
  if (client_id < 0) {
    fprintf(stderr, "can't connect to port %d\n", (int) port);
    ulapi_exit();
    return 1;
  }
  if (debug) {
    printf("serving port %d\n", (int) port);
  }

  client_db.mutex = ulapi_mutex_new(0);
  client_db.number = 0;

  client_task = ulapi_task_new();
  client_args_ptr = reinterpret_cast<client_args *>(malloc(sizeof(client_args)));
  client_args_ptr->client_task = client_task;
  client_args_ptr->client_id = client_id;
  client_args_ptr->client_db_ptr = &client_db;
  client_args_ptr->debug = debug;
  ulapi_task_start(client_task, client_code, client_args_ptr, ulapi_prio_lowest(), 0);

  /* enter application main loop */
  while (!feof(stdin)) {

    if (NULL == fgets(buffer, sizeof(buffer), stdin)) {
      break;
    }

    ptr = buffer;
    while (isspace(*ptr)) ptr++;

    if ('q' == *ptr) break;

    if (0 == *ptr) {
      ulapi_mutex_take(client_db.mutex);
      number = client_db.number;
      ulapi_mutex_give(client_db.mutex);
      printf("%d\n", number);
      continue;
    }

    if (1 == sscanf(ptr, "%d", &number)) {
      ulapi_mutex_take(client_db.mutex);
      client_db.number = number;
      ulapi_mutex_give(client_db.mutex);
      continue;
    }
  }

  ulapi_socket_close(client_id);

  ulapi_exit();

  sample_app_exit();

  return 0;
}