Print the QoS settings to stdout when setting them up

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>
ros2 · Oct 18, 2021 · 90afae3 · 90afae3
1 parent 7c4ae65
commit 90afae3
Showing 1 changed file with 31 additions and 14 deletions.
diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp
@@ -33,22 +33,27 @@
 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
         {
@@ -68,13 +73,16 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
     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
       {
@@ -87,13 +95,16 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
     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
       {
@@ -107,11 +118,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
     {
       try
       {
-        ros2_publisher_qos.deadline(
-          rclcpp::Duration(
-            static_cast<int>(qos_params["deadline"]["secs"]),
-            static_cast<int>(qos_params["deadline"]["nsecs"]))
-        );
+        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)
       {
@@ -126,10 +137,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
     {
       try
       {
-        ros2_publisher_qos.lifespan(
-            rclcpp::Duration(
-                static_cast<int>(qos_params["lifespan"]["secs"]),
-                static_cast<int>(qos_params["lifespan"]["nsecs"])));
+        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)
       {
@@ -146,6 +158,7 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
       {
         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)
       {
@@ -160,10 +173,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
     {
       try
       {
-        ros2_publisher_qos.liveliness_lease_duration(
-            rclcpp::Duration(
-                static_cast<int>(qos_params["liveliness_lease_duration"]["secs"]),
-                static_cast<int>(qos_params["liveliness_lease_duration"]["nsecs"])));
+        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)
       {
@@ -180,6 +194,8 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params)
         stderr,
         "QoS parameters could not be read\n"); // TODO: clearer message
   }
+
+  printf(")");
   return ros2_publisher_qos;
 }
 
@@ -241,8 +257,9 @@ int main(int argc, char * argv[])
       try {
         if (topics[i].hasMember("qos"))
         {
-          printf("Setting up QoS for '%s'\n", topic_name.c_str());
+          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);