main.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <stdio.h>
00036 #include <getopt.h>
00037 #include <execinfo.h>
00038 #include <signal.h>
00039 #include <sys/mman.h>
00040 #include <sys/types.h>
00041 #include <sys/stat.h>
00042 #include <unistd.h>
00043 #include <fcntl.h>
00044 #include <pthread.h>
00045 
00046 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00047 #include <pr2_controller_manager/controller_manager.h>
00048 #include <ethercat_hardware/ethercat_hardware.h>
00049 
00050 #include <ros/ros.h>
00051 #include <std_srvs/Empty.h>
00052 
00053 #include <realtime_tools/realtime_publisher.h>
00054 #include <std_msgs/Float64.h>
00055 
00056 #include <boost/accumulators/accumulators.hpp>
00057 #include <boost/accumulators/statistics/stats.hpp>
00058 #include <boost/accumulators/statistics/max.hpp>
00059 #include <boost/accumulators/statistics/mean.hpp>
00060 using namespace boost::accumulators;
00061 
00062 static const std::string name = "pr2_etherCAT";
00063 
00064 
00065 static struct
00066 {
00067   char *program_;
00068   char *interface_;
00069   char *xml_;
00070   char *rosparam_;
00071   bool allow_unprogrammed_;
00072   bool stats_;
00073 } g_options;
00074 
00075 std::string g_robot_desc;
00076 
00077 void Usage(string msg = "")
00078 {
00079   fprintf(stderr, "Usage: %s [options]\n", g_options.program_);
00080   fprintf(stderr, "  Available options\n");
00081   fprintf(stderr, "    -i, --interface <interface> Connect to EtherCAT devices on this interface\n");
00082   fprintf(stderr, "    -s, --stats                 Publish statistics on the RT loop jitter on \"pr2_etherCAT/realtime\" in seconds\n");
00083   fprintf(stderr, "    -x, --xml <file>            Load the robot description from this file\n");
00084   fprintf(stderr, "    -r, --rosparam <param>      Load the robot description from this parameter name\n");
00085   fprintf(stderr, "    -u, --allow_unprogrammed    Allow control loop to run with unprogrammed devices\n");
00086   fprintf(stderr, "    -h, --help                  Print this message and exit\n");
00087   if (msg != "")
00088   {
00089     fprintf(stderr, "Error: %s\n", msg.c_str());
00090     exit(-1);
00091   }
00092   else
00093   {
00094     exit(0);
00095   }
00096 }
00097 
00098 static int g_quit = 0;
00099 static bool g_reset_motors = true;
00100 static bool g_halt_motors = false;
00101 static bool g_halt_requested = false;
00102 static volatile bool g_publish_trace_requested = false;
00103 static const int NSEC_PER_SECOND = 1e+9;
00104 static const int USEC_PER_SECOND = 1e6;
00105 
00106 static struct
00107 {
00108   accumulator_set<double, stats<tag::max, tag::mean> > ec_acc;
00109   accumulator_set<double, stats<tag::max, tag::mean> > cm_acc;
00110   accumulator_set<double, stats<tag::max, tag::mean> > loop_acc;
00111   accumulator_set<double, stats<tag::max, tag::mean> > jitter_acc;
00112   int overruns;
00113   int recent_overruns;
00114   int last_overrun;
00115   int last_severe_overrun;
00116   double overrun_loop_sec;
00117   double overrun_ec;
00118   double overrun_cm;
00119 
00120   // These values are set when realtime loop does not meet performace expections
00121   bool rt_loop_not_making_timing; 
00122   double halt_rt_loop_frequency;
00123   double rt_loop_frequency;
00124 } g_stats;
00125 
00126 static void publishDiagnostics(realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticArray> &publisher)
00127 {
00128   if (publisher.trylock())
00129   {
00130     accumulator_set<double, stats<tag::max, tag::mean> > zero;
00131     vector<diagnostic_msgs::DiagnosticStatus> statuses;
00132     diagnostic_updater::DiagnosticStatusWrapper status;
00133 
00134     static double max_ec = 0, max_cm = 0, max_loop = 0, max_jitter = 0;
00135     double avg_ec, avg_cm, avg_loop, avg_jitter;
00136 
00137     avg_ec           = extract_result<tag::mean>(g_stats.ec_acc);
00138     avg_cm           = extract_result<tag::mean>(g_stats.cm_acc);
00139     avg_loop         = extract_result<tag::mean>(g_stats.loop_acc);
00140     max_ec           = std::max(max_ec, extract_result<tag::max>(g_stats.ec_acc));
00141     max_cm           = std::max(max_cm, extract_result<tag::max>(g_stats.cm_acc));
00142     max_loop         = std::max(max_loop, extract_result<tag::max>(g_stats.loop_acc));
00143     g_stats.ec_acc   = zero;
00144     g_stats.cm_acc   = zero;
00145     g_stats.loop_acc = zero;
00146 
00147     // Publish average loop jitter
00148     avg_jitter         = extract_result<tag::mean>(g_stats.jitter_acc);
00149     max_jitter         = std::max(max_jitter, extract_result<tag::max>(g_stats.jitter_acc));
00150     g_stats.jitter_acc = zero;
00151 
00152     static bool first = true;
00153     if (first)
00154     {
00155       first = false;
00156       status.add("Robot Description", g_robot_desc);
00157     }
00158 
00159     status.addf("Max EtherCAT roundtrip (us)", "%.2f", max_ec*USEC_PER_SECOND);
00160     status.addf("Avg EtherCAT roundtrip (us)", "%.2f", avg_ec*USEC_PER_SECOND);
00161     status.addf("Max Controller Manager roundtrip (us)", "%.2f", max_cm*USEC_PER_SECOND);
00162     status.addf("Avg Controller Manager roundtrip (us)", "%.2f", avg_cm*USEC_PER_SECOND);
00163     status.addf("Max Total Loop roundtrip (us)", "%.2f", max_loop*USEC_PER_SECOND);
00164     status.addf("Avg Total Loop roundtrip (us)", "%.2f", avg_loop*USEC_PER_SECOND);
00165     status.addf("Max Loop Jitter (us)", "%.2f", max_jitter * USEC_PER_SECOND);
00166     status.addf("Avg Loop Jitter (us)", "%.2f", avg_jitter * USEC_PER_SECOND);
00167     status.addf("Control Loop Overruns", "%d", g_stats.overruns);
00168     status.addf("Recent Control Loop Overruns", "%d", g_stats.recent_overruns);
00169     status.addf("Last Control Loop Overrun Cause", "ec: %.2fus, cm: %.2fus", 
00170                 g_stats.overrun_ec*USEC_PER_SECOND, g_stats.overrun_cm*USEC_PER_SECOND);
00171     status.addf("Last Overrun Loop Time (us)", "%.2f", g_stats.overrun_loop_sec * USEC_PER_SECOND);
00172     status.addf("Realtime Loop Frequency", "%.4f", g_stats.rt_loop_frequency);
00173 
00174     status.name = "Realtime Control Loop";
00175     if (g_stats.overruns > 0 && g_stats.last_overrun < 30)
00176     {
00177       if (g_stats.last_severe_overrun < 30)
00178         status.level = 1;
00179       else
00180         status.level = 0;
00181       status.message = "Realtime loop used too much time in the last 30 seconds.";
00182     }
00183     else
00184     {
00185       status.level = 0;
00186       status.message = "OK";
00187     }
00188     g_stats.recent_overruns = 0;
00189     g_stats.last_overrun++;
00190     g_stats.last_severe_overrun++;
00191 
00192     if (g_stats.rt_loop_not_making_timing)
00193     {
00194       status.mergeSummaryf(status.ERROR, "Halting, realtime loop only ran at %.4f Hz", g_stats.halt_rt_loop_frequency);
00195     }
00196 
00197     statuses.push_back(status);
00198     publisher.msg_.status = statuses;
00199     publisher.msg_.header.stamp = ros::Time::now();
00200     publisher.unlockAndPublish();
00201   }
00202 }
00203 
00204 static inline double now()
00205 {
00206   struct timespec n;
00207   clock_gettime(CLOCK_MONOTONIC, &n);
00208   return double(n.tv_nsec) / NSEC_PER_SECOND + n.tv_sec;
00209 }
00210 
00211 
00212 void *diagnosticLoop(void *args)
00213 {
00214   EthercatHardware *ec((EthercatHardware *) args);
00215   struct timespec tick;
00216   clock_gettime(CLOCK_MONOTONIC, &tick);
00217   while (!g_quit) {
00218     ec->collectDiagnostics();
00219     tick.tv_sec += 1;
00220     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tick, NULL);
00221   }
00222   return NULL;
00223 }
00224 
00225 static void timespecInc(struct timespec &tick, int nsec)
00226 {
00227   tick.tv_nsec += nsec;
00228   while (tick.tv_nsec >= NSEC_PER_SECOND)
00229   {
00230     tick.tv_nsec -= NSEC_PER_SECOND;
00231     tick.tv_sec++;
00232   }
00233 }
00234 
00235 
00236 class RTLoopHistory
00237 {
00238 public:
00239   RTLoopHistory(unsigned length, double default_value) :
00240     index_(0), 
00241     length_(length),
00242     history_(new double[length])
00243   {
00244     for (unsigned i=0; i<length_; ++i) 
00245       history_[i] = default_value;
00246   }
00247 
00248   ~RTLoopHistory()
00249   {
00250     delete[] history_;
00251     history_ = NULL;
00252   }
00253   
00254   void sample(double value) 
00255   {
00256     index_ = (index_+1) % length_;
00257     history_[index_] = value;
00258   }
00259 
00260   double average() const
00261   {
00262     double sum(0.0);
00263     for (unsigned i=0; i<length_; ++i) 
00264       sum+=history_[i];
00265     return sum / double(length_);
00266   }
00267 
00268 protected:
00269   unsigned index_;
00270   unsigned length_;
00271   double *history_;
00272 };
00273 
00274 
00275 void *controlLoop(void *)
00276 {
00277   int rv = 0;
00278   double last_published, last_loop_start;
00279   int period;
00280   int policy;
00281   TiXmlElement *root;
00282   TiXmlElement *root_element;
00283 
00284   ros::NodeHandle node(name);
00285 
00286   realtime_tools::RealtimePublisher<diagnostic_msgs::DiagnosticArray> publisher(node, "/diagnostics", 2);
00287   realtime_tools::RealtimePublisher<std_msgs::Float64> *rtpublisher = 0;
00288 
00289   // Realtime loop should be running at least 750Hz
00290   // Calculate realtime loop frequency every 200mseec
00291   // Halt motors if average frequency over last 600msec is less than 750Hz
00292   double min_acceptable_rt_loop_frequency;
00293   if (!node.getParam("min_acceptable_rt_loop_frequency", min_acceptable_rt_loop_frequency))
00294   {
00295     min_acceptable_rt_loop_frequency = 750.0; 
00296   } 
00297   else 
00298   {
00299     ROS_WARN("min_acceptable_rt_loop_frequency changed to %f", min_acceptable_rt_loop_frequency);
00300   }
00301   unsigned rt_cycle_count = 0;
00302   double last_rt_monitor_time;
00303   double rt_loop_monitor_period = 0.6 / 3;
00304   // Keep history of last 3 calculation intervals.
00305   RTLoopHistory rt_loop_history(3, 1000.0); 
00306 
00307   if (g_options.stats_){
00308     rtpublisher = new realtime_tools::RealtimePublisher<std_msgs::Float64>(node, "realtime", 2);
00309   }
00310 
00311   // Initialize the hardware interface
00312   EthercatHardware ec(name);
00313   ec.init(g_options.interface_, g_options.allow_unprogrammed_);
00314 
00315   // Create controller manager
00316   pr2_controller_manager::ControllerManager cm(ec.hw_);
00317 
00318   // Load robot description
00319   TiXmlDocument xml;
00320   struct stat st;
00321   if (g_options.rosparam_ != NULL)
00322   {
00323     if (ros::param::get(g_options.rosparam_, g_robot_desc))
00324     {
00325       xml.Parse(g_robot_desc.c_str());
00326     }
00327     else
00328     {
00329       ROS_FATAL("Could not load the xml from parameter server: %s", g_options.rosparam_);
00330       rv = -1;
00331       goto end;
00332     }    
00333   }
00334   else if (0 == stat(g_options.xml_, &st))
00335   {
00336     xml.LoadFile(g_options.xml_);
00337   }
00338   else
00339   {
00340     // In ROS Galapagos remove this fall-back to rosparam functionality
00341     ROS_INFO("Xml file not found, reading from parameter server");
00342     ros::NodeHandle top_level_node;
00343     if (top_level_node.getParam(g_options.xml_, g_robot_desc))
00344     {
00345       xml.Parse(g_robot_desc.c_str());
00346       ROS_WARN("Using -x to load robot description from parameter server is depricated.  Use -r instead.");
00347     }
00348     else
00349     {
00350       ROS_FATAL("Could not load the xml from parameter server: %s", g_options.xml_);
00351       rv = -1;
00352       goto end;
00353     }
00354   }
00355   
00356   root_element = xml.RootElement();
00357   root = xml.FirstChildElement("robot");
00358   if (!root || !root_element)
00359   {
00360       ROS_FATAL("Could not parse the xml from %s", g_options.xml_);
00361       rv = -1;
00362       goto end;
00363   }
00364 
00365   // Initialize the controller manager from robot description
00366   if (!cm.initXml(root))
00367   {
00368       ROS_FATAL("Could not initialize the controller manager");
00369       rv = -1;
00370       goto end;
00371   }
00372 
00373   // Publish one-time before entering real-time to pre-allocate message vectors
00374   publishDiagnostics(publisher);
00375 
00376   //Start Non-realtime diagonostic thread
00377   static pthread_t diagnosticThread;
00378   if ((rv = pthread_create(&diagnosticThread, NULL, diagnosticLoop, &ec)) != 0)
00379   {
00380     ROS_FATAL("Unable to create control thread: rv = %d", rv);
00381     goto end;
00382   }
00383   
00384   // Set to realtime scheduler for this thread
00385   struct sched_param thread_param;
00386   policy = SCHED_FIFO;
00387   thread_param.sched_priority = sched_get_priority_max(policy);
00388   pthread_setschedparam(pthread_self(), policy, &thread_param);
00389 
00390   struct timespec tick;
00391   clock_gettime(CLOCK_REALTIME, &tick);
00392   period = 1e+6; // 1 ms in nanoseconds
00393 
00394   // Snap to the nearest second
00395   tick.tv_sec = tick.tv_sec;
00396   tick.tv_nsec = (tick.tv_nsec / period + 1) * period;
00397   clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
00398 
00399   last_published = now();
00400   last_rt_monitor_time = now();
00401   last_loop_start = now();
00402   while (!g_quit)
00403   {
00404     // Track how long the actual loop takes
00405     double this_loop_start = now();
00406     g_stats.loop_acc(this_loop_start - last_loop_start);
00407     last_loop_start = this_loop_start;
00408     
00409     double start = now();
00410     if (g_reset_motors)
00411     {
00412       ec.update(true, g_halt_motors);
00413       g_reset_motors = false;
00414       // Also, clear error flags when motor reset is requested
00415       g_stats.rt_loop_not_making_timing = false;
00416     }
00417     else
00418     {
00419       ec.update(false, g_halt_motors);
00420     }
00421     if (g_publish_trace_requested)
00422     {
00423       g_publish_trace_requested = false;
00424       ec.publishTrace(-1,"",0,0);
00425     }
00426     g_halt_motors = false;
00427     double after_ec = now();
00428     cm.update();
00429     double end = now();
00430 
00431     g_stats.ec_acc(after_ec - start);
00432     g_stats.cm_acc(end - after_ec);
00433 
00434     if ((end - last_published) > 1.0)
00435     {
00436       publishDiagnostics(publisher);
00437       last_published = end;
00438     }
00439 
00440     // Realtime loop should run about 1000Hz.  
00441     // Missing timing on a control cycles usually causes a controller glitch and actuators to jerk.
00442     // When realtime loop misses a lot of cycles controllers will perform poorly and may cause robot to shake.
00443     // Halt motors if realtime loop does not run enough cycles over a given period.
00444     ++rt_cycle_count;
00445     if ((start - last_rt_monitor_time) > rt_loop_monitor_period)
00446     {
00447       // Calculate new average rt loop frequency       
00448       double rt_loop_frequency = double(rt_cycle_count) / rt_loop_monitor_period;
00449 
00450       // Use last X samples of frequency when deciding whether or not to halt
00451       rt_loop_history.sample(rt_loop_frequency);
00452       double avg_rt_loop_frequency = rt_loop_history.average();
00453       if (avg_rt_loop_frequency < min_acceptable_rt_loop_frequency)
00454       {
00455         g_halt_motors = true;
00456         if (!g_stats.rt_loop_not_making_timing)
00457         {
00458           // Only update this value if motors when this first occurs (used for diagnostics error message)
00459           g_stats.halt_rt_loop_frequency = avg_rt_loop_frequency;
00460         }
00461         g_stats.rt_loop_not_making_timing = true;
00462       }
00463       g_stats.rt_loop_frequency = avg_rt_loop_frequency;
00464       rt_cycle_count = 0;
00465       last_rt_monitor_time = start;
00466     }
00467 
00468     // Compute end of next period
00469     timespecInc(tick, period);
00470 
00471     struct timespec before; 
00472     clock_gettime(CLOCK_REALTIME, &before); 
00473     if ((before.tv_sec + double(before.tv_nsec)/NSEC_PER_SECOND) > (tick.tv_sec + double(tick.tv_nsec)/NSEC_PER_SECOND))
00474     {
00475       // Total amount of time the loop took to run
00476       g_stats.overrun_loop_sec = (before.tv_sec + double(before.tv_nsec)/NSEC_PER_SECOND) - 
00477         (tick.tv_sec + double(tick.tv_nsec)/NSEC_PER_SECOND);
00478 
00479       // We overran, snap to next "period"
00480       tick.tv_sec = before.tv_sec;
00481       tick.tv_nsec = (before.tv_nsec / period) * period;
00482       timespecInc(tick, period);
00483 
00484       // initialize overruns
00485       if (g_stats.overruns == 0){
00486         g_stats.last_overrun = 1000;
00487         g_stats.last_severe_overrun = 1000;
00488       }
00489       // check for overruns
00490       if (g_stats.recent_overruns > 10)
00491         g_stats.last_severe_overrun = 0;
00492       g_stats.last_overrun = 0;
00493 
00494       g_stats.overruns++;
00495       g_stats.recent_overruns++;
00496       g_stats.overrun_ec = after_ec - start;
00497       g_stats.overrun_cm = end - after_ec;
00498     }
00499 
00500     // Sleep until end of period
00501     clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
00502 
00503     // Calculate RT loop jitter
00504     struct timespec after; 
00505     clock_gettime(CLOCK_REALTIME, &after); 
00506     double jitter = (after.tv_sec - tick.tv_sec + double(after.tv_nsec-tick.tv_nsec)/NSEC_PER_SECOND);
00507 
00508     g_stats.jitter_acc(jitter);
00509 
00510     // Publish realtime loops statistics, if requested
00511     if (rtpublisher)
00512     {
00513       if (rtpublisher->trylock()) 
00514       { 
00515         rtpublisher->msg_.data  = jitter; 
00516         rtpublisher->unlockAndPublish(); 
00517       }
00518     }
00519 
00520     // Halt the motors, if requested by a service call
00521     if (g_halt_requested)
00522     {
00523       g_halt_motors = true;
00524       g_halt_requested = false;
00525     }
00526   }
00527 
00528   /* Shutdown all of the motors on exit */
00529   for (pr2_hardware_interface::ActuatorMap::const_iterator it = ec.hw_->actuators_.begin(); it != ec.hw_->actuators_.end(); ++it)
00530   {
00531     it->second->command_.enable_ = false;
00532     it->second->command_.effort_ = 0;
00533   }
00534   ec.update(false, true);
00535 
00536   //pthread_join(diagnosticThread, 0);  
00537 
00538 end:
00539   publisher.stop();
00540   delete rtpublisher;
00541 
00542   ros::shutdown();
00543 
00544   return (void *)rv;
00545 }
00546 
00547 void quitRequested(int sig)
00548 {
00549   g_quit = 1;
00550 }
00551 
00552 bool resetMotorsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00553 {
00554   g_reset_motors = true;
00555   return true;
00556 }
00557 
00558 bool haltMotorsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00559 {
00560   g_halt_requested = true;
00561   return true;
00562 }
00563 
00564 bool publishTraceService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
00565 {
00566   g_publish_trace_requested = true;
00567   return true;
00568 }
00569 
00570 static int
00571 lock_fd(int fd)
00572 {
00573   struct flock lock;
00574   int rv;
00575 
00576   lock.l_type = F_WRLCK;
00577   lock.l_whence = SEEK_SET;
00578   lock.l_start = 0;
00579   lock.l_len = 0;
00580 
00581   rv = fcntl(fd, F_SETLK, &lock);
00582   return rv;
00583 }
00584 
00585 
00586 static const char* PIDDIR = "/var/tmp/run/";
00587 
00588 std::string generatePIDFilename(const char* interface)
00589 {
00590   std::string filename;
00591   if (interface != NULL)
00592   {
00593     // There should a lock file for each EtherCAT interface instead of for entire computer
00594     // It is entirely possible to have different EtherCAT utilities operating indepedantly
00595     //  on different interfaces
00596     filename = std::string(PIDDIR) + "EtherCAT_" +  std::string(interface) + ".pid";
00597   }
00598   else 
00599   {
00600     filename = std::string(PIDDIR) + std::string("pr2_etherCAT.pid"); 
00601   }
00602   return filename;
00603 }
00604 
00605 
00606 static int setupPidFile(const char* interface)
00607 {
00608   int rv = -1;
00609   pid_t pid;
00610   int fd;
00611   FILE *fp = NULL;
00612 
00613   std::string filename = generatePIDFilename(interface);
00614 
00615   umask(0);
00616   mkdir(PIDDIR, 0777);
00617   fd = open(filename.c_str(), O_RDWR | O_CREAT | O_EXCL, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IWOTH | S_IROTH);
00618   if (fd == -1)
00619   {
00620     if (errno != EEXIST)
00621     {
00622       ROS_FATAL("Unable to create pid file '%s': %s", filename.c_str(), strerror(errno));
00623       goto end;
00624     }
00625 
00626     if ((fd = open(filename.c_str(), O_RDWR)) < 0)
00627     {
00628       ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno));
00629       goto end;
00630     }
00631 
00632     if ((fp = fdopen(fd, "rw")) == NULL)
00633     {
00634       ROS_FATAL("Can't read from '%s': %s", filename.c_str(), strerror(errno));
00635       goto end;
00636     }
00637     pid = -1;
00638     if ((fscanf(fp, "%d", &pid) != 1) || (pid == getpid()) || (lock_fd(fileno(fp)) == 0))
00639     {
00640       int rc;
00641 
00642       if ((rc = unlink(filename.c_str())) == -1)
00643       {
00644         ROS_FATAL("Can't remove stale pid file '%s': %s", filename.c_str(), strerror(errno));
00645         goto end;
00646       }
00647     } else {
00648       ROS_FATAL("Another instance of pr2_etherCAT is already running with pid: %d", pid);
00649       goto end;
00650     }
00651   }
00652 
00653   unlink(filename.c_str());
00654   fd = open(filename.c_str(), O_RDWR | O_CREAT | O_EXCL, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IWOTH | S_IROTH);
00655 
00656   if (fd == -1)
00657   {
00658     ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno));
00659     goto end;
00660   }
00661 
00662   if (lock_fd(fd) == -1)
00663   {
00664     ROS_FATAL("Unable to lock pid file '%s': %s", filename.c_str(), strerror(errno));
00665     goto end;
00666   }
00667 
00668   if ((fp = fdopen(fd, "w")) == NULL)
00669   {
00670     ROS_FATAL("fdopen failed: %s", strerror(errno));
00671     goto end;
00672   }
00673 
00674   fprintf(fp, "%d\n", getpid());
00675 
00676   /* We do NOT close fd, since we want to keep the lock. */
00677   fflush(fp);
00678   fcntl(fd, F_SETFD, (long) 1);
00679   rv = 0;
00680 end:
00681   return rv;
00682 }
00683 
00684 static void cleanupPidFile(const char* interface)
00685 {
00686   std::string filename = generatePIDFilename(interface);
00687   unlink(filename.c_str());
00688 }
00689 
00690 #define CLOCK_PRIO 0
00691 #define CONTROL_PRIO 0
00692 
00693 static pthread_t controlThread;
00694 static pthread_attr_t controlThreadAttr;
00695 int main(int argc, char *argv[])
00696 {
00697   // Keep the kernel from swapping us out
00698   if (mlockall(MCL_CURRENT | MCL_FUTURE) < 0) {
00699     perror("mlockall");
00700     return -1;
00701   }
00702 
00703   // Initialize ROS and parse command-line arguments
00704   ros::init(argc, argv, "realtime_loop");
00705 
00706   // Parse options
00707   g_options.program_ = argv[0];
00708   g_options.xml_ = NULL;
00709   g_options.rosparam_ = NULL;
00710   while (1)
00711   {
00712     static struct option long_options[] = {
00713       {"help", no_argument, 0, 'h'},
00714       {"stats", no_argument, 0, 's'},
00715       {"allow_unprogrammed", no_argument, 0, 'u'},
00716       {"interface", required_argument, 0, 'i'},
00717       {"xml", required_argument, 0, 'x'},
00718       {"rosparam", required_argument, 0, 'r'},
00719     };
00720     int option_index = 0;
00721     int c = getopt_long(argc, argv, "hi:usx:r:", long_options, &option_index);
00722     if (c == -1) break;
00723     switch (c)
00724     {
00725       case 'h':
00726         Usage();
00727         break;
00728       case 'u':
00729         g_options.allow_unprogrammed_ = 1;
00730         break;
00731       case 'i':
00732         g_options.interface_ = optarg;
00733         break;
00734       case 'x':
00735         g_options.xml_ = optarg;
00736         break;
00737       case 'r':
00738         g_options.rosparam_ = optarg;
00739         break;
00740       case 's':
00741         g_options.stats_ = 1;
00742         break;
00743     }
00744   }
00745   if (optind < argc)
00746   {
00747     Usage("Extra arguments");
00748   }
00749 
00750   if (!g_options.interface_)
00751     Usage("You must specify a network interface");
00752   if (!g_options.xml_ && !g_options.rosparam_)
00753   {
00754     Usage("You must specify either an XML file or rosparam for robot description");
00755   }
00756   if (g_options.xml_ && g_options.rosparam_)
00757   {
00758     Usage("You must not specify both a rosparm and XML file for robot description");
00759   }
00760 
00761   // The current EtherCAT software creates a lock for any EtherCAT master.
00762   // This lock prevents two EtherCAT masters from running on the same computer.
00763   // However, this locking scheme is too restrictive.  
00764   // Two EtherCAT masters can run without conflicting with each other
00765   // as long as they are communication with different sets of EtherCAT devices.
00766   // A better locking scheme would be to prevent two EtherCAT 
00767   // masters from running on same Ethernet interface.  
00768   // To transition to having only lock per interface, this will create both 
00769   // th global and per-interface lock files for next ROS release (Fuerte).  
00770   // In the Galapogos ROS release, the global EtherCAT lock will be removed 
00771   // and only the pre-interface lock will remain.
00772   if (setupPidFile(NULL) < 0) return -1;
00773   if (setupPidFile(g_options.interface_) < 0) return -1;
00774 
00775   ros::NodeHandle node(name);
00776 
00777   // Catch attempts to quit
00778   signal(SIGTERM, quitRequested);
00779   signal(SIGINT, quitRequested);
00780   signal(SIGHUP, quitRequested);
00781 
00782   ros::ServiceServer reset = node.advertiseService("reset_motors", resetMotorsService);
00783   ros::ServiceServer halt = node.advertiseService("halt_motors", haltMotorsService);
00784   ros::ServiceServer publishTrace = node.advertiseService("publish_trace", publishTraceService);
00785 
00786   //Start thread
00787   int rv;
00788   if ((rv = pthread_create(&controlThread, &controlThreadAttr, controlLoop, 0)) != 0)
00789   {
00790     ROS_FATAL("Unable to create control thread: rv = %d", rv);
00791     exit(EXIT_FAILURE);
00792   }
00793 
00794   ros::spin();
00795   pthread_join(controlThread, (void **)&rv);
00796 
00797   // Cleanup pid files
00798   cleanupPidFile(NULL);
00799   cleanupPidFile(g_options.interface_);
00800 
00801   return rv;
00802 }


pr2_etherCAT
Author(s): Rob Wheeler/wheeler@willowgarage.com
autogenerated on Fri Dec 6 2013 20:23:22