Add gazebo_ros::QoS class

gazebo_ros/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rcl REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rmw REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tinyxml_vendor REQUIRED)
 
@@ -31,6 +32,7 @@ ament_python_install_package(scripts/)
 add_library(gazebo_ros_node SHARED
   src/executor.cpp
   src/node.cpp
+  src/qos.cpp
 )
 target_include_directories(gazebo_ros_node
   PUBLIC
@@ -43,6 +45,7 @@ ament_target_dependencies(gazebo_ros_node
   "gazebo_dev"
   "rclcpp"
   "rcl"
+  "rmw"
 )
 ament_export_libraries(gazebo_ros_node)
 

gazebo_ros/include/gazebo_ros/node.hpp

@@ -18,6 +18,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <gazebo_ros/executor.hpp>
+#include <gazebo_ros/qos.hpp>
 
 #include <atomic>
 #include <memory>
@@ -55,7 +56,7 @@ class Node : public rclcpp::Node
    * \details This will create a new node; the node's name will be the same as the name argument
    * on the <plugin> tag.
    * \details This will call rclcpp::init if it hasn't been called yet.
-   * \details Sets node name, namespace, remappings, and parameters from SDF.
+   * \details Sets node name, namespace, remappings, parameters, and quality of service from SDF.
    * SDF is in the form:
    * \code{.xml}
    * <!-- Node name will be the same as the plugin name -->
@@ -72,6 +73,10 @@ class Node : public rclcpp::Node
    *    <parameter name="publish_odom" type="bool">True</parameter>
    *    <!-- Remapping rules for node -->
    *    <remapping>my_topic:=new_topic</remapping>
+   *    <!-- QoS for node publishers and subscriptions -->
+   *    <qos>
+   *      <!-- See #gazebo_ros::QoS -->
+   *    </qos>
    *   </ros>
    * </plugin>
    * \endcode
@@ -108,6 +113,17 @@ class Node : public rclcpp::Node
    */
   static rclcpp::Parameter sdf_to_ros_parameter(sdf::ElementPtr const & _sdf);
 
+  inline const gazebo_ros::QoS & get_qos() const &
+  {
+    return this->qos_;
+  }
+
+  // binds to everything else
+  inline gazebo_ros::QoS get_qos() &&
+  {
+    return this->qos_;
+  }
+
 private:
   /// Inherit constructor
   using rclcpp::Node::Node;
@@ -116,6 +132,9 @@ class Node : public rclcpp::Node
   /// executor thread is too
   std::shared_ptr<Executor> executor_;
 
+  /// QoS for node entities
+  gazebo_ros::QoS qos_;
+
   /// Locks #initialized_ and #executor_
   static std::mutex lock_;
 

gazebo_ros/include/gazebo_ros/qos.hpp

@@ -0,0 +1,177 @@
+// Copyright 2020 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__QOS_HPP_
+#define GAZEBO_ROS__QOS_HPP_
+
+#include <sdf/sdf.hh>
+
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/qos.hpp>
+
+#include <map>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+namespace gazebo_ros
+{
+
+/// Exception thrown when there is an invalid QoS configuration.
+class InvalidQoSException : public std::runtime_error
+{
+public:
+  explicit InvalidQoSException(const std::string & msg)
+  : std::runtime_error(msg)
+  {}
+};
+
+// Forward declare private implementation
+struct QoSPrivate;
+
+/// Quality of service profile for ROS node entities
+/**
+ * \class QoS qos.hpp <gazebo_ros/qos.hpp>
+ * Encapsulates QoS profiles for a group of ROS publishers and subscriptions.
+ * Supports parsing SDF <qos> tags.
+ *
+ * Note, this implementation groups QoS profiles by entity type (publisher or subscription) and
+ * topic name.
+ * For example, two publishers using the same topic will have the same QoS returned by this class.
+ * \include gazebo_ros/qos.hpp
+ */
+class QoS
+{
+public:
+  QoS();
+
+  /// Constructor with SDF
+  /**
+   * \details Create a new QoS profile for a collection of node entities from SDF.
+   * SDF should have the form:
+   * \code{.xml}
+   * <qos>
+   *  <!-- Zero or more topic tags with topic names to configure -->
+   *  <topic name="my/topic/name">
+   *    <!-- Optional tag for setting the QoS profile for subscriptions -->
+   *    <subscription>
+   *      <!-- Optional tags for overriding default settings (see below)-->
+   *    </subscription>
+   *    <!-- Optional tag for setting the QoS profile for publishers -->
+   *    <publisher>
+   *      <!-- Optionally tags for overriding default settings (see below) -->
+   *    </publisher>
+   *  </topic>
+   * </qos>
+   * \endcode
+   *
+   * The available QoS settings that can be overridden are outlined below.
+   * For more information on each option and their possible values, see
+   * http://design.ros2.org/articles/qos.html and
+   * http://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html.
+   *
+   * The <reliability> element must contain one of the following: "reliable", "best_effort",
+   * or "system".
+   *
+   * The <durability> element must contain one of the following: "volatile", "transient_local",
+   * or "system".
+   *
+   * The <history> element must contain one of the following: "keep_last", "keep_all", or "system".
+   * If "keep_last" is used, a "depth" attribute must be present and set to a positive integer.
+   *
+   * The <deadline> element must contain a positive integer representing a duration in
+   * milliseconds.
+   *
+   * The <lifespan> element must contain a positive integer representing a duration in
+   * milliseconds.
+   *
+   * The <liveliness> element must contain one of the following: "automatic", "manual_by_topic",
+   * or "system".
+   *
+   * The <liveliness_lease_duration> element must contain a positive integer representing a
+   * duration in milliseconds.
+   *
+   * For any QoS setting that is not specified in the SDF, the default QoS object passed
+   * to the getter methods is used.
+   *
+   * The topic name should be the name expected after any remapping occurs.
+   * For example, if a <remapping> tag remaps a topic 'foo' to the name 'bar', then the <topic>
+   * tag's name attribute should have the value 'bar' to override any QoS settings:
+   *
+   * \code{.xml}
+   * <remapping>foo:=bar</remapping>
+   * <qos>
+   *   <topic name='bar'>
+   *   </topic>
+   * </qos>
+   * \endcode
+   *
+   * \param[in] _sdf An SDF element in the style documented above
+   * \param[in] node_name The name of the node associated with these QoS settings.
+   * \param[in] node_namespace The namespace of the node associated with these QoS settings.
+   * \param[in] options Node options that were also passed to the node associated with these QoS
+   *   settings.
+   *   This contains the necessary information extracting the remappings.
+   * \throws gazebo_ros::InvalidQoSException if there is an invalid QoS value or a topic element
+   *   is missing a "name" attribute.
+   */
+  QoS(
+    sdf::ElementPtr _sdf,
+    const std::string node_name,
+    const std::string node_namespace,
+    const rclcpp::NodeOptions & options);
+
+  /// Get the QoS for a publisher
+  /*
+   * \param[in] topic: Get QoS for publishers on this topic name.
+   * \param[in] default_qos: The default quality of service used for settings that have not been
+   *   overridden.
+   */
+  rclcpp::QoS get_publisher_qos(
+    const std::string topic, rclcpp::QoS default_qos = rclcpp::QoS(10)) const;
+
+  /// Get the QoS for a subscription
+  /*
+   * \param[in] topic: Get QoS for subscriptions on this topic name.
+   * \param[in] default_qos: The default quality of service used for settings that have not been
+   *   overridden.
+   */
+  rclcpp::QoS get_subscription_qos(
+    const std::string topic, rclcpp::QoS default_qos = rclcpp::QoS(10)) const;
+
+  /// QoS is copyable
+  // need explicit copy constructor due to `std::unique_ptr` `impl_`
+  QoS(const QoS & other);
+
+  /// QoS is movable
+  // this is needed due to rule of five
+  QoS(QoS && other);
+
+  /// QoS is copy assignable
+  // need explicit copy assignment due to `std::unique_ptr` `impl_`
+  QoS & operator=(const QoS & other);
+
+  /// QoS is move assignable
+  // this is needed due to rule of five
+  QoS & operator=(QoS && other);
+
+  // this is needed due to rule of five
+  ~QoS();
+
+private:
+  std::unique_ptr<QoSPrivate> impl_;
+};
+
+}  // namespace gazebo_ros
+#endif  // GAZEBO_ROS__QOS_HPP_

gazebo_ros/package.xml

@@ -28,6 +28,7 @@
   <depend>rcl</depend>
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
+  <depend>rmw</depend>
   <depend>std_srvs</depend>
   <depend>tinyxml_vendor</depend>
 

gazebo_ros/src/node.cpp

@@ -38,7 +38,7 @@ Node::SharedPtr Node::Get(sdf::ElementPtr sdf)
 {
   // Initialize arguments
   std::string name = "";
-  std::string ns = "";
+  std::string ns = "/";
   std::vector<std::string> arguments;
   std::vector<rclcpp::Parameter> parameter_overrides;
 
@@ -99,7 +99,12 @@ Node::SharedPtr Node::Get(sdf::ElementPtr sdf)
   node_options.parameter_overrides(parameter_overrides);
 
   // Create node with parsed arguments
-  return CreateWithArgs(name, ns, node_options);
+  std::shared_ptr<gazebo_ros::Node> node = CreateWithArgs(name, ns, node_options);
+
+  // Parse the qos tag
+  node->qos_ = gazebo_ros::QoS(sdf, name, ns, node_options);
+
+  return node;
 }
 
 Node::SharedPtr Node::Get()