From 130eff3f68975e4941204fa1f02c3feca6655a88 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 11 Feb 2022 12:18:36 +0100
Subject: [PATCH] map optional in yaml
---
demos/hello_wolf/yaml/hello_wolf_config.yaml | 4 --
src/problem/problem.cpp | 41 +++++++++++-----
test/gtest_problem.cpp | 26 +++++++++-
test/yaml/params_problem_autosetup.yaml | 49 +++++++++++++++++++
.../yaml/params_problem_autosetup_no_map.yaml | 46 +++++++++++++++++
test/yaml/params_problem_odom_3d.yaml | 3 --
test/yaml/params_tree_manager1.yaml | 3 --
test/yaml/params_tree_manager2.yaml | 3 --
.../params_tree_manager_sliding_window1.yaml | 3 --
.../params_tree_manager_sliding_window2.yaml | 3 --
...ree_manager_sliding_window_dual_rate1.yaml | 3 --
...ree_manager_sliding_window_dual_rate2.yaml | 3 --
...ree_manager_sliding_window_dual_rate3.yaml | 3 --
...ger_sliding_window_dual_rate_baseline.yaml | 3 --
14 files changed, 149 insertions(+), 44 deletions(-)
create mode 100644 test/yaml/params_problem_autosetup.yaml
create mode 100644 test/yaml/params_problem_autosetup_no_map.yaml
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index c13af9ed2..4260a8c8e 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 05e45510a..a3981d2f9 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 d5fa0054e..d86942de2 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 000000000..c61eab581
--- /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 000000000..6c5ed47c2
--- /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 c61eab581..6c5ed47c2 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 4f3b8f336..59aec3c70 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 c58498238..419125468 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 81658b183..61498d1b6 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 c9e7aeb04..db0a176fc 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 704953933..2b6313f5b 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 a3b4f469d..609fb96f5 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 817447929..aed7a0c7e 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 099c1d228..d00b201f9 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"
--
GitLab