Remove internal logic to check init, add more tests

gazebo_ros/CMakeLists.txt

@@ -27,15 +27,7 @@ ament_export_dependencies(rclcpp)
 ament_export_dependencies(gazebo_dev)
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(std_msgs REQUIRED)
-  add_library(gazebo_ros_plugin_example SHARED test/gazebo_ros_plugin_example.cpp)
-  target_link_libraries(gazebo_ros_plugin_example gazebo_ros_node)
-  ament_target_dependencies(gazebo_ros_plugin_example "rclcpp" "gazebo_dev" "std_msgs")
-  ament_add_gtest(test_gazebo_ros_plugin_example test/test_gazebo_ros_plugin_example.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR})
-  ament_target_dependencies(test_gazebo_ros_plugin_example "rclcpp" "std_msgs")
+  add_subdirectory(test)
 endif()
 
 ament_package()

gazebo_ros/include/gazebo_ros/executor.hpp

@@ -16,7 +16,7 @@
 #define GAZEBO_ROS__EXECUTOR_HPP_
 
 #include <rclcpp/rclcpp.hpp>
-#include <gazebo/common/common.hh>
+#include <gazebo/common/Events.hh>
 
 namespace gazebo_ros
 {

gazebo_ros/include/gazebo_ros/node.hpp

@@ -35,13 +35,6 @@ namespace gazebo_ros
  */
 class Node : public rclcpp::Node
 {
-  /// Exception thrown when a #Create is called for a Node before #InitROS has ever been called
-  class NotInitializedException : public std::runtime_error
-  {
-public:
-    NotInitializedException();
-  };
-
 public:
   /// Shared pointer to a #gazebo_ros::Node
   typedef std::shared_ptr<Node> SharedPtr;
@@ -51,61 +44,20 @@ class Node : public rclcpp::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
+   * \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.
   /**
-   * \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
+   * \return A shared pointer to a new #gazebo_ros::Node, or nullptr if failed to create.
    */
   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;
@@ -114,9 +66,6 @@ class Node : public rclcpp::Node
   /// 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_;
 
@@ -127,17 +76,26 @@ class Node : public rclcpp::Node
 template<typename ... Args>
 Node::SharedPtr Node::Create(Args && ... args)
 {
-  // Throw exception is Node is created before ROS is initialized
-  if (!initialized_) {
-    throw NotInitializedException();
-  }
-
   // Contruct Node by forwarding arguments
-  Node::SharedPtr node = std::make_shared<Node>(std::forward<Args>(args) ...);
+  // TODO(chapulina): use rclcpp::isInitialized() 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_WARN(rclcpp::get_logger("gazebo_ros_node"),
+      "Called Node::Create before rclcpp::init, node not created.");
+    return nullptr;
+  }
 
   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 contructed yet, do so now
   if (!node->executor_) {
     node->executor_ = std::make_shared<Executor>();
@@ -146,6 +104,7 @@ Node::SharedPtr Node::Create(Args && ... args)
 
   // Add new node to the executor so its callbacks are called
   node->executor_->add_node(node);
+
   return node;
 }
 }  // namespace gazebo_ros

gazebo_ros/src/node.cpp

@@ -22,30 +22,11 @@ namespace gazebo_ros
 
 std::weak_ptr<Executor> Node::static_executor_;
 std::mutex Node::lock_;
-std::atomic_bool Node::initialized_(false);
-
-Node::NotInitializedException::NotInitializedException()
-: std::runtime_error("Called LoadNode before InitROS. Be sure to load the ros_api plugin first")
-{
-}
 
 Node::~Node()
 {
 }
 
-void Node::InitROS(int argc, char ** argv)
-{
-  std::lock_guard<std::mutex> l(lock_);
-  if (initialized_) {
-    RCLCPP_WARN(rclcpp::get_logger("gazebo_ros_node"),
-      "Called Node::InitROS twice, ignoring second call.");
-    return;
-  }
-
-  rclcpp::init(argc, argv);
-  initialized_ = true;
-}
-
 Node::SharedPtr Node::Create(const std::string & node_name)
 {
   return Create<const std::string &>(node_name);

gazebo_ros/test/CMakeLists.txt

@@ -0,0 +1,41 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(ament_lint_auto REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# Linters
+ament_lint_auto_find_test_dependencies()
+
+# Plugins
+set(plugins
+  create_before_init
+  multiple_nodes
+  proper_init
+)
+
+foreach(plugin ${plugins})
+  add_library(${plugin} SHARED plugins/${plugin}.cpp)
+  target_link_libraries(${plugin} gazebo_ros_node)
+  ament_target_dependencies(${plugin}
+    "rclcpp"
+    "gazebo_dev"
+    "std_msgs"
+  )
+endforeach()
+
+# Tests
+set(tests
+  test_plugins
+)
+
+foreach(test ${tests})
+  ament_add_gtest(${test}
+    ${test}.cpp
+    WORKING_DIRECTORY
+      ${CMAKE_CURRENT_BINARY_DIR}
+  )
+  ament_target_dependencies(${test}
+    "rclcpp"
+    "std_msgs"
+  )
+endforeach()
+

gazebo_ros/test/plugins/create_before_init.cpp

@@ -0,0 +1,68 @@
+// 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.
+
+#include <gazebo/common/Plugin.hh>
+
+#include <std_msgs/msg/string.hpp>
+#include <gazebo_ros/node.hpp>
+
+#include <memory>
+
+/// Simple example of a gazebo system plugin which uses a ROS2 node with gazebo_ros::Node.
+class CreateBeforeInit : public gazebo::SystemPlugin
+{
+public:
+  /// Called by gazebo to load plugin. Creates #node, #timer_, and #pub
+  /// \param[in] argc Argument count.
+  /// \param[in] argv Argument values.
+  void Load(int argc, char ** argv);
+
+private:
+  /// Timer called to publish a message every second
+  std::shared_ptr<rclcpp::TimerBase> timer_;
+};
+
+void CreateBeforeInit::Load(int argc, char ** argv)
+{
+  // Try creating before initializing
+  auto node = gazebo_ros::Node::Create("create_before_init");
+  assert(nullptr == node);
+
+  // Initialize ROS with arguments
+  rclcpp::init(argc, argv);
+
+  // Now it should be created
+  node = gazebo_ros::Node::Create("create_before_init");
+  assert(nullptr != node);
+
+  // Create a publisher
+  auto pub = node->create_publisher<std_msgs::msg::String>("test");
+
+  // Run lambda every 1 second
+  using namespace std::chrono_literals;
+  timer_ = node->create_wall_timer(1s,
+      [node, pub]() {
+        // Create string message
+        auto msg = std_msgs::msg::String();
+        msg.data = "Hello world";
+
+        // Warn with this node's name (to test logging)
+        RCLCPP_WARN(node->get_logger(), "Publishing");
+
+        // Publish message
+        pub->publish(msg);
+      });
+}
+
+GZ_REGISTER_SYSTEM_PLUGIN(CreateBeforeInit)