From c925ab830545f50c7409eb063176e7d38054baa9 Mon Sep 17 00:00:00 2001
From: Gilwoo Lee <gilwoo301@gmail.com>
Date: Mon, 3 Apr 2017 20:09:38 -0400
Subject: [PATCH 1/5] Initial commit for Executor

---
 include/aikido/util/ExecutorMultiplexer.hpp | 28 ++++++++++
 include/aikido/util/ExecutorThread.hpp      | 37 +++++++++++++
 src/control/CMakeLists.txt                  |  2 +
 src/control/ros/CMakeLists.txt              | 48 +++++++++++++++++
 src/util/CMakeLists.txt                     |  6 ++-
 src/util/ExecutorMultiplexer.cpp            | 23 ++++++++
 src/util/ExecutorThread.cpp                 | 60 +++++++++++++++++++++
 7 files changed, 202 insertions(+), 2 deletions(-)
 create mode 100644 include/aikido/util/ExecutorMultiplexer.hpp
 create mode 100644 include/aikido/util/ExecutorThread.hpp
 create mode 100644 src/control/ros/CMakeLists.txt
 create mode 100644 src/util/ExecutorMultiplexer.cpp
 create mode 100644 src/util/ExecutorThread.cpp

diff --git a/include/aikido/util/ExecutorMultiplexer.hpp b/include/aikido/util/ExecutorMultiplexer.hpp
new file mode 100644
index 0000000000..e958339472
--- /dev/null
+++ b/include/aikido/util/ExecutorMultiplexer.hpp
@@ -0,0 +1,28 @@
+#ifndef AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
+#define AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
+#include <functional>
+#include <mutex>
+#include <vector>
+
+namespace aikido {
+namespace util {
+
+class ExecutorMultiplexer final
+{
+public:
+  ExecutorMultiplexer() = default;
+  ~ExecutorMultiplexer() = default;
+
+  void addCallback(std::function<void ()> _callback);
+
+  void operator ()();
+
+private:
+  std::mutex mMutex;
+  std::vector<std::function<void ()>> mCallbacks;
+};
+
+} // namespace util
+} // namespace aikido
+
+#endif // ifndef AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
diff --git a/include/aikido/util/ExecutorThread.hpp b/include/aikido/util/ExecutorThread.hpp
new file mode 100644
index 0000000000..5e7ff5194a
--- /dev/null
+++ b/include/aikido/util/ExecutorThread.hpp
@@ -0,0 +1,37 @@
+#ifndef AIKIDO_UTIL_EXECUTORTHREAD_HPP_
+#define AIKIDO_UTIL_EXECUTORTHREAD_HPP_
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <mutex>
+#include <thread>
+
+namespace aikido {
+namespace util {
+
+class ExecutorThread final
+{
+public:
+  ExecutorThread(
+    std::function<void ()> _callback,
+    std::chrono::milliseconds _period);
+
+  ~ExecutorThread();
+
+  bool is_running() const;
+
+  void stop();
+
+private:
+  void spin();
+
+  std::function<void ()> mCallback;
+  std::chrono::milliseconds mPeriod;
+  std::atomic<bool> mIsRunning;
+  std::thread mThread;
+};
+
+} // namespace util
+} // namespace aikido
+
+#endif // ifndef AIKIDO_UTIL_EXECUTORTHREAD_HPP_
diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt
index 75ca7ef7b6..ea7b7b8704 100644
--- a/src/control/CMakeLists.txt
+++ b/src/control/CMakeLists.txt
@@ -17,3 +17,5 @@ add_component_targets(${PROJECT_NAME} control "${PROJECT_NAME}_control")
 add_component_dependencies(${PROJECT_NAME} control statespace trajectory)
 
 coveralls_add_sources(${sources})
