Parametrize Quality of Service in parameter_bridge. (ros2#331)

* Extend create_bidirectional_bridge to take qos param for ROS2 publisher Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Busy setting up a way to read QoS parameters from ROS1 params Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Parse history qos params Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Call qos_from_params when setting up topics Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Configure deadline, lifespan, liveliness_lease_durations Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Configure liveliness Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Add some basic debug text Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Print the QoS settings to stdout when setting them up Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Catch XmlRpc::XmlRpcExceptions when constructing QoS from parameters Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Parse liveliness as either int enum value or upper/lower case string representation of liveliness enum values Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Fix formatting with uncrustify Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Fix cpplint formatting In 2 cases the formatting conflicts with what uncrustify wants; // NOLINT got rid of the complaints of cpplint, favouring uncrustify Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Clearer logging as suggested by code review Co-authored-by: Geoffrey Biggs <gbiggs@killbots.net> Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> * Clarify keep_last vs keep_all setting for history Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de> Co-authored-by: Geoffrey Biggs <gbiggs@killbots.net>
hsd-dev · Dec 9, 2022 · b8f8a4f · b8f8a4f
1 parent ae614dc
commit b8f8a4f
Show file tree

Hide file tree

Showing 6 changed files with 290 additions and 3 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rmw_implementation_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(xmlrpcpp REQUIRED)
 
 # find ROS 1 packages
 set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake)

