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 
320 
321  PollManager::instance()->addPollThreadListener(checkForShutdown);
322  XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
323 
325 
326  TopicManager::instance()->start();
327  ServiceManager::instance()->start();
328  ConnectionManager::instance()->start();
329  PollManager::instance()->start();
330  XMLRPCManager::instance()->start();
331 
332  if (!(g_init_options & init_options::NoSigintHandler))
333  {
334  signal(SIGINT, basicSigintHandler);
335  }
336 
337  ros::Time::init();
338 
339  if (!(g_init_options & init_options::NoRosout))
340  {
341  g_rosout_appender = new ROSOutAppender;
342  ros::console::register_appender(g_rosout_appender);
343  }
344 
345  if (g_shutting_down) goto end;
346 
347  {
349  ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
351  ServiceManager::instance()->advertiseService(ops);
352  }
353 
354  if (g_shutting_down) goto end;
355 
356  {
358  ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
360  ServiceManager::instance()->advertiseService(ops);
361  }
362 
363  if (g_shutting_down) goto end;
364 
365  if (enable_debug)
366  {
368  ops.init<roscpp::Empty>(names::resolve("~debug/close_all_connections"), closeAllConnections);
370  ServiceManager::instance()->advertiseService(ops);
371  }
372 
373  if (g_shutting_down) goto end;
374 
375  {
376  bool use_sim_time = false;
377  param::param("/use_sim_time", use_sim_time, use_sim_time);
378 
379  if (use_sim_time)
380  {
382  }
383 
384  if (g_shutting_down) goto end;
385 
386  if (use_sim_time)
387  {
389  ops.init<rosgraph_msgs::Clock>(names::resolve("/clock"), 1, clockCallback);
391  TopicManager::instance()->subscribe(ops);
392  }
393  }
394 
395  if (g_shutting_down) goto end;
396 
397  g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
399 
400  ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], using [%s] time",
401  this_node::getName().c_str(), getpid(), network::getHost().c_str(),
402  XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(),
403  Time::useSystemTime() ? "real" : "sim");
404 
405  // Label used to abort if we've started shutting down in the middle of start(), which can happen in
406  // threaded code or if Ctrl-C is pressed while we're initializing
407 end:
408  // If we received a shutdown request while initializing, wait until we've shutdown to continue
409  if (g_shutting_down)
410  {
411  boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
412  }
413 }
414 
416  char* env_ipv6 = NULL;
417 #ifdef _MSC_VER
418  _dupenv_s(&env_ipv6, NULL, "ROS_IPV6");
419 #else
420  env_ipv6 = getenv("ROS_IPV6");
421 #endif
422 
423  bool use_ipv6 = (env_ipv6 && strcmp(env_ipv6,"on") == 0);
424  TransportTCP::s_use_ipv6_ = use_ipv6;
426 
427 #ifdef _MSC_VER
428  if (env_ipv6)
429  {
430  free(env_ipv6);
431  }
432 #endif
433 }
434 
435 void init(const M_string& remappings, const std::string& name, uint32_t options)
436 {
437  if (!g_atexit_registered)
438  {
439  g_atexit_registered = true;
440  atexit(atexitCallback);
441  }
442 
443  if (!g_global_queue)
444  {
445  g_global_queue.reset(new CallbackQueue);
446  }
447 
448  if (!g_initialized)
449  {
450  g_init_options = options;
451  g_ok = true;
452 
454  // Disable SIGPIPE
455 #ifndef WIN32
456  signal(SIGPIPE, SIG_IGN);
457 #else
458  WSADATA wsaData;
459  WSAStartup(MAKEWORD(2, 0), &wsaData);
460 #endif
462  network::init(remappings);
463  master::init(remappings);
464  // names:: namespace is initialized by this_node
465  this_node::init(name, remappings, options);
466  file_log::init(remappings);
467  param::init(remappings);
468 
469  g_initialized = true;
470  }
471 }
472 
473 void init(int& argc, char** argv, const std::string& name, uint32_t options)
474 {
475  M_string remappings;
476 
477  int full_argc = argc;
478  // now, move the remapping argv's to the end, and decrement argc as needed
479  for (int i = 0; i < argc; )
480  {
481  std::string arg = argv[i];
482  size_t pos = arg.find(":=");
483  if (pos != std::string::npos)
484  {
485  std::string local_name = arg.substr(0, pos);
486  std::string external_name = arg.substr(pos + 2);
487 
488  ROSCPP_LOG_DEBUG("remap: %s => %s", local_name.c_str(), external_name.c_str());
489  remappings[local_name] = external_name;
490 
491  // shuffle everybody down and stuff this guy at the end of argv
492  char *tmp = argv[i];
493  for (int j = i; j < full_argc - 1; j++)
494  argv[j] = argv[j+1];
495  argv[argc-1] = tmp;
496  argc--;
497  }
498  else
499  {
500  i++; // move on, since we didn't shuffle anybody here to replace it
501  }
502  }
503 
504  init(remappings, name, options);
505 }
506 
507 void init(const VP_string& remappings, const std::string& name, uint32_t options)
508 {
509  M_string remappings_map;
510  VP_string::const_iterator it = remappings.begin();
511  VP_string::const_iterator end = remappings.end();
512  for (; it != end; ++it)
513  {
514  remappings_map[it->first] = it->second;
515  }
516 
517  init(remappings_map, name, options);
518 }
519 
520 std::string getROSArg(int argc, const char* const* argv, const std::string& arg)
521 {
522  for (int i = 0; i < argc; ++i)
523  {
524  std::string str_arg = argv[i];
525  size_t pos = str_arg.find(":=");
526  if (str_arg.substr(0,pos) == arg)
527  {
528  return str_arg.substr(pos+2);
529  }
530  }
531  return "";
532 }
533 
534 void removeROSArgs(int argc, const char* const* argv, V_string& args_out)
535 {
536  for (int i = 0; i < argc; ++i)
537  {
538  std::string arg = argv[i];
539  size_t pos = arg.find(":=");
540  if (pos == std::string::npos)
541  {
542  args_out.push_back(arg);
543  }
544  }
545 }
546 
547 void spin()
548 {
550  spin(s);
551 }
552 
553 void spin(Spinner& s)
554 {
555  s.spin();
556 }
557 
558 void spinOnce()
559 {
560  g_global_queue->callAvailable(ros::WallDuration());
561 }
562 
564 {
565  while (ok())
566  {
567  WallDuration(0.05).sleep();
568  }
569 }
570 
572 {
573  return g_global_queue.get();
574 }
575 
576 bool ok()
577 {
578  return g_ok;
579 }
580 
581 void shutdown()
582 {
583  boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
584  if (g_shutting_down)
585  return;
586  else
587  g_shutting_down = true;
588 
590 
591  g_global_queue->disable();
592  g_global_queue->clear();
593 
594  if (g_internal_queue_thread.get_id() != boost::this_thread::get_id())
595  {
596  g_internal_queue_thread.join();
597  }
598  //ros::console::deregister_appender(g_rosout_appender);
599  delete g_rosout_appender;
600  g_rosout_appender = 0;
601 
602  if (g_started)
603  {
604  TopicManager::instance()->shutdown();
605  ServiceManager::instance()->shutdown();
606  PollManager::instance()->shutdown();
607  ConnectionManager::instance()->shutdown();
608  XMLRPCManager::instance()->shutdown();
609  }
610 
611  g_started = false;
612  g_ok = false;
613  Time::shutdown();
614 }
615 
616 }
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
ROSCPP_DECL XmlRpc::XmlRpcValue responseInt(int code, const std::string &msg, int response)
void init(const M_string &remappings)
Definition: file_log.cpp:60
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
#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()
void check_ipv6_environment()
Definition: init.cpp:415
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
Returns a pointer to the global default callback queue.
Definition: init.cpp:571
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
Type const & getType() const
std::vector< std::string > V_string
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:576
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
bool sleep() const
ROSCPP_DECL void spin()
Enter simple event loop.
Definition: init.cpp:547
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:520
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
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>&...
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:581
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:534
static bool g_initialized
Definition: init.cpp:99
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:558
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:563
static const TopicManagerPtr & instance()


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Wed Oct 16 2019 03:27:43