Create base Node class for gazebo plugins with ROS2

gazebo_ros/CMakeLists.txt

@@ -1,132 +1,37 @@
-cmake_minimum_required(VERSION 3.6.3)
+cmake_minimum_required(VERSION 3.5)
 project(gazebo_ros)
 
-find_package(catkin REQUIRED COMPONENTS
-  gazebo_dev
-  cmake_modules
-  roslib
-  roscpp
-  geometry_msgs
-  std_srvs
-  tf
-  rosgraph_msgs
-  dynamic_reconfigure
-  std_msgs
-  gazebo_msgs
-)
 
-# Through transitive dependencies in the packages above, gazebo_ros depends
-# on Simbody.  There is a bug in the Ubuntu Artful (17.10) version of the
-# Simbody package where it includes /usr/lib/libblas.so and
-# /usr/lib/liblapack.so in the CMake list of libraries even though neither of
-# those two paths exist (they both really live in /usr/lib/<arch>-linux-gnu).
-# We remove these two during build-time on artful below; this works because
-# they both will get resolved to the proper paths during runtime linking.
-find_program(LSB_RELEASE_EXEC lsb_release)
-if(NOT LSB_RELEASE_EXEC STREQUAL "LSB_RELEASE_EXEC-NOTFOUND")
-  execute_process(COMMAND ${LSB_RELEASE_EXEC} -cs
-    OUTPUT_VARIABLE OS_CODENAME
-    OUTPUT_STRIP_TRAILING_WHITESPACE
-  )
-  if(OS_CODENAME STREQUAL "artful")
-    list(FILTER catkin_LIBRARIES EXCLUDE REGEX "/usr/lib/libblas.so")
-    list(FILTER catkin_LIBRARIES EXCLUDE REGEX "/usr/lib/liblapack.so")
-  endif()
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
 endif()
