From 8058f6a9b9944d0afb720abcfa42fb9e2c419f0a Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Tue, 17 Jul 2018 14:11:09 -0700 Subject: [PATCH 1/8] Move gazebo_ros files for porting --- {.ros1_unported/gazebo_ros => gazebo_ros}/CHANGELOG.rst | 0 {.ros1_unported/gazebo_ros => gazebo_ros}/CMakeLists.txt | 0 {.ros1_unported/gazebo_ros => gazebo_ros}/package.xml | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename {.ros1_unported/gazebo_ros => gazebo_ros}/CHANGELOG.rst (100%) rename {.ros1_unported/gazebo_ros => gazebo_ros}/CMakeLists.txt (100%) rename {.ros1_unported/gazebo_ros => gazebo_ros}/package.xml (100%) 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/.ros1_unported/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt similarity index 100% rename from .ros1_unported/gazebo_ros/CMakeLists.txt rename to gazebo_ros/CMakeLists.txt diff --git a/.ros1_unported/gazebo_ros/package.xml b/gazebo_ros/package.xml similarity index 100% rename from .ros1_unported/gazebo_ros/package.xml rename to gazebo_ros/package.xml From cd236f7da6e68658adb6d6f86ca4b91525c4f3ff Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Tue, 17 Jul 2018 14:12:09 -0700 Subject: [PATCH 2/8] Create base Node class for gazebo plugins with ROS2 --- gazebo_ros/CMakeLists.txt | 145 ++++---------------- gazebo_ros/include/gazebo_ros/executor.hpp | 52 +++++++ gazebo_ros/include/gazebo_ros/node.hpp | 152 +++++++++++++++++++++ gazebo_ros/package.xml | 30 ++-- gazebo_ros/src/executor.cpp | 46 +++++++ gazebo_ros/src/node.cpp | 51 +++++++ 6 files changed, 336 insertions(+), 140 deletions(-) create mode 100644 gazebo_ros/include/gazebo_ros/executor.hpp create mode 100644 gazebo_ros/include/gazebo_ros/node.hpp create mode 100644 gazebo_ros/src/executor.cpp create mode 100644 gazebo_ros/src/node.cpp diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 8494db6a9..8508a9b1d 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -1,132 +1,37 @@ -cmake_minimum_required(VERSION 3.6.3) +cmake_minimum_required(VERSION 3.5) 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() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) 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") +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(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}) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(gazebo_dev REQUIRED) -## Tests +include_directories(include) -add_subdirectory(test) +add_library(gazebo_ros_node SHARED src/node.cpp src/executor.cpp) +ament_target_dependencies(gazebo_ros_node rclcpp gazebo_dev) -# Install Gazebo System Plugins -install(TARGETS gazebo_ros_api_plugin gazebo_ros_paths_plugin - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ) +ament_export_include_directories(include) +ament_export_libraries(gazebo_ros_node) +ament_export_dependencies(rclcpp) +ament_export_dependencies(gazebo_dev) -# 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} -) +ament_package() -# 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(DIRECTORY include/ + DESTINATION include) -# Install Gazebo launch files -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) +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..51527fd0f --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/executor.hpp @@ -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. + +#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 diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp new file mode 100644 index 000000000..900b730f7 --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -0,0 +1,152 @@ +// 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 + +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: + /// Exception thrown when a #Create is called for a Node before #InitROS has ever been called + class NotInitializedException : public std::runtime_error + { + public: + NotInitializedException(); + }; + + /// 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. + /** + * \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 + */ + 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 + */ + 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; + + /// Points to #static_executor_, so that when all #gazebo_ros::Node instances are destroyed, the 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_; + + /// 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) +{ + // Throw exception if Node is created before ROS is initialized + if (!initialized_) + throw NotInitializedException(); + + // Construct Node by forwarding arguments + Node::SharedPtr node = std::make_shared(std::forward(args)...); + + std::lock_guard l(lock_); + // Store shared pointer to static executor in this object + node->executor_ = static_executor_.lock(); + // If executor has not been constructed 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 diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 471c0b03d..b05bfb82c 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,10 +1,10 @@ - + + gazebo_ros 2.8.4 - Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. - Formally simulator_gazebo/gazebo + Utilities to interface with Gazebo through ROS. Jose Luis Rivero @@ -19,25 +19,15 @@ Nate Koenig Dave Coleman - catkin + ament_cmake - cmake_modules + rclcpp gazebo_dev - + + rclcpp gazebo_dev - gazebo_msgs - roslib - roscpp - tf - std_srvs - rosgraph_msgs - dynamic_reconfigure - std_msgs - geometry_msgs - tinyxml - python-argparse + + ament_cmake + diff --git a/gazebo_ros/src/executor.cpp b/gazebo_ros/src/executor.cpp new file mode 100644 index 000000000..27efecac0 --- /dev/null +++ b/gazebo_ros/src/executor.cpp @@ -0,0 +1,46 @@ +// 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..d62f0cb93 --- /dev/null +++ b/gazebo_ros/src/node.cpp @@ -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. + +#include + +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); +} + +} // namespace gazebo_ros From 112aec3ee11c9984a428a73cf135d407dec0cee9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 18 Jul 2018 11:10:58 -0700 Subject: [PATCH 3/8] Enable linters and make them happy --- gazebo_ros/CMakeLists.txt | 6 +++ gazebo_ros/include/gazebo_ros/executor.hpp | 9 ++-- gazebo_ros/include/gazebo_ros/node.hpp | 62 +++++++++++----------- gazebo_ros/package.xml | 3 ++ gazebo_ros/src/executor.cpp | 13 ++--- gazebo_ros/src/node.cpp | 23 ++++---- 6 files changed, 64 insertions(+), 52 deletions(-) diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 8508a9b1d..fbda8602d 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -26,6 +26,11 @@ ament_export_libraries(gazebo_ros_node) ament_export_dependencies(rclcpp) ament_export_dependencies(gazebo_dev) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + ament_package() install(DIRECTORY include/ @@ -35,3 +40,4 @@ 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 index 51527fd0f..853f86696 100644 --- a/gazebo_ros/include/gazebo_ros/executor.hpp +++ b/gazebo_ros/include/gazebo_ros/executor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef GAZEBO_ROS_EXECUTOR_HPP -#define GAZEBO_ROS_EXECUTOR_HPP +#ifndef GAZEBO_ROS__EXECUTOR_HPP_ +#define GAZEBO_ROS__EXECUTOR_HPP_ #include #include @@ -47,6 +47,5 @@ class Executor : public rclcpp::executors::MultiThreadedExecutor /// Connection to gazebo sigint event gazebo::event::ConnectionPtr sigint_handle_; }; - -} // namespace gazebo_ros -#endif +} // 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 index 900b730f7..6d80bc781 100644 --- a/gazebo_ros/include/gazebo_ros/node.hpp +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -12,21 +12,21 @@ // 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 +#ifndef GAZEBO_ROS__NODE_HPP_ +#define GAZEBO_ROS__NODE_HPP_ #include -#include #include +#include +#include +#include +#include +#include + namespace gazebo_ros { - /// ROS Node for gazebo plugins /** * \class Node node.hpp @@ -35,14 +35,14 @@ namespace gazebo_ros */ class Node : public rclcpp::Node { -public: /// 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: + NotInitializedException(); }; +public: /// Shared pointer to a #gazebo_ros::Node typedef std::shared_ptr SharedPtr; @@ -55,7 +55,7 @@ class Node : public rclcpp::Node * \param[in] node_name Name for the new node to create * \return A shared pointer to a new #gazebo_ros::Node */ - static SharedPtr Create(const std::string& node_name); + static SharedPtr Create(const std::string & node_name); /// Create a #gazebo_ros::Node and add it to the global #gazebo_ros::Executor. /** @@ -86,7 +86,7 @@ class Node : public rclcpp::Node * \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); + 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. /** @@ -95,8 +95,8 @@ class Node : public rclcpp::Node * \param[in] args List of arguments to pass to rclcpp::Node * \return A shared pointer to a new #gazebo_ros::Node */ - template - static SharedPtr Create(Args && ...args); + template + static SharedPtr Create(Args && ... args); /// Initialize ROS with the command line arguments. /** @@ -104,16 +104,17 @@ class Node : public rclcpp::Node * \param[in] argc Number of arguments * \param[in] argv Vector of c-strings of length \a argc */ - static void InitROS(int argc, char** argv); + static void InitROS(int argc, char ** argv); private: - /// Inherit constructor + /// 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 + /// Points to #static_executor_, so that when all #gazebo_ros::Node instances are destroyed, the + /// executor thread is too std::shared_ptr executor_; - /// true if #InitROS has been called and future calls will be ignored + /// True if #InitROS has been called and future calls will be ignored static std::atomic_bool initialized_; /// Locks #initialized_ and #executor_ @@ -123,22 +124,22 @@ class Node : public rclcpp::Node static std::weak_ptr static_executor_; }; -template -Node::SharedPtr Node::Create(Args && ...args) +template +Node::SharedPtr Node::Create(Args && ... args) { - // Throw exception if Node is created before ROS is initialized - if (!initialized_) + // Throw exception is Node is created before ROS is initialized + if (!initialized_) { throw NotInitializedException(); + } - // Construct Node by forwarding arguments - Node::SharedPtr node = std::make_shared(std::forward(args)...); + // Contruct Node by forwarding arguments + Node::SharedPtr node = std::make_shared(std::forward(args) ...); std::lock_guard l(lock_); // Store shared pointer to static executor in this object node->executor_ = static_executor_.lock(); - // If executor has not been constructed yet, do so now - if (!node->executor_) - { + // If executor has not been contructed yet, do so now + if (!node->executor_) { node->executor_ = std::make_shared(); static_executor_ = node->executor_; } @@ -147,6 +148,5 @@ Node::SharedPtr Node::Create(Args && ...args) node->executor_->add_node(node); return node; } - -} // namespace gazebo_ros -#endif +} // namespace gazebo_ros +#endif // GAZEBO_ROS__NODE_HPP_ diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index b05bfb82c..caec6ca41 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -27,6 +27,9 @@ rclcpp gazebo_dev + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/gazebo_ros/src/executor.cpp b/gazebo_ros/src/executor.cpp index 27efecac0..89ffbf859 100644 --- a/gazebo_ros/src/executor.cpp +++ b/gazebo_ros/src/executor.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include +#include + namespace gazebo_ros { -Executor::Executor() : - spin_thread_(std::bind(&Executor::run, this)) +Executor::Executor() +: spin_thread_(std::bind(&Executor::run, this)) { sigint_handle_ = gazebo::event::Events::ConnectSigInt(std::bind(&Executor::shutdown, this)); } @@ -28,8 +28,9 @@ Executor::Executor() : Executor::~Executor() { // If ros was not already shutdown by SIGINT handler, do it now - if (rclcpp::ok()) + if (rclcpp::ok()) { rclcpp::shutdown(); + } spin_thread_.join(); } @@ -43,4 +44,4 @@ void Executor::shutdown() rclcpp::shutdown(); } -} // namespace gazebo_ros +} // namespace gazebo_ros diff --git a/gazebo_ros/src/node.cpp b/gazebo_ros/src/node.cpp index d62f0cb93..9e4230698 100644 --- a/gazebo_ros/src/node.cpp +++ b/gazebo_ros/src/node.cpp @@ -14,6 +14,9 @@ #include +#include +#include + namespace gazebo_ros { @@ -21,8 +24,8 @@ 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::NotInitializedException::NotInitializedException() +: std::runtime_error("Called LoadNode before InitROS. Be sure to load the ros_api plugin first") { } @@ -30,12 +33,12 @@ Node::~Node() { } -void Node::InitROS(int argc, char** argv) +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."); + std::lock_guard l(lock_); + if (initialized_) { + RCLCPP_WARN(rclcpp::get_logger("gazebo_ros_node"), + "Called Node::InitROS twice, ignoring second call."); return; } @@ -43,9 +46,9 @@ void Node::InitROS(int argc, char** argv) initialized_ = true; } -Node::SharedPtr Node::Create(const std::string& node_name) +Node::SharedPtr Node::Create(const std::string & node_name) { - return Create(node_name); + return Create(node_name); } -} // namespace gazebo_ros +} // namespace gazebo_ros From dcf67e24c7f69fdaf73a605a592193dc2e856d63 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Wed, 18 Jul 2018 13:51:01 -0700 Subject: [PATCH 4/8] Add simple test for gazebo_ros::Node --- gazebo_ros/CMakeLists.txt | 9 +- gazebo_ros/package.xml | 1 + gazebo_ros/test/gazebo_ros_plugin_example.cpp | 52 +++++++++ gazebo_ros/test/gazebo_ros_plugin_example.hpp | 43 ++++++++ .../test/test_gazebo_ros_plugin_example.cpp | 104 ++++++++++++++++++ 5 files changed, 208 insertions(+), 1 deletion(-) create mode 100644 gazebo_ros/test/gazebo_ros_plugin_example.cpp create mode 100644 gazebo_ros/test/gazebo_ros_plugin_example.hpp create mode 100644 gazebo_ros/test/test_gazebo_ros_plugin_example.cpp 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(); +} From d2790f39df93491e3964bcddfa402c2f308939c4 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Wed, 18 Jul 2018 16:18:53 -0700 Subject: [PATCH 5/8] Remove Node::Create using sdf until it is implemented --- gazebo_ros/include/gazebo_ros/node.hpp | 33 +------------------------- 1 file changed, 1 insertion(+), 32 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp index 6d80bc781..7cb86a3fc 100644 --- a/gazebo_ros/include/gazebo_ros/node.hpp +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -59,38 +59,7 @@ class Node : public rclcpp::Node /// 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 + * \note Must NOT be 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 From 8a64d976d55354e130e3b2e1ae0eab67f3ec3a3d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 19 Jul 2018 13:30:58 -0700 Subject: [PATCH 6/8] Remove internal logic to check init, add more tests --- gazebo_ros/CMakeLists.txt | 10 +- gazebo_ros/include/gazebo_ros/executor.hpp | 2 +- gazebo_ros/include/gazebo_ros/node.hpp | 46 +++----- gazebo_ros/src/node.cpp | 19 ---- gazebo_ros/test/CMakeLists.txt | 41 +++++++ gazebo_ros/test/gazebo_ros_plugin_example.cpp | 52 --------- gazebo_ros/test/gazebo_ros_plugin_example.hpp | 43 -------- .../test/plugins/create_before_init.cpp | 68 ++++++++++++ gazebo_ros/test/plugins/multiple_nodes.cpp | 70 ++++++++++++ gazebo_ros/test/plugins/proper_init.cpp | 77 +++++++++++++ ...os_plugin_example.cpp => test_plugins.cpp} | 103 +++++++++++------- 11 files changed, 339 insertions(+), 192 deletions(-) create mode 100644 gazebo_ros/test/CMakeLists.txt delete mode 100644 gazebo_ros/test/gazebo_ros_plugin_example.cpp delete mode 100644 gazebo_ros/test/gazebo_ros_plugin_example.hpp create mode 100644 gazebo_ros/test/plugins/create_before_init.cpp create mode 100644 gazebo_ros/test/plugins/multiple_nodes.cpp create mode 100644 gazebo_ros/test/plugins/proper_init.cpp rename gazebo_ros/test/{test_gazebo_ros_plugin_example.cpp => test_plugins.cpp} (51%) 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 7cb86a3fc..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,30 +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. /** - * \note Must NOT be 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; @@ -83,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_; @@ -96,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(); @@ -115,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); From 4f5d13dfc88357c76f341878021369182dba899c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 20 Jul 2018 11:32:28 -0700 Subject: [PATCH 7/8] Call init from node in case it hasn't been called yet --- gazebo_ros/include/gazebo_ros/node.hpp | 15 +++++++++------ gazebo_ros/test/CMakeLists.txt | 4 ++-- .../plugins/{proper_init.cpp => args_init.cpp} | 0 ...fore_init.cpp => create_node_without_init.cpp} | 14 ++++---------- gazebo_ros/test/test_plugins.cpp | 4 ++-- 5 files changed, 17 insertions(+), 20 deletions(-) rename gazebo_ros/test/plugins/{proper_init.cpp => args_init.cpp} (100%) rename gazebo_ros/test/plugins/{create_before_init.cpp => create_node_without_init.cpp} (84%) diff --git a/gazebo_ros/include/gazebo_ros/node.hpp b/gazebo_ros/include/gazebo_ros/node.hpp index 31b20fc60..c0fa46370 100644 --- a/gazebo_ros/include/gazebo_ros/node.hpp +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -44,6 +44,7 @@ class Node : public rclcpp::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. */ @@ -51,6 +52,7 @@ class Node : public rclcpp::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. * \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. @@ -76,8 +78,10 @@ class Node : public rclcpp::Node template Node::SharedPtr Node::Create(Args && ... args) { + std::lock_guard l(lock_); + // Contruct Node by forwarding arguments - // TODO(chapulina): use rclcpp::isInitialized() once that's available, see + // TODO(chapulina): use rclcpp::is_initialized() once that's available, see // https://github.com/ros2/rclcpp/issues/518 Node::SharedPtr node; try @@ -86,13 +90,12 @@ Node::SharedPtr Node::Create(Args && ... 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; + rclcpp::init(0, nullptr); + RCLCPP_INFO(rclcpp::get_logger("gazebo_ros_node"), + "ROS was initialized without arguments."); + node = std::make_shared(std::forward(args) ...); } - std::lock_guard l(lock_); - // Store shared pointer to static executor in this object node->executor_ = static_executor_.lock(); diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index d2a6a94f3..dd9befe6a 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -7,9 +7,9 @@ ament_lint_auto_find_test_dependencies() # Plugins set(plugins - create_before_init + args_init + create_node_without_init multiple_nodes - proper_init ) foreach(plugin ${plugins}) diff --git a/gazebo_ros/test/plugins/proper_init.cpp b/gazebo_ros/test/plugins/args_init.cpp similarity index 100% rename from gazebo_ros/test/plugins/proper_init.cpp rename to gazebo_ros/test/plugins/args_init.cpp diff --git a/gazebo_ros/test/plugins/create_before_init.cpp b/gazebo_ros/test/plugins/create_node_without_init.cpp similarity index 84% rename from gazebo_ros/test/plugins/create_before_init.cpp rename to gazebo_ros/test/plugins/create_node_without_init.cpp index 238965769..979d0778c 100644 --- a/gazebo_ros/test/plugins/create_before_init.cpp +++ b/gazebo_ros/test/plugins/create_node_without_init.cpp @@ -33,17 +33,10 @@ class CreateBeforeInit : public gazebo::SystemPlugin std::shared_ptr timer_; }; -void CreateBeforeInit::Load(int argc, char ** argv) +void CreateBeforeInit::Load(int, char **) { - // 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"); + // 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 @@ -53,6 +46,7 @@ void CreateBeforeInit::Load(int argc, char ** argv) 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"; diff --git a/gazebo_ros/test/test_plugins.cpp b/gazebo_ros/test/test_plugins.cpp index 9780ff63d..75d2e1cf7 100644 --- a/gazebo_ros/test/test_plugins.cpp +++ b/gazebo_ros/test/test_plugins.cpp @@ -114,8 +114,8 @@ TEST_P(TestPlugins, Run) } INSTANTIATE_TEST_CASE_P(Plugins, TestPlugins, ::testing::Values( - TestParams({"./libgazebo_ros_plugin_example.so", {"test"}}), - TestParams({"./libcreate_before_init.so", {"test"}}), + TestParams({"./libargs_init.so", {"test"}}), + TestParams({"./libcreate_node_without_init.so", {"test"}}), TestParams({"./libmultiple_nodes.so", {"testA", "testB"}}) )); From d5e448bd61a414197b4563ef8075105f852a22d7 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Fri, 20 Jul 2018 15:29:02 -0700 Subject: [PATCH 8/8] Fix bug in test_plugins not ensuring all topics were received --- gazebo_ros/test/test_plugins.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/gazebo_ros/test/test_plugins.cpp b/gazebo_ros/test/test_plugins.cpp index 75d2e1cf7..71d4ff6db 100644 --- a/gazebo_ros/test/test_plugins.cpp +++ b/gazebo_ros/test/test_plugins.cpp @@ -82,22 +82,30 @@ void TestPlugins::Run(TestParams params) // Subscribe to topics published by plugin using StringPtr = std_msgs::msg::String::SharedPtr; - std::vector msgs; + + // Track which topics we have received a message from + size_t topics_received_from = 0; + std::vector received(topics.size()); std::vector>> subs; - for (auto topic : topics) + for (size_t i = 0; i < topics.size(); ++i) { - subs.push_back(node->create_subscription(topic, - [&msgs](const StringPtr msg) { - printf("message received!!\n"); - msgs.push_back(msg); + 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 (msgs.size() != topics.size() && node->now() < timeout) { + while (topics_received_from != topics.size() && node->now() < timeout) { executor.spin_once(50ms); } @@ -105,7 +113,7 @@ void TestPlugins::Run(TestParams params) rclcpp::sleep_for(1s); // Assert a message was received - EXPECT_EQ(msgs.size(), topics.size()); + EXPECT_EQ(topics_received_from, topics.size()); } TEST_P(TestPlugins, Run) @@ -117,7 +125,7 @@ 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) {