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


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Fri Aug 28 2015 12:33:10