resolve merge conflicts

backend/CMakeLists.txt

@@ -1,5 +1,9 @@
 include (${project_cmake_dir}/Utils.cmake)
+include_directories(${PYTHON_INCLUDE_DIRS})
+include_directories(${pybind11_INCLUDE_DIRS})
 
+# ----------------------------------------
+# Prepare sources
 set (sources
   automotive_simulator.cc
   simulation_runner.cc
@@ -10,6 +14,8 @@ set (gtest_sources
   automotive_simulator_test.cc
 )
 
+# ----------------------------------------
+# Automotive demo
 add_executable(automotive-demo
   automotive_demo.cc
   ${sources}
@@ -21,6 +27,8 @@ target_link_libraries(automotive-demo
   ${drake_LIBRARIES}
   ${IGNITION-COMMON_LIBRARIES}
   ${IGNITION-TRANSPORT_LIBRARIES}
+  pybind11::module
+  ${PYTHON_LIBRARIES}
 )
 
 install(TARGETS automotive-demo DESTINATION ${CMAKE_INSTALL_PREFIX}/bin)
@@ -51,10 +59,15 @@ target_link_libraries(simulation-support
   ${drake_LIBRARIES}
   ${IGNITION-COMMON_LIBRARIES}
   ${IGNITION-TRANSPORT_LIBRARIES}
+  pybind11::module
+  ${PYTHON_LIBRARIES}
 )
 
 target_include_directories(simulation-support PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
 
+# ----------------------------------------
+# Python bindings
+
 # Define the wrapper library for python
 add_library(simulation_runner_py
   SHARED
@@ -70,7 +83,7 @@ target_link_libraries(simulation_runner_py
   ${drake_LIBRARIES}
   ${IGNITION-COMMON_LIBRARIES}
   ${IGNITION-TRANSPORT_LIBRARIES}
-  ${Boost_LIBRARIES}
+  pybind11::module
   ${PYTHON_LIBRARIES}
   )
 

backend/python_bindings_example.py

@@ -41,7 +41,10 @@
 from __future__ import print_function
 
 import os
+import random
 import sys
+import time
+
 from launcher import Launcher
 from simulation_runner_py import SimulatorRunner
 
@@ -62,8 +65,46 @@ def get_from_env_or_fail(var):
     return value
 
 
+class SimulationStats(object):
+    """This is a simple class to keep statistics of the simulation, just
+    averaging the time it takes to execute a simulation step from the outside
+    world. Every 1000 measures, the values are printed to stdout"""
+    def __init__(self):
+        self.reset()
+
+    def reset(self):
+        self._time_sum = 0.0
+        self._samples_count = 0
+        self._current_start_time = None
+
+    def print_stats(self):
+        print(
+            "Average simulation step takes {delta}ms"
+            .format(delta=(self._time_sum / self._samples_count) * 1000))
+
+    def start(self):
+        self._current_start_time = time.time()
+
+    def record_tick(self):
+        end_time = time.time()
+        delta = end_time - self._current_start_time
+        self._time_sum += delta
+        self._samples_count += 1
+        if self._samples_count == 1000:
+            self.print_stats()
+            self.reset()
+        self.start()
+
+
+def random_print():
+    """Print a message at random, roughly every 500 calls"""
+    if (random.randint(1, 500) == 1):
+        print("One in five hundred")
+
+
 def main():
     """Spawn an automotive simulator making use of the python bindings"""
+    stats = SimulationStats()
     launcher = Launcher()
 
     delphyne_ws_dir = get_from_env_or_fail('DELPHYNE_WS_DIR')
@@ -80,10 +121,20 @@ def main():
         launcher.launch([ign_visualizer, teleop_config])
 
         runner = SimulatorRunner()
-        runner.start()
+        # Add a callback to record and print statistics
+        runner.AddStepCallback(stats.record_tick)
+        # Add a second callback that prints a message roughly every 500 calls
+        runner.AddStepCallback(random_print)
+
+        stats.start()
+        runner.Start()
 
         launcher.wait(float("Inf"))
     finally:
+        runner.Stop()
+        # This is needed to avoid a possible deadlock. See SimulatorRunner
+        # class description.
+        time.sleep(0.5)
         launcher.kill()
 
 

backend/simulation_runner.cc

@@ -36,12 +36,14 @@
 #include <ignition/common/Console.hh>
 #include <ignition/msgs.hh>
 #include <ignition/transport/Node.hh>
+#include <pybind11/pybind11.h>
 
 #include "backend/simulation_runner.h"
 
+namespace py = pybind11;
+
 namespace delphyne {
 namespace backend {
-
 /// \brief Flag to detect SIGINT or SIGTERM while the code is executing
 /// WaitForShutdown().
 static bool g_shutdown = false;
@@ -94,22 +96,37 @@ SimulatorRunner::SimulatorRunner(
     ignerr << "Error advertising service [" << kRobotRequestServiceName
               << "]" << std::endl;
   }
+  // Initialize the python machinery so we can invoke a python callback
+  // function on each simulation step.
+  Py_Initialize();
+  PyEval_InitThreads();
 
   this->simulator->Start();
 }
 
 //////////////////////////////////////////////////
 SimulatorRunner::~SimulatorRunner() {
-  {
-    // Tell the main loop thread to terminate.
-    std::lock_guard<std::mutex> lock(this->mutex);
-    this->enabled = false;
-  }
+  Stop();
   if (this->mainThread.joinable()) {
     this->mainThread.join();
   }
 }
 
+//////////////////////////////////////////////////
+void SimulatorRunner::Stop() {
+  // Only do this if we are running the simulation
+  if (this->enabled) {
+    // Tell the main loop thread to terminate.
+    this->enabled = false;
+  }
+}
+
+//////////////////////////////////////////////////
+void SimulatorRunner::AddStepCallback(std::function<void()> callable) {
+  std::lock_guard<std::mutex> lock(this->mutex);
+  step_callbacks_.push_back(callable);
+}
+
 //////////////////////////////////////////////////
 void SimulatorRunner::Start() {
   // The main loop is already running.
@@ -123,11 +140,14 @@ void SimulatorRunner::Start() {
 
 //////////////////////////////////////////////////
 void SimulatorRunner::Run() {
-  bool stayAlive = true;
-  while (stayAlive) {
+  while (this->enabled) {
     // Start a timer to measure the time we spend doing tasks.
     auto stepStart = std::chrono::steady_clock::now();
 
+    // A copy of the python callbacks so we can process them in a thread-safe
+    // way
+    std::vector<std::function<void()>> callbacks;
+
     // 1. Process incoming messages (requests).
     {
       std::lock_guard<std::mutex> lock(this->mutex);
@@ -150,8 +170,19 @@ void SimulatorRunner::Run() {
       // 3. Process outgoing messages (notifications).
       this->SendOutgoingMessages();
 
-      // Do we have to exit?
-      stayAlive = this->enabled;
+      // Make a temporal copy of the python callbacks while we have the lock.
+      callbacks = step_callbacks_;
+    }
+
+    // This if is here so that we only grab the python global interpreter lock
+    // if there is at least one callback.
+    if (callbacks.size() > 0) {
+      // 1. Acquire the lock to the python interpreter
+      py::gil_scoped_acquire acquire;
+      // 2. Perform the callbacks
+      for (std::function<void()> callback : callbacks) {
+        callback();
+      }
     }
 
     // Stop the timer.

backend/simulation_runner.h

@@ -28,21 +28,27 @@
 
 #pragma once
 
+#include <atomic>
 #include <memory>
 #include <mutex>
 #include <queue>
 #include <string>
 #include <thread>
+#include <vector>
+
 #include <ignition/msgs.hh>
 #include <ignition/transport/Node.hh>
 
+#include <Python.h>
+#include <pybind11/pybind11.h>
+
 #include "backend/automotive_simulator.h"
 #include "protobuf/robot_model_request.pb.h"
+
 #include "protobuf/simulation_in_message.pb.h"
 
 namespace delphyne {
 namespace backend {
-
 /// @brief Block the current thread until a SIGINT or SIGTERM is received.
 /// Note that this function registers a signal handler. Do not use this
 /// function if you want to manage yourself SIGINT/SIGTERM.
@@ -56,7 +62,7 @@ void WaitForShutdown();
 /// Ignition Transport messages. All these operations occur in a main loop with
 /// the following structure:
 ///
-/// * Process incoming messages: All incoming requestes are queued
+/// * Process incoming messages: All incoming requests are queued
 ///   asynchronously when received and in this step we process all of them.
 ///   This request might involve flag the simulation as paused, insert a new
 ///   model, remove an existing model, etc.
@@ -71,6 +77,59 @@ void WaitForShutdown();
 ///   occur. E.g.: When the simulation is paused or a model is removed. These
 ///   notifications should be used by all the Visualizer instances to update its
 ///   state/widgets in order to keep them in sync with the back-end.
+///
+/// * Provides a mechanism to register callbacks to be triggered on each
+///   simulation step. Since this was originally conceived to be used with the
+///   Python bindings and plain typedef-based functions are not supported
+///   (see http://www.boost.org/doc/libs/1_65_1/libs/python/doc/html/faq.html)
+///   we need to pass a PyObject as an argument.
+///
+/// Finally, it is important to clarify how the Python-C++ callback mechanism
+/// is implemented, so we can understand a possible deadlock. The key
+/// elements here are:
+///   - A typical main program will create a SimulatorRunner instance,
+///   configure it and call Start(). Let's call the thread the program runs on
+///   the MainThread.
+///   - Calling Start() will in turn spawn a new thread. This thread will
+///   execute a tight loop (see the `Run()` method), making the simulation
+///   itself move forward. We will call this thread the RunThread.
+///   - When the SimulatorRunner destructor is called, it will try to join the
+///   RunThread into the MainThread, to perform a clean shutdown. To do that
+///   a boolean flag is set to exit the tight loop in `Run()` and the `join()`
+///   method is called on the RunThread, effectively blocking the MainThread
+///   until RunThread is done.
+///   - When calling a Python-defined callback function from C++ we need to
+///   acquire the GIL (Python's Global Interpreter Lock), which basically
+///   gives us thread-safety between the C++ and Python worlds. The typical
+///   steps to invoke a Python callback from C++ are:
+///     - Acquire the GIL.
+///     - Invoke the callback.
+///     - Release the GIL.
+///   Note that acquiring the GIL is a potentially blocking call, as it is
+///   basically getting the lock of a mutex.
+///
+/// Now, the following interleaving may occur when running a python-scripted
+/// simulation:
+///
+/// - Create the simulator runner and configure a python callback.
+/// - Start the simulator. By this time we have the MainThread (which is the
+///   same thread that executes the python code) and the RunThread executing.
+/// - Stop the simulator from the MainThread (calling the `Stop()` method) and
+///   consider the following events:
+///     - The RunThread yielded the processor right before acquiring the GIL.
+///     - The MainTread execute the `Stop()` method and then the destructor.
+///       Now the MainThread is waiting for RunThread to join.
+///     - Execution is now returned to RunThread, which tries to acquire the
+///       GIL. However, the Python thread is blocked on the `join()`, so we
+///       are effectively in a deadlock.
+///
+///   It is interesting to note however that the lock on the GIL happens only
+///   if the code is blocked on the C++ side; in other words, if the code was
+///   blocked on the Python side (e.g. due to a `sleep` call), C++ has no
+///   problem to acquire the GIL. Because of this, the current recommendation
+///   is to add a sleep after calling `stop()` from the Python side, so the
+///   processor is yielded and the RunThread can finish its current (and last)
+///   loop before exiting the while.
 class SimulatorRunner {
   // @brief Default constructor.
   // @param[in] _sim A pointer to a simulator. Note that we take ownership of
@@ -85,11 +144,25 @@ class SimulatorRunner {
   /// @brief Default destructor.
   virtual ~SimulatorRunner();
 
+  /// @brief Add a python callback to be invoked on each simulation step. It is
+  /// important to note that the simulation step will be effectively blocked
+  /// by this the execution of the callbacks, so please consider this when
+  /// adding it.
+  /// @param[in] callable A pointer to a callback function, coming from the
+  /// python world.
+  void AddStepCallback(std::function<void()> callable);
+
   /// @brief Start the thread that runs the simulation loop. If there was a
   /// previous call to Start(), this call will be ignored.
   void Start();
 
+  /// @brief Stop the thread that runs the simulation loop. If there was a
+  /// previous call to Stop(), this call will be ignored.
+ public:
+  void Stop();
+
   /// @brief Run the main simulation loop.
+ public:
   void Run();
 
  private:
@@ -180,8 +253,9 @@ class SimulatorRunner {
   // externally.
   double customTimeStep = 0.001;
 
-  // @brief Whether the main loop has been started or not.
-  bool enabled = false;
+  /// \brief Whether the main loop has been started or not.
+ private:
+  std::atomic<bool> enabled{false};
 
   // @brief A pointer to the Drake simulator.
   std::unique_ptr<delphyne::backend::AutomotiveSimulator<double>> simulator;
@@ -209,6 +283,11 @@ class SimulatorRunner {
 
   // @brief An Ignition Transport publisher for sending notifications.
   ignition::transport::Node::Publisher notificationsPub;
+
+  // \brief A vector that holds all the registered callbacks that need to be
+  // triggered on each simulation step.
+ private:
+  std::vector<std::function<void()>> step_callbacks_;
 };
 
 }  // namespace backend