diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index c13af9ed22c38ba4fb62b3f1c54880dad810deef..4260a8c8efb26928a6d2242edbd43830b4467875 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -19,10 +19,6 @@ config:
type: "none"
plugin: "core"
- map:
- type: "MapBase"
- plugin: "core"
-
solver:
max_num_iterations: 100
verbose: 0
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 05e45510ac36c608d3e1378befe3c4179951551b..a3981d2f9e1a8e2b08a25f290ee2e7cf60ffd6be 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -66,7 +66,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
- map_ptr_(std::make_shared<MapBase>()),
+ map_ptr_(_map),
motion_provider_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
@@ -119,17 +119,19 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
- auto problem = Problem::create(frame_structure, dim);
+ auto problem = Problem::create(frame_structure, dim, nullptr);
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path;
- try{
+ try
+ {
plugins_path = _server.getParam<std::string>("plugins_path");
}
- catch(MissingValueException& e){
+ catch (MissingValueException& e)
+ {
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
plugins_path="/usr/local/lib/";
@@ -147,9 +149,11 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load Packages for subscribers and publishers
std::string packages_path;
- try {
+ try
+ {
packages_path = _server.getParam<std::string>("packages_path");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e) {
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
@@ -170,9 +174,12 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
// load raw libs
std::vector<std::string> raw_libs;
- try {
+ try
+ {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
- } catch (MissingValueException& e) {
+ }
+ catch (MissingValueException& e)
+ {
WOLF_TRACE("No raw libraries to load...");
raw_libs = std::vector<std::string>();
}
@@ -198,10 +205,20 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
_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");
+ // Map (optional)
+ std::string map_type, map_plugin;
+ try
+ {
+ map_type = _server.getParam<std::string>("map/type");
+ map_plugin = _server.getParam<std::string>("map/plugin");
+ }
+ catch (MissingValueException& e)
+ {
+ WOLF_TRACE("No map/type and/or map/plugin specified. Emplacing the default map: MapBase.");
+ map_type = "MapBase";
+ map_plugin = "core";
+ }
+ WOLF_TRACE("Map Type: ", map_type, " in plugin ", map_plugin);
if (map_plugin != "core" and map_plugin != "wolf")
{
std::string plugin = plugins_path + "libwolf" + map_plugin + lib_extension;
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index d5fa0054e26e64664dba42b3d0fc66a717bd8709..d86942de20f525f0b0995e2a5b43e18750311697 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -512,11 +512,35 @@ TEST(Problem, check)
ASSERT_TRUE(problem->check(true, std::cout));
}
+TEST(Problem, autoSetupMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
+TEST(Problem, autoSetupNoMap)
+{
+ std::string wolf_root = _WOLF_ROOT_DIR;
+
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup_no_map.yaml", wolf_root);
+ auto server = ParamsServer(parser.getParams());
+
+ auto P = Problem::autoSetup(server);
+
+ ASSERT_TRUE(P->check(true, std::cout));
+}
+
TEST(Problem, getState)
{
std::string wolf_root = _WOLF_ROOT_DIR;
- auto parser = ParserYaml("test/yaml/params_problem_odom_3d.yaml", wolf_root);
+ auto parser = ParserYaml("test/yaml/params_problem_autosetup.yaml", wolf_root);
auto server = ParamsServer(parser.getParams());
auto P = Problem::autoSetup(server);
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c61eab58146a9a5917c1130a14e234afa08a01b6
--- /dev/null
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -0,0 +1,49 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ map:
+ type: "MapBase"
+ plugin: "core"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6c5ed47c2efc2afc3ba960b075e60cea73b58a73
--- /dev/null
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -0,0 +1,46 @@
+config:
+ problem:
+ frame_structure: "PO"
+ dimension: 3
+ prior:
+ mode: "factor"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ time_tolerance: 0.1
+ tree_manager:
+ type: "None"
+ sensors:
+ -
+ type: "SensorOdom3d"
+ name: "odom"
+ plugin: "core"
+ k_disp_to_disp: 0.1
+ k_disp_to_rot: 0.1
+ k_rot_to_rot: 0.1
+ min_disp_var: 0.1
+ min_rot_var: 0.1
+ extrinsic:
+ pose: [1,2,3,0,0,0,1]
+ processors:
+ -
+ type: "ProcessorOdom3d"
+ name: "my_proc_odom3d"
+ sensor_name: "odom"
+ plugin: "core"
+ apply_loss_function: false
+ time_tolerance: 0.01 # seconds
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 1.95 # seconds
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ unmeasured_perturbation_std: 0.00111
+
+ state_getter: true
+ state_priority: 1
diff --git a/test/yaml/params_problem_odom_3d.yaml b/test/yaml/params_problem_odom_3d.yaml
index c61eab58146a9a5917c1130a14e234afa08a01b6..6c5ed47c2efc2afc3ba960b075e60cea73b58a73 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -13,9 +13,6 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
- map:
- type: "MapBase"
- plugin: "core"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index 4f3b8f336b643ece24537d1b0d21afab52bfbd61..59aec3c70cee0e147e38dbfb9408c8879bc203e2 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -19,9 +19,6 @@ config:
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 c58498238dc8fca1cd108cfddd875b102830cace..419125468ba5155eba8f0f75c972a5d52b5dbcef 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -17,9 +17,6 @@ config:
time_tolerance: 0.1
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 81658b1833d024e935e367af23b5109fa3fb99ab..61498d1b6c182a6ae95930047940c56bcc3ca4ae 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -17,9 +17,6 @@ config:
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 c9e7aeb048ef6b2336db594a13131a2c9b43a02a..db0a176fce7534934e9a10da0c5f84a5ae431517 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -17,9 +17,6 @@ config:
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 704953933ddf02972e4cbe4d644344f2cceaf49f..2b6313f5b9702ab0f7c3dc0187dbde23ae944d00 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -19,6 +19,3 @@ config:
rate_old_frames: 2
n_fix_first_frames: 2
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 a3b4f469d3dc11eff15f0fbbceaa40e025319351..609fb96f585545376756279956377ee6730dbc0d 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -19,6 +19,3 @@ config:
rate_old_frames: 2
n_fix_first_frames: 0
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 81744792993a9cb21e3cd7be51b3870a3cf59dd2..aed7a0c7e4da1313e261501c87e3748fb64cd2b5 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -21,9 +21,6 @@ config:
rate_old_frames: 2
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_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 099c1d228b7bbba68277b3d2dab8289f96c94429..d00b201f9d84f53cf0b332315cd7617f60a490a5 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
@@ -15,9 +15,6 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
- map:
- type: "MapBase"
- plugin: "core"
sensors:
-
type: "SensorOdom3d"