Harden ROS 2 MPC lifecycle and policy handoff

.github/workflows/ros-build-test.yml

@@ -2,9 +2,9 @@ name: Build and Test OCS2 (ROS2 Jazzy)
 
 on:
   push:
-    branches: [ main ]
+    branches: [ ros2 ]
   pull_request:
-    branches: [ main ]
+    branches: [ ros2 ]
 
 jobs:
   build:
@@ -83,6 +83,7 @@ jobs:
         git clone --depth 1 --filter=blob:none --sparse --branch ros2 --single-branch https://github.com/leggedrobotics/elevation_mapping_cupy.git src/elevation_mapping_cupy
         cd src/elevation_mapping_cupy
         git sparse-checkout set plane_segmentation
+        rm -f plane_segmentation/COLCON_IGNORE
 
     - name: Rosdep
       shell: bash

ocs2_core/src/thread_support/ThreadPool.cpp

@@ -30,6 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/thread_support/SetThreadPriority.h>
 #include <ocs2_core/thread_support/ThreadPool.h>
 
+#include <exception>
+
 namespace ocs2 {
 
 /**************************************************************************************************/
@@ -112,13 +114,31 @@ void ThreadPool::runParallel(std::function<void(int)> taskFunction, int N) {
     }
   }
 
+  std::exception_ptr firstException;
+
   // Execute one instance in this thread.
   const auto workerId = static_cast<int>(numThreads());  // threadpool workers use ID 0 -> nThreads - 1
-  taskFunction(workerId);
+  try {
+    taskFunction(workerId);
+  } catch (...) {
+    firstException = std::current_exception();
+  }
 
-  // Wait for helpers to finish.
+  // Always drain every helper before propagating an exception. Otherwise a
+  // caller can start another parallel operation while failed-call helpers are
+  // still using its state.
   for (auto&& fut : futures) {
-    fut.get();
+    try {
+      fut.get();
+    } catch (...) {
+      if (!firstException) {
+        firstException = std::current_exception();
+      }
+    }
+  }
+
+  if (firstException) {
+    std::rethrow_exception(firstException);
   }
 }
 

ocs2_core/test/thread_support/testThreadPool.cpp

@@ -1,5 +1,11 @@
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <future>
 #include <gtest/gtest.h>
+#include <mutex>
 #include <ocs2_core/thread_support/ThreadPool.h>
+#include <stdexcept>
 
 using namespace ocs2;
 