+
+add_subdirectory("ros") # Dependencies: control, statespace, trajectory
diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt
new file mode 100644
index 0000000000..6a9fc33193
--- /dev/null
+++ b/src/control/ros/CMakeLists.txt
@@ -0,0 +1,48 @@
+#==============================================================================
+# Dependencies
+#
+find_package(actionlib QUIET)
+aikido_check_package(actionlib "aikido::control::ros" "actionlib")
+
+find_package(control_msgs QUIET)
+aikido_check_package(control_msgs "aikido::control::ros" "control_msgs")
+
+find_package(roscpp QUIET)
+aikido_check_package(roscpp "aikido::control::ros" "roscpp")
+
+find_package(trajectory_msgs QUIET)
+aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")
+
+#==============================================================================
+# Libraries
+#
+set(sources
+  Conversions.cpp
+  RosJointStateClient.cpp
+  RosTrajectoryExecutor.cpp
+)
+
+add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})
+target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM
+  PUBLIC
+    ${actionlib_INCLUDE_DIRS}
+    ${control_msgs_INCLUDE_DIRS}
+    ${roscpp_INCLUDE_DIRS}
+    ${trajectory_msgs_INCLUDE_DIRS}
+)
+target_link_libraries("${PROJECT_NAME}_control_ros"
+  "${PROJECT_NAME}_control"
+  "${PROJECT_NAME}_statespace"
+  "${PROJECT_NAME}_trajectory"
+  ${DART_LIBRARIES}
+  ${actionlib_LIBRARIES}
+  ${control_msgs_LIBRARIES}
+  ${roscpp_LIBRARIES}
+  ${trajectory_msgs_LIBRARIES}
+)
+
+add_component(${PROJECT_NAME} control_ros)
+add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros")
+add_component_dependencies(${PROJECT_NAME} control control statespace trajectory)
+
+coveralls_add_sources(${sources})
diff --git a/src/util/CMakeLists.txt b/src/util/CMakeLists.txt
index 30c45f7f78..720f1e9c57 100644
--- a/src/util/CMakeLists.txt
+++ b/src/util/CMakeLists.txt
@@ -9,10 +9,12 @@ find_package(TinyXML2 REQUIRED)
 #
 set(sources
   CatkinResourceRetriever.cpp
+  ExecutorMultiplexer.cpp
+  ExecutorThread.cpp
   PseudoInverse.cpp
-  VanDerCorput.cpp
-  StepSequence.cpp
   RNG.cpp
+  StepSequence.cpp
+  VanDerCorput.cpp
   stream.cpp
 )
 
diff --git a/src/util/ExecutorMultiplexer.cpp b/src/util/ExecutorMultiplexer.cpp
new file mode 100644
index 0000000000..1f94be9db5
--- /dev/null
+++ b/src/util/ExecutorMultiplexer.cpp
@@ -0,0 +1,23 @@
+#include <aikido/util/ExecutorMultiplexer.hpp>
+
+namespace aikido {
+namespace util {
+
+//=============================================================================
+void ExecutorMultiplexer::addCallback(std::function<void ()> _callback)
+{
+  std::lock_guard<std::mutex> lock{mMutex};
+  mCallbacks.emplace_back(std::move(_callback));
+}
+
+//=============================================================================
+void ExecutorMultiplexer::operator ()()
+{
+  std::lock_guard<std::mutex> lock{mMutex};
+
+  for (auto& callback : mCallbacks)
+    callback();
+}
+
+} // namespace util
+} // namespace aikido
diff --git a/src/util/ExecutorThread.cpp b/src/util/ExecutorThread.cpp
new file mode 100644
index 0000000000..496cb9b1ce
--- /dev/null
+++ b/src/util/ExecutorThread.cpp
@@ -0,0 +1,60 @@
+#include <iostream>
+#include <aikido/util/ExecutorThread.hpp>
+
+namespace aikido {
+namespace util {
+
+//=============================================================================
+ExecutorThread::ExecutorThread(
+      std::function<void ()> _callback,
+      std::chrono::milliseconds _period)
+  : mCallback{std::move(_callback)}
+  , mPeriod{_period}
+  , mIsRunning{true}
+{
+  mThread = std::thread(&ExecutorThread::spin, this);
+}
+
+//=============================================================================
+ExecutorThread::~ExecutorThread()
+{
+  stop();
+}
+
+//=============================================================================
+bool ExecutorThread::is_running() const
+{
+  return mIsRunning.load();
+}
+
+//=============================================================================
+void ExecutorThread::stop()
+{
+  mIsRunning.store(false);
+  mThread.join();
+}
+
+//=============================================================================
+void ExecutorThread::spin()
+{
+  auto currentTime = std::chrono::steady_clock::now();
+
+  while (mIsRunning.load())
+  {
+    try
+    {
+      mCallback();
+    }
+    catch (const std::exception& e)
+    {
+      std::cerr << "Exception thrown by callback: " << e.what() << std::endl;
+      mIsRunning.store(false);
+    }
+
+    currentTime += mPeriod;
+    std::this_thread::sleep_until(currentTime);
+  }
+}
+
+} // namespace util
+} // namespace aikido