diff --git a/README.md b/README.md
@@ -458,3 +458,47 @@ source ${ROS2_INSTALL_PATH}/setup.bash
 ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}"
 ```
 If all is well, the output should contain `example_interfaces.srv.AddTwoInts_Response(sum=3)`
+
+### Parametrizing Quality of Service
+An advantage of ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic.
+The parameter bridge optionally allows for this as well.
+For some topics, like `/tf_static` this is actually required, as this is a latching topic in ROS 1.
+In ROS 2 with the `parameter_bridge`, this requires that topic to be configured as such:
+
+```yaml
+topics:
+  -
+    topic: /tf_static
+    type: tf2_msgs/msg/TFMessage
+    queue_size: 1
+    qos:
+      history: keep_all
+      durability: transient_local
+```
+
+All other QoS options (as documented here in https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html) are available:
+
+```yaml
+topics:
+  -
+    topic: /some_ros1_topic
+    type: std_msgs/msg/String
+    queue_size: 1
+    qos:
+      history: keep_last  # OR keep_all, then you can omit `depth` parameter below
+      depth: 10  # Only required when history == keep_last
+      reliability: reliable  # OR best_effort
+      durability: transient_local  # OR volatile
+      deadline:
+          secs: 10
+          nsecs: 2345
+      lifespan:
+          secs: 20
+          nsecs: 3456
+      liveliness: liveliness_system_default  # Values from https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html, eg. LIVELINESS_AUTOMATIC
+      liveliness_lease_duration:
+          secs: 40
+          nsecs: 5678
+```
+
+Note that the `qos` section can be omitted entirely and options not set are left default.
diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp
@@ -127,6 +127,16 @@ create_bidirectional_bridge(
   const std::string & topic_name,
   size_t queue_size = 10);
 
+BridgeHandles
+create_bidirectional_bridge(
+  ros::NodeHandle ros1_node,
+  rclcpp::Node::SharedPtr ros2_node,
+  const std::string & ros1_type_name,
+  const std::string & ros2_type_name,
+  const std::string & topic_name,
+  size_t queue_size,
+  const rclcpp::QoS & publisher_qos);
+
 }  // namespace ros1_bridge
 
 #endif  // ROS1_BRIDGE__BRIDGE_HPP_
diff --git a/package.xml b/package.xml
@@ -29,6 +29,7 @@
   <build_depend>rcutils</build_depend>
   <build_depend>rmw_implementation_cmake</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>xmlrpcpp</build_depend>
 
   <buildtool_export_depend>pkg-config</buildtool_export_depend>
 

diff --git a/src/bridge.cpp b/src/bridge.cpp
@@ -139,4 +139,28 @@ create_bidirectional_bridge(
   return handles;
 }
 
+BridgeHandles
+create_bidirectional_bridge(
+  ros::NodeHandle ros1_node,
+  rclcpp::Node::SharedPtr ros2_node,
+  const std::string & ros1_type_name,
+  const std::string & ros2_type_name,
+  const std::string & topic_name,
+  size_t queue_size,
+  const rclcpp::QoS & publisher_qos)
+{
+  RCLCPP_INFO(
+    ros2_node->get_logger(), "create bidirectional bridge for topic %s",
+    topic_name.c_str());
+  BridgeHandles handles;
+  handles.bridge1to2 = create_bridge_from_1_to_2(
+    ros1_node, ros2_node,
+    ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, publisher_qos);
+  handles.bridge2to1 = create_bridge_from_2_to_1(
+    ros2_node, ros1_node,
+    ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size,
+    handles.bridge1to2.ros2_publisher);
+  return handles;
+}
+
 }  // namespace ros1_bridge
diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp
@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <xmlrpcpp/XmlRpcException.h>
+
 #include <list>
 #include <string>
 
@@ -30,6 +32,202 @@
 
 #include "ros1_bridge/bridge.hpp"
 
+rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
+{
+  auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10));
+
+  printf("Qos(");
+
+  if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
+    if (qos_params.hasMember("history")) {
+      auto history = static_cast<std::string>(qos_params["history"]);
+      printf("history: ");
+      if (history == "keep_all") {
+        ros2_publisher_qos.keep_all();
+        printf("keep_all, ");
+      } else if (history == "keep_last") {
+        if (qos_params.hasMember("depth")) {
+          auto depth = static_cast<int>(qos_params["depth"]);
+          ros2_publisher_qos.keep_last(depth);
+          printf("keep_last(%i), ", depth);
+        } else {
+          fprintf(
+            stderr,
+            "history: keep_last requires that also a depth is set\n");
+        }
+      } else {
+        fprintf(
+          stderr,
+          "invalid value for 'history': '%s', allowed values are 'keep_all',"
+          "'keep_last' (also requires 'depth' to be set)\n",
+          history.c_str());
+      }
+    }
+
+    if (qos_params.hasMember("reliability")) {
+      auto reliability = static_cast<std::string>(qos_params["reliability"]);
+      printf("reliability: ");
+      if (reliability == "best_effort") {
+        ros2_publisher_qos.best_effort();
+        printf("best_effort, ");
+      } else if (reliability == "reliable") {
+        ros2_publisher_qos.reliable();
+        printf("reliable, ");
+      } else {
+        fprintf(
+          stderr,
+          "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n",
+          reliability.c_str());
+      }
+    }
+
+    if (qos_params.hasMember("durability")) {
+      auto durability = static_cast<std::string>(qos_params["durability"]);
+      printf("durability: ");
+      if (durability == "transient_local") {
+        ros2_publisher_qos.transient_local();
+        printf("transient_local, ");
+      } else if (durability == "volatile") {
+        ros2_publisher_qos.durability_volatile();
+        printf("volatile, ");
+      } else {
+        fprintf(
+          stderr,
+          "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n",
+          durability.c_str());
+      }
+    }
+
+    if (qos_params.hasMember("deadline")) {
+      try {
+        rclcpp::Duration dur = rclcpp::Duration(
+          static_cast<int>(qos_params["deadline"]["secs"]),
+          static_cast<int>(qos_params["deadline"]["nsecs"]));
+        ros2_publisher_qos.deadline(dur);
+        printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds());
+      } catch (std::runtime_error & e) {
+        fprintf(
+          stderr,
+          "failed to parse deadline: '%s'\n",
+          e.what());
+      } catch (XmlRpc::XmlRpcException & e) {
+        fprintf(
+          stderr,
+          "failed to parse deadline: '%s'\n",
+          e.getMessage().c_str());
+      }
+    }
+
+    if (qos_params.hasMember("lifespan")) {
+      try {
+        rclcpp::Duration dur = rclcpp::Duration(
+          static_cast<int>(qos_params["lifespan"]["secs"]),
+          static_cast<int>(qos_params["lifespan"]["nsecs"]));
+        ros2_publisher_qos.lifespan(dur);
+        printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds());
+      } catch (std::runtime_error & e) {
+        fprintf(
+          stderr,
+          "failed to parse lifespan: '%s'\n",
+          e.what());
+      } catch (XmlRpc::XmlRpcException & e) {
+        fprintf(
+          stderr,
+          "failed to parse lifespan: '%s'\n",
+          e.getMessage().c_str());
+      }
+    }
+
+    if (qos_params.hasMember("liveliness")) {
+      if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) {
+        try {
+          auto liveliness = static_cast<int>(qos_params["liveliness"]);
+          ros2_publisher_qos.liveliness(static_cast<rmw_qos_liveliness_policy_t>(liveliness));
+          printf("liveliness: %i, ", static_cast<int>(liveliness));
+        } catch (std::runtime_error & e) {
+          fprintf(
+            stderr,
+            "failed to parse liveliness: '%s'\n",
+            e.what());
+        } catch (XmlRpc::XmlRpcException & e) {
+          fprintf(
+            stderr,
+            "failed to parse liveliness: '%s'\n",
+            e.getMessage().c_str());
+        }
+      } else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) {
+        try {
+          rmw_qos_liveliness_policy_t liveliness =
+            rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
+          auto liveliness_str = static_cast<std::string>(qos_params["liveliness"]);
+          if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" ||
+            liveliness_str == "liveliness_system_default")
+          {
+            liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
+          } else if (liveliness_str == "LIVELINESS_AUTOMATIC" ||  // NOLINT
+            liveliness_str == "liveliness_automatic")
+          {
+            liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+          } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" ||  // NOLINT
+            liveliness_str == "liveliness_manual_by_topic")
+          {
+            liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
+          } else {
+            fprintf(
+              stderr,
+              "invalid value for 'liveliness': '%s', allowed values are "
+              "LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n",
+              liveliness_str.c_str());
+          }
+
+          ros2_publisher_qos.liveliness(liveliness);
+          printf("liveliness: %s, ", liveliness_str.c_str());
+        } catch (std::runtime_error & e) {
+          fprintf(
+            stderr,
+            "failed to parse liveliness: '%s'\n",
+            e.what());
+        } catch (XmlRpc::XmlRpcException & e) {
+          fprintf(
+            stderr,
+            "failed to parse liveliness: '%s'\n",
+            e.getMessage().c_str());
+        }
+      } else {
+        fprintf(
+          stderr,
+          "failed to parse liveliness, parameter was not a string or int \n");
+      }
+    }
+
+    if (qos_params.hasMember("liveliness_lease_duration")) {
+      try {
+        rclcpp::Duration dur = rclcpp::Duration(
+          static_cast<int>(qos_params["liveliness_lease_duration"]["secs"]),
+          static_cast<int>(qos_params["liveliness_lease_duration"]["nsecs"]));
+        ros2_publisher_qos.liveliness_lease_duration(dur);
+        printf("liveliness_lease_duration: Duration(nsecs: %ld), ", dur.nanoseconds());
+      } catch (std::runtime_error & e) {
+        fprintf(
+          stderr,
+          "failed to parse liveliness_lease_duration: '%s'\n",
+          e.what());
+      } catch (XmlRpc::XmlRpcException & e) {
+        fprintf(
+          stderr,
+          "failed to parse liveliness_lease_duration: '%s'\n",
+          e.getMessage().c_str());
+      }
+    }
+  } else {
+    fprintf(
+      stderr,
+      "QoS parameters could not be read\n");
+  }
+
+  printf(")");
+  return ros2_publisher_qos;
+}
 
 int main(int argc, char * argv[])
 {
@@ -89,9 +287,18 @@ int main(int argc, char * argv[])
         topic_name.c_str(), type_name.c_str());
 
       try {
-        ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge(
-          ros1_node, ros2_node, "", type_name, topic_name, queue_size);
-        all_handles.push_back(handles);
+        if (topics[i].hasMember("qos")) {
+          printf("Setting up QoS for '%s': ", topic_name.c_str());
+          auto qos_settings = qos_from_params(topics[i]["qos"]);
+          printf("\n");
+          ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge(
+            ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings);
+          all_handles.push_back(handles);
+        } else {
+          ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge(
+            ros1_node, ros2_node, "", type_name, topic_name, queue_size);
+          all_handles.push_back(handles);
+        }
       } catch (std::runtime_error & e) {
         fprintf(
           stderr,