roscpp_initializer.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "moveit/py_bindings_tools/roscpp_initializer.h"
#include "moveit/py_bindings_tools/py_conversions.h"
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <ros/ros.h>

static std::vector<std::string>& ROScppArgs()
{
  static std::vector<std::string> args;
  return args;
}

static std::string& ROScppNodeName()
{
  static std::string node_name("moveit_python_wrappers");
  return node_name;
}

void moveit::py_bindings_tools::roscpp_set_arguments(const std::string &node_name, boost::python::list &argv)
{
  ROScppNodeName() = node_name;
  ROScppArgs() = stringFromList(argv);
}

namespace
{

struct InitProxy
{
  InitProxy()
  {
    const std::vector<std::string> &args = ROScppArgs();
    int fake_argc = args.size();
    char **fake_argv = new char*[args.size()];
    for (std::size_t i = 0 ; i < args.size() ; ++i)
      fake_argv[i] = strdup(args[i].c_str());

    ros::init(fake_argc, fake_argv, ROScppNodeName(), ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
    for (int i = 0 ; i < fake_argc ; ++i)
      delete[] fake_argv[i];
    delete[] fake_argv;
  }

  ~InitProxy()
  {
    if (ros::isInitialized() && !ros::isShuttingDown())
      ros::shutdown();
  }
};

}

static void roscpp_init_or_stop(bool init)
{
  // ensure we do not accidentally initialize ROS multiple times per process
  static boost::mutex lock;
  boost::mutex::scoped_lock slock(lock);

  // once per process, we start a spinner
  static bool once = true;
  static boost::scoped_ptr<InitProxy> proxy;
  static boost::scoped_ptr<ros::AsyncSpinner> spinner;

  // initialize only once
  if (once && init)
  {
    once = false;

    // if ROS (cpp) is not initialized, we initialize it
    if (!ros::isInitialized())
    {
      proxy.reset(new InitProxy());
      spinner.reset(new ros::AsyncSpinner(1));
      spinner->start();
    }
  }

  // shutdown if needed
  if (!init)
  {
    once = false;
    proxy.reset();
    spinner.reset();
  }
}

void moveit::py_bindings_tools::roscpp_init()
{
  roscpp_init_or_stop(true);
}

void moveit::py_bindings_tools::roscpp_init(const std::string &node_name, boost::python::list &argv)
{
  roscpp_set_arguments(node_name, argv);
  roscpp_init();
}

void moveit::py_bindings_tools::roscpp_init(boost::python::list &argv)
{
  ROScppArgs() = stringFromList(argv);
  roscpp_init();
}

void moveit::py_bindings_tools::roscpp_shutdown()
{
  roscpp_init_or_stop(false);
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer()
{
  roscpp_init();
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(boost::python::list &argv)
{
  roscpp_init(argv);
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(const std::string &node_name, boost::python::list &argv)
{
  roscpp_init(node_name, argv);
}