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