static void request_server_thread_code(request_server_thread_args *args) { int id; int connection_id; ulapi_task_struct *request_connection_thread; request_connection_thread_args *request_connection_args; id = args->id; while (true) { if (debug) printf("waiting for a trajectory point connection...\n"); connection_id = ulapi_socket_get_connection_id(id); if (connection_id < 0) { fprintf(stderr, "can't get a trajectory point connecton\n"); break; } if (debug) printf("got a trajectory point connection on id %d\n", connection_id); // spawn a connection thread request_connection_thread = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*request_connection_thread))); request_connection_args = reinterpret_cast<request_connection_thread_args *>(malloc(sizeof(*request_connection_args))); ulapi_task_init(request_connection_thread); request_connection_args->id = connection_id; request_connection_args->thread = request_connection_thread; ulapi_task_start(request_connection_thread, reinterpret_cast<ulapi_task_code>(request_connection_thread_code), reinterpret_cast<void *>(request_connection_args), ulapi_prio_highest(), 0); } // while (true) ulapi_socket_close(id); if (debug) printf("server on %d done\n", id); return; }
static void emove_server_task_code(emove_server_task_args *args) { ulapi_task_struct *task; int id; int period_nsecs; ulapi_task_struct *emove_client_write_task; emove_client_write_task_args *emove_client_write_args; int emove_connection_id; task = args->task; id = args->id; period_nsecs = args->period_nsecs; bool done = false; while (! done) { if (debug) printf("waiting for an Emove HMI connection on %d...\n", id); emove_connection_id = ulapi_socket_get_connection_id(id); if (emove_connection_id < 0) { fprintf(stderr, "can't get an Emove HMI connection\n"); break; } if (debug) printf("got an Emove HMI connection on id %d\n", emove_connection_id); // spawn connection tasks for reading and writing emove_client_write_task = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*emove_client_write_task))); emove_client_write_args = reinterpret_cast<emove_client_write_task_args *>(malloc(sizeof(*emove_client_write_args))); ulapi_task_init(emove_client_write_task); emove_client_write_args->task = emove_client_write_task; emove_client_write_args->id = emove_connection_id; emove_client_write_args->period_nsecs = period_nsecs; ulapi_task_start(emove_client_write_task, reinterpret_cast<ulapi_task_code>(emove_client_write_task_code), reinterpret_cast<void *>(emove_client_write_args), ulapi_prio_highest(), 0); } // while (true) ulapi_socket_close(id); if (debug) printf("server on %d done\n", id); ulapi_task_delete(task); return; }
rtapi_integer rtapi_socket_get_client(rtapi_integer socket_fd) { return ulapi_socket_get_connection_id(socket_fd); }
int main(int argc, char *argv[]) { int ros_argc; char **ros_argv; std::string node_name(NODE_NAME_DEFAULT); int option; int ival; double dval; int ws_port = WS_PORT_DEFAULT; int emove_port = EMOVE_PORT_DEFAULT; double period = 1; int ws_server_id; int ws_connection_id; ulapi_task_struct *ws_client_read_task; ws_client_read_task_args *ws_client_read_args; ulapi_task_struct *ws_client_write_task; ws_client_write_task_args *ws_client_write_args; int emove_server_id; ulapi_task_struct emove_server_task; emove_server_task_args emove_server_args; enum {INBUF_LEN = 1024}; char inbuf[INBUF_LEN]; opterr = 0; while (true) { option = getopt(argc, argv, ":i:n:p:t:Whd"); if (option == -1) break; switch (option) { case 'i': inifile_name = std::string(optarg); break; case 'n': // first check for valid name if (optarg[0] == '-') { fprintf(stderr, "invalid node name: %s\n", optarg); return 1; } node_name = std::string(optarg); break; case 'w': ival = atoi(optarg); ws_port = ival; break; case 'e': ival = atoi(optarg); emove_port = ival; break; case 't': dval = atof(optarg); if (dval < FLT_EPSILON) { fprintf(stderr, "bad value for period: %s\n", optarg); return 1; } period = dval; break; // FIXME -- ipad case 'W': ws_all = true; break; case 'h': print_help(); break; case 'd': debug = true; break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } // switch (option) } // while (true) for getopt if (ULAPI_OK != ulapi_init()) { fprintf(stderr, "can't init ulapi\n"); return 1; } if (! inifile_name.empty()) { if (0 != ini_load(inifile_name, &ws_port, &emove_port)) { fprintf(stderr, "error reading ini file %s\n", inifile_name.c_str()); return 1; } } ulapi_mutex_init(&ws_stat_mutex, 1); ulapi_mutex_init(&emove_stat_mutex, 1); // pass everything after a '--' separator to ROS ros_argc = argc - optind; ros_argv = &argv[optind]; ros::init(ros_argc, ros_argv, node_name); ros::NodeHandle nh; ros::Subscriber wssub; ros::Subscriber emovesub; wssub = nh.subscribe(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN, ws_stat_callback); emovesub = nh.subscribe(KITTING_EMOVE_STAT_TOPIC, TOPIC_QUEUE_LEN, emove_stat_callback); /* Run two processes, one that serves up the Workstation-level commands and status, and the other that serves up the Emove status. This main thread will handle the Workstation level. */ ws_server_id = ulapi_socket_get_server_id(ws_port); if (ws_server_id < 0) { fprintf(stderr, "can't serve WS port %d\n", ws_port); return 1; } emove_server_id = ulapi_socket_get_server_id(emove_port); if (emove_server_id < 0) { fprintf(stderr, "can't serve Emove port %d\n", emove_port); return 1; } ulapi_task_init(&emove_server_task); emove_server_args.task = &emove_server_task; emove_server_args.id = emove_server_id; emove_server_args.period_nsecs = (int) (period * 1.0e9); ulapi_task_start(&emove_server_task, reinterpret_cast<ulapi_task_code>(emove_server_task_code), reinterpret_cast<void *>(&emove_server_args), ulapi_prio_highest(), 0); bool done = false; while (! done) { if (debug) printf("waiting for a WS HMI connection on %d...\n", ws_server_id); ws_connection_id = ulapi_socket_get_connection_id(ws_server_id); if (ws_connection_id < 0) { fprintf(stderr, "can't get a WS HMI connection\n"); break; } if (debug) printf("got a WS HMI connection on id %d\n", ws_connection_id); // spawn connection tasks for reading and writing ws_client_read_task = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*ws_client_read_task))); ws_client_read_args = reinterpret_cast<ws_client_read_task_args *>(malloc(sizeof(*ws_client_read_args))); ulapi_task_init(ws_client_read_task); ws_client_read_args->task = ws_client_read_task; ws_client_read_args->id = ws_connection_id; ulapi_task_start(ws_client_read_task, reinterpret_cast<ulapi_task_code>(ws_client_read_task_code), reinterpret_cast<void *>(ws_client_read_args), ulapi_prio_highest(), 0); ws_client_write_task = reinterpret_cast<ulapi_task_struct *>(malloc(sizeof(*ws_client_write_task))); ws_client_write_args = reinterpret_cast<ws_client_write_task_args *>(malloc(sizeof(*ws_client_write_args))); ulapi_task_init(ws_client_write_task); ws_client_write_args->task = ws_client_write_task; ws_client_write_args->id = ws_connection_id; ws_client_write_args->period_nsecs = (int) (period * 1.0e9); ulapi_task_start(ws_client_write_task, reinterpret_cast<ulapi_task_code>(ws_client_write_task_code), reinterpret_cast<void *>(ws_client_write_args), ulapi_prio_highest(), 0); } // while (true) ulapi_socket_close(ws_server_id); if (debug) printf("server on %d done\n", ws_server_id); return 0; }