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 ros_argc; char **ros_argv; std::string node_name(NODE_NAME_DEFAULT); int option; int ival; double dval; nist_kitting::ws_stat ws_stat_buf; ws_stat_buf.stat.period = PERIOD_DEFAULT; opterr = 0; while (true) { option = getopt(argc, argv, ":i:n:t:p:f:hd"); 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 't': dval = atof(optarg); if (dval < FLT_EPSILON) { fprintf(stderr, "bad value for period: %s\n", optarg); return 1; } ws_stat_buf.stat.period = dval; break; case 'p': planning_app = std::string(optarg); break; case 'f': plan_file = std::string(optarg); break; case 'h': print_help(); break; case 'd': debug = 1; ulapi_set_debug(ULAPI_DEBUG_ALL); break; case ':': fprintf(stderr, "missing value for -%c\n", optopt); return 1; break; default: fprintf (stderr, "unrecognized option -%c\n", optopt); return 1; break; } } if (ULAPI_OK != ulapi_init()) { fprintf(stderr, "can't init ulapi\n"); return 1; } if (inifile_name.empty()) { fprintf(stderr, "no ini file provided\n"); return 1; } if (0 != ini_load(inifile_name, planning_app, plan_file)) { fprintf(stderr, "error reading ini file %s\n", inifile_name.c_str()); return 1; } if (planning_app.empty()) { fprintf(stderr, "no planning application provided\n"); return 1; } if (plan_file.empty()) { fprintf(stderr, "no plan file provided\n"); return 1; } if (debug) { ROS_INFO("Using planning application '%s', plan file '%s'\n", planning_app.c_str(), plan_file.c_str()); } // 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 ws_cmd_sub; ros::Subscriber emove_stat_sub; ros::Publisher ws_stat_pub; ros::Publisher emove_cmd_pub; ros::Rate loop_rate(1.0 / ws_stat_buf.stat.period); ws_cmd_sub = nh.subscribe(KITTING_WS_CMD_TOPIC, TOPIC_QUEUE_LEN, ws_cmd_callback); emove_stat_sub = nh.subscribe(KITTING_EMOVE_STAT_TOPIC, TOPIC_QUEUE_LEN, emove_stat_callback); ws_stat_pub = nh.advertise<nist_kitting::ws_stat>(KITTING_WS_STAT_TOPIC, TOPIC_QUEUE_LEN); emove_cmd_pub = nh.advertise<nist_kitting::emove_cmd>(KITTING_EMOVE_CMD_TOPIC, TOPIC_QUEUE_LEN); // stuff a NOP command ws_stat_buf.stat.type = ws_cmd_buf.cmd.type = KITTING_NOP; ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number = 0; ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND; ws_stat_buf.stat.status = RCS_STATUS_EXEC; ws_stat_buf.stat.heartbeat = 0; signal(SIGINT, quit); double start, end, last_start = ulapi_time() - ws_stat_buf.stat.period; while (true) { ros::spinOnce(); start = ulapi_time(); ws_stat_buf.stat.cycle = start - last_start; last_start = start; if (ws_stat_buf.stat.serial_number != ws_cmd_buf.cmd.serial_number) { ws_stat_buf.stat.type = ws_cmd_buf.cmd.type; ws_stat_buf.stat.serial_number = ws_cmd_buf.cmd.serial_number; ws_stat_buf.stat.state = RCS_STATE_NEW_COMMAND; ws_stat_buf.stat.status = RCS_STATUS_EXEC; if (NULL != planning_process) { ulapi_process_stop(planning_process); ulapi_process_delete(planning_process); planning_process = NULL; } } switch (ws_cmd_buf.cmd.type) { case KITTING_NOP: do_cmd_kitting_nop(ws_stat_buf); break; case KITTING_INIT: do_cmd_kitting_init(ws_stat_buf, emove_cmd_pub, emove_stat_buf); break; case KITTING_HALT: do_cmd_halt(ws_stat_buf); break; case KITTING_WS_ASSEMBLE_KIT: do_cmd_kitting_ws_assemble_kit(ws_cmd_buf.assemble_kit, ws_stat_buf, emove_cmd_pub, emove_stat_buf); break; default: // unrecognized command -- FIXME break; } ws_stat_buf.stat.heartbeat++; end = ulapi_time(); ws_stat_buf.stat.duration = ulapi_time() - start; ws_stat_pub.publish(ws_stat_buf); loop_rate.sleep(); } return 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; }
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; }
int main(int argc, char *argv[]) { bool done; int option; int cmdConnection; int script = 1; char msgOut[buffSize]; RCS_TIMER *cycleBlock = new RCS_TIMER(KUKA_DEFAULT_CYCLE); CRCLStatus status; ulapi_task_struct statusTask; RobotStatus robotStatus; GripperStatus gripperStatus; bool useFile = false; char *fileName; std::ifstream inFile; std::istream *inputSource; float counter = 0; debug = 1; while (true) { option = getopt(argc, argv, ":cdf:"); if (option == -1) break; switch (option) { case 'd': debug = 1; break; case 'c': script = 0; break; case 'f': script = 0; useFile = true; fileName = optarg; 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 // this code uses the ULAPI library to provide portability // between different operating systems and architectures if (ULAPI_OK != ulapi_init()) { printf ("crclClient:: can't initialize ulapi"); return 1; } cmdConnection = ulapi_socket_get_client_id(CRCL_CMD_PORT_DEFAULT, "localhost"); if(cmdConnection < 0) return -1; // start status thread ulapi_task_init(&statusTask); ulapi_task_start(&statusTask, reinterpret_cast<ulapi_task_code>(statusThread), reinterpret_cast<void*>(&status), ulapi_prio_lowest(), 0); if(!script) { if( useFile ) { inFile.open(fileName); inputSource = &inFile; if( !inputSource->good() ) { printf( "unable to open %s\n", fileName ); exit(1); } } else inputSource = &std::cin; while(inputSource->good()) { inputSource->getline(msgOut, buffSize); if( msgOut[0] == ' ' || msgOut[0] == '/' ) continue; ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut)); printf( "Sent: %s\n", msgOut); done = false; if(useFile) { for(int ii=0; ii<5; ii++) cycleBlock->wait(); } while( !done ) { cycleBlock->wait(); gripperStatus = status.getGripperStatus(); robotStatus = status.getRobotStatus(); if( !useFile || status.getCurrentStatus()==CRCL_DONE ) done = true; // printf( "Fmod: %f\n", fmod(counter, 100.)); if( fmod(counter++, 100.)==0 || done ) { printf( "Status Cmd: %s Status: %s\n", getCRCLCmdString((status.getCurrentCmd()).cmd).c_str(), getCRCLStatusString(status.getCurrentStatus()).c_str()); printf( "\x1b[32mCart Status: <%3.4f, %3.4f, %3.4f> <%3.4f, %3.4f, %3.4f>\x1b[0m\n", robotStatus.pose.x, robotStatus.pose.y, robotStatus.pose.z, robotStatus.pose.xrot, robotStatus.pose.yrot, robotStatus.pose.zrot); printf( "\x1b[32mJoint Status: <%4.2f %4.2f %4.2f %4.2f %4.2f %4.2f>\x1b[0m\n", robotStatus.joint[0], robotStatus.joint[1], robotStatus.joint[2], robotStatus.joint[3], robotStatus.joint[4], robotStatus.joint[5]); } } } } for(int i=0; i<1; i++ ) { ulapi_strncpy(msgOut, "InitCanon", strlen("InitCanon")); ulapi_socket_write(cmdConnection, msgOut, strlen(msgOut)); /* printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "MoveTo 1. 2. 3. 4. 5. 6."; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "Dwell 1"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "MoveTo 7. 8. 9. 10. 11. 12."; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "Dwell 4"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); msgOut = "EndCanon"; ulapi_socket_write(cmdConnection, msgOut.c_str(), msgOut.length()); printf( "crclClient writing: %s\n", msgOut.c_str()); sleep(2); */ cycleBlock->wait(); } }