ros_plugin.cpp

Go to the documentation of this file.

/***************************************************************************
  tag: Ruben Smits  Tue Nov 16 09:19:02 CET 2010  ros_plugin.cpp

                        ros_plugin.cpp -  description
                           -------------------
    begin                : Tue November 16 2010
    copyright            : (C) 2010 Ruben Smits
    email                : first.last@mech.kuleuven.be

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/


#include <rtt/plugin/Plugin.hpp>
#include <rtt/TaskContext.hpp>
#include <rtt/Activity.hpp>
#include <rtt/internal/GlobalService.hpp>
#include <rtt/Logger.hpp>
#include <rtt/os/startstop.h>
#include <ros/ros.h>

using namespace RTT;

static void loadROSService()
{
  RTT::Service::shared_ptr ros = RTT::internal::GlobalService::Instance()->provides("ros");

  ros->addOperation("getNodeName", &ros::this_node::getName)
          .doc("Return full name of ROS node.");
  ros->addOperation("getNamespace", &ros::this_node::getNamespace)
          .doc("Return ROS node namespace.");
}

extern "C" {
  bool loadRTTPlugin(RTT::TaskContext* c){

    // Initialize ROS if necessary
    if (!ros::isInitialized()) {
      log(Info) << "Initializing ROS node in Orocos plugin..." << endlog();

      int argc = __os_main_argc();
      char ** argv = __os_main_argv();

      // copy the argv array of C strings into a std::vector<char *>
      // Rationale: ros::init(int &argc, char **argv) removes some of the
      // command line arguments and rearranges the remaining ones in the argv
      // vector.
      // See https://github.com/orocos/rtt_ros_integration/issues/54
      std::vector<char *> argvv(argv, argv + argc);
      assert(argvv.size() == argc);
      ros::init(argc, argvv.data(), "rtt", ros::init_options::AnonymousName);
      argvv.resize(argc);

      if (ros::master::check()) {
        ros::start();
      } else {
        log(Warning) << "'roscore' is not running: no ROS functions will be available." << endlog();
        ros::shutdown();
        return true;
      }

          // Register new operations in global ros Service
          loadROSService();
    }
          
    // Defaults the number of threads to the number of CPUs available on the machine
    int thread_count = 0;
    // get number of spinners from parameter server, if available
    ros::param::get("~spinner_threads", thread_count);

    // Create an asynchronous spinner to handle the default callback queue 
    static ros::AsyncSpinner spinner(thread_count); // Use thread_count threads

    // TODO: Check spinner.canStart() to suppress errors / warnings once it's incorporated into ROS
    spinner.start();
    log(Info) << "ROS node spinner started (" << thread_count << " " << (thread_count > 1 ? "threads" : "thread") << ")." << endlog();

    return true;
  }

  std::string getRTTPluginName () {
    return "rosnode";
  }

  std::string getRTTTargetName () {
    return OROCOS_TARGET_NAME;
  }
}