diff --git a/.ros1_unported/gazebo_ros/CMakeLists.txt b/.ros1_unported/gazebo_ros/CMakeLists.txt deleted file mode 100644 index 8494db6a9..000000000 --- a/.ros1_unported/gazebo_ros/CMakeLists.txt +++ /dev/null @@ -1,132 +0,0 @@ -cmake_minimum_required(VERSION 3.6.3) -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/-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() -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") -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}) - -## Tests - -add_subdirectory(test) - -# Install Gazebo System Plugins -install(TARGETS gazebo_ros_api_plugin gazebo_ros_paths_plugin - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ) - -# 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} -) - -# 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 Gazebo launch files -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) diff --git a/.ros1_unported/gazebo_ros/package.xml b/.ros1_unported/gazebo_ros/package.xml deleted file mode 100644 index 471c0b03d..000000000 --- a/.ros1_unported/gazebo_ros/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - gazebo_ros - 2.8.4 - - Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. - Formally simulator_gazebo/gazebo - - - Jose Luis Rivero - - Apache 2.0 - - http://gazebosim.org/tutorials?cat=connect_ros - https://github.com/ros-simulation/gazebo_ros_pkgs/issues - https://github.com/ros-simulation/gazebo_ros_pkgs - - John Hsu - Nate Koenig - Dave Coleman - - catkin - - cmake_modules - gazebo_dev - - gazebo_dev - gazebo_msgs - roslib - roscpp - tf - std_srvs - rosgraph_msgs - dynamic_reconfigure - std_msgs - geometry_msgs - tinyxml - python-argparse - - diff --git a/.ros1_unported/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst similarity index 100% rename from .ros1_unported/gazebo_ros/CHANGELOG.rst rename to gazebo_ros/CHANGELOG.rst diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt new file mode 100644 index 000000000..3fbaaed4f --- /dev/null +++ b/gazebo_ros/CMakeLists.txt @@ -0,0 +1,42 @@ +cmake_minimum_required(VERSION 3.5) +project(gazebo_ros) + + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +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(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +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_export_include_directories(include) +ament_export_libraries(gazebo_ros_node) +ament_export_dependencies(rclcpp) +ament_export_dependencies(gazebo_dev) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +ament_package() + +install(DIRECTORY include/ + DESTINATION include) + +install(TARGETS gazebo_ros_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + diff --git a/gazebo_ros/include/gazebo_ros/executor.hpp b/gazebo_ros/include/gazebo_ros/executor.hpp new file mode 100644 index 000000000..664aca544 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/executor.hpp @@ -0,0 +1,51 @@ +// 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 +#include + +namespace gazebo_ros +{ + +/// Executor run in a separate thread to handle events from all #gazebo_ros::Node instances +/** + * \class Executor 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__EXECUTOR_HPP_ diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp new file mode 100644 index 000000000..c0fa46370 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -0,0 +1,114 @@ +// 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 + +#include + +#include +#include +#include +#include +#include + +namespace gazebo_ros +{ +/// ROS Node for gazebo plugins +/** + * \class Node 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: + /// Shared pointer to a #gazebo_ros::Node + typedef std::shared_ptr SharedPtr; + + /// Destructor + virtual ~Node(); + + /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor. + /** + * \details This will call rclcpp::init if it hasn't been called yet. + * \param[in] node_name Name for the new node to create + * \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. + /** + * \details This will call rclcpp::init if it hasn't been called yet. + * \details Forwards arguments to the constructor for rclcpp::Node + * \param[in] args List of arguments to pass to rclcpp::Node + * \return A shared pointer to a new #gazebo_ros::Node, or nullptr if failed to create. + */ + template + static SharedPtr Create(Args && ... args); + +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_; + + /// 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 static_executor_; +}; + +template +Node::SharedPtr Node::Create(Args && ... args) +{ + std::lock_guard l(lock_); + + // Contruct Node by forwarding arguments + // TODO(chapulina): use rclcpp::is_initialized() once that's available, see + // https://github.com/ros2/rclcpp/issues/518 + Node::SharedPtr node; + try + { + node = std::make_shared(std::forward(args) ...); + } + catch(rclcpp::exceptions::RCLError e) + { + rclcpp::init(0, nullptr); + RCLCPP_INFO(rclcpp::get_logger("gazebo_ros_node"), + "ROS was initialized without arguments."); + node = std::make_shared(std::forward(args) ...); + } + + // 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(); + 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 // GAZEBO_ROS__NODE_HPP_ diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml new file mode 100644 index 000000000..610681423 --- /dev/null +++ b/gazebo_ros/package.xml @@ -0,0 +1,37 @@ + + + + gazebo_ros + 2.8.4 + + Utilities to interface with Gazebo through ROS. + + + Jose Luis Rivero + + Apache 2.0 + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + John Hsu + Nate Koenig + Dave Coleman + + ament_cmake + + rclcpp + gazebo_dev + + rclcpp + gazebo_dev + + ament_lint_auto + ament_lint_common + std_msgs + + + ament_cmake + + diff --git a/gazebo_ros/src/executor.cpp b/gazebo_ros/src/executor.cpp new file mode 100644 index 000000000..89ffbf859 --- /dev/null +++ b/gazebo_ros/src/executor.cpp @@ -0,0 +1,47 @@ +// 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 + +namespace gazebo_ros +{ + +Executor::Executor() +: spin_thread_(std::bind(&Executor::run, this)) +{ + sigint_handle_ = gazebo::event::Events::ConnectSigInt(std::bind(&Executor::shutdown, this)); +} + +Executor::~Executor() +{ + // If ros was not already shutdown by SIGINT handler, do it now + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + spin_thread_.join(); +} + +void Executor::run() +{ + spin(); +} + +void Executor::shutdown() +{ + rclcpp::shutdown(); +} + +} // namespace gazebo_ros diff --git a/gazebo_ros/src/node.cpp b/gazebo_ros/src/node.cpp new file mode 100644 index 000000000..debd02931 --- /dev/null +++ b/gazebo_ros/src/node.cpp @@ -0,0 +1,35 @@ +// 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 + +namespace gazebo_ros +{ + +std::weak_ptr Node::static_executor_; +std::mutex Node::lock_; + +Node::~Node() +{ +} + +Node::SharedPtr Node::Create(const std::string & node_name) +{ + return Create(node_name); +} + +} // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt new file mode 100644 index 000000000..dd9befe6a --- /dev/null +++ b/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 + args_init + create_node_without_init + multiple_nodes +) + +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() + diff --git a/gazebo_ros/test/plugins/args_init.cpp b/gazebo_ros/test/plugins/args_init.cpp new file mode 100644 index 000000000..baad8440b --- /dev/null +++ b/gazebo_ros/test/plugins/args_init.cpp @@ -0,0 +1,77 @@ +// 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 + +/// Simple example of a gazebo system plugin which uses a ROS2 node with gazebo_ros::Node. +class ProperInit : public gazebo::SystemPlugin +{ +public: + /// Constructor + ProperInit(); + + /// Destructor + virtual ~ProperInit(); + + /// 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 timer_; +}; + +ProperInit::ProperInit() +{ +} + +ProperInit::~ProperInit() +{ +} + +void ProperInit::Load(int argc, char ** argv) +{ + // Initialize ROS with arguments + rclcpp::init(argc, argv); + + // Create the ROS node + auto node = gazebo_ros::Node::Create("proper_init"); + + // Create a publisher + auto pub = node->create_publisher("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(ProperInit) diff --git a/gazebo_ros/test/plugins/create_node_without_init.cpp b/gazebo_ros/test/plugins/create_node_without_init.cpp new file mode 100644 index 000000000..979d0778c --- /dev/null +++ b/gazebo_ros/test/plugins/create_node_without_init.cpp @@ -0,0 +1,62 @@ +// 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 + +/// 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 timer_; +}; + +void CreateBeforeInit::Load(int, char **) +{ + // It should be ok to create a node without calling init first. + auto node = gazebo_ros::Node::Create("create_node_without_init"); + assert(nullptr != node); + + // Create a publisher + auto pub = node->create_publisher("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) diff --git a/gazebo_ros/test/plugins/multiple_nodes.cpp b/gazebo_ros/test/plugins/multiple_nodes.cpp new file mode 100644 index 000000000..122bc6aee --- /dev/null +++ b/gazebo_ros/test/plugins/multiple_nodes.cpp @@ -0,0 +1,70 @@ +// 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 + +/// Simple example of a gazebo system plugin which uses a ROS2 node with gazebo_ros::Node. +class MultipleNodes : 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 timer_; +}; + +void MultipleNodes::Load(int argc, char ** argv) +{ + // Initialize ROS with arguments + rclcpp::init(argc, argv); + + // Create nodes + auto nodeA = gazebo_ros::Node::Create("multiple_nodes_A"); + assert(nullptr != nodeA); + + auto nodeB = gazebo_ros::Node::Create("multiple_nodes_B"); + assert(nullptr != nodeB); + + // Create publishers + auto pubA = nodeA->create_publisher("testA"); + auto pubB = nodeB->create_publisher("testB"); + + // Run lambdas every 1 second + using namespace std::chrono_literals; + timer_ = nodeA->create_wall_timer(1s, + [nodeA, nodeB, pubA, pubB]() { + // Create string message + auto msg = std_msgs::msg::String(); + msg.data = "Hello world"; + + // Warn with this node's name (to test logging) + RCLCPP_WARN(nodeA->get_logger(), "Publishing A"); + RCLCPP_WARN(nodeB->get_logger(), "Publishing B"); + + // Publish message + pubA->publish(msg); + pubB->publish(msg); + }); +} + +GZ_REGISTER_SYSTEM_PLUGIN(MultipleNodes) diff --git a/gazebo_ros/test/test_plugins.cpp b/gazebo_ros/test/test_plugins.cpp new file mode 100644 index 000000000..71d4ff6db --- /dev/null +++ b/gazebo_ros/test/test_plugins.cpp @@ -0,0 +1,135 @@ +// 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 +#include +#include +#include + +struct TestParams +{ + std::string plugin; + std::vector topics; +}; + +class TestPlugins : public ::testing::TestWithParam +{ +public: + // Documentation inherited + void TearDown() override; + + /// Run gzserver with the given system plugin + /// \param[in] params Path of plugin library to load and a vector of topics to subscribe to. + void Run(TestParams params); + +private: + int pid_ = -1; +}; + +void TestPlugins::TearDown() +{ + // If fork failed, don't need to do anything + ASSERT_GE(pid_, 0); + + // Kill gazebo (simulating ^C command) + EXPECT_FALSE(kill(pid_, SIGINT)); + + // Wait for gazebo to terminate + wait(nullptr); +} + +void TestPlugins::Run(TestParams params) +{ + const char * plugin = params.plugin.c_str(); + auto topics = params.topics; + + // 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 a plugin with publishes a string to /test + ASSERT_TRUE(execlp("gzserver", "/usr/bin/gzserver", "-s", plugin, NULL)); + + exit(1); + } + + // Create node and executor + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared("my_node"); + executor.add_node(node); + + // Subscribe to topics published by plugin + using StringPtr = std_msgs::msg::String::SharedPtr; + + // Track which topics we have received a message from + size_t topics_received_from = 0; + std::vector received(topics.size()); + + std::vector>> subs; + for (size_t i = 0; i < topics.size(); ++i) + { + subs.push_back(node->create_subscription(topics[i], + [&topics_received_from, &received, i](const StringPtr msg) { + (void) msg; + // If this is the first message from this topic, increment the counter + if (!received[i]) + { + received[i] = true; + ++topics_received_from; + } + })); + } + + // Wait until message is received or timeout after 5 seconds + using namespace std::literals::chrono_literals; + auto timeout = node->now() + rclcpp::Duration(5s); + while (topics_received_from != topics.size() && 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 + EXPECT_EQ(topics_received_from, topics.size()); +} + +TEST_P(TestPlugins, Run) +{ + Run(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(Plugins, TestPlugins, ::testing::Values( + TestParams({"./libargs_init.so", {"test"}}), + TestParams({"./libcreate_node_without_init.so", {"test"}}), + TestParams({"./libmultiple_nodes.so", {"testA", "testB"}}) +),); + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}