[Geomagic] Fix crash when drawDevice is set to true and add option to log haptic thread speed

applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.cpp

@@ -72,6 +72,9 @@ HDCallbackCode HDCALLBACK stateCallback(void * userData)
     HDErrorInfo error;
     GeomagicDriver * driver = (GeomagicDriver * ) userData;
 
+    static auto last_time = std::chrono::high_resolution_clock::now();
+    static int counter = 0;
+
     hdMakeCurrentDevice(driver->m_hHD);
     if (HD_DEVICE_ERROR(error = hdGetError())) return HD_CALLBACK_CONTINUE;
 
@@ -146,6 +149,23 @@ HDCallbackCode HDCALLBACK stateCallback(void * userData)
 
     hdEndFrame(driver->m_hHD);
 
+    // Measure period
+    if (driver->m_logThreadSpeed)
+    {
+        counter++;
+
+        // Print averaged frequency every ~1000 loops
+        if (counter >= 1000)
+        {
+            auto now = std::chrono::high_resolution_clock::now();
+            double elapsed_ms = std::chrono::duration<double, std::milli>(now - last_time).count();
+            double avg_freq = counter / (elapsed_ms / 1000.0);
+            msg_info("GeomagicDriver") << "Haptic loop avg freq: " << avg_freq << " Hz";
+            last_time = now;
+            counter = 0;
+        }
+    }
+
     return HD_CALLBACK_CONTINUE;
 }
 
@@ -178,6 +198,15 @@ GeomagicDriver::GeomagicDriver()
     m_forceFeedback = nullptr;
     m_GeomagicVisualModel = std::make_unique<GeomagicVisualModel>();
     sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Loading);
+
+    this->addUpdateCallback("logHaptics", {&f_printLog},
+                            [this](const core::DataTracker& t)
+                            {
+                                SOFA_UNUSED(t);
+                                m_logThreadSpeed = f_printLog.getValue();
+                                return sofa::core::objectmodel::ComponentState::Valid;
+                            },
+                            {});
 }
 
 
@@ -206,6 +235,8 @@ void GeomagicDriver::init()
         msg_warning() << "No forceFeedBack component found in the scene. Only the motion of the haptic tool will be simulated.";
     }
 
+    // Use internal bool to know if haptic thread should log its speed without calling GetValue each time
+    m_logThreadSpeed = f_printLog.getValue();
 
     // 2- init device and Hd scheduler
     if (d_manualStart.getValue() == false)

applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.h

@@ -132,7 +132,7 @@ class SOFA_GEOMAGIC_API GeomagicDriver : public Controller
 
 protected:
     // Pointer to the Geomagic visual model to draw device in scene
-    std::unique_ptr<GeomagicVisualModel> m_GeomagicVisualModel;
+    std::unique_ptr<GeomagicVisualModel> m_GeomagicVisualModel = nullptr;
 
 public:
     ///These data are written by the omni they cnnot be accessed in the simulation loop
@@ -147,6 +147,7 @@ class SOFA_GEOMAGIC_API GeomagicDriver : public Controller
     // Public members exchanged between Driver and HD scheduler
     bool m_simulationStarted; ///< Boolean to warn scheduler when SOFA has started the simulation (changed by AnimateBeginEvent)
     bool m_isInContact; ///< Boolean to warn SOFA side when scheduler has computer contact (forcefeedback no null)
+    bool m_logThreadSpeed = false; ///< Boolean to enable/disable logging of the scheduler thread speed (for debug purpose), will be set to f_printLog
     DeviceData m_hapticData; ///< data structure used by scheduler
     DeviceData m_simuData; ///< data structure used by SOFA loop, values are copied from @sa m_hapticData
     SHHD m_hHD; ///< ID the device

applications/plugins/Geomagic/src/Geomagic/GeomagicVisualModel.cpp

@@ -122,8 +122,6 @@ void GeomagicVisualModel::initDisplay(sofa::simulation::Node::SPtr node, const s
         visualNode[i].visu->d_vtexcoords.setParent(&visualNode[i].loader->d_texCoords);
 
         visualNode[i].visu->init();
-        visualNode[i].visu->initVisual(sofa::core::visual::visualparams::defaultInstance());
-        visualNode[i].visu->updateVisual(sofa::core::visual::visualparams::defaultInstance());
 
         // create the visual mapping and at it to the graph //
         visualNode[i].mapping = sofa::core::objectmodel::New< sofa::component::mapping::nonlinear::RigidMapping< Rigid3Types, Vec3Types > >();