main.cc
Go to the documentation of this file.
00001 #include "FRIComm.hh"
00002 
00003 #include "FRICheck.hh"
00004 
00005 #include "YARPComm.hh"
00006 #include "ROSComm.hh"
00007 
00008 #include <signal.h> // SIGINT signal handling
00009 
00010 #include <string.h>
00011 #include <time.h>
00012 #include <sys/time.h>
00013 #include <unistd.h>
00014 
00015 int should_exit=0;
00016 
00018 void catchsignal(int signo)
00019 {
00020   should_exit=1;
00021   printf("\n# caught Ctrl-C, exiting...\n");
00022 }
00023 
00024 
00025 // This is our setup at TUM.
00026 bool friConfigure(const char* side, FRIThread *kuka, char* yarp_prefix)
00027 {
00028   if(strcmp(side, "left") != 0 && strcmp(side, "right") != 0)
00029     return false;
00030 
00031   if(strcmp(side, "left") == 0) {
00032     strcpy(yarp_prefix, "/lwr/left/robot");
00033     kuka->configureNetwork(7001, "192.168.139.12", 7010);
00034     kuka->limitChecker()->setHandSide(FRICheck::LEFT);
00035   }
00036 
00037   if(strcmp(side, "right") == 0)
00038   {
00039     strcpy(yarp_prefix, "/lwr/right/robot");
00040     kuka->configureNetwork(7002, "192.168.139.10", 7010);
00041     kuka->limitChecker()->setHandSide(FRICheck::RIGHT);
00042   }
00043 
00044   return true;
00045 }
00046 
00047 int main(int argc, char** argv)
00048 {
00049   // install Ctrl-C handler
00050   struct sigaction act;
00051   act.sa_handler = catchsignal;
00052   act.sa_flags = 0;
00053   if ((sigemptyset(&act.sa_mask) == -1) ||
00054       (sigaction(SIGINT, &act, NULL) == -1)) {
00055     printf("# Failed to set SIGINT to handle Ctrl-C, oh well\n");
00056   }
00057 
00058   char yarp_prefix[1024] = "/lwr";
00059   bool configured = false;
00060 
00061   FRIThread kuka;
00062 
00063   // configure according to command line option
00064   if(argc >= 2)
00065     configured = friConfigure(argv[1], &kuka, yarp_prefix);
00066 
00067   // create YARP Comm object
00068   YARPComm yarp(yarp_prefix);
00069 
00070   // create ROS Comm object
00071   ros::init(argc, argv, "lwr", ros::init_options::NoSigintHandler);
00072   ROSComm ros;
00073   
00074   // configure according to ROS settings
00075   configured |= ros.configure(&kuka);
00076 
00077 
00078   // must be configured!
00079   if(!configured)
00080   {
00081     printf("usage: fri [left|right]\n");
00082     return 0;
00083   }
00084 
00085   //printf("# connecting %d -> %s:%d\n", local_port, remote_ip, remote_port);
00086   // now start FRI communication
00087   if(!kuka.start()) {
00088     printf("# FRI part failed to start, exiting before doing more harm...\n");
00089     return -1;
00090   }
00091 
00092   
00093   yarp.start(&kuka);
00094   ros.start(&kuka);
00095 
00096   int seq=0;
00097 
00098   bool showOutput = isatty(1);
00099 
00100   while(!should_exit) {
00101     RobotData data = kuka.data();
00102     RobotCommand cmd = kuka.cmd();
00103     RobotStatus status = kuka.status();
00104 
00105     if(seq != data.seq && showOutput) {
00106       char buffer[4096];
00107       FRIComm::printStatus(buffer, data, cmd, status);
00108       printf("%s", buffer);
00109       seq = data.seq;
00110     }
00111 
00112     usleep(500000);
00113   }
00114 
00115   kuka.finish();
00116   ros.finish();
00117   yarp.finish();
00118 
00119   printf("# exiting program.\n");
00120 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kuka_fri
Author(s): Ingo Kresse, Alexis Maldonado
autogenerated on Thu May 23 2013 05:28:18