diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt
index fbda8602d..524279d3a 100644
--- a/gazebo_ros/CMakeLists.txt
+++ b/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()
diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml
index caec6ca41..610681423 100644
--- a/gazebo_ros/package.xml
+++ b/gazebo_ros/package.xml
@@ -29,6 +29,7 @@
ament_lint_auto
ament_lint_common
+ std_msgs
ament_cmake
diff --git a/gazebo_ros/test/gazebo_ros_plugin_example.cpp b/gazebo_ros/test/gazebo_ros_plugin_example.cpp
new file mode 100644
index 000000000..ff14bd370
--- /dev/null
+++ b/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("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)
diff --git a/gazebo_ros/test/gazebo_ros_plugin_example.hpp b/gazebo_ros/test/gazebo_ros_plugin_example.hpp
new file mode 100644
index 000000000..434d32c8d
--- /dev/null
+++ b/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
+
+#include
+#include
+
+#include
+
+/// 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 timer_;
+ /// Example publisher
+ std::shared_ptr> pub_;
+};
+
+#endif // GAZEBO_ROS_PLUGIN_EXAMPLE_HPP_
diff --git a/gazebo_ros/test/test_gazebo_ros_plugin_example.cpp b/gazebo_ros/test/test_gazebo_ros_plugin_example.cpp
new file mode 100644
index 000000000..3ae53f7d1
--- /dev/null
+++ b/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
+#include
+
+#include
+#include
+#include
+
+#include
+
+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("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("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();
+}