diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 524279d3a..3fbaaed4f 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/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() diff --git a/gazebo_ros/include/gazebo_ros/executor.hpp b/gazebo_ros/include/gazebo_ros/executor.hpp index 853f86696..664aca544 100644 --- a/gazebo_ros/include/gazebo_ros/executor.hpp +++ b/gazebo_ros/include/gazebo_ros/executor.hpp @@ -16,7 +16,7 @@ #define GAZEBO_ROS__EXECUTOR_HPP_ #include -#include +#include namespace gazebo_ros { diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp index 6d80bc781..31b20fc60 100644 --- a/gazebo_ros/include/gazebo_ros/node.hpp +++ b/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 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} - * - * - * - * - * - * - * - * - * my_topic:=new_topic - * __name:=super_cool_node - * - * - * - * 55 - * True - * - * - * \endcode - * \param[in] node_name Name of node to create - * \param[in] _sdf An SDF element which either is a element or contains a 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 rclcpp::Node - * \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 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_; - /// 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 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(std::forward(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(std::forward(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 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(); @@ -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 diff --git a/gazebo_ros/src/node.cpp b/gazebo_ros/src/node.cpp index 9e4230698..debd02931 100644 --- a/gazebo_ros/src/node.cpp +++ b/gazebo_ros/src/node.cpp @@ -22,30 +22,11 @@ namespace gazebo_ros std::weak_ptr 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 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(node_name); diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt new file mode 100644 index 000000000..d2a6a94f3 --- /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 + 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() + diff --git a/gazebo_ros/test/gazebo_ros_plugin_example.cpp b/gazebo_ros/test/gazebo_ros_plugin_example.cpp deleted file mode 100644 index ff14bd370..000000000 --- a/gazebo_ros/test/gazebo_ros_plugin_example.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// 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 deleted file mode 100644 index 434d32c8d..000000000 --- a/gazebo_ros/test/gazebo_ros_plugin_example.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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/plugins/create_before_init.cpp b/gazebo_ros/test/plugins/create_before_init.cpp new file mode 100644 index 000000000..238965769 --- /dev/null +++ b/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 + +#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 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("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/plugins/proper_init.cpp b/gazebo_ros/test/plugins/proper_init.cpp new file mode 100644 index 000000000..baad8440b --- /dev/null +++ b/gazebo_ros/test/plugins/proper_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/test_gazebo_ros_plugin_example.cpp b/gazebo_ros/test/test_plugins.cpp similarity index 51% rename from gazebo_ros/test/test_gazebo_ros_plugin_example.cpp rename to gazebo_ros/test/test_plugins.cpp index 3ae53f7d1..9780ff63d 100644 --- a/gazebo_ros/test/test_gazebo_ros_plugin_example.cpp +++ b/gazebo_ros/test/test_plugins.cpp @@ -20,72 +20,84 @@ #include #include +#include +#include +#include -using namespace std::chrono_literals; - -class TestGazeboRosPluginExample : public ::testing::Test +struct TestParams { -public: - static void SetUpTestCase(); - static void TearDownTestCase(); - static int pid; + std::string plugin; + std::vector topics; }; -int TestGazeboRosPluginExample::pid = -1; - -void TestGazeboRosPluginExample::SetUpTestCase() +class TestPlugins : public ::testing::TestWithParam { - // Fork process so gazebo can be run as child - pid = fork(); - if (pid < 0) { - throw std::runtime_error("fork failed"); - } +public: + // Documentation inherited + void TearDown() override; - // 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); - } - } -} + /// 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); -void TestGazeboRosPluginExample::TearDownTestCase() +private: + int pid_ = -1; +}; + +void TestPlugins::TearDown() { // If fork failed, don't need to do anything - if (pid < 0) { - return; - } + ASSERT_GE(pid_, 0); // Kill gazebo (simulating ^C command) - if (kill(pid, SIGINT)) { - throw std::runtime_error("could not kill"); - } + EXPECT_FALSE(kill(pid_, SIGINT)); // Wait for gazebo to terminate wait(nullptr); } -TEST_F(TestGazeboRosPluginExample, todo) +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 topic published by plugin + // Subscribe to topics 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; - }); + std::vector msgs; + + std::vector>> subs; + for (auto topic : topics) + { + subs.push_back(node->create_subscription(topic, + [&msgs](const StringPtr msg) { + printf("message received!!\n"); + msgs.push_back(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) { + while (msgs.size() != topics.size() && node->now() < timeout) { executor.spin_once(50ms); } @@ -93,9 +105,20 @@ TEST_F(TestGazeboRosPluginExample, todo) rclcpp::sleep_for(1s); // Assert a message was received - ASSERT_NE(msg, nullptr); + EXPECT_EQ(msgs.size(), topics.size()); } +TEST_P(TestPlugins, Run) +{ + Run(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(Plugins, TestPlugins, ::testing::Values( + TestParams({"./libgazebo_ros_plugin_example.so", {"test"}}), + TestParams({"./libcreate_before_init.so", {"test"}}), + TestParams({"./libmultiple_nodes.so", {"testA", "testB"}}) +)); + int main(int argc, char ** argv) { rclcpp::init(argc, argv);