roscpp_initializer.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 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 /* Author: Ioan Sucan */
36 
39 #include <boost/thread.hpp>
40 #include <ros/ros.h>
41 #include <memory>
42 
43 static std::vector<std::string>& ROScppArgs()
44 {
45  static std::vector<std::string> args;
46  return args;
47 }
48 
49 static std::string& ROScppNodeName()
50 {
51  static std::string node_name("moveit_python_wrappers");
52  return node_name;
53 }
54 
55 void moveit::py_bindings_tools::roscpp_set_arguments(const std::string& node_name, boost::python::list& argv)
56 {
57  ROScppNodeName() = node_name;
58  ROScppArgs() = stringFromList(argv);
59 }
60 
61 namespace
62 {
63 struct InitProxy
64 {
65  InitProxy()
66  {
67  const std::vector<std::string>& args = ROScppArgs();
68  int fake_argc = args.size();
69  char** fake_argv = new char*[args.size()];
70  for (std::size_t i = 0; i < args.size(); ++i)
71  fake_argv[i] = strdup(args[i].c_str());
72 
73  ros::init(fake_argc, fake_argv, ROScppNodeName(),
75  for (int i = 0; i < fake_argc; ++i)
76  delete[] fake_argv[i];
77  delete[] fake_argv;
78  }
79 
80  ~InitProxy()
81  {
83  ros::shutdown();
84  }
85 };
86 }
87 
88 static void roscpp_init_or_stop(bool init)
89 {
90  // ensure we do not accidentally initialize ROS multiple times per process
91  static boost::mutex lock;
92  boost::mutex::scoped_lock slock(lock);
93 
94  // once per process, we start a spinner
95  static bool once = true;
96  static std::unique_ptr<InitProxy> proxy;
97  static std::unique_ptr<ros::AsyncSpinner> spinner;
98 
99  // initialize only once
100  if (once && init)
101  {
102  once = false;
103 
104  // if ROS (cpp) is not initialized, we initialize it
105  if (!ros::isInitialized())
106  {
107  proxy.reset(new InitProxy());
108  spinner.reset(new ros::AsyncSpinner(1));
109  spinner->start();
110  }
111  }
112 
113  // shutdown if needed
114  if (!init)
115  {
116  once = false;
117  proxy.reset();
118  spinner.reset();
119  }
120 }
121 
123 {
124  roscpp_init_or_stop(true);
125 }
126 
127 void moveit::py_bindings_tools::roscpp_init(const std::string& node_name, boost::python::list& argv)
128 {
129  roscpp_set_arguments(node_name, argv);
130  roscpp_init();
131 }
132 
133 void moveit::py_bindings_tools::roscpp_init(boost::python::list& argv)
134 {
135  ROScppArgs() = stringFromList(argv);
136  roscpp_init();
137 }
138 
140 {
141  roscpp_init_or_stop(false);
142 }
143 
145 {
146  roscpp_init();
147 }
148 
150 {
151  roscpp_init(argv);
152 }
153 
154 moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(const std::string& node_name, boost::python::list& argv)
155 {
156  roscpp_init(node_name, argv);
157 }
void roscpp_set_arguments(const std::string &node_name, boost::python::list &argv)
This function can be used to specify the ROS command line arguments for the internal ROScpp instance;...
ROSCPP_DECL bool isInitialized()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< std::string > stringFromList(const boost::python::object &values)
static void roscpp_init_or_stop(bool init)
void spinner()
static std::vector< std::string > & ROScppArgs()
void roscpp_init(const std::string &node_name, boost::python::list &argv)
Initialize ROScpp with specified command line args.
ROSCPP_DECL bool isShuttingDown()
static std::string & ROScppNodeName()
ROSCPP_DECL void shutdown()


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:50