From b102958902b920566e403218e66cb864aa26126b Mon Sep 17 00:00:00 2001
From: Gilwoo Lee <gilwoo301@gmail.com>
Date: Mon, 3 Apr 2017 20:11:57 -0400
Subject: [PATCH 2/5] CMakeLists cleanup

---
 src/control/CMakeLists.txt     |  2 --
 src/control/ros/CMakeLists.txt | 48 ----------------------------------
 2 files changed, 50 deletions(-)
 delete mode 100644 src/control/ros/CMakeLists.txt

diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt
index ea7b7b8704..75ca7ef7b6 100644
--- a/src/control/CMakeLists.txt
+++ b/src/control/CMakeLists.txt
@@ -17,5 +17,3 @@ add_component_targets(${PROJECT_NAME} control "${PROJECT_NAME}_control")
 add_component_dependencies(${PROJECT_NAME} control statespace trajectory)
 
 coveralls_add_sources(${sources})
-
-add_subdirectory("ros") # Dependencies: control, statespace, trajectory
diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt
deleted file mode 100644
index 6a9fc33193..0000000000
--- a/src/control/ros/CMakeLists.txt
+++ /dev/null
@@ -1,48 +0,0 @@
-#==============================================================================
-# Dependencies
-#
-find_package(actionlib QUIET)
-aikido_check_package(actionlib "aikido::control::ros" "actionlib")
-
-find_package(control_msgs QUIET)
-aikido_check_package(control_msgs "aikido::control::ros" "control_msgs")
-
-find_package(roscpp QUIET)
-aikido_check_package(roscpp "aikido::control::ros" "roscpp")
-
-find_package(trajectory_msgs QUIET)
-aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")
-
-#==============================================================================
-# Libraries
-#
-set(sources
-  Conversions.cpp
-  RosJointStateClient.cpp
-  RosTrajectoryExecutor.cpp
-)
-
-add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})
-target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM
-  PUBLIC
-    ${actionlib_INCLUDE_DIRS}
-    ${control_msgs_INCLUDE_DIRS}
-    ${roscpp_INCLUDE_DIRS}
-    ${trajectory_msgs_INCLUDE_DIRS}
-)
-target_link_libraries("${PROJECT_NAME}_control_ros"
-  "${PROJECT_NAME}_control"
-  "${PROJECT_NAME}_statespace"
-  "${PROJECT_NAME}_trajectory"
-  ${DART_LIBRARIES}
-  ${actionlib_LIBRARIES}
-  ${control_msgs_LIBRARIES}
-  ${roscpp_LIBRARIES}
-  ${trajectory_msgs_LIBRARIES}
-)
-
-add_component(${PROJECT_NAME} control_ros)
-add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros")
-add_component_dependencies(${PROJECT_NAME} control control statespace trajectory)
-
-coveralls_add_sources(${sources})

From f912a93224787fc23021802785b14c3a00a6b745 Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Mon, 3 Apr 2017 21:56:48 -0400
Subject: [PATCH 3/5] Add documentation and tests; fix style

---
 include/aikido/util/ExecutorMultiplexer.hpp   | 24 ++++++++++-
 include/aikido/util/ExecutorThread.hpp        | 39 ++++++++++++++---
 .../util/detail/ExecutorThread-impl.hpp       | 19 ++++++++
 src/util/ExecutorMultiplexer.cpp              | 31 +++++++++++--
 src/util/ExecutorThread.cpp                   | 17 ++------
 tests/util/CMakeLists.txt                     |  3 ++
 tests/util/test_Executor.cpp                  | 43 +++++++++++++++++++
 7 files changed, 153 insertions(+), 23 deletions(-)
 create mode 100644 include/aikido/util/detail/ExecutorThread-impl.hpp
 create mode 100644 tests/util/test_Executor.cpp

