Merge pull request #27 from rohanpsingh/topic/check-timesteps

jrl-umi3218 · Feb 8, 2023 · b2a28aa · b2a28aa
2 parents 22ad3da + 9832bf9
commit b2a28aa
Showing 1 changed file with 21 additions and 0 deletions.
diff --git a/src/MCControl.cpp b/src/MCControl.cpp
@@ -631,6 +631,27 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id)
 
     if(controller.running)
     {
+      // confirm mc-rtc timestep is same as IOB timestep
+      if(!m_is_simulation)
+      {
+        if(!open_iob())
+        {
+          failed_iob("open_iob", "(mc_openrtm constructor)");
+          mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Cannot verify timesteps of IOB and mc-rtc.");
+        }
+        double iob_ts = get_signal_period() / 1e9;
+        if(fabs(iob_ts - controller.controller().timeStep) > 1e-6)
+        {
+          mc_rtc::log::error_and_throw<std::runtime_error>(
+              "[mc_openrtm] Missmatch between IOB timestep ({}ms) and mc_rtc ({}ms).", iob_ts,
+              controller.controller().timeStep);
+        }
+        if(!close_iob())
+        {
+          failed_iob("close_iob", "(mc_openrtm constructor)");
+          mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Could not close IOB.");
+        }
+      }
       if(!init)
       {
         mc_rtc::log::info("Init controller");