plugin also for tree_manager

CMakeLists.txt

@@ -186,6 +186,7 @@ SET(HDRS_TRAJECTORY
   include/core/trajectory/trajectory_base.h
   )
 SET(HDRS_MAP
+  include/core/map/factory_map.h
   include/core/map/map_base.h
   )
 SET(HDRS_FRAME

src/problem/problem.cpp

@@ -170,11 +170,38 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
         procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
     }
 
+    // Map
+    std::string map_type = _server.getParam<std::string>("map/type");
+    WOLF_TRACE("Map Type: ", map_type);
+    std::string map_plugin = _server.getParam<std::string>("map/plugin");
+    if (map_plugin != "core" and map_plugin != "wolf")
+    {
+        std::string plugin = plugins_path + "libwolf" + map_plugin + lib_extension;
+        WOLF_TRACE("Loading plugin " + plugin);
+        auto l = new LoaderRaw(plugin);
+        l->load();
+        loaders.push_back(l);
+    }
+    auto map = AutoConfFactoryMap::create(map_type, _server);
+    map->setProblem(problem);
+    problem->setMap(map);
+
     // Tree manager
     std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
     WOLF_TRACE("Tree Manager Type: ", tree_manager_type);
     if (tree_manager_type != "None" and tree_manager_type != "none")
+    {
+        std::string tm_plugin = _server.getParam<std::string>("problem/tree_manager/plugin");
+        if (tm_plugin != "core" and tm_plugin != "wolf")
+        {
+            std::string plugin = plugins_path + "libwolf" + tm_plugin + lib_extension;
+            WOLF_TRACE("Loading plugin " + plugin);
+            auto l = new LoaderRaw(plugin);
+            l->load();
+            loaders.push_back(l);
+        }
         problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
+    }
 
     // Prior
     std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
@@ -199,18 +226,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
                                  _server.getParam<VectorComposite>("problem/prior/state"));
     }
 
-    // Map
-    std::string map_type = _server.getParam<std::string>("map/type");
-    WOLF_TRACE("Map Type: ", map_type);
-    auto map = AutoConfFactoryMap::create(map_type, _server);
-    WOLF_INFO("Map created");
-    WOLF_INFO_COND(problem->getMap() == nullptr, "Problem map is nullptr");
-    WOLF_INFO_COND(problem->getMap() != nullptr, "Problem map is OK");
-    map->setProblem(problem);
-    WOLF_INFO("Map problem set");
-    problem->setMap(map);
-    WOLF_INFO("Problem map set");
-
     // Done
     return problem;
 }

test/yaml/params_problem_odom_3d.yaml

@@ -14,7 +14,8 @@ config:
     tree_manager: 
       type: "None"
   map:
-     type: "MapBase"
+    type: "MapBase"
+    plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager1.yaml

@@ -17,9 +17,11 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerDummy"
+      plugin: "core"
       toy_param: 0
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager2.yaml

@@ -15,10 +15,11 @@ config:
         O: [0.31, 0.31, 0.31]
         V: [0.31, 0.31, 0.31]
       time_tolerance: 0.1
-    tree_manager: 
+    tree_manager:
       type: "None"
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager_sliding_window1.yaml

@@ -13,11 +13,13 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
+      plugin: "core"
       n_frames: 3
       n_fix_first_frames: 2
       viral_remove_empty_parent: true
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager_sliding_window2.yaml

@@ -13,11 +13,13 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindow"
+      plugin: "core"
       n_frames: 3
       n_fix_first_frames: 0
       viral_remove_empty_parent: false
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml

@@ -13,6 +13,7 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindowDualRate"
+      plugin: "core"
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
@@ -20,3 +21,4 @@ config:
       viral_remove_empty_parent: true
   map:
      type: "MapBase"
+     plugin: "core"

test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml

@@ -13,6 +13,7 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindowDualRate"
+      plugin: "core"
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
@@ -20,3 +21,4 @@ config:
       viral_remove_empty_parent: false
   map:
      type: "MapBase"
+     plugin: "core"

test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml

@@ -19,6 +19,7 @@ config:
       time_tolerance: 0.1
     tree_manager:
       type: "TreeManagerSlidingWindowDualRate"
+      plugin: "core"
       n_frames: 5
       n_frames_recent: 3
       rate_old_frames: 2
@@ -26,6 +27,7 @@ config:
       viral_remove_empty_parent: true
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"

test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml

@@ -20,6 +20,7 @@ config:
       type: "None"
   map:
      type: "MapBase"
+     plugin: "core"
   sensors: 
     -
       type: "SensorOdom3d"