Add simple test for gazebo_ros::Node

gazebo_ros/CMakeLists.txt

@@ -19,7 +19,7 @@ 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_target_dependencies(gazebo_ros_node "rclcpp" "gazebo_dev")
 
 ament_export_include_directories(include)
 ament_export_libraries(gazebo_ros_node)
@@ -29,6 +29,13 @@ 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")
 endif()
 
 ament_package()

gazebo_ros/package.xml

@@ -29,6 +29,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>std_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

gazebo_ros/test/gazebo_ros_plugin_example.cpp

@@ -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.
+
+#include "gazebo_ros_plugin_example.hpp"
+
+GazeboRosNodeExample::GazeboRosNodeExample()
+{
+}
+
+GazeboRosNodeExample::~GazeboRosNodeExample()
+{
+}
+
+void GazeboRosNodeExample::Load(int argc, char ** argv)
+{
+  // Initialize ROS with arguments
+  gazebo_ros::Node::InitROS(argc, argv);
+
+  // Create the ROS node
+  node_ = gazebo_ros::Node::Create("gazebo_ros_node_example");
+
+  // Create a publisher
+  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,
+      [this]() {
+        // 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
+        this->pub_->publish(msg);
+      });
+}
+
+GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosNodeExample)

gazebo_ros/test/gazebo_ros_plugin_example.hpp

@@ -0,0 +1,43 @@
+// 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_PLUGIN_EXAMPLE_HPP_
+#define GAZEBO_ROS_PLUGIN_EXAMPLE_HPP_
+
+#include <gazebo/common/common.hh>
+
+#include <std_msgs/msg/string.hpp>
+#include <gazebo_ros/node.hpp>
+
+#include <memory>
+
+/// Simple example of a gazebo plugin which uses a ROS2 node with gazebo_ros::Node
+class GazeboRosNodeExample : public gazebo::SystemPlugin
+{
+public:
+  GazeboRosNodeExample();
+  virtual ~GazeboRosNodeExample();
+  /// Called by gazebo to load plugin. Creates #node_, #timer_, and #pub_
+  void Load(int argc, char ** argv);
+
+private:
+  /// ROS node used for publisher and timer
+  gazebo_ros::Node::SharedPtr node_;
+  /// Timer called to publish a message every second
+  std::shared_ptr<rclcpp::TimerBase> timer_;
+  /// Example publisher
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::String>> pub_;
+};
+
+#endif  // GAZEBO_ROS_PLUGIN_EXAMPLE_HPP_

gazebo_ros/test/test_gazebo_ros_plugin_example.cpp

@@ -0,0 +1,104 @@
+// 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 <unistd.h>
+#include <stdlib.h>
+
+#include <gtest/gtest.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <memory>
+
+using namespace std::chrono_literals;
+
+class TestGazeboRosPluginExample : public ::testing::Test
+{
+public:
+  static void SetUpTestCase();
+  static void TearDownTestCase();
+  static int pid;
+};
+
+int TestGazeboRosPluginExample::pid = -1;
+
+void TestGazeboRosPluginExample::SetUpTestCase()
+{
+  // Fork process so gazebo can be run as child
+  pid = fork();
+  if (pid < 0) {
+    throw std::runtime_error("fork failed");
+  }
+
+  // Child process
+  if (0 == pid) {
+    // Run gazebo with the example plugin with publishes a string to /test
+    if (execlp("gzserver", "/usr/bin/gzserver", "-s", "./libgazebo_ros_plugin_example.so", NULL)) {
+      exit(1);
+    }
+  }
+}
+
+void TestGazeboRosPluginExample::TearDownTestCase()
+{
+  // If fork failed, don't need to do anything
+  if (pid < 0) {
+    return;
+  }
+
+  // Kill gazebo (simulating ^C command)
+  if (kill(pid, SIGINT)) {
+    throw std::runtime_error("could not kill");
+  }
+
+  // Wait for gazebo to terminate
+  wait(nullptr);
+}
+
+TEST_F(TestGazeboRosPluginExample, todo)
+{
+  // Create node and executor
+  rclcpp::executors::SingleThreadedExecutor executor;
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+  executor.add_node(node);
+
+  // Subscribe to topic published by plugin
+  using StringPtr = std_msgs::msg::String::SharedPtr;
+  StringPtr msg;
+  auto subscriber = node->create_subscription<std_msgs::msg::String>("test",
+      [&msg](const StringPtr _msg) {
+        printf("message received!!\n");
+        msg = _msg;
+      });
+
+  // Wait until message is received or timeout after 5 seconds
+  using namespace std::literals::chrono_literals;
+  auto timeout = node->now() + rclcpp::Duration(5s);
+  while (nullptr == msg && node->now() < timeout) {
+    executor.spin_once(50ms);
+  }
+
+  // Wait a little while so gazebo isn't torn down before created
+  rclcpp::sleep_for(1s);
+
+  // Assert a message was received
+  ASSERT_NE(msg, nullptr);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}