-
-include (FindPkgConfig)
-if (PKG_CONFIG_FOUND)
-  pkg_check_modules(XML libxml-2.0)
-else()
-  message(FATAL_ERROR "pkg-config is required; please install it")
+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(Boost REQUIRED COMPONENTS thread)
-
-find_package(TinyXML REQUIRED)
-
-catkin_python_setup()
-
-generate_dynamic_reconfigure_options(cfg/Physics.cfg)
-
-catkin_package(
-  LIBRARIES
-    gazebo_ros_api_plugin
-    gazebo_ros_paths_plugin
-
-  CATKIN_DEPENDS
-    roslib
-    roscpp
-    geometry_msgs
-    std_srvs
-    tf
-    rosgraph_msgs
-    dynamic_reconfigure
-    std_msgs
-    gazebo_msgs
-
-  DEPENDS
-    TinyXML
-)
-
-include_directories(
-  include
-  ${Boost_INCLUDE_DIRS}
-  ${catkin_INCLUDE_DIRS}
-  ${TinyXML_INCLUDE_DIRS})
-
-link_directories(${catkin_LIBRARY_DIRS})
-
-set(cxx_flags)
-foreach (item ${GAZEBO_CFLAGS})
-  set(cxx_flags "${cxx_flags} ${item}")
-endforeach ()
-
-set(ld_flags)
-foreach (item ${GAZEBO_LDFLAGS})
-  set(ld_flags "${ld_flags} ${item}")
-endforeach ()
-
-## Plugins
-add_library(gazebo_ros_api_plugin src/gazebo_ros_api_plugin.cpp)
-add_dependencies(gazebo_ros_api_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-set_target_properties(gazebo_ros_api_plugin PROPERTIES LINK_FLAGS "${ld_flags}")
-set_target_properties(gazebo_ros_api_plugin PROPERTIES COMPILE_FLAGS "${cxx_flags}")
-target_link_libraries(gazebo_ros_api_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TinyXML_LIBRARIES})
-
-add_library(gazebo_ros_paths_plugin src/gazebo_ros_paths_plugin.cpp)
-add_dependencies(gazebo_ros_paths_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-set_target_properties(gazebo_ros_paths_plugin PROPERTIES COMPILE_FLAGS "${cxx_flags}")
-set_target_properties(gazebo_ros_paths_plugin PROPERTIES LINK_FLAGS "${ld_flags}")
-target_link_libraries(gazebo_ros_paths_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(gazebo_dev REQUIRED)
 
-## Tests
+include_directories(include)
 
-add_subdirectory(test)
+add_library(gazebo_ros_node SHARED src/node.cpp src/executor.cpp)
+ament_target_dependencies(gazebo_ros_node rclcpp gazebo_dev)
 
-# Install Gazebo System Plugins
-install(TARGETS gazebo_ros_api_plugin gazebo_ros_paths_plugin
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  )
+ament_export_include_directories(include)
+ament_export_libraries(gazebo_ros_node)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(gazebo_dev)
 
-# Install Gazebo Scripts
-install(PROGRAMS scripts/gazebo
-                 scripts/debug
-		 scripts/gzclient
-		 scripts/gzserver
-		 scripts/gdbrun
-		 scripts/perf
-		 scripts/libcommon.sh
-        DESTINATION
-                 ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+ament_package()
 
-# This one is a Python program, not a shell script, so install it separately
-catkin_install_python(PROGRAMS scripts/spawn_model
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+install(DIRECTORY include/
+        DESTINATION include)
 
-# Install Gazebo launch files
-install(DIRECTORY launch/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
+install(TARGETS gazebo_ros_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)

gazebo_ros/include/gazebo_ros/executor.hpp

@@ -0,0 +1,52 @@
+// 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/common.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/include/gazebo_ros/node.hpp

@@ -0,0 +1,152 @@
+// 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 <memory>
+#include <mutex>
+#include <atomic>
+
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo/common/common.hh>
+
+#include <gazebo_ros/executor.hpp>
+
+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:
+  /// Exception thrown when a #Create is called for a Node before #InitROS has ever been called
+  class NotInitializedException : public std::runtime_error
+  {
+    public:
+      NotInitializedException();
+  };
+
+  /// 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.
+  /**
+   * \note Must NOT called until #InitROS has been called
+   * \param[in] node_name Name for the new node to create
+   * \return A shared pointer to a new #gazebo_ros::Node
+   */
+  static SharedPtr Create(const std::string& node_name);
+
+  /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
+  /**
+   * \todo Implement
+   * \note Must NOT called until #InitROS has been called
+   * \details Sets namespace, remappings, and parameters from SDF.
+   * SDF is in the form:
+   * \code{.xml}
+   * <!-- Optional configurations for a plugin's Node -->
+   * <ros>
+   *  <!-- Name of node, only use if plugin is one node -->
+   *  <node_name></node_name>
+   *  <!-- Namespace of the node -->
+   *  <namespace></namespace>
+   *  <!-- Command line arguments sent to Node's constructor for remappings -->
+   *  <arguments>
+   *     <argument>my_topic:=new_topic</argument>
+   *     <argument>__name:=super_cool_node</argument>
+   *  </arguments>
+   *  <!-- Initial ROS params set for node -->
+   *  <parameters>
+   *     <parameter name="max_velocity">55</parameter>
+   *     <parameter name="publish_odom">True</parameter>
+   *  </parameters>
+   * </ros>
+   * \endcode
+   * \param[in] node_name Name of node to create
+   * \param[in] _sdf An SDF element which either is a <ros> element or contains a <ros> element
+   * \return A shared pointer to a new #gazebo_ros::Node
+   */
+  static SharedPtr Create(const std::string& node_name, sdf::ElementPtr _sdf);
+
+  /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor.
+  /**
+   * \note Must NOT called until #InitROS has been called
+   * \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
+   */
+  template <typename ...Args>
+  static SharedPtr Create(Args && ...args);
+
+  /// Initialize ROS with the command line arguments.
+  /**
+   * \note Must be called ONCE before any #gazebo_ros::Node instances are created. Subsequent calls are ignored.
+   * \param[in] argc Number of arguments
+   * \param[in] argv Vector of c-strings of length \a argc
+   */
+  static void InitROS(int argc, char** argv);
+
+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_;
+
+  /// true if #InitROS has been called and future calls will be ignored
+  static std::atomic_bool initialized_;
+
+  /// 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)
+{
+  // Throw exception if Node is created before ROS is initialized
+  if (!initialized_)
+    throw NotInitializedException();
+
+  // Construct Node by forwarding arguments
+  Node::SharedPtr node = std::make_shared<Node>(std::forward<Args>(args)...);
+
+  std::lock_guard<std::mutex> l(lock_);
+  // Store shared pointer to static executor in this object
+  node->executor_ = static_executor_.lock();
+  // If executor has not been constructed 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