From d474d0e3b41948219a7cdfbc982fffdf79ca9095 Mon Sep 17 00:00:00 2001
From: William Woodall <william@osrfoundation.org>
Date: Tue, 2 Sep 2014 18:30:10 -0700
Subject: [PATCH] add sigint singal handler chaining, err checking

---
 rclcpp/include/rclcpp/executor.hpp  | 41 +++++++++--------------------
 rclcpp/include/rclcpp/utilities.hpp | 20 +++++++++++++-
 2 files changed, 32 insertions(+), 29 deletions(-)

diff --git a/rclcpp/include/rclcpp/executor.hpp b/rclcpp/include/rclcpp/executor.hpp
index 4568a8e..40f4d1d 100644
--- a/rclcpp/include/rclcpp/executor.hpp
+++ b/rclcpp/include/rclcpp/executor.hpp
@@ -89,7 +89,6 @@ public:
 
   void spin_node_some(rclcpp::node::Node::SharedPtr &node)
   {
-    reset_subscriber_handles();
     this->add_node(node);
     // non-blocking = true
     std::shared_ptr<AnyExecutable> any_exec;
@@ -98,7 +97,6 @@ public:
       execute_any_executable(any_exec);
     }
     this->remove_node(node);
-    reset_subscriber_handles();
   }
 
 protected:
@@ -163,29 +161,6 @@ protected:
     timer->callback_();
   }
 
-/*** Reseting class storage ***/
-
-  void
-  reset_all_handles()
-  {
-    reset_subscriber_handles();
-    reset_guard_condition_handles();
-  }
-
-  void
-  reset_subscriber_handles()
-  {
-    subscriber_handles_.clear();
-  }
-
-  void
-  reset_guard_condition_handles()
-  {
-    guard_condition_handles_.clear();
-  }
-
-/******************************/
-
 /*** Populate class storage from stored weak node pointers and wait. ***/
 
   void
@@ -283,6 +258,15 @@ protected:
     ros_middleware_interface::wait(subscriber_handles,
                                    guard_condition_handles,
                                    nonblocking);
+    // If ctrl-c guard condition, return directly
+    if (guard_condition_handles.guard_conditions_[0] != 0)
+    {
+      // Make sure to free memory
+      // TODO: Remove theses when "Avoid redundant malloc's" todo is addressed
+      std::free(subscriber_handles.subscribers_);
+      std::free(guard_condition_handles.guard_conditions_);
+      return;
+    }
     // Add the new work to the class's list of things waiting to be executed
     // Starting with the subscribers
     for (size_t i = 0; i < number_of_subscriptions; ++i)
@@ -587,15 +571,16 @@ protected:
   }
 
   ros_middleware_interface::GuardConditionHandle interrupt_guard_condition_;
+
+private:
+  RCLCPP_DISABLE_COPY(Executor);
+
   std::vector<std::weak_ptr<rclcpp::node::Node>> weak_nodes_;
   typedef std::list<void*> SubscriberHandles;
   SubscriberHandles subscriber_handles_;
   typedef std::list<void*> GuardConditionHandles;
   GuardConditionHandles guard_condition_handles_;
 
-private:
-  RCLCPP_DISABLE_COPY(Executor);
-
 };
 
 } /* executor */
diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp
index 6459ccb..46d3743 100644
--- a/rclcpp/include/rclcpp/utilities.hpp
+++ b/rclcpp/include/rclcpp/utilities.hpp
@@ -16,11 +16,14 @@
 #ifndef RCLCPP_RCLCPP_UTILITIES_HPP_
 #define RCLCPP_RCLCPP_UTILITIES_HPP_
 
+// TODO: remove
 #include <iostream>
 
+#include <cerrno>
 #include <chrono>
 #include <condition_variable>
 #include <csignal>
+#include <cstring>
 #include <mutex>
 #include <thread>
 
@@ -35,14 +38,21 @@ namespace
   std::condition_variable g_interrupt_condition_variable;
   std::mutex g_interrupt_mutex;
 
+  void (*old_signal_handler)(int) = 0;
+
   void
   signal_handler(int signal_value)
   {
+    // TODO: remove
     std::cout << "signal_handler(" << signal_value << ")" << std::endl;
     g_signal_status = signal_value;
     using ros_middleware_interface::trigger_guard_condition;
     trigger_guard_condition(g_sigint_guard_cond_handle);
     g_interrupt_condition_variable.notify_all();
+    if (old_signal_handler)
+    {
+      return old_signal_handler(signal_value);
+    }
   }
 }
 
@@ -57,7 +67,15 @@ namespace utilities
 void
 init(int argc, char *argv[])
 {
-  std::signal(SIGINT, ::signal_handler);
+  ros_middleware_interface::init();
+  ::old_signal_handler = std::signal(SIGINT, ::signal_handler);
+  if (::old_signal_handler == SIG_ERR)
+  {
+    throw std::runtime_error(
+      std::string("Failed to set SIGINT signal handler: (" +
+                  std::to_string(errno) + ")") +
+      std::strerror(errno));
+  }
 }
 
 bool