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