Force load plugin lib at launch time

test/gtest_factor_force_torque_inertial.cpp

@@ -42,8 +42,8 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
     ProblemPtr                            problem;
     SensorForceTorqueInertialPtr          sensor;
     ProcessorForceTorqueInertialPreintPtr processor;
-    FrameBasePtr                          frame_origin, frame;
-    CaptureMotionPtr                      capture_origin, capture;
+    FrameBasePtr                          frame_origin, frame_last, frame;
+    CaptureMotionPtr                      capture_origin, capture_last, capture;
     FeatureMotionPtr                      feature;
     FactorForceTorqueInertialPtr          factor;
 
@@ -57,8 +57,8 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
   protected:
     void SetUp() override
     {
-        data         = VectorXd::Random(12); // [ a, w, f, t ]
-        data_cov     = MatrixXd::Identity(12, 12);
+        data     = VectorXd::Random(12);  // [ a, w, f, t ]
+        data_cov = MatrixXd::Identity(12, 12);
 
         std::string  wolf_root = _WOLF_BODYDYNAMICS_ROOT_DIR;
         ParserYaml   parser    = ParserYaml("problem_force_torque_inertial.yaml", wolf_root + "/test/yaml/");
@@ -66,24 +66,46 @@ class Test_FactorForceTorqueInertialPreint : public testing::Test
         problem                = Problem::autoSetup(server);
 
         sensor = std::static_pointer_cast<SensorForceTorqueInertial>(problem->getHardware()->getSensorList().front());
+        processor = std::static_pointer_cast<ProcessorForceTorqueInertialPreint>(sensor->getProcessorList().front());
 
         problem->print(4, 1, 1, 1);
 
-        capture_origin = std::make_shared<CaptureMotion>("CaptureForceTorqueInertial", 0.0, sensor, data, data_cov, nullptr);
-        capture_origin->process();
+        capture = std::make_shared<CaptureMotion>("CaptureForceTorqueInertial", 0.0, sensor, data, data_cov, nullptr);
+        capture->process();
 
         problem->print(4, 1, 1, 1);
 
-        frame_origin   = problem->getLastFrame();
+        capture_origin = std::static_pointer_cast<CaptureMotion>(processor->getOrigin());
+        frame_origin   = capture_origin->getFrame();
 
         ASSERT_EQ(capture_origin->getTimeStamp(), frame_origin->getTimeStamp());
         ASSERT_EQ(capture_origin->getTimeStamp(), frame_origin->getCaptureOf(sensor)->getTimeStamp());
+
+        capture = std::make_shared<CaptureMotion>("CaptureForceTorqueInertial", 0.1, sensor, data, data_cov, nullptr);
+        capture->process();
+
+        capture_last = std::static_pointer_cast<CaptureMotion>(processor->getLast());
+        frame_last   = capture_last->getFrame();
     }
 };
 
+TEST(Dummy, force_load_libwolfbodydynamics_so_at_launch_time)
+{
+    ParamsSensorForceTorqueInertialPtr p = std::make_shared<ParamsSensorForceTorqueInertial>();
+    p->acc_noise_std                     = 1;
+    p->force_noise_std                   = 1;
+    p->gyro_noise_std                    = 1;
+    p->torque_noise_std                  = 1;
+    SensorForceTorqueInertial s((Vector7d() << 1, 2, 3, 0, 0, 0, 1).finished(), p);
+}
+
 TEST_F(Test_FactorForceTorqueInertialPreint, constructor)
 {
-    // FactorForceTorqueInertial f(feature, capture_origin, processor, false);
+    feature = FeatureMotion::emplace<FeatureMotion>(capture_last, "FeatureFTI", VectorXd(19),
+                                                    MatrixXd::Identity(18, 18), VectorXd(6), MatrixXd(18, 6));
+    FactorForceTorqueInertial f(feature, capture_origin, processor, false);
+
+    WOLF_INFO("f id: ", f.id());
 }
 
 int main(int argc, char **argv)