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


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Sun Feb 3 2019 03:29:54