@@ -40,23 +46,24 @@ TEST(testThreadPool, testCanExecuteMultipleTasks) {
   std::promise<void> barrier_promise;
   std::shared_future<void> barrier = barrier_promise.get_future();
 
-  std::string data1, data2;
+  std::atomic_int state1{0};
+  std::atomic_int state2{0};
 
-  res1 = pool.run([&data1, barrier](int) {
-    data1 = "running";
+  res1 = pool.run([&state1, barrier](int) {
+    state1 = 1;
     barrier.wait();
-    data1 = "done";
+    state1 = 2;
   });
-  res2 = pool.run([&data2, barrier](int) {
-    data2 = "running";
+  res2 = pool.run([&state2, barrier](int) {
+    state2 = 1;
     barrier.wait();
-    data2 = "done";
+    state2 = 2;
   });
 
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
   // check that both threads are blocking
-  EXPECT_EQ(data1, "running");
-  EXPECT_EQ(data2, "running");
+  EXPECT_EQ(state1, 1);
+  EXPECT_EQ(state2, 1);
 
   // signal threads
   barrier_promise.set_value();
@@ -68,8 +75,8 @@ TEST(testThreadPool, testCanExecuteMultipleTasks) {
   std::future_status status2 = res2.wait_for(std::chrono::seconds(1));
   ASSERT_EQ(status2, std::future_status::ready);
 
-  EXPECT_EQ(data1, "done");
-  EXPECT_EQ(data2, "done");
+  EXPECT_EQ(state1, 2);
+  EXPECT_EQ(state2, 2);
 
   res1.get();
   res2.get();
@@ -85,6 +92,139 @@ TEST(testThreadPool, testRunMultiple) {
   EXPECT_EQ(counter, 42);
 }
 
+TEST(testThreadPool, runParallelDrainsHelpersAfterCallerFailure) {
+  ThreadPool pool(2);
+
+  std::mutex mutex;
+  std::condition_variable helpersStartedCondition;
+  int helpersStarted = 0;
+  std::atomic_int helpersFinished{0};
+  std::promise<void> releaseHelpersPromise;
+  const auto releaseHelpers = releaseHelpersPromise.get_future().share();
+
+  auto result = std::async(std::launch::async, [&] {
+    pool.runParallel(
+        [&](int workerIndex) {
+          if (workerIndex == static_cast<int>(pool.numThreads())) {
+            throw std::runtime_error("caller failure");
+          }
+
+          {
+            std::lock_guard<std::mutex> lock(mutex);
+            ++helpersStarted;
+          }
+          helpersStartedCondition.notify_one();
+          releaseHelpers.wait();
+          ++helpersFinished;
+        },
+        3);
+  });
+
+  bool allHelpersStarted = false;
+  {
+    std::unique_lock<std::mutex> lock(mutex);
+    allHelpersStarted = helpersStartedCondition.wait_for(
+        lock, std::chrono::seconds(1), [&] { return helpersStarted == 2; });
+  }
+  const auto statusBeforeRelease =
+      result.wait_for(std::chrono::milliseconds(50));
+  releaseHelpersPromise.set_value();
+
+  EXPECT_TRUE(allHelpersStarted);
+  EXPECT_EQ(statusBeforeRelease, std::future_status::timeout);
+  EXPECT_THROW(result.get(), std::runtime_error);
+  EXPECT_EQ(helpersFinished, 2);
+
+  std::atomic_int subsequentCalls{0};
+  pool.runParallel([&](int) { ++subsequentCalls; }, 3);
+  EXPECT_EQ(subsequentCalls, 3);
+}
+
+TEST(testThreadPool, runParallelDrainsLaterHelpersAfterHelperFailure) {
+  ThreadPool pool(1);
+
+  std::mutex mutex;
+  std::condition_variable blockingHelperStartedCondition;
+  bool blockingHelperStarted = false;
+  std::atomic_int helperCalls{0};
+  std::atomic_int helpersFinished{0};
+  std::promise<void> releaseHelperPromise;
+  const auto releaseHelper = releaseHelperPromise.get_future().share();
+
+  auto result = std::async(std::launch::async, [&] {
+    pool.runParallel(
+        [&](int workerIndex) {
+          if (workerIndex == static_cast<int>(pool.numThreads())) {
+            std::unique_lock<std::mutex> lock(mutex);
+            blockingHelperStartedCondition.wait(
+                lock, [&] { return blockingHelperStarted; });
+            return;
+          }
+
+          if (helperCalls++ == 0) {
+            throw std::runtime_error("helper failure");
+          }
+
+          {
+            std::lock_guard<std::mutex> lock(mutex);
+            blockingHelperStarted = true;
+          }
+          blockingHelperStartedCondition.notify_all();
+          releaseHelper.wait();
+          ++helpersFinished;
+        },
+        3);
+  });
+
+  bool helperStarted = false;
+  {
+    std::unique_lock<std::mutex> lock(mutex);
+    helperStarted = blockingHelperStartedCondition.wait_for(
+        lock, std::chrono::seconds(1), [&] { return blockingHelperStarted; });
+  }
+  const auto statusBeforeRelease =
+      result.wait_for(std::chrono::milliseconds(50));
+  releaseHelperPromise.set_value();
+
+  EXPECT_TRUE(helperStarted);
+  EXPECT_EQ(statusBeforeRelease, std::future_status::timeout);
+  EXPECT_THROW(result.get(), std::runtime_error);
+  EXPECT_EQ(helperCalls, 2);
+  EXPECT_EQ(helpersFinished, 1);
+
+  std::atomic_int subsequentCalls{0};
+  pool.runParallel([&](int) { ++subsequentCalls; }, 3);
+  EXPECT_EQ(subsequentCalls, 3);
+}
+
+TEST(testThreadPool, runParallelPropagatesFailureFromEachHelperPosition) {
+  ThreadPool pool(1);
+
+  for (int failingHelper = 0; failingHelper < 3; ++failingHelper) {
+    std::atomic_int helperCalls{0};
+    std::atomic_int helpersFinished{0};
+
+    EXPECT_THROW(
+        pool.runParallel(
+            [&](int workerIndex) {
+              if (workerIndex == static_cast<int>(pool.numThreads())) {
+                return;
+              }
+
+              const int helperCall = helperCalls++;
+              if (helperCall == failingHelper) {
+                throw std::runtime_error("helper failure");
+              }
+              ++helpersFinished;
+            },
+            4),
+        std::runtime_error);
+
+    EXPECT_EQ(helperCalls, 3);
+    EXPECT_EQ(helpersFinished, 2);
+  }
+}
+
 TEST(testThreadPool, testNoThreads) {
   ThreadPool pool(0);
 

ocs2_mpc/CMakeLists.txt

@@ -67,11 +67,17 @@ ament_package()
 ## Testing ##
 #############
 
-#catkin_add_gtest(testMPC_OCS2
-#  test/testMPC_OCS2.cpp
-#)
-#target_link_libraries(testMPC_OCS2
-#  ${catkin_LIBRARIES}
-#  ${Boost_LIBRARIES}
-#)
-#target_compile_options(testMPC_OCS2 PRIVATE ${OCS2_CXX_FLAGS})
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+
+  ament_add_gtest(ocs2_mpc_test_mpc_base
+    test/testMPC_BASE.cpp
+  )
+  target_compile_definitions(ocs2_mpc_test_mpc_base PRIVATE
+    OCS2_MPC_TEST_DATA_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/data"
+  )
+  target_link_libraries(ocs2_mpc_test_mpc_base
+    ${PROJECT_NAME}
+    gtest_main
+  )
+endif()

ocs2_mpc/include/ocs2_mpc/MPC_BASE.h

@@ -61,6 +61,7 @@ class MPC_BASE {
 
   /**
    * The main routine of MPC which runs MPC for the given state and time.
+   * If a solve throws, later calls fail until reset() succeeds.
    *
    * @param [in] currentTime: The given time.
    * @param [in] currentState: The given state.
@@ -94,6 +95,8 @@ class MPC_BASE {
 
  private:
   bool initRun_ = true;
+  bool solverHealthy_ = true;
+  scalar_t lastRunTime_ = 0.0;
   const mpc::Settings mpcSettings_;
 
   benchmark::RepeatedTimer mpcTimer_;

ocs2_mpc/include/ocs2_mpc/MRT_BASE.h

@@ -64,7 +64,7 @@ class MRT_BASE {
   /**
    * Resets the class to its instantiated state.
    */
-  void reset();
+  virtual void reset();
 
   /**
    * Request the MPC node to reset. This method is a blocking method.

ocs2_mpc/package.xml

@@ -17,10 +17,11 @@
   <depend>eigen3_cmake_module</depend>
   <depend>boost</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
 
 </package>
 
-

ocs2_mpc/src/MPC_BASE.cpp

@@ -28,6 +28,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
 #include <algorithm>
+#include <stdexcept>
 
 #include <ocs2_mpc/MPC_BASE.h>
 
@@ -42,20 +43,29 @@ MPC_BASE::MPC_BASE(mpc::Settings mpcSettings) : mpcSettings_(std::move(mpcSettin
 /******************************************************************************************************/
 /******************************************************************************************************/
 void MPC_BASE::reset() {
+  getSolverPtr()->reset();
   initRun_ = true;
+  solverHealthy_ = true;
+  lastRunTime_ = 0.0;
   mpcTimer_.reset();
-  getSolverPtr()->reset();
 }
 
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
 bool MPC_BASE::run(scalar_t currentTime, const vector_t& currentState) {
-  // check if the current time exceeds the solver final limit
-  if (!initRun_ && currentTime >= getSolverPtr()->getFinalTime()) {
-    std::cerr << "WARNING: The MPC time-horizon is smaller than the MPC starting time.\n";
-    std::cerr << "currentTime: " << currentTime << "\t Controller finalTime: " << getSolverPtr()->getFinalTime() << '\n';
-    return false;
+  if (!solverHealthy_) {
+    throw std::runtime_error("The previous MPC solve failed. Reset the MPC before running it again.");
+  }
+
+  if (!initRun_) {
+    const scalar_t previousFinalTime = getSolverPtr()->getFinalTime();
+    if (currentTime >= previousFinalTime || currentTime < lastRunTime_) {
+      std::cerr << "WARNING: MPC observation time discontinuity detected. Resetting the solver.\n";
+      std::cerr << "currentTime: " << currentTime << "\t previousRunTime: " << lastRunTime_
+                << "\t previousFinalTime: " << previousFinalTime << '\n';
+      reset();
+    }
   }
 
   const scalar_t finalTime = currentTime + mpcSettings_.timeHorizon_;
@@ -72,10 +82,16 @@ bool MPC_BASE::run(scalar_t currentTime, const vector_t& currentState) {
   }
 
   // calculate the MPC policy
-  calculateController(currentTime, currentState, finalTime);
+  try {
+    calculateController(currentTime, currentState, finalTime);
+  } catch (...) {
+    solverHealthy_ = false;
+    throw;
+  }
 
   // set initRun flag to false
   initRun_ = false;
+  lastRunTime_ = currentTime;
 
   // display
   if (mpcSettings_.debugPrint_) {

ocs2_mpc/test/data/lockstep_replay_expected.csv

@@ -0,0 +1,5 @@
+step,event,time,state,target,reset_epoch,policy_sequence,solve_outcome,state_finite,input_finite,cost,inequality_constraints_sse,solver_reset_count
+0,reset_observation,1.000000,0.000000,1.000000,1,1,1,1,1,1.000000,0.000000,1
+1,observation,2.000000,0.250000,1.000000,1,2,1,1,1,0.562500,0.000000,1
+2,target_change_observation,3.000000,0.250000,-0.500000,1,3,1,1,1,0.562500,0.000000,1
+3,time_discontinuity,1.500000,0.000000,-0.500000,1,4,1,1,1,0.250000,0.000000,2