ros_control/Executor (#151)

include/aikido/util/ExecutorMultiplexer.hpp

@@ -0,0 +1,59 @@
+#ifndef AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
+#define AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_
+
+#include <functional>
+#include <mutex>
+#include <vector>
+
+namespace aikido {
+namespace util {
+
+/// 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:
+  /// Default constructor.
+  ExecutorMultiplexer() = default;
+
+  /// Default destructor.
+  ~ExecutorMultiplexer() = default;
+
+  /// 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);
+
+  /// 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;
+};
+
+} // namespace util
+} // namespace aikido
+
+#endif // ifndef AIKIDO_UTIL_EXECUTORMULTIPLEXER_HPP_

include/aikido/util/ExecutorThread.hpp

@@ -0,0 +1,71 @@
+#ifndef AIKIDO_UTIL_EXECUTORTHREAD_HPP_
+#define AIKIDO_UTIL_EXECUTORTHREAD_HPP_
+
+#include <atomic>
+#include <chrono>
+#include <functional>
+#include <thread>
+
+namespace aikido {
+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::milliseconds(10));
+///
+/// // thread is running
+///
+/// // The destructor of ExecutorThread stops the thread.
+/// \endcode
+///
+/// \sa ExecutorMultiplexer
+class ExecutorThread final
+{
+public:
+  /// Constructs from callback and period. The thread begins execution
+  /// immediately upon construction.
+  /// \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 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
+  /// 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_

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

src/util/CMakeLists.txt

@@ -9,6 +9,8 @@ find_package(TinyXML2 REQUIRED)
 #
 set(sources
   CatkinResourceRetriever.cpp
+  ExecutorMultiplexer.cpp
+  ExecutorThread.cpp
   KinBodyParser.cpp
   PseudoInverse.cpp
   RNG.cpp

src/util/ExecutorMultiplexer.cpp

@@ -0,0 +1,46 @@
+#include <aikido/util/ExecutorMultiplexer.hpp>
+
+#include <dart/dart.hpp>
+
+namespace aikido {
+namespace util {
+
+//=============================================================================
+void ExecutorMultiplexer::addCallback(std::function<void ()> callback)
+{
+  std::lock_guard<std::mutex> lock{mMutex};
+  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()()
+{
+  std::lock_guard<std::mutex> lock{mMutex};
+  DART_UNUSED(lock);
+
+  for (const auto& callback : mCallbacks)
+    callback();
+}
+
+} // namespace util
+} // namespace aikido

src/util/ExecutorThread.cpp

@@ -0,0 +1,56 @@
+#include <iostream>
+#include <aikido/util/ExecutorThread.hpp>
+
+namespace aikido {
+namespace util {
+
+//=============================================================================
+ExecutorThread::~ExecutorThread()
+{
+  stop();
+}
+
+//=============================================================================
+bool ExecutorThread::isRunning() const
+{
+  return mIsRunning.load();
+}
+
+//=============================================================================
+void ExecutorThread::stop()
+{
+  mIsRunning.store(false);
+
+  if (mThread.joinable())
+    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;
+      // 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;
+    std::this_thread::sleep_until(currentTime);
+  }
+}
+
+} // namespace util
+} // namespace aikido

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_KinBodyParser test_KinBodyParser.cpp)
 target_link_libraries(test_KinBodyParser "${PROJECT_NAME}_util")
 target_compile_definitions(test_KinBodyParser
@@ -53,4 +56,3 @@ target_link_libraries(test_SplineProblem "${PROJECT_NAME}_util")
 
 aikido_add_test(test_string test_string.cpp)
 target_link_libraries(test_string "${PROJECT_NAME}_util")
-

tests/util/test_Executor.cpp

@@ -0,0 +1,57 @@
+#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());
+}
+
+//==============================================================================
+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());
+}