ROS2: Add base class for Node in gazebo plugins

gazebo_ros/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.5)
+project(gazebo_ros)
+
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(gazebo_dev REQUIRED)
+
+include_directories(include)
+
+add_library(gazebo_ros_node SHARED src/node.cpp src/executor.cpp)
+ament_target_dependencies(gazebo_ros_node "rclcpp" "gazebo_dev")
+
+ament_export_include_directories(include)
+ament_export_libraries(gazebo_ros_node)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(gazebo_dev)
+
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
+ament_package()
+
+install(DIRECTORY include/
+        DESTINATION include)
+
+install(TARGETS gazebo_ros_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+

gazebo_ros/include/gazebo_ros/executor.hpp

@@ -0,0 +1,51 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GAZEBO_ROS__EXECUTOR_HPP_
+#define GAZEBO_ROS__EXECUTOR_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo/common/Events.hh>
+
+namespace gazebo_ros
+{
+
+/// Executor run in a separate thread to handle events from all #gazebo_ros::Node instances
+/**
+ * \class Executor executor.hpp <gazebo_ros/executor.hpp>
+ */
+class Executor : public rclcpp::executors::MultiThreadedExecutor
+{
+public:
+  /// Create an instance and start the internal thread
+  Executor();
+
+  /// Stops the internal executor and joins with the internal thread
+  virtual ~Executor();
+
+private:
+  /// Thread where the executor spins until destruction
+  std::thread spin_thread_;
+
+  /// Spin executor, called in #spin_thread_
+  void run();
+
+  /// Shutdown ROS, called when gazebo sigint handle arrives so ROS is shutdown cleanly
+  void shutdown();
+
+  /// Connection to gazebo sigint event
+  gazebo::event::ConnectionPtr sigint_handle_;
+};
+}  // namespace gazebo_ros
+#endif  // GAZEBO_ROS__EXECUTOR_HPP_

gazebo_ros/include/gazebo_ros/node.hpp

@@ -0,0 +1,114 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GAZEBO_ROS__NODE_HPP_
+#define GAZEBO_ROS__NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gazebo_ros/executor.hpp>
+
+#include <atomic>
+#include <memory>
+#include <mutex>
+#include <string>
+#include <utility>
+
+namespace gazebo_ros
+{
+/// ROS Node for gazebo plugins
+/**
+ * \class Node node.hpp <gazebo_ros/node.hpp>
+ * Wrapper around an rclcpp::Node which ensures all instances share an executor.
+ * \include gazebo_ros/node.hpp
+ */
+class Node : public rclcpp::Node
+{
+public:
+  /// Shared pointer to a #gazebo_ros::Node
+  typedef std::shared_ptr<Node> SharedPtr;
+
+  /// Destructor
+  virtual ~Node();
+
+  /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
+  /**
+   * \details This will call rclcpp::init if it hasn't been called yet.
+   * \param[in] node_name Name for the new node to create
+   * \return A shared pointer to a new #gazebo_ros::Node, or nullptr if failed to create.
+   */
+  static SharedPtr Create(const std::string & node_name);
+
+  /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
+  /**
+   * \details This will call rclcpp::init if it hasn't been called yet.
+   * \details Forwards arguments to the constructor for rclcpp::Node
+   * \param[in] args List of arguments to pass to <a href="http://docs.ros2.org/latest/api/rclcpp/classrclcpp_1_1_node.html">rclcpp::Node</a>
+   * \return A shared pointer to a new #gazebo_ros::Node, or nullptr if failed to create.
+   */
+  template<typename ... Args>
+  static SharedPtr Create(Args && ... args);
+
+private:
+  /// Inherit constructor
+  using rclcpp::Node::Node;
+
+  /// Points to #static_executor_, so that when all #gazebo_ros::Node instances are destroyed, the
+  /// executor thread is too
+  std::shared_ptr<Executor> executor_;
+
+  /// Locks #initialized_ and #executor_
+  static std::mutex lock_;
+
+  /// Points to an #gazebo_ros::Executor shared between all #gazebo_ros::Node instances
+  static std::weak_ptr<Executor> static_executor_;
+};
+
+template<typename ... Args>
+Node::SharedPtr Node::Create(Args && ... args)
+{
+  std::lock_guard<std::mutex> l(lock_);
+
+  // Contruct Node by forwarding arguments
+  // TODO(chapulina): use rclcpp::is_initialized() once that's available, see
+  // https://github.com/ros2/rclcpp/issues/518
+  Node::SharedPtr node;
+  try
+  {
+    node = std::make_shared<Node>(std::forward<Args>(args) ...);
+  }
+  catch(rclcpp::exceptions::RCLError e)
+  {
+    rclcpp::init(0, nullptr);
+    RCLCPP_INFO(rclcpp::get_logger("gazebo_ros_node"),
+      "ROS was initialized without arguments.");
+    node = std::make_shared<Node>(std::forward<Args>(args) ...);
+  }
+
+  // Store shared pointer to static executor in this object
+  node->executor_ = static_executor_.lock();
+
+  // If executor has not been contructed yet, do so now
+  if (!node->executor_) {
+    node->executor_ = std::make_shared<Executor>();
+    static_executor_ = node->executor_;
+  }
+
+  // Add new node to the executor so its callbacks are called
+  node->executor_->add_node(node);
+
+  return node;
+}
+}  // namespace gazebo_ros
+#endif  // GAZEBO_ROS__NODE_HPP_