From 01b388c78160b784de703c7581b0bc927edaaa14 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 28 Jul 2021 16:50:31 +0200
Subject: [PATCH] plugin also for tree_manager
---
CMakeLists.txt | 1 +
src/problem/problem.cpp | 39 +++++++++++++------
test/yaml/params_problem_odom_3d.yaml | 3 +-
test/yaml/params_tree_manager1.yaml | 2 +
test/yaml/params_tree_manager2.yaml | 3 +-
.../params_tree_manager_sliding_window1.yaml | 2 +
.../params_tree_manager_sliding_window2.yaml | 2 +
...ree_manager_sliding_window_dual_rate1.yaml | 2 +
...ree_manager_sliding_window_dual_rate2.yaml | 2 +
...ree_manager_sliding_window_dual_rate3.yaml | 2 +
...ger_sliding_window_dual_rate_baseline.yaml | 1 +
11 files changed, 45 insertions(+), 14 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c6c2db62a..bbf2bd890 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 8258db880..daf1b4990 100644
--- a/src/problem/problem.cpp
+++ b/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;
}
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index 387924859..c61eab581 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index a27f60df2..4f3b8f336 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index 10759a6ed..c58498238 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index d0c3d81dd..81658b183 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index e7a7bc9b9..c9e7aeb04 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
index 299fa63e0..704953933 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
index 928408fcb..a3b4f469d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 9e64db064..11b3c1c7b 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/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"
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 16e421556..7716b669a 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/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"
--
GitLab