From a7b4cb23b3fb059174a8293b644827f7d63bf403 Mon Sep 17 00:00:00 2001
From: Loy van Beek <loy.vanbeek@mojin-robotics.de>
Date: Mon, 18 Oct 2021 16:52:32 +0200
Subject: [PATCH] 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>
---
 README.md                | 42 +++++++++++++++++++++
 src/parameter_bridge.cpp | 79 +++++++++++++++++++++++++++++++++-------
 2 files changed, 108 insertions(+), 13 deletions(-)

diff --git a/README.md b/README.md
index 112df87f..a5e01f21 100644
--- a/README.md
+++ b/README.md
@@ -419,3 +419,45 @@ This should start printing text like `I heard: [hello world ...]` with a timesta
 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 or 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: the `qos` section can be omitted entirely and options not set are left default.
+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_last, then you can omit `depth` 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
+```
diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp
index a16244e1..74e6564e 100644
--- a/src/parameter_bridge.cpp
+++ b/src/parameter_bridge.cpp
@@ -169,25 +169,78 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
 
     if (qos_params.hasMember("liveliness"))
     {
-      try
+      if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt)
       {
-        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));
+        printf("liviness is an int\n");
+        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 create parametrize liveliness: '%s'\n",
+              e.what());
+        }
+        catch (XmlRpc::XmlRpcException &e)
+        {
+          fprintf(
+              stderr,
+              "failed to create parametrize liveliness: '%s'\n",
+              e.getMessage().c_str());
+        }
       }
-      catch (std::runtime_error &e)
+      else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString)
       {
-        fprintf(
-            stderr,
-            "failed to create parametrize liveliness: '%s'\n",
-            e.what());
+        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" || liveliness_str == "liveliness_automatic")
+          {
+            liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
+          }
+          else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || 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 create parametrize liveliness: '%s'\n",
+              e.what());
+        }
+        catch (XmlRpc::XmlRpcException &e)
+        {
+          fprintf(
+              stderr,
+              "failed to create parametrize liveliness: '%s'\n",
+              e.getMessage().c_str());
+        }
       }
-      catch (XmlRpc::XmlRpcException &e)
+      else
       {
         fprintf(
-            stderr,
-            "failed to create parametrize liveliness: '%s'\n",
-            e.getMessage().c_str());
+          stderr,
+          "failed to create parametrize liveliness, parameter was not a string or int \n");
       }
     }