static int open_device(struct olimex_transport *tr, struct usb_device *dev) { struct usb_config_descriptor *c = &dev->config[0]; int i; for (i = 0; i < c->bNumInterfaces; i++) { struct usb_interface *intf = &c->interface[i]; struct usb_interface_descriptor *desc = &intf->altsetting[0]; if (desc->bInterfaceClass == V1_INTERFACE_CLASS && !open_interface(tr, dev, desc->bInterfaceNumber)) { printc_dbg("olimex: rev 1 device\n"); tr->in_ep = V1_IN_EP; tr->out_ep = V1_OUT_EP; return 0; } else if (desc->bInterfaceClass == V2_INTERFACE_CLASS && !open_interface(tr, dev, desc->bInterfaceNumber)) { printc_dbg("olimex: rev 2 device\n"); tr->in_ep = V2_IN_EP; tr->out_ep = V2_OUT_EP; return 0; } } return -1; }
/* Parse a servers part of config */ void parse_servers_line(char *buf) { char *p, *n; int inum; p = buf; strsep(&p, " \t"); if (!open_server(buf)) { logd(LOG_WARNING, "Can't open server %s. Ignored.", buf); return; } inum = 0; while ((n = strsep(&p, " \t")) != NULL) { if (*n != '\0') { if (open_interface(n)) inum++; else { logd(LOG_WARNING, "Interface %s does not exist. Ignored.", n); } } } /* We found no interfaces for listening on this computer */ if (inum == 0) { srv_num--; free(servers[srv_num]); } }
/* Open all configured ports on all interfaces. */ static unsigned open_all_ports(struct lshd_context *ctx, struct resource_list *resources, struct object_queue *interfaces, struct string_queue *ports) { /* FIXME: Move this default behaviour to lshd_config_handler where other defaults are set up. */ if (object_queue_is_empty (interfaces)) return open_interface(ctx, resources, NULL, ports); else { unsigned done = 0; FOR_OBJECT_QUEUE(interfaces, o) { CAST(lshd_interface, interface, o); done += open_interface(ctx, resources, interface, ports); } return done; }
static int open_device(struct olimex_transport *tr, struct usb_device *dev) { struct usb_config_descriptor *c = &dev->config[0]; int i; for (i = 0; i < c->bNumInterfaces; i++) { struct usb_interface *intf = &c->interface[i]; struct usb_interface_descriptor *desc = &intf->altsetting[0]; if (desc->bInterfaceClass == USB_FET_INTERFACE_CLASS && !open_interface(tr, dev, desc->bInterfaceNumber)) return 0; } return -1; }
int main(int argc, char *argv[]) { int c, i, j, configured = 0; pid_t opid; char prgname[80], filename[256], *p; struct servent *servent; pthread_t tid; /* Default plugin_base */ strlcpy(plugin_base, PLUGIN_PATH, sizeof(plugin_base)); if ((servent = getservbyname("bootps", 0)) == NULL) errx(EX_UNAVAILABLE, "getservbyname(bootps)"); bootps_port = servent->s_port; if ((servent = getservbyname("bootpc", 0)) == NULL) errx(EX_UNAVAILABLE, "getservbyname(bootpc)"); bootpc_port = servent->s_port; openlog("dhcprelya", LOG_NDELAY | LOG_PID, LOG_DAEMON); strlcpy(prgname, argv[0], sizeof(prgname)); filename[0] = '\0'; while ((c = getopt(argc, argv, "A:c:df:hi:p:")) != -1) { switch (c) { case 'A': if (configured == 2) errx(1, "Either config file or command line options allowed. Not both."); max_packet_size = strtol(optarg, NULL, 10); if (max_packet_size < 300 || max_packet_size > DHCP_MTU_MAX) errx(1, "Wrong packet size"); break; case 'c': if (configured == 2) errx(1, "Either config file or command line options allowed. Not both."); max_hops = strtol(optarg, NULL, 10); if (max_hops < 1 || max_hops > 16) errx(1, "Wrong hops number"); break; case 'd': debug++; break; case 'f': if (configured == 1) errx(1, "Either config file or command line options allowed. Not both."); if (configured == 2) errx(1, "only one config file allowed"); configured = 2; read_config(optarg); break; case 'i': if (configured == 2) errx(1, "Either config file or command line options allowed. Not both."); configured = 1; if (!open_interface(optarg)) logd(LOG_DEBUG, "Interface %s does not exist. Ignored.", optarg); break; case 'p': strlcpy(filename, optarg, sizeof(filename)); break; case 'h': default: usage(prgname); } } argc -= optind; argv += optind; if (optind == 0) argc--; if ((configured == 1 && argc < 1) || (configured == 2 && argc >= 1)) usage(prgname); /* Initialize polugins */ for (i = 0; i < plugins_number; i++) { if (plugins[i]->init) if ((plugins[i]->init) (options_heads[i]) == 0) errx(1, "Can't initialize a plugin %s\n", plugins[i]->name); } for (i = 0; i < argc; i++) { open_server(argv[i]); for (j = 0; j < if_num; j++) { if (i > ifs[j]->srv_num - 1) { ifs[j]->srv_num++; ifs[j]->srvrs = realloc(ifs[j]->srvrs, ifs[j]->srv_num * sizeof(int)); } ifs[j]->srvrs[ifs[j]->srv_num - 1] = i; } } if (if_num == 0) errx(1, "No interfaces found to listen. Exiting."); logd(LOG_WARNING, "Total interfaces: %d", if_num); /* Make a PID filename */ if (filename[0] == '\0') { strlcpy(filename, "/var/run/", sizeof(filename)); p = strrchr(prgname, '/'); if (p == NULL) p = prgname; else p++; strlcat(filename, p, sizeof(filename)); strlcat(filename, ".pid", sizeof(filename)); } /* Create a PID file and daemonize if no debug flag */ if (!debug && (pfh = pidfile_open(filename, 0644, &opid)) == NULL) { if (errno == EEXIST) errx(1, "Already run with PID %lu. Exiting.", (unsigned long)opid); errx(1, "Can't create PID file"); } signal(SIGHUP, SIG_IGN); if (!debug) { if (daemon(0, 0) == -1) process_error(1, "Can't daemonize. Exiting."); else logd(LOG_DEBUG, "Runned as %d", getpid()); } if (pfh) pidfile_write(pfh); STAILQ_INIT(&q_head); pthread_mutex_init(&queue_lock, NULL); /* Create listeners for every interface */ for (i = 0; i < if_num; i++) { pthread_create(&tid, NULL, listener, ifs[i]); pthread_detach(tid); } /* Main loop */ while (1) { if (queue_size > 0) process_queue(); /* Process one packet from server(s) */ process_server_answer(); } /* Destroy polugins */ for (i = 0; i < plugins_number; i++) { if (plugins[i]->destroy) (plugins[i]->destroy) (); } pthread_mutex_destroy(&queue_lock); }
int main(int argc, char* const argv[]) { memset(&config, 0, sizeof(config)); const char *pidfile = "/var/run/6relayd.pid"; bool daemonize = false; int verbosity = 0; int c; while ((c = getopt(argc, argv, "ASR:D:Nsucn::l:a:rt:m:oi:p:dvh")) != -1) { switch (c) { case 'A': config.enable_router_discovery_relay = true; config.enable_dhcpv6_relay = true; config.enable_ndp_relay = true; config.send_router_solicitation = true; config.enable_route_learning = true; break; case 'S': config.enable_router_discovery_relay = true; config.enable_router_discovery_server = true; config.enable_dhcpv6_relay = true; config.enable_dhcpv6_server = true; break; case 'R': config.enable_router_discovery_relay = true; if (!strcmp(optarg, "server")) config.enable_router_discovery_server = true; else if (strcmp(optarg, "relay")) return print_usage(argv[0]); break; case 'D': config.enable_dhcpv6_relay = true; if (!strcmp(optarg, "server")) config.enable_dhcpv6_server = true; else if (strcmp(optarg, "relay")) return print_usage(argv[0]); break; case 'N': config.enable_ndp_relay = true; break; case 's': config.send_router_solicitation = true; break; case 'u': config.always_announce_default_router = true; break; case 'c': config.deprecate_ula_if_public_avail = true; break; case 'n': config.always_rewrite_dns = true; if (optarg) inet_pton(AF_INET6, optarg, &config.dnsaddr); break; case 'l': config.dhcpv6_statefile = strtok(optarg, ","); if (config.dhcpv6_statefile) config.dhcpv6_cb = strtok(NULL, ","); break; case 'a': config.dhcpv6_lease = realloc(config.dhcpv6_lease, sizeof(char*) * ++config.dhcpv6_lease_len); config.dhcpv6_lease[config.dhcpv6_lease_len - 1] = optarg; break; case 'r': config.enable_route_learning = true; break; case 't': config.static_ndp = realloc(config.static_ndp, sizeof(char*) * ++config.static_ndp_len); config.static_ndp[config.static_ndp_len - 1] = optarg; break; case 'm': config.ra_managed_mode = atoi(optarg); break; case 'o': config.ra_not_onlink = true; break; case 'i': if (!strcmp(optarg, "low")) config.ra_preference = -1; else if (!strcmp(optarg, "high")) config.ra_preference = 1; break; case 'p': pidfile = optarg; break; case 'd': daemonize = true; break; case 'v': verbosity++; break; default: return print_usage(argv[0]); } } openlog("6relayd", LOG_PERROR | LOG_PID, LOG_DAEMON); if (verbosity == 0) setlogmask(LOG_UPTO(LOG_WARNING)); else if (verbosity == 1) setlogmask(LOG_UPTO(LOG_INFO)); if (argc - optind < 1) return print_usage(argv[0]); if (getuid() != 0) { syslog(LOG_ERR, "Must be run as root. stopped."); return 2; } #if defined(__NR_epoll_create1) && defined(EPOLL_CLOEXEC) epoll = epoll_create1(EPOLL_CLOEXEC); #else epoll = epoll_create(32); epoll = fflags(epoll, O_CLOEXEC); #endif if (epoll < 0) { syslog(LOG_ERR, "Unable to open epoll: %s", strerror(errno)); return 2; } #ifdef SOCK_CLOEXEC ioctl_sock = socket(AF_INET6, SOCK_DGRAM | SOCK_CLOEXEC, 0); #else ioctl_sock = socket(AF_INET6, SOCK_DGRAM, 0); ioctl_sock = fflags(ioctl_sock, O_CLOEXEC); #endif if ((rtnl_socket = relayd_open_rtnl_socket()) < 0) { syslog(LOG_ERR, "Unable to open socket: %s", strerror(errno)); return 2; } if (open_interface(&config.master, argv[optind++], false)) return 3; config.slavecount = argc - optind; config.slaves = calloc(config.slavecount, sizeof(*config.slaves)); for (size_t i = 0; i < config.slavecount; ++i) { const char *name = argv[optind + i]; bool external = (name[0] == '~'); if (external) ++name; if (open_interface(&config.slaves[i], name, external)) return 3; } if ((urandom_fd = open("/dev/urandom", O_RDONLY | O_CLOEXEC)) < 0) return 4; struct sigaction sa = {.sa_handler = SIG_IGN}; sigaction(SIGUSR1, &sa, NULL); if (init_router_discovery_relay(&config)) return 4; if (init_dhcpv6_relay(&config)) return 4; if (init_ndp_proxy(&config)) return 4; if (epoll_registered == 0) { syslog(LOG_WARNING, "No relays enabled or no slave " "interfaces specified. stopped."); return 5; } if (daemonize) { openlog("6relayd", LOG_PID, LOG_DAEMON); // Disable LOG_PERROR if (daemon(0, 0)) { syslog(LOG_ERR, "Failed to daemonize: %s", strerror(errno)); return 6; } FILE *fp = fopen(pidfile, "w"); if (fp) { fprintf(fp, "%i\n", getpid()); fclose(fp); } } signal(SIGTERM, set_stop); signal(SIGHUP, set_stop); signal(SIGINT, set_stop); signal(SIGCHLD, wait_child); // Main loop while (!do_stop) { struct epoll_event ev[16]; int len = epoll_wait(epoll, ev, 16, -1); for (int i = 0; i < len; ++i) { struct relayd_event *event = ev[i].data.ptr; if (event->handle_event) event->handle_event(event); else if (event->handle_dgram) relayd_receive_packets(event); } } syslog(LOG_WARNING, "Termination requested by signal."); deinit_ndp_proxy(); deinit_router_discovery_relay(); free(config.slaves); close(urandom_fd); return 0; }
int main(int argc, char **argv) { printf("ur5 Server Start\n"); // JointVector tcp_home= {0.0, -191.0, 600.0, 0.0, -2.2211, -2.2211}; JointVector joint_home_rad={0.0000, -1.5708, 0.0000, -1.5708, 0.0000, 0.0000}; JointVector joint_home={0.0000, -90, 0.0000, -90, 0.0000, 0.0000}; JointVector init_two ={-6.5018, -95.3469, -29.8123, -194.7758, -59.0822, -62.3032}; JointVector init_right_side = {0.309199, -1.240009, 0.323497, -1.039543, 0.541075, 0.486453}; JointVector initialP_rad = {-0.330294, -1.881878, -0.290919, -2.243194, -0.660115, -0.704284}; JointVector viereck[4] = {{43.24, -140.88, -30.05, -12.83, 129.28, -0.27}, {130.67, -140.87, -30.05, -12.75, 44.08, -0.27}, {130.67, -187.90, 6.46, -1.22, 43.35, -0.27}, {43.24, -187.90, -30.05, 3.87, 100.32, -0.27} }; JointVector PosOne= {36.12, -112.88, -8.78, -24.53, 88.39, 16.64}; int i,h,n=0; for(i=0; i<4;i++){ for(h=0;h<6;h++){ viereck[i][h]= deg_to_rad(viereck[i][h]); } } for(i=0;i<MSG_BUFFER_SIZE;i++) msg_buffer[i].text = msg_text_buffers[i]; // JointVector initialP={-30.4347, -132.2621, -40.8718, -149.8236, -59.1151, -62.2781}; // int i; // printf("joint_home ={"); // for(i=0; i<6; i++){ // joint_home_rad[i]= deg_to_rad(joint_home[i]); // joint_home[i] = rad_to_deg(joint_home_rad[i]); // printf("%3.4f%s",joint_home_rad[i], i<5 ? ", " : "};"); //// } // MovePTPPacket move_ptp_packet; for(i=0; i<6; i++){ PosOne[i]= deg_to_rad(PosOne[i]);} // move_ptp(joint_home_rad, PosOne , &move_ptp_packet); // return 0; struct connection_data server; fd_set masterfds, readfds; struct timeval timeout,send_time_start, send_time_end; int rc; pthread_t tcp_server_thread; MovePTPPacket move_ptp_packet; server.initialize_direction = -1.0; read_args(argc, argv, &server); initialize_direction = server.initialize_direction; printf("initialize direction: %f", initialize_direction); rc = pthread_create(&tcp_server_thread, NULL, &start_tcp_server, &server); if(rc < 0){ quit_program=true; } int connection_timeout_count=0; pva_packet.header.protocol_id=PVA_PACKET_ID; p_packet.header.protocol_id=PPACKET_ID; //--------------------------- Init Robot ------------------------------------------------------- configuration_load(); open_interface(); power_up(); set_wrench(); if(!initialize(0)){ puts("could not initialize robot"); exit(EXIT_FAILURE); } settle(); setup_sigint(); setup_sigpipe(); memcpy(&last_pva_packet, &pva_packet,sizeof(PVAPacket)); for(i=0;i<6;i++) last_pva_packet.acceleration[i] = 0.0; for(i=0;i<6;i++) last_pva_packet.velocity[i] = 0.0; bool timeout_flag=false; char *buff[sizeof(PVAPacket)]; //------------------------------------------------------------------------------------------------------ //------------------------------------------------------------------------------------------------------ // for(pva_packet.header.cycle_number =0; quit_program == false; pva_packet.header.cycle_number++) { // robotinterface_read_state_blocking(); // pthread_mutex_lock(&connect_mutex); // if(connection_timeout_count > TIMEOUT_TOLERANCE){ // connection_timeout_count=0; // connected=false; // } // //--------------------------- Stuff roboter does ------------------------------------------------------- // robotinterface_get_actual(servo_packet.position, servo_packet.velocity); // //------------------------------------------------------------------------------------------------------ // //------------------------------------------------------------------------------------------------------ // //---------------------------- send ------------------------------------------------------------------- // if(connected){ // gettimeofday(&send_time_start,0); // if(send(client_fd, (char*) &pva_packet , sizeof(pva_packet),0) < 1){ // connected=false; // } // } // //------------------------------------------------------------------------------------------------------ // //------------------------------------------------------------------------------------------------------ // //---------------------------- recieve ----------------------------------------------------------------- // timeout_flag=false; // if(connected){ // FD_ZERO(&masterfds); // FD_SET(client_fd, &masterfds); // memcpy(&readfds, &masterfds, sizeof(fd_set)); // gettimeofday(&send_time_end,0); // timeval_diff(&timeout, &send_time_start, &send_time_end); // timeout.tv_usec = 6000 - timeout.tv_usec; // if (select(client_fd +1, &readfds, NULL, NULL, &timeout) < 0) { // pthread_mutex_lock(&connect_mutex); // connected=false; // pthread_mutex_unlock(&connect_mutex); // } // if(FD_ISSET(client_fd, &readfds)){ // n=recv(client_fd, buff, sizeof(buff), 0); // if (n < 1 || n != sizeof(PVAPacket)){ // pthread_mutex_lock(&connect_mutex); // connected=false; // pthread_mutex_unlock(&connect_mutex); // } // }else{ // connection_timeout_count++; // if(connection_timeout_count< TIMEOUT_TOLERANCE) // timeout_flag=true; // } // if(!timeout_flag && connected){ // memcpy(&pva_packet, buff, n); // if(connection_timeout_count) connection_timeout_count--; // } // } // //------------------------------------------------------------------------------------------------------ // //------------------------------------------------------------------------------------------------------ // //---------------------------- do stuff with data ------------------------------------------------------ // // TODO check for errors----- // //------------------------------------------------------------------------------------------------------ // if(!timeout_flag && connected){ // // save pva to last if something went wrong; // memcpy(&last_pva_packet, &pva_packet, sizeof(PVAPacket); // } // //------------------------------------------------------------------------------------------------------ // //------------------------------------------------------------------------------------------------------ // //--------------------------- Stuff roboter does ------------------------------------------------------- // if(connected){ // if(!timeout){ // robotinterface_command_position_velocity_acceleration(pva_packet.position, // pva_packet.velocity, // pva_packet.acceleration); // }else{ //timeout // if(connection_timeout_count<3){ // connected=false; // robotinterface_command_velocity(zero_vector); // } // // cause of timeout we take the last pva_packet // robotinterface_command_position_velocity_acceleration(last_pva_packet.position, // last_pva_packet.velocity, // last_pva_packet.acceleration); // } // }else{ // robotinterface_command_velocity(zero_vector); // } // pthread_mutex_unlock(&connect_mutex); // robotinterface_send(); // //------------------------------------------------------------------------------------------------------ // //------------------------------------------------------------------------------------------------------ // } // // ---- quit programm printf("shutdown robot...\n"); //---- shutdown robot --------- // move to home ---- move_to_position(viereck[0]); // move_to_position(joint_home_rad); // move_to_position(PosOne); // --- puts("- Speeding down"); int j; for (j = 0; j < 10; j++) { robotinterface_read_state_blocking(); robotinterface_command_velocity(zero_vector); robotinterface_send(); } puts("close robotinterface"); robotinterface_close(); puts("Done!"); printf("shutdown program\n"); return 0; }