diff --git a/include/aikido/util/ExecutorMultiplexer.hpp b/include/aikido/util/ExecutorMultiplexer.hpp
index e958339472..2a8c0f9554 100644
--- a/include/aikido/util/ExecutorMultiplexer.hpp
+++ b/include/aikido/util/ExecutorMultiplexer.hpp
@@ -1,5 +1,6 @@
 #ifndef AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
 #define AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
+
 #include <functional>
 #include <mutex>
 #include <vector>
@@ -7,18 +8,39 @@
 namespace aikido {
 namespace util {
 
+/// ExecutorMultiplexer stores callbacks and calls them in order of they added.
 class ExecutorMultiplexer final
 {
 public:
+  /// Default constructor.
   ExecutorMultiplexer() = default;
+
+  /// Default destructor.
   ~ExecutorMultiplexer() = default;
 
-  void addCallback(std::function<void ()> _callback);
+  /// Adds callback. The added callbacks will be called order of they added.
+  /// \param[in] callback Any callable object that doesn't return and take any
+  /// parameters.
+  void addCallback(std::function<void ()> callback);
+
+  /// Removes all the added callbacks.
+  void removeAllCallbacks();
 
+  /// Returns true if no callback is added. Otherwise, returns false.
+  bool isEmpty() const;
+
+  /// Returns the number of added callbacks.
+  std::size_t getNumCallbacks() const;
+
+  /// Executes all the added callbacked in order of they added.
   void operator ()();
 
 private:
+  /// Mutex for the list of callbacks. The array of callbacks will be locked
+  /// during it's modified and the callbacks are called.
   std::mutex mMutex;
+
+  /// Array of callbacks.
   std::vector<std::function<void ()>> mCallbacks;
 };
 
diff --git a/include/aikido/util/ExecutorThread.hpp b/include/aikido/util/ExecutorThread.hpp
index 5e7ff5194a..35424f7b57 100644
--- a/include/aikido/util/ExecutorThread.hpp
+++ b/include/aikido/util/ExecutorThread.hpp
@@ -1,37 +1,66 @@
 #ifndef AIKIDO_UTIL_EXECUTORTHREAD_HPP_
 #define AIKIDO_UTIL_EXECUTORTHREAD_HPP_
+
 #include <atomic>
 #include <chrono>
 #include <functional>
-#include <mutex>
 #include <thread>
 
 namespace aikido {
 namespace util {
 
+/// ExecutorThread is a wrapper of std::thread that calls a callback
+/// periodically.
+///
+/// \code
+/// ExecutorThread exec(
+///     []() { std::cout << "running\n";}, std::chrono::nanoseconds(1));
+///
+/// // thread is running
+///
+/// exec.stop();
+/// \endcode
 class ExecutorThread final
 {
 public:
-  ExecutorThread(
-    std::function<void ()> _callback,
-    std::chrono::milliseconds _period);
+  /// Constructs from callback and period. The thread begins execution
+  /// immediately upon construction.
+  /// \param[in] callback
+  /// \param[in] period
+  template <typename Duration>
+  ExecutorThread(std::function<void ()> callback, const Duration& period);
 
+  /// Default destructor. The thread is stopped as ExecutorThread is destructed.
   ~ExecutorThread();
 
-  bool is_running() const;
+  /// Returns true if the thread is running.
+  bool isRunning() const;
 
+  /// Stops the thread. It is safe to call this function even when the thread is
+  /// already stopped.
   void stop();
 
 private:
+  /// The loop function that will be executed by the thread.
   void spin();
 
+private:
+  /// Callback to be periodically executed by the thread.
   std::function<void ()> mCallback;
+
+  /// The callback is called in this period.
   std::chrono::milliseconds mPeriod;
+
+  /// Flag whether the thread is running.
   std::atomic<bool> mIsRunning;
+
+  /// Thread.
   std::thread mThread;
 };
 
 } // namespace util
 } // namespace aikido
 
+#include <aikido/util/detail/ExecutorThread-impl.hpp>
+
 #endif // ifndef AIKIDO_UTIL_EXECUTORTHREAD_HPP_
diff --git a/include/aikido/util/detail/ExecutorThread-impl.hpp b/include/aikido/util/detail/ExecutorThread-impl.hpp
new file mode 100644
index 0000000000..551e2df0f4
--- /dev/null
+++ b/include/aikido/util/detail/ExecutorThread-impl.hpp
@@ -0,0 +1,19 @@
+#include <aikido/util/ExecutorThread.hpp>
+
+namespace aikido {
+namespace util {
+
+//==============================================================================
+template <typename Duration>
+ExecutorThread::ExecutorThread(
+    std::function<void ()> callback, const Duration& period)
+  : mCallback{std::move(callback)},
+    mPeriod{std::chrono::duration_cast<std::chrono::milliseconds>(period)},
+    mIsRunning{true},
+    mThread{std::thread{&ExecutorThread::spin, this}}
+{
+  // Do nothing
+}
+
+} // namespace util
+} // namespace aikido
diff --git a/src/util/ExecutorMultiplexer.cpp b/src/util/ExecutorMultiplexer.cpp
index 1f94be9db5..52e53600ea 100644
--- a/src/util/ExecutorMultiplexer.cpp
+++ b/src/util/ExecutorMultiplexer.cpp
@@ -1,21 +1,44 @@
 #include <aikido/util/ExecutorMultiplexer.hpp>
 
+#include <dart/dart.hpp>
+
 namespace aikido {
 namespace util {
 
 //=============================================================================
-void ExecutorMultiplexer::addCallback(std::function<void ()> _callback)
+void ExecutorMultiplexer::addCallback(std::function<void ()> callback)
 {
   std::lock_guard<std::mutex> lock{mMutex};
-  mCallbacks.emplace_back(std::move(_callback));
+  DART_UNUSED(lock);
+
+  mCallbacks.emplace_back(std::move(callback));
+}
+
+//=============================================================================
+void ExecutorMultiplexer::removeAllCallbacks()
+{
+  mCallbacks.clear();
+}
+
+//=============================================================================
+bool ExecutorMultiplexer::isEmpty() const
+{
+  return mCallbacks.empty();
+}
+
+//=============================================================================
+std::size_t ExecutorMultiplexer::getNumCallbacks() const
+{
+  return mCallbacks.size();
 }
 
 //=============================================================================
-void ExecutorMultiplexer::operator ()()
+void ExecutorMultiplexer::operator()()
 {
   std::lock_guard<std::mutex> lock{mMutex};
+  DART_UNUSED(lock);
 
-  for (auto& callback : mCallbacks)
+  for (const auto& callback : mCallbacks)
     callback();
 }
 
diff --git a/src/util/ExecutorThread.cpp b/src/util/ExecutorThread.cpp
index 496cb9b1ce..a187179a7c 100644
--- a/src/util/ExecutorThread.cpp
+++ b/src/util/ExecutorThread.cpp
@@ -4,17 +4,6 @@
 namespace aikido {
 namespace util {
 
-//=============================================================================
-ExecutorThread::ExecutorThread(
-      std::function<void ()> _callback,
-      std::chrono::milliseconds _period)
-  : mCallback{std::move(_callback)}
-  , mPeriod{_period}
-  , mIsRunning{true}
-{
-  mThread = std::thread(&ExecutorThread::spin, this);
-}
-
 //=============================================================================
 ExecutorThread::~ExecutorThread()
 {
@@ -22,7 +11,7 @@ ExecutorThread::~ExecutorThread()
 }
 
 //=============================================================================
-bool ExecutorThread::is_running() const
+bool ExecutorThread::isRunning() const
 {
   return mIsRunning.load();
 }
@@ -31,7 +20,9 @@ bool ExecutorThread::is_running() const
 void ExecutorThread::stop()
 {
   mIsRunning.store(false);
-  mThread.join();
+
+  if (mThread.joinable())
+    mThread.join();
 }
 
 //=============================================================================
diff --git a/tests/util/CMakeLists.txt b/tests/util/CMakeLists.txt
index 295765b11a..e796d88bed 100644
--- a/tests/util/CMakeLists.txt
+++ b/tests/util/CMakeLists.txt
@@ -31,6 +31,9 @@ target_compile_definitions(test_CatkinResourceRetriever_catkin_build
 #==============================================================================
 # Miscellaneous
 #
+aikido_add_test(test_Executor test_Executor.cpp)
+target_link_libraries(test_Executor "${PROJECT_NAME}_util")
+
 aikido_add_test(test_PseudoInverse test_PseudoInverse.cpp)
 target_link_libraries(test_PseudoInverse "${PROJECT_NAME}_util")
 
diff --git a/tests/util/test_Executor.cpp b/tests/util/test_Executor.cpp
new file mode 100644
index 0000000000..53da70b6ff
--- /dev/null
+++ b/tests/util/test_Executor.cpp
@@ -0,0 +1,43 @@
+#include <aikido/util/ExecutorMultiplexer.hpp>
+#include <aikido/util/ExecutorThread.hpp>
+#include <gtest/gtest.h>
+
+using namespace aikido::util;
+
+static int numCalled = 0;
+
+void foo() { numCalled++; }
+
+//==============================================================================
+TEST(ExecutorMultiplexer, Execute)
+{
+  ExecutorMultiplexer exec;
+  EXPECT_TRUE(exec.isEmpty());
+  EXPECT_TRUE(exec.getNumCallbacks() == 0u);
+
+  exec.addCallback(foo);
+  exec.addCallback([]() {});
+  EXPECT_TRUE(!exec.isEmpty());
+  EXPECT_TRUE(exec.getNumCallbacks() == 2u);
+
+  EXPECT_TRUE(numCalled == 0);
+  exec();
+  EXPECT_TRUE(numCalled == 1);
+
+  exec.removeAllCallbacks();
+  EXPECT_TRUE(exec.isEmpty());
+
+  EXPECT_TRUE(numCalled == 1);
+  exec();
+  EXPECT_TRUE(numCalled == 1);
+}
+
+//==============================================================================
+TEST(ExecutorThread, Execute)
+{
+  ExecutorThread exec([]() {}, std::chrono::nanoseconds(1));
+
+  EXPECT_TRUE(exec.isRunning());
+  exec.stop();
+  EXPECT_TRUE(!exec.isRunning());
+}

From f722010418c1e3009d0be1ff2982ef3f3706a90d Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Mon, 3 Apr 2017 22:42:00 -0400
Subject: [PATCH 4/5] Address Mike's comments

---
 include/aikido/util/ExecutorThread.hpp |  8 ++++----
 src/util/ExecutorThread.cpp            |  5 +++++
 tests/util/test_Executor.cpp           | 11 +++++++++++
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/include/aikido/util/ExecutorThread.hpp b/include/aikido/util/ExecutorThread.hpp
index 35424f7b57..f39b72760b 100644
--- a/include/aikido/util/ExecutorThread.hpp
+++ b/include/aikido/util/ExecutorThread.hpp
@@ -25,18 +25,18 @@ class ExecutorThread final
 public:
   /// Constructs from callback and period. The thread begins execution
   /// immediately upon construction.
-  /// \param[in] callback
-  /// \param[in] period
+  /// \param[in] callback Callback to be repeatedly executed by the thread.
+  /// \param[in] period The period of calling the callback.
   template <typename Duration>
   ExecutorThread(std::function<void ()> callback, const Duration& period);
 
-  /// Default destructor. The thread is stopped as ExecutorThread is destructed.
+  /// Default destructor. The thread stops as ExecutorThread is destructed.
   ~ExecutorThread();
 
   /// Returns true if the thread is running.
   bool isRunning() const;
 
-  /// Stops the thread. It is safe to call this function even when the thread is
+  /// Stops the thread. It is safe to call this function even when the thread
   /// already stopped.
   void stop();
 
diff --git a/src/util/ExecutorThread.cpp b/src/util/ExecutorThread.cpp
index a187179a7c..25cb4ccfb4 100644
--- a/src/util/ExecutorThread.cpp
+++ b/src/util/ExecutorThread.cpp
@@ -39,7 +39,12 @@ void ExecutorThread::spin()
     catch (const std::exception& e)
     {
       std::cerr << "Exception thrown by callback: " << e.what() << std::endl;
+      // TODO: We should find another way to handle this error, so we don't
+      // print directly to std::cerr. Unfortunately, we don't have a better
+      // solution yet since Aikido doesn't use any particular logging framework.
       mIsRunning.store(false);
+
+      break;
     }
 
     currentTime += mPeriod;
diff --git a/tests/util/test_Executor.cpp b/tests/util/test_Executor.cpp
index 53da70b6ff..7c98308852 100644
--- a/tests/util/test_Executor.cpp
+++ b/tests/util/test_Executor.cpp
@@ -41,3 +41,14 @@ TEST(ExecutorThread, Execute)
   exec.stop();
   EXPECT_TRUE(!exec.isRunning());
 }
+
+//==============================================================================
+TEST(ExecutorThread, ExceptionThrownByCallback)
+{
+  ExecutorThread exec(
+      []() { throw std::exception(); }, std::chrono::milliseconds(1));
+
+  std::this_thread::sleep_for(std::chrono::seconds(3));
+
+  EXPECT_TRUE(!exec.isRunning());
+}

From d6c937df35b0ada33a8c0bb6f0adbf84bae0972b Mon Sep 17 00:00:00 2001
From: Jeongseok Lee <jslee02@gmail.com>
Date: Wed, 5 Apr 2017 22:21:38 -0400
Subject: [PATCH 5/5] Update docstring of executor classes per Mike's comment

---
 include/aikido/util/ExecutorMultiplexer.hpp | 13 +++++++++++--
 include/aikido/util/ExecutorThread.hpp      |  9 +++++++--
 tests/util/test_Executor.cpp                |  3 +++
 3 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/include/aikido/util/ExecutorMultiplexer.hpp b/include/aikido/util/ExecutorMultiplexer.hpp
index 2a8c0f9554..a715e705b8 100644
--- a/include/aikido/util/ExecutorMultiplexer.hpp
+++ b/include/aikido/util/ExecutorMultiplexer.hpp
@@ -8,7 +8,12 @@
 namespace aikido {
 namespace util {
 
-/// ExecutorMultiplexer stores callbacks and calls them in order of they added.
+/// Combine multiple executors (i.e. no argument callbacks) into one executor.
+///
+/// This helper class allows one ExecutorThread to call multiple executors by
+/// sequentially calling the callbacks added to this class.
+///
+/// \sa ExecutorThread
 class ExecutorMultiplexer final
 {
 public:
@@ -18,7 +23,11 @@ class ExecutorMultiplexer final
   /// Default destructor.
   ~ExecutorMultiplexer() = default;
 
-  /// Adds callback. The added callbacks will be called order of they added.
+  /// Adds a callback. The added callbacks will be called by operator().
+  ///
+  /// The order of callback calling is implementation detail that is subject to
+  /// change.
+  ///
   /// \param[in] callback Any callable object that doesn't return and take any
   /// parameters.
   void addCallback(std::function<void ()> callback);
diff --git a/include/aikido/util/ExecutorThread.hpp b/include/aikido/util/ExecutorThread.hpp
index f39b72760b..3604027125 100644
--- a/include/aikido/util/ExecutorThread.hpp
+++ b/include/aikido/util/ExecutorThread.hpp
@@ -12,14 +12,19 @@ namespace util {
 /// ExecutorThread is a wrapper of std::thread that calls a callback
 /// periodically.
 ///
+/// If you want to let ExecutorThread calls multiple callbacks then consider
+/// using ExecutorMultiplexer.
+///
 /// \code
 /// ExecutorThread exec(
-///     []() { std::cout << "running\n";}, std::chrono::nanoseconds(1));
+///     []() { std::cout << "running...\n"; }, std::chrono::milliseconds(10));
 ///
 /// // thread is running
 ///
-/// exec.stop();
+/// // The destructor of ExecutorThread stops the thread.
 /// \endcode
+///
+/// \sa ExecutorMultiplexer
 class ExecutorThread final
 {
 public:
diff --git a/tests/util/test_Executor.cpp b/tests/util/test_Executor.cpp
index 7c98308852..1e69b2b1d4 100644
--- a/tests/util/test_Executor.cpp
+++ b/tests/util/test_Executor.cpp
@@ -48,6 +48,9 @@ TEST(ExecutorThread, ExceptionThrownByCallback)
   ExecutorThread exec(
       []() { throw std::exception(); }, std::chrono::milliseconds(1));
 
+  // Assumed that the 3 second sleep is essentially a timeout for the test. If
+  // this is not the case, either increase the sleep time or remove this test
+  // (unless there is a better test for this).
   std::this_thread::sleep_for(std::chrono::seconds(3));
 
   EXPECT_TRUE(!exec.isRunning());