init.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 Willow Garage, Inc. 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 "ros/init.h"
00036 #include "ros/names.h"
00037 #include "ros/xmlrpc_manager.h"
00038 #include "ros/poll_manager.h"
00039 #include "ros/connection_manager.h"
00040 #include "ros/topic_manager.h"
00041 #include "ros/service_manager.h"
00042 #include "ros/this_node.h"
00043 #include "ros/network.h"
00044 #include "ros/file_log.h"
00045 #include "ros/callback_queue.h"
00046 #include "ros/param.h"
00047 #include "ros/rosout_appender.h"
00048 #include "ros/subscribe_options.h"
00049 #include "ros/transport/transport_tcp.h"
00050 #include "ros/internal_timer_manager.h"
00051 
00052 #include "roscpp/GetLoggers.h"
00053 #include "roscpp/SetLoggerLevel.h"
00054 #include "roscpp/Empty.h"
00055 
00056 #include <ros/console.h>
00057 #include <ros/time.h>
00058 #include <rosgraph_msgs/Clock.h>
00059 
00060 #include <algorithm>
00061 
00062 #include <signal.h>
00063 
00064 #include <cstdlib>
00065 
00066 #ifdef _MSC_VER
00067   #include "log4cxx/helpers/transcoder.h" // Have to be able to encode wchar LogStrings on windows.
00068 #endif
00069 
00070 namespace ros
00071 {
00072 
00073 namespace master
00074 {
00075 void init(const M_string& remappings);
00076 }
00077 
00078 namespace this_node
00079 {
00080 void init(const std::string& names, const M_string& remappings, uint32_t options);
00081 }
00082 
00083 namespace network
00084 {
00085 void init(const M_string& remappings);
00086 }
00087 
00088 namespace param
00089 {
00090 void init(const M_string& remappings);
00091 }
00092 
00093 namespace file_log
00094 {
00095 void init(const M_string& remappings);
00096 }
00097 
00098 CallbackQueuePtr g_global_queue;
00099 ROSOutAppenderPtr g_rosout_appender;
00100 static CallbackQueuePtr g_internal_callback_queue;
00101 
00102 static bool g_initialized = false;
00103 static bool g_started = false;
00104 static bool g_atexit_registered = false;
00105 static boost::mutex g_start_mutex;
00106 static bool g_ok = false;
00107 static uint32_t g_init_options = 0;
00108 static bool g_shutdown_requested = false;
00109 static volatile bool g_shutting_down = false;
00110 static boost::recursive_mutex g_shutting_down_mutex;
00111 static boost::thread g_internal_queue_thread;
00112 
00113 bool isInitialized()
00114 {
00115   return g_initialized;
00116 }
00117 
00118 bool isShuttingDown()
00119 {
00120   return g_shutting_down;
00121 }
00122 
00123 void checkForShutdown()
00124 {
00125   if (g_shutdown_requested)
00126   {
00127     // Since this gets run from within a mutex inside PollManager, we need to prevent ourselves from deadlocking with
00128     // another thread that's already in the middle of shutdown()
00129     boost::recursive_mutex::scoped_try_lock lock(g_shutting_down_mutex, boost::defer_lock);
00130     while (!lock.try_lock() && !g_shutting_down)
00131     {
00132       ros::WallDuration(0.001).sleep();
00133     }
00134 
00135     if (!g_shutting_down)
00136     {
00137       shutdown();
00138     }
00139 
00140     g_shutdown_requested = false;
00141   }
00142 }
00143 
00144 void requestShutdown()
00145 {
00146   g_shutdown_requested = true;
00147 }
00148 
00149 void atexitCallback()
00150 {
00151   if (ok() && !isShuttingDown())
00152   {
00153     ROSCPP_LOG_DEBUG("shutting down due to exit() or end of main() without cleanup of all NodeHandles");
00154     shutdown();
00155   }
00156 }
00157 
00158 void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
00159 {
00160   int num_params = 0;
00161   if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
00162     num_params = params.size();
00163   if (num_params > 1)
00164   {
00165     std::string reason = params[1];
00166     ROS_WARN("Shutdown request received.");
00167     ROS_WARN("Reason given for shutdown: [%s]", reason.c_str());
00168     requestShutdown();
00169   }
00170 
00171   result = xmlrpc::responseInt(1, "", 0);
00172 }
00173 
00174 bool getLoggers(roscpp::GetLoggers::Request&, roscpp::GetLoggers::Response& resp)
00175 {
00176   log4cxx::spi::LoggerRepositoryPtr repo = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME)->getLoggerRepository();
00177 
00178   log4cxx::LoggerList loggers = repo->getCurrentLoggers();
00179   log4cxx::LoggerList::iterator it = loggers.begin();
00180   log4cxx::LoggerList::iterator end = loggers.end();
00181   for (; it != end; ++it)
00182   {
00183     roscpp::Logger logger;
00184     #ifdef _MSC_VER
00185       LOG4CXX_ENCODE_CHAR(tmp_name_str, (*it)->getName()); // has to handle LogString with wchar types.
00186       logger.name = tmp_name_str; // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro
00187     #else
00188     logger.name = (*it)->getName();
00189     #endif
00190     const log4cxx::LevelPtr& level = (*it)->getEffectiveLevel();
00191     if (level)
00192     {
00193           #ifdef _MSC_VER
00194             LOG4CXX_ENCODE_CHAR(tmp_level_str, level->toString()); // has to handle LogString with wchar types.
00195             logger.level = tmp_level_str;   // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro
00196           #else
00197       logger.level = level->toString();
00198       #endif
00199     }
00200     resp.loggers.push_back(logger);
00201   }
00202 
00203   return true;
00204 }
00205 
00206 bool setLoggerLevel(roscpp::SetLoggerLevel::Request& req, roscpp::SetLoggerLevel::Response&)
00207 {
00208   log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(req.logger);
00209   log4cxx::LevelPtr level;
00210 
00211   std::transform(req.level.begin(), req.level.end(), req.level.begin(), (int(*)(int))std::toupper);
00212 
00213   if (req.level == "DEBUG")
00214   {
00215     level = log4cxx::Level::getDebug();
00216   }
00217   else if (req.level == "INFO")
00218   {
00219     level = log4cxx::Level::getInfo();
00220   }
00221   else if (req.level == "WARN")
00222   {
00223     level = log4cxx::Level::getWarn();
00224   }
00225   else if (req.level == "ERROR")
00226   {
00227     level = log4cxx::Level::getError();
00228   }
00229   else if (req.level == "FATAL")
00230   {
00231     level = log4cxx::Level::getFatal();
00232   }
00233 
00234   if (level)
00235   {
00236     logger->setLevel(level);
00237     console::notifyLoggerLevelsChanged();
00238     return true;
00239   }
00240 
00241   return false;
00242 }
00243 
00244 bool closeAllConnections(roscpp::Empty::Request&, roscpp::Empty::Response&)
00245 {
00246   ROSCPP_LOG_DEBUG("close_all_connections service called, closing connections");
00247   ConnectionManager::instance()->clear(Connection::TransportDisconnect);
00248   return true;
00249 }
00250 
00251 void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg)
00252 {
00253   Time::setNow(msg->clock);
00254 }
00255 
00256 CallbackQueuePtr getInternalCallbackQueue()
00257 {
00258   if (!g_internal_callback_queue)
00259   {
00260     g_internal_callback_queue.reset(new CallbackQueue);
00261   }
00262 
00263   return g_internal_callback_queue;
00264 }
00265 
00266 void basicSigintHandler(int sig)
00267 {
00268   ros::requestShutdown();
00269 }
00270 
00271 void internalCallbackQueueThreadFunc()
00272 {
00273   disableAllSignalsInThisThread();
00274 
00275   CallbackQueuePtr queue = getInternalCallbackQueue();
00276 
00277   while (!g_shutting_down)
00278   {
00279     queue->callAvailable(WallDuration(0.1));
00280   }
00281 }
00282 
00283 bool isStarted()
00284 {
00285   return g_started;
00286 }
00287 
00288 void start()
00289 {
00290   boost::mutex::scoped_lock lock(g_start_mutex);
00291   if (g_started)
00292   {
00293     return;
00294   }
00295 
00296   g_shutdown_requested = false;
00297   g_shutting_down = false;
00298   g_started = true;
00299   g_ok = true;
00300 
00301   bool enable_debug = false;
00302   std::string enable_debug_env;
00303   if ( get_environment_variable(enable_debug_env,"ROSCPP_ENABLE_DEBUG") )
00304   {
00305     try
00306     {
00307       enable_debug = boost::lexical_cast<bool>(enable_debug_env.c_str());
00308     }
00309     catch (boost::bad_lexical_cast&)
00310     {
00311     }
00312   }
00313 
00314   param::param("/tcp_keepalive", TransportTCP::s_use_keepalive_, TransportTCP::s_use_keepalive_);
00315 
00316   PollManager::instance()->addPollThreadListener(checkForShutdown);
00317   XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
00318 
00319   initInternalTimerManager();
00320 
00321   TopicManager::instance()->start();
00322   ServiceManager::instance()->start();
00323   ConnectionManager::instance()->start();
00324   PollManager::instance()->start();
00325   XMLRPCManager::instance()->start();
00326 
00327   if (!(g_init_options & init_options::NoSigintHandler))
00328   {
00329     signal(SIGINT, basicSigintHandler);
00330   }
00331 
00332   ros::Time::init();
00333 
00334   if (!(g_init_options & init_options::NoRosout))
00335   {
00336     g_rosout_appender = new ROSOutAppender;
00337     const log4cxx::LoggerPtr& logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
00338     logger->addAppender(g_rosout_appender);
00339   }
00340 
00341   if (g_shutting_down) goto end;
00342 
00343   {
00344     ros::AdvertiseServiceOptions ops;
00345     ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
00346     ops.callback_queue = getInternalCallbackQueue().get();
00347     ServiceManager::instance()->advertiseService(ops);
00348   }
00349 
00350   if (g_shutting_down) goto end;
00351 
00352   {
00353     ros::AdvertiseServiceOptions ops;
00354     ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
00355     ops.callback_queue = getInternalCallbackQueue().get();
00356     ServiceManager::instance()->advertiseService(ops);
00357   }
00358 
00359   if (g_shutting_down) goto end;
00360 
00361   if (enable_debug)
00362   {
00363     ros::AdvertiseServiceOptions ops;
00364     ops.init<roscpp::Empty>(names::resolve("~debug/close_all_connections"), closeAllConnections);
00365     ops.callback_queue = getInternalCallbackQueue().get();
00366     ServiceManager::instance()->advertiseService(ops);
00367   }
00368 
00369   if (g_shutting_down) goto end;
00370 
00371   {
00372     bool use_sim_time = false;
00373     param::param("/use_sim_time", use_sim_time, use_sim_time);
00374 
00375     if (use_sim_time)
00376     {
00377       Time::setNow(ros::Time());
00378     }
00379 
00380     if (g_shutting_down) goto end;
00381 
00382     if (use_sim_time)
00383     {
00384       ros::SubscribeOptions ops;
00385       ops.init<rosgraph_msgs::Clock>("/clock", 1, clockCallback);
00386       ops.callback_queue = getInternalCallbackQueue().get();
00387       TopicManager::instance()->subscribe(ops);
00388     }
00389   }
00390 
00391   if (g_shutting_down) goto end;
00392 
00393   g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
00394   getGlobalCallbackQueue()->enable();
00395 
00396   ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], using [%s] time", 
00397                    this_node::getName().c_str(), getpid(), network::getHost().c_str(), 
00398                    XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(), 
00399                    Time::useSystemTime() ? "real" : "sim");
00400 
00401   // Label used to abort if we've started shutting down in the middle of start(), which can happen in
00402   // threaded code or if Ctrl-C is pressed while we're initializing
00403 end:
00404   // If we received a shutdown request while initializing, wait until we've shutdown to continue
00405   if (g_shutting_down)
00406   {
00407     boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
00408   }
00409 }
00410 
00411 void init(const M_string& remappings, const std::string& name, uint32_t options)
00412 {
00413   if (!g_atexit_registered)
00414   {
00415     g_atexit_registered = true;
00416     atexit(atexitCallback);
00417   }
00418 
00419   if (!g_global_queue)
00420   {
00421     g_global_queue.reset(new CallbackQueue);
00422   }
00423 
00424   if (!g_initialized)
00425   {
00426     g_init_options = options;
00427     g_ok = true;
00428 
00429     ROSCONSOLE_AUTOINIT;
00430     // Disable SIGPIPE
00431 #ifndef WIN32
00432     signal(SIGPIPE, SIG_IGN);
00433 #endif
00434     network::init(remappings);
00435     master::init(remappings);
00436     // names:: namespace is initialized by this_node
00437     this_node::init(name, remappings, options);
00438     file_log::init(remappings);
00439     param::init(remappings);
00440 
00441     g_initialized = true;
00442   }
00443 }
00444 
00445 void init(int& argc, char** argv, const std::string& name, uint32_t options)
00446 {
00447   M_string remappings;
00448 
00449   int full_argc = argc;
00450   // now, move the remapping argv's to the end, and decrement argc as needed
00451   for (int i = 0; i < argc; )
00452   {
00453     std::string arg = argv[i];
00454     size_t pos = arg.find(":=");
00455     if (pos != std::string::npos)
00456     {
00457       std::string local_name = arg.substr(0, pos);
00458       std::string external_name = arg.substr(pos + 2);
00459 
00460       ROSCPP_LOG_DEBUG("remap: %s => %s", local_name.c_str(), external_name.c_str());
00461       remappings[local_name] = external_name;
00462 
00463       // shuffle everybody down and stuff this guy at the end of argv
00464       char *tmp = argv[i];
00465       for (int j = i; j < full_argc - 1; j++)
00466         argv[j] = argv[j+1];
00467       argv[argc-1] = tmp;
00468       argc--;
00469     }
00470     else
00471     {
00472       i++; // move on, since we didn't shuffle anybody here to replace it
00473     }
00474   }
00475 
00476   init(remappings, name, options);
00477 }
00478 
00479 void init(const VP_string& remappings, const std::string& name, uint32_t options)
00480 {
00481   M_string remappings_map;
00482   VP_string::const_iterator it = remappings.begin();
00483   VP_string::const_iterator end = remappings.end();
00484   for (; it != end; ++it)
00485   {
00486     remappings_map[it->first] = it->second;
00487   }
00488 
00489   init(remappings_map, name, options);
00490 }
00491 
00492 void removeROSArgs(int argc, const char* const* argv, V_string& args_out)
00493 {
00494   for (int i = 0; i < argc; ++i)
00495   {
00496     std::string arg = argv[i];
00497     size_t pos = arg.find(":=");
00498     if (pos == std::string::npos)
00499     {
00500       args_out.push_back(arg);
00501     }
00502   }
00503 }
00504 
00505 void spin()
00506 {
00507   SingleThreadedSpinner s;
00508   spin(s);
00509 }
00510 
00511 void spin(Spinner& s)
00512 {
00513   s.spin();
00514 }
00515 
00516 void spinOnce()
00517 {
00518   g_global_queue->callAvailable(ros::WallDuration());
00519 }
00520 
00521 void waitForShutdown()
00522 {
00523   while (ok())
00524   {
00525     WallDuration(0.05).sleep();
00526   }
00527 }
00528 
00529 CallbackQueue* getGlobalCallbackQueue()
00530 {
00531   return g_global_queue.get();
00532 }
00533 
00534 bool ok()
00535 {
00536   return g_ok;
00537 }
00538 
00539 void shutdown()
00540 {
00541   boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
00542   if (g_shutting_down)
00543     return;
00544   else
00545     g_shutting_down = true;
00546 
00547   ros::console::shutdown();
00548 
00549   g_global_queue->disable();
00550   g_global_queue->clear();
00551 
00552   if (g_internal_queue_thread.get_id() != boost::this_thread::get_id())
00553   {
00554     g_internal_queue_thread.join();
00555   }
00556 
00557   const log4cxx::LoggerPtr& logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
00558   logger->removeAppender(g_rosout_appender);
00559   g_rosout_appender = 0;
00560 
00561   // reset this so that the logger doesn't get crashily destroyed
00562   // again during global destruction.  
00563   //
00564   // See https://code.ros.org/trac/ros/ticket/3271
00565   //
00566   log4cxx::Logger::getRootLogger()->getLoggerRepository()->shutdown();
00567   
00568   if (g_started)
00569   {
00570     TopicManager::instance()->shutdown();
00571     ServiceManager::instance()->shutdown();
00572     PollManager::instance()->shutdown();
00573     ConnectionManager::instance()->shutdown();
00574     XMLRPCManager::instance()->shutdown();
00575   }
00576 
00577   WallTime end = WallTime::now();
00578 
00579   g_started = false;
00580   g_ok = false;
00581   Time::shutdown();
00582 }
00583 
00584 }


roscpp
Author(s): Morgan Quigley mquigley@cs.stanford.edu, Josh Faust jfaust@willowgarage.com, Brian Gerkey gerkey@willowgarage.com, Troy Straszheim straszheim@willowgarage.com
autogenerated on Sat Dec 28 2013 17:35:52