From 4a2e9d8af92315c7bf838e6642d25495b4f09969 Mon Sep 17 00:00:00 2001
From: dhood <dhood@users.noreply.github.com>
Date: Thu, 14 Sep 2017 11:03:24 -0700
Subject: [PATCH] Reset rcl errors (#374)

---
 rclcpp/src/rclcpp/executor.cpp | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp
index 3b4ae4d..6cc7905 100644
--- a/rclcpp/src/rclcpp/executor.cpp
+++ b/rclcpp/src/rclcpp/executor.cpp
@@ -60,9 +60,11 @@ Executor::Executor(const ExecutorArgs & args)
   {
     fprintf(stderr,
       "[rclcpp::error] failed to create waitset: %s\n", rcl_get_error_string_safe());
+    rcl_reset_error();
     if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
       fprintf(stderr,
         "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+      rcl_reset_error();
     }
     throw std::runtime_error("Failed to create waitset in Executor constructor");
   }
@@ -74,11 +76,13 @@ Executor::~Executor()
   if (rcl_wait_set_fini(&waitset_) != RCL_RET_OK) {
     fprintf(stderr,
       "[rclcpp::error] failed to destroy waitset: %s\n", rcl_get_error_string_safe());
+    rcl_reset_error();
   }
   // Finalize the interrupt guard condition.
   if (rcl_guard_condition_fini(&interrupt_guard_condition_) != RCL_RET_OK) {
     fprintf(stderr,
       "[rclcpp::error] failed to destroy guard condition: %s\n", rcl_get_error_string_safe());
+    rcl_reset_error();
   }
   // Remove and release the sigint guard condition
   memory_strategy_->remove_guard_condition(
@@ -270,6 +274,7 @@ Executor::execute_subscription(
     fprintf(stderr,
       "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
       subscription->get_topic_name(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
   subscription->return_message(message);
 }
@@ -292,6 +297,7 @@ Executor::execute_intra_process_subscription(
     fprintf(stderr,
       "[rclcpp::error] take failed for intra process subscription on topic '%s': %s\n",
       subscription->get_topic_name(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -318,6 +324,7 @@ Executor::execute_service(
     fprintf(stderr,
       "[rclcpp::error] take request failed for server of service '%s': %s\n",
       service->get_service_name().c_str(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -337,6 +344,7 @@ Executor::execute_client(
     fprintf(stderr,
       "[rclcpp::error] take response failed for client of service '%s': %s\n",
       client->get_service_name().c_str(), rcl_get_error_string_safe());
+    rcl_reset_error();
   }
 }
 
@@ -410,7 +418,10 @@ Executor::wait_for_work(std::chrono::nanoseconds timeout)
   if (status == RCL_RET_WAIT_SET_EMPTY) {
     fprintf(stderr, "Warning: empty waitset received in rcl_wait(). This should never happen.\n");
   } else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT) {
-    throw std::runtime_error(std::string("rcl_wait() failed: ") + rcl_get_error_string_safe());
+    std::string msg = "rcl_wait() failed: ";
+    msg += rcl_get_error_string_safe();
+    rcl_reset_error();
+    throw std::runtime_error(msg);
   }
 
   // check the null handles in the waitset and remove them from the handles in memory strategy