From a7af591957efcd375df92a318752fa4597435d71 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Tue, 17 Jul 2018 14:12:09 -0700 Subject: [PATCH] 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 | 153 +++++++++++++++++++++ gazebo_ros/package.xml | 30 ++-- gazebo_ros/src/executor.cpp | 46 +++++++ gazebo_ros/src/node.cpp | 51 +++++++ 6 files changed, 337 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..750ec240e --- /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 seperate 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 exeuctor and joins with the internal thread + virtual ~Executor(); + +private: + /// Thread where the exeuctor 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..7693b222a --- /dev/null +++ b/gazebo_ros/include/gazebo_ros/node.hpp @@ -0,0 +1,153 @@ +// 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 exeuctor. + * \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 contructor 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 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)...); + + 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(); + static_executor_ = node->executor_; + } + + // Add new node to the exeuctor 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