init.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include "ros/init.h"
36 #include "ros/names.h"
37 #include "ros/xmlrpc_manager.h"
38 #include "ros/poll_manager.h"
39 #include "ros/connection_manager.h"
40 #include "ros/topic_manager.h"
41 #include "ros/service_manager.h"
42 #include "ros/this_node.h"
43 #include "ros/network.h"
44 #include "ros/file_log.h"
45 #include "ros/callback_queue.h"
46 #include "ros/param.h"
47 #include "ros/rosout_appender.h"
48 #include "ros/subscribe_options.h"
51 #include "xmlrpcpp/XmlRpcSocket.h"
52 
53 #include "roscpp/GetLoggers.h"
54 #include "roscpp/SetLoggerLevel.h"
55 #include "roscpp/Empty.h"
56 
57 #include <ros/console.h>
58 #include <ros/time.h>
59 #include <rosgraph_msgs/Clock.h>
60 
61 #include <algorithm>
62 
63 #include <signal.h>
64 
65 #include <cstdlib>
66 
67 namespace ros
68 {
69 
70 namespace master
71 {
72 void init(const M_string& remappings);
73 }
74 
75 namespace this_node
76 {
77 void init(const std::string& names, const M_string& remappings, uint32_t options);
78 }
79 
80 namespace network
81 {
82 void init(const M_string& remappings);
83 }
84 
85 namespace param
86 {
87 void init(const M_string& remappings);
88 }
89 
90 namespace file_log
91 {
92 void init(const M_string& remappings);
93 }
94 
98 
99 static bool g_initialized = false;
100 static bool g_started = false;
101 static bool g_atexit_registered = false;
102 static boost::mutex g_start_mutex;
103 static bool g_ok = false;
104 static uint32_t g_init_options = 0;
105 static bool g_shutdown_requested = false;
106 static volatile bool g_shutting_down = false;
107 static boost::recursive_mutex g_shutting_down_mutex;
108 static boost::thread g_internal_queue_thread;
109 
111 {
112  return g_initialized;
113 }
114 
116 {
117  return g_shutting_down;
118 }
119 
121 {
122  if (g_shutdown_requested)
123  {
124  // Since this gets run from within a mutex inside PollManager, we need to prevent ourselves from deadlocking with
125  // another thread that's already in the middle of shutdown()
126  boost::recursive_mutex::scoped_try_lock lock(g_shutting_down_mutex, boost::defer_lock);
127  while (!lock.try_lock() && !g_shutting_down)
128  {
129  ros::WallDuration(0.001).sleep();
130  }
131 
132  if (!g_shutting_down)
133  {
134  shutdown();
135  }
136 
137  g_shutdown_requested = false;
138  }
139 }
140 
142 {
143  g_shutdown_requested = true;
144 }
145 
147 {
148  if (ok() && !isShuttingDown())
149  {
150  ROSCPP_LOG_DEBUG("shutting down due to exit() or end of main() without cleanup of all NodeHandles");
151  g_started = false; // don't shutdown singletons, because they are already destroyed
152  shutdown();
153  }
154 }
155 
157 {
158  int num_params = 0;
159  if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
160  num_params = params.size();
161  if (num_params > 1)
162  {
163  std::string reason = params[1];
164  ROS_WARN("Shutdown request received.");
165  ROS_WARN("Reason given for shutdown: [%s]", reason.c_str());
166  requestShutdown();
167  }
168 
169  result = xmlrpc::responseInt(1, "", 0);
170 }
171 
172 bool getLoggers(roscpp::GetLoggers::Request&, roscpp::GetLoggers::Response& resp)
173 {
174  std::map<std::string, ros::console::levels::Level> loggers;
175  bool success = ::ros::console::get_loggers(loggers);
176  if (success)
177  {
178  for (std::map<std::string, ros::console::levels::Level>::const_iterator it = loggers.begin(); it != loggers.end(); it++)
179  {
180  roscpp::Logger logger;
181  logger.name = it->first;
182  ros::console::levels::Level level = it->second;
183  if (level == ros::console::levels::Debug)
184  {
185  logger.level = "debug";
186  }
187  else if (level == ros::console::levels::Info)
188  {
189  logger.level = "info";
190  }
191  else if (level == ros::console::levels::Warn)
192  {
193  logger.level = "warn";
194  }
195  else if (level == ros::console::levels::Error)
196  {
197  logger.level = "error";
198  }
199  else if (level == ros::console::levels::Fatal)
200  {
201  logger.level = "fatal";
202  }
203  resp.loggers.push_back(logger);
204  }
205  }
206  return success;
207 }
208 
209 bool setLoggerLevel(roscpp::SetLoggerLevel::Request& req, roscpp::SetLoggerLevel::Response&)
210 {
211  std::transform(req.level.begin(), req.level.end(), req.level.begin(), (int(*)(int))std::toupper);
212 
214  if (req.level == "DEBUG")
215  {
217  }
218  else if (req.level == "INFO")
219  {
221  }
222  else if (req.level == "WARN")
223  {
225  }
226  else if (req.level == "ERROR")
227  {
229  }
230  else if (req.level == "FATAL")
231  {
233  }
234  else
235  {
236  return false;
237  }
238 
239  bool success = ::ros::console::set_logger_level(req.logger, level);
240  if (success)
241  {
243  }
244 
245  return success;
246 }
247 
248 bool closeAllConnections(roscpp::Empty::Request&, roscpp::Empty::Response&)
249 {
250  ROSCPP_LOG_DEBUG("close_all_connections service called, closing connections");
252  return true;
253 }
254 
255 void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg)
256 {
257  Time::setNow(msg->clock);
258 }
259 
261 {
262  if (!g_internal_callback_queue)
263  {
264  g_internal_callback_queue.reset(new CallbackQueue);
265  }
266 
268 }
269 
270 void basicSigintHandler(int sig)
271 {
272  (void)sig;
274 }
275 
277 {
278  disableAllSignalsInThisThread();
279 
281 
282  while (!g_shutting_down)
283  {
284  queue->callAvailable(WallDuration(0.1));
285  }
286 }
287 
288 bool isStarted()
289 {
290  return g_started;
291 }
292 
293 void start()
294 {
295  boost::mutex::scoped_lock lock(g_start_mutex);
296  if (g_started)
297  {
298  return;
299  }
300 
301  g_shutdown_requested = false;
302  g_shutting_down = false;
303  g_started = true;
304  g_ok = true;
305 
306  bool enable_debug = false;
307  std::string enable_debug_env;
308  if ( get_environment_variable(enable_debug_env,"ROSCPP_ENABLE_DEBUG") )
309  {
310  try
311  {
312  enable_debug = boost::lexical_cast<bool>(enable_debug_env.c_str());
313  }
314  catch (boost::bad_lexical_cast&)
315  {
316  }
317  }
318 
319  char* env_ipv6 = NULL;
320 #ifdef _MSC_VER
321  _dupenv_s(&env_ipv6, NULL, "ROS_IPV6");
322 #else
323  env_ipv6 = getenv("ROS_IPV6");
324 #endif
325 
326  bool use_ipv6 = (env_ipv6 && strcmp(env_ipv6,"on") == 0);
327  TransportTCP::s_use_ipv6_ = use_ipv6;
329 
330 #ifdef _MSC_VER
331  if (env_ipv6)
332  {
333  free(env_ipv6);
334  }
335 #endif
336 
338 
339  PollManager::instance()->addPollThreadListener(checkForShutdown);
340  XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
341 
343 
344  TopicManager::instance()->start();
345  ServiceManager::instance()->start();
346  ConnectionManager::instance()->start();
347  PollManager::instance()->start();
348  XMLRPCManager::instance()->start();
349 
350  if (!(g_init_options & init_options::NoSigintHandler))
351  {
352  signal(SIGINT, basicSigintHandler);
353  }
354 
355  ros::Time::init();
356 
357  if (!(g_init_options & init_options::NoRosout))
358  {
359  g_rosout_appender = new ROSOutAppender;
360  ros::console::register_appender(g_rosout_appender);
361  }
362 
363  if (g_shutting_down) goto end;
364 
365  {
367  ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
369  ServiceManager::instance()->advertiseService(ops);
370  }
371 
372  if (g_shutting_down) goto end;
373 
374  {
376  ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
378  ServiceManager::instance()->advertiseService(ops);
379  }
380 
381  if (g_shutting_down) goto end;
382 
383  if (enable_debug)
384  {
386  ops.init<roscpp::Empty>(names::resolve("~debug/close_all_connections"), closeAllConnections);
388  ServiceManager::instance()->advertiseService(ops);
389  }
390 
391  if (g_shutting_down) goto end;
392 
393  {
394  bool use_sim_time = false;
395  param::param("/use_sim_time", use_sim_time, use_sim_time);
396 
397  if (use_sim_time)
398  {
400  }
401 
402  if (g_shutting_down) goto end;
403 
404  if (use_sim_time)
405  {
407  ops.init<rosgraph_msgs::Clock>(names::resolve("/clock"), 1, clockCallback);
409  TopicManager::instance()->subscribe(ops);
410  }
411  }
412 
413  if (g_shutting_down) goto end;
414 
415  g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
417 
418  ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], using [%s] time",
419  this_node::getName().c_str(), getpid(), network::getHost().c_str(),
420  XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(),
421  Time::useSystemTime() ? "real" : "sim");
422 
423  // Label used to abort if we've started shutting down in the middle of start(), which can happen in
424  // threaded code or if Ctrl-C is pressed while we're initializing
425 end:
426  // If we received a shutdown request while initializing, wait until we've shutdown to continue
427  if (g_shutting_down)
428  {
429  boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
430  }
431 }
432 
433 void init(const M_string& remappings, const std::string& name, uint32_t options)
434 {
435  if (!g_atexit_registered)
436  {
437  g_atexit_registered = true;
438  atexit(atexitCallback);
439  }
440 
441  if (!g_global_queue)
442  {
443  g_global_queue.reset(new CallbackQueue);
444  }
445 
446  if (!g_initialized)
447  {
448  g_init_options = options;
449  g_ok = true;
450 
452  // Disable SIGPIPE
453 #ifndef WIN32
454  signal(SIGPIPE, SIG_IGN);
455 #endif
456  network::init(remappings);
457  master::init(remappings);
458  // names:: namespace is initialized by this_node
459  this_node::init(name, remappings, options);
460  file_log::init(remappings);
461  param::init(remappings);
462 
463  g_initialized = true;
464  }
465 }
466 
467 void init(int& argc, char** argv, const std::string& name, uint32_t options)
468 {
469  M_string remappings;
470 
471  int full_argc = argc;
472  // now, move the remapping argv's to the end, and decrement argc as needed
473  for (int i = 0; i < argc; )
474  {
475  std::string arg = argv[i];
476  size_t pos = arg.find(":=");
477  if (pos != std::string::npos)
478  {
479  std::string local_name = arg.substr(0, pos);
480  std::string external_name = arg.substr(pos + 2);
481 
482  ROSCPP_LOG_DEBUG("remap: %s => %s", local_name.c_str(), external_name.c_str());
483  remappings[local_name] = external_name;
484 
485  // shuffle everybody down and stuff this guy at the end of argv
486  char *tmp = argv[i];
487  for (int j = i; j < full_argc - 1; j++)
488  argv[j] = argv[j+1];
489  argv[argc-1] = tmp;
490  argc--;
491  }
492  else
493  {
494  i++; // move on, since we didn't shuffle anybody here to replace it
495  }
496  }
497 
498  init(remappings, name, options);
499 }
500 
501 void init(const VP_string& remappings, const std::string& name, uint32_t options)
502 {
503  M_string remappings_map;
504  VP_string::const_iterator it = remappings.begin();
505  VP_string::const_iterator end = remappings.end();
506  for (; it != end; ++it)
507  {
508  remappings_map[it->first] = it->second;
509  }
510 
511  init(remappings_map, name, options);
512 }
513 
514 std::string getROSArg(int argc, const char* const* argv, const std::string& arg)
515 {
516  for (int i = 0; i < argc; ++i)
517  {
518  std::string str_arg = argv[i];
519  size_t pos = str_arg.find(":=");
520  if (str_arg.substr(0,pos) == arg)
521  {
522  return str_arg.substr(pos+2);
523  }
524  }
525  return "";
526 }
527 
528 void removeROSArgs(int argc, const char* const* argv, V_string& args_out)
529 {
530  for (int i = 0; i < argc; ++i)
531  {
532  std::string arg = argv[i];
533  size_t pos = arg.find(":=");
534  if (pos == std::string::npos)
535  {
536  args_out.push_back(arg);
537  }
538  }
539 }
540 
541 void spin()
542 {
544  spin(s);
545 }
546 
547 void spin(Spinner& s)
548 {
549  s.spin();
550 }
551 
552 void spinOnce()
553 {
554  g_global_queue->callAvailable(ros::WallDuration());
555 }
556 
558 {
559  while (ok())
560  {
561  WallDuration(0.05).sleep();
562  }
563 }
564 
566 {
567  return g_global_queue.get();
568 }
569 
570 bool ok()
571 {
572  return g_ok;
573 }
574 
575 void shutdown()
576 {
577  boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
578  if (g_shutting_down)
579  return;
580  else
581  g_shutting_down = true;
582 
584 
585  g_global_queue->disable();
586  g_global_queue->clear();
587 
588  if (g_internal_queue_thread.get_id() != boost::this_thread::get_id())
589  {
590  g_internal_queue_thread.join();
591  }
592 
593  g_rosout_appender = 0;
594 
595  if (g_started)
596  {
597  TopicManager::instance()->shutdown();
598  ServiceManager::instance()->shutdown();
599  PollManager::instance()->shutdown();
600  ConnectionManager::instance()->shutdown();
601  XMLRPCManager::instance()->shutdown();
602  }
603 
604  WallTime end = WallTime::now();
605 
606  g_started = false;
607  g_ok = false;
608  Time::shutdown();
609 }
610 
611 }
CallbackQueueInterface * callback_queue
Queue to add callbacks to. If NULL, the global callback queue will be used.
bool param(const std::string &param_name, T &param_val, const T &default_val)
Assign value from parameter server, with default.
Definition: param.h:608
void init(const std::string &_service, const boost::function< bool(MReq &, MRes &)> &_callback)
Templated convenience method for filling out md5sum/etc. based on the service request/response types...
This is the default implementation of the ros::CallbackQueueInterface.
virtual void spin(CallbackQueue *queue=0)=0
Spin on a callback queue (defaults to the global one). Blocks until roscpp has been shutdown...
ROSCPP_DECL void start()
Actually starts the internals of the node (spins up threads, starts the network polling and xmlrpc lo...
Definition: init.cpp:293
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
bool closeAllConnections(roscpp::Empty::Request &, roscpp::Empty::Response &)
Definition: init.cpp:248
void enable()
Enable the queue (queue is enabled by default)
static boost::recursive_mutex g_shutting_down_mutex
Definition: init.cpp:107
std::vector< std::pair< std::string, std::string > > VP_string
XmlRpc::XmlRpcValue responseInt(int code, const std::string &msg, int response)
void init(const M_string &remappings)
Definition: file_log.cpp:60
int size() const
ROSCPP_DECL bool isInitialized()
Returns whether or not ros::init() has been called.
Definition: init.cpp:110
void init(const M_string &remappings)
Definition: master.cpp:50
XmlRpcServer s
ROSCPP_DECL const std::string & getName()
Returns the name of the current node.
Definition: this_node.cpp:81
static bool useSystemTime()
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
Resolve a graph resource name into a fully qualified graph resource name.
Definition: names.cpp:136
#define ROSCONSOLE_AUTOINIT
Type const & getType() const
#define ROS_WARN(...)
static void setNow(const Time &new_now)
static bool g_ok
Definition: init.cpp:103
void internalCallbackQueueThreadFunc()
Definition: init.cpp:276
bool getLoggers(roscpp::GetLoggers::Request &, roscpp::GetLoggers::Response &resp)
Definition: init.cpp:172
static const ServiceManagerPtr & instance()
void init(const std::string &names, const M_string &remappings, uint32_t options)
Definition: this_node.cpp:101
Encapsulates all options available for creating a ServiceServer.
static void shutdown()
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
Returns a pointer to the global default callback queue.
Definition: init.cpp:565
std::map< std::string, std::string > M_string
#define ROSCPP_LOG_DEBUG(...)
Definition: file_log.h:35
Abstract interface for classes which spin on a callback queue.
Definition: spinner.h:44
void basicSigintHandler(int sig)
Definition: init.cpp:270
std::vector< std::string > V_string
bool sleep() const
void init(const M_string &remappings)
Definition: param.cpp:810
ROSCPP_DECL const std::string & getHost()
Definition: network.cpp:50
void shutdownCallback(XmlRpc::XmlRpcValue &params, XmlRpc::XmlRpcValue &result)
Definition: init.cpp:156
static bool s_use_ipv6_
Definition: transport_tcp.h:60
static const ConnectionManagerPtr & instance()
Encapsulates all options available for creating a Subscriber.
ROSCPP_DECL void initInternalTimerManager()
static void init()
ROSCPP_DECL bool ok()
Check whether it&#39;s time to exit.
Definition: init.cpp:570
static bool s_use_ipv6_
static bool g_started
Definition: init.cpp:100
ROSCPP_DECL bool isShuttingDown()
Returns whether or not ros::shutdown() has been (or is being) called.
Definition: init.cpp:115
bool get_environment_variable(std::string &str, const char *environment_variable)
CallbackQueuePtr getInternalCallbackQueue()
Definition: init.cpp:260
static bool g_atexit_registered
Definition: init.cpp:101
ROSCPP_DECL void spin()
Enter simple event loop.
Definition: init.cpp:541
Spinner which runs in a single thread.
Definition: spinner.h:58
Don&#39;t broadcast rosconsole output to the /rosout topic.
Definition: init.h:63
void checkForShutdown()
Definition: init.cpp:120
static volatile bool g_shutting_down
Definition: init.cpp:106
static bool g_shutdown_requested
Definition: init.cpp:105
ROSCPP_DECL std::string getROSArg(int argc, const char *const *argv, const std::string &arg)
searches the command line arguments for the given arg parameter. In case this argument is not found a...
Definition: init.cpp:514
void clockCallback(const rosgraph_msgs::Clock::ConstPtr &msg)
Definition: init.cpp:255
CallbackQueuePtr g_global_queue
Definition: init.cpp:95
static bool s_use_keepalive_
Definition: transport_tcp.h:59
static const PollManagerPtr & instance()
static CallbackQueuePtr g_internal_callback_queue
Definition: init.cpp:97
ROSCPP_DECL void requestShutdown()
Request that the node shut itself down from within a ROS thread.
Definition: init.cpp:141
static WallTime now()
ROSCPP_DECL bool isStarted()
Returns whether or not the node has been started through ros::start()
Definition: init.cpp:288
ROSCONSOLE_DECL void shutdown()
static boost::mutex g_start_mutex
Definition: init.cpp:102
ROSOutAppender * g_rosout_appender
Definition: init.cpp:96
static const XMLRPCManagerPtr & instance()
ROSCPP_DECL void shutdown()
Disconnects everything and unregisters from the master. It is generally not necessary to call this fu...
Definition: init.cpp:575
ROSCPP_DECL void removeROSArgs(int argc, const char *const *argv, V_string &args_out)
returns a vector of program arguments that do not include any ROS remapping arguments. Useful if you need to parse your arguments to determine your node name
Definition: init.cpp:528
static bool g_initialized
Definition: init.cpp:99
void init(const std::string &_topic, uint32_t _queue_size, const boost::function< void(const boost::shared_ptr< M const > &)> &_callback, const boost::function< boost::shared_ptr< M >(void)> &factory_fn=DefaultMessageCreator< M >())
Templated initialization, templated on message type. Only supports "const boost::shared_ptr<M const>&...
void atexitCallback()
Definition: init.cpp:146
bool setLoggerLevel(roscpp::SetLoggerLevel::Request &req, roscpp::SetLoggerLevel::Response &)
Definition: init.cpp:209
ROSCPP_DECL void spinOnce()
Process a single round of callbacks.
Definition: init.cpp:552
ROSCONSOLE_DECL void register_appender(LogAppender *appender)
static uint32_t g_init_options
Definition: init.cpp:104
CallbackQueueInterface * callback_queue
Queue to add callbacks to. If NULL, the global callback queue will be used.
static boost::thread g_internal_queue_thread
Definition: init.cpp:108
void init(const M_string &remappings)
Definition: network.cpp:186
ROSCPP_DECL void waitForShutdown()
Wait for this node to be shutdown, whether through Ctrl-C, ros::shutdown(), or similar.
Definition: init.cpp:557
static const TopicManagerPtr & instance()


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Wed Dec 20 2017 03:58:41