From edc18861739985db875130ae350e79697cd0160f Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Tue, 14 Jul 2015 19:34:28 -0700
Subject: [PATCH] incorporating changes from #53

---
 rclcpp/include/rclcpp/client.hpp    |  8 ++++++--
 rclcpp/include/rclcpp/executor.hpp  | 22 ++++++++++++++--------
 rclcpp/include/rclcpp/publisher.hpp |  8 +++++++-
 rclcpp/include/rclcpp/service.hpp   |  8 +++++++-
 4 files changed, 34 insertions(+), 12 deletions(-)

diff --git a/rclcpp/include/rclcpp/client.hpp b/rclcpp/include/rclcpp/client.hpp
index 6b957d4..553c9b5 100644
--- a/rclcpp/include/rclcpp/client.hpp
+++ b/rclcpp/include/rclcpp/client.hpp
@@ -146,8 +146,12 @@ public:
     CallbackType cb)
   {
     int64_t sequence_number;
-    // TODO(wjwwood): Check the return code.
-    rmw_send_request(get_client_handle(), request.get(), &sequence_number);
+    if (RMW_RET_OK != rmw_send_request(get_client_handle(), request.get(), &sequence_number)) {
+      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      throw std::runtime_error(
+        std::string("failed to send request: ") + rmw_get_error_string_safe());
+      // *INDENT-ON*
+    }
 
     SharedPromise call_promise = std::make_shared<Promise>();
     SharedFuture f(call_promise->get_future());
diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index d3e9051..d2e28a8 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -162,8 +162,9 @@ protected:
         subscription->handle_message(message);
       }
     } else {
-      std::cout << "[rclcpp::error] take failed for subscription on topic: " <<
-        subscription->get_topic_name() << std::endl;
+      fprintf(stderr,
+        "[rclcpp::error] take failed for subscription on topic '%s': %s\n",
+        subscription->get_topic_name().c_str(), rmw_get_error_string_safe());
     }
   }
 
@@ -191,8 +192,9 @@ protected:
         service->handle_request(request_header, request);
       }
     } else {
-      std::cout << "[rclcpp::error] take failed for service on service: " <<
-        service->get_service_name() << std::endl;
+      fprintf(stderr,
+        "[rclcpp::error] take request failed for server of service '%s': %s\n",
+        service->get_service_name().c_str(), rmw_get_error_string_safe());
     }
   }
 
@@ -203,15 +205,19 @@ protected:
     std::shared_ptr<void> request_header = client->create_request_header();
     std::shared_ptr<void> response = client->create_response();
     bool taken = false;
-    rmw_take_response(
+    rmw_ret_t status = rmw_take_response(
       client->client_handle_,
       request_header.get(),
       response.get(),
       &taken);
-    if (taken) {
-      client->handle_response(request_header, response);
+    if (status == RMW_RET_OK) {
+      if (taken) {
+        client->handle_response(request_header, response);
+      }
     } else {
-      std::cout << "[rclcpp::error] take failed for service on client" << std::endl;
+      fprintf(stderr,
+        "[rclcpp::error] take response failed for client of service '%s': %s\n",
+        client->get_service_name().c_str(), rmw_get_error_string_safe());
     }
   }
 
diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index f7810bd..b31b2dc 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -63,7 +63,13 @@ public:
   void
   publish(std::shared_ptr<MessageT> & msg)
   {
-    rmw_publish(publisher_handle_, msg.get());
+    rmw_ret_t status = rmw_publish(publisher_handle_, msg.get());
+    if (status != RMW_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      throw std::runtime_error(
+        std::string("failed to publish message: ") + rmw_get_error_string_safe());
+      // *INDENT-ON*
+    }
   }
 
 private:
diff --git a/rclcpp/include/rclcpp/service.hpp b/rclcpp/include/rclcpp/service.hpp
index 3c9d7c3..6c756c1 100644
--- a/rclcpp/include/rclcpp/service.hpp
+++ b/rclcpp/include/rclcpp/service.hpp
@@ -154,7 +154,13 @@ public:
     std::shared_ptr<rmw_request_id_t> & req_id,
     std::shared_ptr<typename ServiceT::Response> & response)
   {
-    rmw_send_response(get_service_handle(), req_id.get(), response.get());
+    rmw_ret_t status = rmw_send_response(get_service_handle(), req_id.get(), response.get());
+    if (status != RMW_RET_OK) {
+      // *INDENT-OFF* (prevent uncrustify from making unecessary indents here)
+      throw std::runtime_error(
+        std::string("failed to send response: ") + rmw_get_error_string_safe());
+      // *INDENT-ON*
+    }
   }
 
 private: