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(); +}