diff --git a/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.cpp b/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.cpp index 6fe8894b442..7860a5133b5 100644 --- a/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.cpp +++ b/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(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(); 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) diff --git a/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.h b/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.h index 6f13cb51d21..480a99553cf 100644 --- a/applications/plugins/Geomagic/src/Geomagic/GeomagicDriver.h +++ b/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 m_GeomagicVisualModel; + std::unique_ptr 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 diff --git a/applications/plugins/Geomagic/src/Geomagic/GeomagicVisualModel.cpp b/applications/plugins/Geomagic/src/Geomagic/GeomagicVisualModel.cpp index db50cd53960..29b69a86840 100644 --- a/applications/plugins/Geomagic/src/Geomagic/GeomagicVisualModel.cpp +++ b/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 > >();