Adjust for new rclcpp::Rate API

Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
ros2 · Jun 6, 2023 · 3583a4a · 3583a4a
1 parent 419067b
commit 3583a4a
Show file tree

Hide file tree

Showing 3 changed files with 69 additions and 42 deletions.
diff --git a/test_communication/test/test_publisher.cpp b/test_communication/test/test_publisher.cpp
@@ -33,21 +33,30 @@ void publish(
 
   auto publisher = node->create_publisher<T>(std::string("test/message/") + message_type, qos);
 
-  rclcpp::WallRate cycle_rate(10);
-  rclcpp::WallRate message_rate(100);
-  size_t cycle_index = 0;
-  // publish all messages up to number_of_cycles times, longer sleep between each cycle
-  while (rclcpp::ok() && cycle_index < number_of_cycles) {
-    size_t message_index = 0;
-    // publish all messages one by one, shorter sleep between each message
-    while (rclcpp::ok() && message_index < messages.size()) {
-      printf("publishing message #%zu\n", message_index + 1);
-      publisher->publish(*messages[message_index]);
-      ++message_index;
-      message_rate.sleep();
+  try {
+    rclcpp::WallRate cycle_rate(10);
+    rclcpp::WallRate message_rate(100);
+    size_t cycle_index = 0;
+    // publish all messages up to number_of_cycles times, longer sleep between each cycle
+    while (rclcpp::ok() && cycle_index < number_of_cycles) {
+      size_t message_index = 0;
+      // publish all messages one by one, shorter sleep between each message
+      while (rclcpp::ok() && message_index < messages.size()) {
+        printf("publishing message #%zu\n", message_index + 1);
+        publisher->publish(*messages[message_index]);
+        ++message_index;
+        message_rate.sleep();
+      }
+      ++cycle_index;
+      cycle_rate.sleep();
+    }
+  } catch (const std::exception & ex) {
+    // It is expected to get into invalid context during the sleep, since rclcpp::shutdown()
+    // might be called earlier (e.g. when running *AfterShutdown case)
+    if (ex.what() != std::string("context cannot be slept with because it's invalid")) {
+      printf("ERROR: got unexpected exception: %s\n", ex.what());
+      throw ex;
     }
-    ++cycle_index;
-    cycle_rate.sleep();
   }
 
   auto end = std::chrono::steady_clock::now();

diff --git a/test_communication/test/test_publisher_subscriber.cpp b/test_communication/test/test_publisher_subscriber.cpp
@@ -38,21 +38,30 @@ void publish(
 
   auto publisher = node->create_publisher<T>(std::string("test/message/") + message_type, qos);
 
-  rclcpp::WallRate cycle_rate(10);
-  rclcpp::WallRate message_rate(100);
-  size_t cycle_index = 0;
-  // publish all messages up to number_of_cycles times, longer sleep between each cycle
-  while (rclcpp::ok() && cycle_index < number_of_cycles) {
-    size_t message_index = 0;
-    // publish all messages one by one, shorter sleep between each message
-    while (rclcpp::ok() && message_index < messages.size()) {
-      printf("publishing message #%zu\n", message_index + 1);
-      publisher->publish(*messages[message_index]);
-      ++message_index;
-      message_rate.sleep();
+  try {
+    rclcpp::WallRate cycle_rate(10);
+    rclcpp::WallRate message_rate(100);
+    size_t cycle_index = 0;
+    // publish all messages up to number_of_cycles times, longer sleep between each cycle
+    while (rclcpp::ok() && cycle_index < number_of_cycles) {
+      size_t message_index = 0;
+      // publish all messages one by one, shorter sleep between each message
+      while (rclcpp::ok() && message_index < messages.size()) {
+        printf("publishing message #%zu\n", message_index + 1);
+        publisher->publish(*messages[message_index]);
+        ++message_index;
+        message_rate.sleep();
+      }
+      ++cycle_index;
+      cycle_rate.sleep();
+    }
+  } catch (const std::exception & ex) {
+    // It is expected to get into invalid context during the sleep, since rclcpp::shutdown()
+    // might be called earlier (e.g. by subscribe() routine or when running *AfterShutdown case)
+    if (ex.what() != std::string("context cannot be slept with because it's invalid")) {
+      printf("ERROR: got unexpected exception: %s\n", ex.what());
+      throw ex;
     }
-    ++cycle_index;
-    cycle_rate.sleep();
   }
 }
 

diff --git a/test_security/test/test_secure_publisher.cpp b/test_security/test/test_secure_publisher.cpp
@@ -32,21 +32,30 @@ int8_t attempt_publish(
 
   auto publisher = node->create_publisher<T>(topic_name, messages.size());
 
-  rclcpp::WallRate cycle_rate(10);
-  rclcpp::WallRate message_rate(100);
-  size_t cycle_index = 0;
-  // publish all messages up to number_of_cycles times, longer sleep between each cycle
-  while (rclcpp::ok() && cycle_index < number_of_cycles) {
-    size_t message_index = 0;
-    // publish all messages one by one, shorter sleep between each message
-    while (rclcpp::ok() && message_index < messages.size()) {
-      printf("publishing message #%zu\n", message_index + 1);
-      publisher->publish(*messages[message_index]);
-      ++message_index;
-      message_rate.sleep();
+  try {
+    rclcpp::WallRate cycle_rate(10);
+    rclcpp::WallRate message_rate(100);
+    size_t cycle_index = 0;
+    // publish all messages up to number_of_cycles times, longer sleep between each cycle
+    while (rclcpp::ok() && cycle_index < number_of_cycles) {
+      size_t message_index = 0;
+      // publish all messages one by one, shorter sleep between each message
+      while (rclcpp::ok() && message_index < messages.size()) {
+        printf("publishing message #%zu\n", message_index + 1);
+        publisher->publish(*messages[message_index]);
+        ++message_index;
+        message_rate.sleep();
+      }
+      ++cycle_index;
+      cycle_rate.sleep();
+    }
+  } catch (const std::exception & ex) {
+    // It is expected to get into invalid context during the sleep, since rclcpp::shutdown()
+    // might be called earlier (e.g. when running *AfterShutdown case)
+    if (ex.what() != std::string("context cannot be slept with because it's invalid")) {
+      printf("ERROR: got unexpected exception: %s\n", ex.what());
+      throw ex;
     }
-    ++cycle_index;
-    cycle_rate.sleep();
   }
 
   auto end = std::chrono::steady_clock::now();