From 71ee1fd4d5198c8c6c5bd72778fc998c6656a94e Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 7 Jul 2022 21:49:28 +0200
Subject: [PATCH] [skip ci] working on gtests
---
schema/Setup.schema | 14 ------
schema/map/MapBase.schema | 12 +++++
schema/problem/Problem.schema | 46 +++++++++++++------
schema/processor/MotionProvider.schema | 10 ++++
schema/processor/ProcessorBase.schema | 33 ++++++++++++-
schema/processor/ProcessorMotion.schema | 28 +++++++++++
schema/processor/ProcessorOdom3d.schema | 1 +
schema/processor/ProcessorTracker.schema | 11 +++--
schema/sensor/SensorBase.schema | 14 +++++-
schema/sensor/SensorDiffDrive.schema | 18 ++++++++
schema/sensor/SensorOdom2d.schema | 5 ++
schema/sensor/SensorOdom3d.schema | 22 +++++++++
src/problem/problem.cpp | 12 ++++-
test/dummy/processor_tracker_feature_dummy.h | 2 +
test/yaml/params_problem_autosetup.yaml | 2 +-
.../yaml/params_problem_autosetup_no_map.yaml | 2 +-
test/yaml/processor_odom_3d.yaml | 2 +
.../yaml/processor_tracker_feature_dummy.yaml | 6 ++-
18 files changed, 201 insertions(+), 39 deletions(-)
delete mode 100644 schema/Setup.schema
create mode 100644 schema/map/MapBase.schema
create mode 100644 schema/processor/MotionProvider.schema
create mode 100644 schema/processor/ProcessorMotion.schema
create mode 100644 schema/processor/ProcessorOdom3d.schema
diff --git a/schema/Setup.schema b/schema/Setup.schema
deleted file mode 100644
index d5c2e7d71..000000000
--- a/schema/Setup.schema
+++ /dev/null
@@ -1,14 +0,0 @@
-problem:
- follow: Poblem.schema
-map:
- follow: Map.schema
-solver:
- follow: Solver.schema
-sensors:
- yaml_type: sequence
- mandatory: true
- doc: A sequence of all the sensors.
-processors:
- yaml_type: sequence
- mandatory: true
- doc: A sequence of all the processors.
diff --git a/schema/map/MapBase.schema b/schema/map/MapBase.schema
new file mode 100644
index 000000000..50484d21e
--- /dev/null
+++ b/schema/map/MapBase.schema
@@ -0,0 +1,12 @@
+type:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: MapBase
+ doc: Type of the Map used in the problem.
+type:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: core
+ doc: Name of the wolf plugin where the map type is implemented.
\ No newline at end of file
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index 76ed61a80..587f7bdb1 100644
--- a/schema/problem/Problem.schema
+++ b/schema/problem/Problem.schema
@@ -1,17 +1,35 @@
-tree_manager:
+problem:
+ tree_manager:
+ type: derived
+ yaml_type: scalar
+ mandatory: true
+ doc: Tree manager parameters
+ frame_structure:
+ yaml_type: scalar
+ type: string
+ mandatory: true
+ doc: The keys of the default frames in problems
+ dimension:
+ yaml_type: scalar
+ type: int
+ mandatory: true
+ options: [2, 3]
+ doc: Dimension of the problem. '2' for 2D or '3' for 3D
+ #prior:
+map:
+ type: derived
yaml_type: scalar
- type: string
+ mandatory: false
+ doc: The map used in the wolf problem.
+solver:
+ follow: Solver.schema
+sensors:
+ yaml_type: sequence
+ type: derived
mandatory: true
- doc: Tree manager parameters
-frame_structure:
- yaml_type: scalar
- type: string
- mandatory: true
- doc: The keys of the default frames in problems
-dimension:
- yaml_type: scalar
- type: int
+ doc: A sequence of all the sensors.
+processors:
+ yaml_type: sequence
+ type: derived
mandatory: true
- options: [2, 3]
- doc: Dimension of the problem: '2' for 2D or '3' for 3D
-#prior:
\ No newline at end of file
+ doc: A sequence of all the processors.
diff --git a/schema/processor/MotionProvider.schema b/schema/processor/MotionProvider.schema
new file mode 100644
index 000000000..a557389eb
--- /dev/null
+++ b/schema/processor/MotionProvider.schema
@@ -0,0 +1,10 @@
+state_getter:
+ mandatory: true
+ yaml_type: scalar
+ type: bool
+ doc: If the processor is used by the problem as provider of state.
+state_priority:
+ mandatory: false
+ yaml_type: scalar
+ type: double
+ doc: The priority of this processor when problem gets the state (only used if state_getter = true). Two processors cannot have the same priority (if so, when installing the second is increased).
\ No newline at end of file
diff --git a/schema/processor/ProcessorBase.schema b/schema/processor/ProcessorBase.schema
index 63d4b609b..d5dcf3694 100644
--- a/schema/processor/ProcessorBase.schema
+++ b/schema/processor/ProcessorBase.schema
@@ -2,4 +2,35 @@ name:
mandatory: true
type: string
yaml_type: scalar
- doc: The processor's name. It has to be unique.
\ No newline at end of file
+ doc: The processor's name. It has to be unique.
+
+type:
+ mandatory: true
+ type: string
+ yaml_type: scalar
+ doc: The processor's class name.
+
+plugin:
+ mandatory: true
+ type: string
+ yaml_type: scalar
+ doc: The name of the wolf plugin where the processor is implemented.
+
+time_tolerance:
+ mandatory: true
+ type: double
+ yaml_type: scalar
+ doc: The time tolerance for joining captures into frames [s].
+
+keyframe_vote:
+ voting_active:
+ mandatory: true
+ type: bool
+ yaml_type: scalar
+ doc: If the processor is allowed to decide to create a frame.
+
+apply_loss_function:
+ mandatory: true
+ type: bool
+ yaml_type: scalar
+ doc: If the factors created by the processor have loss function.
\ No newline at end of file
diff --git a/schema/processor/ProcessorMotion.schema b/schema/processor/ProcessorMotion.schema
new file mode 100644
index 000000000..057fc7725
--- /dev/null
+++ b/schema/processor/ProcessorMotion.schema
@@ -0,0 +1,28 @@
+follow: ProcessorBase.schema
+follow: MotionProvider.schema
+keyframe_vote:
+ max_time_span:
+ mandatory: true
+ yaml_type: scalar
+ type: double
+ doc: Time threshold to create a new frame [s].
+ max_buff_length:
+ mandatory: true
+ yaml_type: scalar
+ type: unsigned int
+ doc: Maximum size of the buffer of motions.
+ dist_traveled:
+ mandatory: true
+ yaml_type: scalar
+ type: double
+ doc: Threshold of distance traveled to create a new frame [m].
+ angle_turned:
+ mandatory: true
+ yaml_type: scalar
+ type: double
+ doc: Threshold of angle turned to create a new frame [rad].
+unmeasured_perturbation_std:
+ mandatory: true
+ yaml_type: scalar
+ type: double
+ doc: Noise (standard deviation) of the integrated movement in the not observed directions.
\ No newline at end of file
diff --git a/schema/processor/ProcessorOdom3d.schema b/schema/processor/ProcessorOdom3d.schema
new file mode 100644
index 000000000..3823a0814
--- /dev/null
+++ b/schema/processor/ProcessorOdom3d.schema
@@ -0,0 +1 @@
+follow: ProcessorMotion.schema
\ No newline at end of file
diff --git a/schema/processor/ProcessorTracker.schema b/schema/processor/ProcessorTracker.schema
index 73323ef25..1e40aa054 100644
--- a/schema/processor/ProcessorTracker.schema
+++ b/schema/processor/ProcessorTracker.schema
@@ -1,9 +1,10 @@
follow: ProcessorBase.schema
-min_features_for_keyframe:
- mandatory: true
- type: unsigned int
- yaml_type: scalar
- doc: Minimum number of features to vote for keyframe.
+keyframe_vote:
+ min_features_for_keyframe:
+ mandatory: true
+ type: unsigned int
+ yaml_type: scalar
+ doc: Minimum number of features to vote for keyframe.
max_new_features:
mandatory: true
diff --git a/schema/sensor/SensorBase.schema b/schema/sensor/SensorBase.schema
index f06ab44a7..07ebae6f5 100644
--- a/schema/sensor/SensorBase.schema
+++ b/schema/sensor/SensorBase.schema
@@ -2,4 +2,16 @@ name:
mandatory: true
type: string
yaml_type: scalar
- doc: The sensor's name. It has to be unique.
\ No newline at end of file
+ doc: The sensor's name. It has to be unique.
+
+type:
+ mandatory: true
+ type: string
+ yaml_type: scalar
+ doc: The sensor's class name.
+
+plugin:
+ mandatory: true
+ type: string
+ yaml_type: scalar
+ doc: The name of the wolf plugin where the sensor is implemented.
\ No newline at end of file
diff --git a/schema/sensor/SensorDiffDrive.schema b/schema/sensor/SensorDiffDrive.schema
index d685842ed..ca425eed8 100644
--- a/schema/sensor/SensorDiffDrive.schema
+++ b/schema/sensor/SensorDiffDrive.schema
@@ -1,9 +1,27 @@
follow: SensorBase.schema
+
+type:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: SensorDiffDrive
+ options: [SensorDiffDrive]
+ doc: The sensor's class name.
+
+plugin:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: core
+ options: [core]
+ doc: The name of the wolf plugin where the sensor is implemented.
+
ticks_per_wheel_revolution:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of displacement variance to displacement, for odometry noise calculation.
+
ticks_std_factor:
mandatory: true
type: double
diff --git a/schema/sensor/SensorOdom2d.schema b/schema/sensor/SensorOdom2d.schema
index bd4574a7a..4983ec87a 100644
--- a/schema/sensor/SensorOdom2d.schema
+++ b/schema/sensor/SensorOdom2d.schema
@@ -1,24 +1,29 @@
follow: SensorBase.schema
+
k_disp_to_disp:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of displacement variance to displacement, for odometry noise calculation.
+
k_disp_to_rot:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of displacement variance to rotation, for odometry noise calculation.
+
k_rot_to_rot:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of rotation variance to rotation, for odometry noise calculation.
+
min_disp_var:
mandatory: true
type: double
yaml_type: scalar
doc: minimum displacement variance, for odometry noise calculation.
+
min_rot_var:
mandatory: true
type: double
diff --git a/schema/sensor/SensorOdom3d.schema b/schema/sensor/SensorOdom3d.schema
index 36bc601c9..0866f2278 100644
--- a/schema/sensor/SensorOdom3d.schema
+++ b/schema/sensor/SensorOdom3d.schema
@@ -1,30 +1,52 @@
follow: SensorBase.schema
+
+type:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: SensorOdom3d
+ options: [SensorOdom3d]
+ doc: The sensor's class name.
+
+plugin:
+ mandatory: false
+ type: string
+ yaml_type: scalar
+ default: core
+ options: [core]
+ doc: The name of the wolf plugin where the sensor is implemented.
+
k_disp_to_disp:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of displacement variance to displacement, for odometry noise calculation.
+
k_disp_to_rot:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of displacement variance to rotation, for odometry noise calculation.
+
k_rot_to_rot:
mandatory: true
type: double
yaml_type: scalar
doc: ratio of rotation variance to rotation, for odometry noise calculation.
+
min_disp_var:
mandatory: true
type: double
yaml_type: scalar
doc: minimum displacement variance, for odometry noise calculation.
+
min_rot_var:
mandatory: true
type: double
yaml_type: scalar
doc: minimum rotation variance, for odometry noise calculation.
+
states:
P:
follow: PriorP3d.schema
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index ee4631c8d..63b8a78f5 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -120,26 +120,33 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
schema_folders.push_back(_WOLF_SCHEMAS_DIR);
// Create yaml server
+ WOLF_INFO("Loading yaml file");
auto server = yaml_schema_cpp::YamlServer(schema_folders, _input_yaml_file);
-
+ WOLF_INFO("loaded node:\n", server.getNode());
+
// Validate agains schema files
+ WOLF_INFO("Applying schema");
if (not server.applySchema("Problem.schema"))
{
WOLF_ERROR(server.getLog().str());
return nullptr;
}
+ WOLF_INFO("schema applied");
+
// Get param node
YAML::Node param_node = server.getNode();
// PROBLEM ===============================================================================
// structure and dimension
+ WOLF_INFO("Loading problem parameters");
YAML::Node problem_node = param_node["problem"];
std::string frame_structure = problem_node["frame_structure"].as<std::string>();
int dim = problem_node["dimension"].as<int>();
auto problem = Problem::create(frame_structure, dim, nullptr);
// Tree manager
+ WOLF_INFO("Loading tree manager parameters");
YAML::Node tree_manager_node = problem_node["tree_manager"];
std::string tree_manager_type = tree_manager_node["type"].as<std::string>();
problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node));
@@ -167,6 +174,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
// SENSORS ===============================================================================
// load plugin if sensor is not registered
+ WOLF_INFO("Loading sensors parameters");
YAML::Node sensors_node = param_node["sensors"];
for (auto sensor_n : sensors_node)
{
@@ -180,6 +188,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
// PROCESSORS ===============================================================================
// load plugin if processor is not registered
+ WOLF_INFO("Loading processors parameters");
YAML::Node processors_node = param_node["processors"];
for (auto processor_n : processors_node)
{
@@ -193,6 +202,7 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve
// MAP (optional) ===============================================================================
// Default MapBase
+ WOLF_INFO("Loading map parameters");
if (not param_node["map"])
{
param_node["map"]["type"] = "MapBase";
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index ffc25ac1a..81781d042 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.h
@@ -38,7 +38,9 @@ struct ParamsProcessorTrackerFeatureDummy : public ParamsProcessorTrackerFeature
ParamsProcessorTrackerFeatureDummy(const YAML::Node& _n):
ParamsProcessorTrackerFeature(_n)
{
+ WOLF_INFO("ParamsProcessorTrackerFeatureDummy input node:\n", _n);
n_tracks_lost = _n["n_tracks_lost"].as<unsigned int>();
+ WOLF_INFO("ParamsProcessorTrackerFeatureDummy loaded");
}
};
diff --git a/test/yaml/params_problem_autosetup.yaml b/test/yaml/params_problem_autosetup.yaml
index 00cc15c55..fdd5b2f39 100644
--- a/test/yaml/params_problem_autosetup.yaml
+++ b/test/yaml/params_problem_autosetup.yaml
@@ -18,7 +18,7 @@ config:
plugin: "core"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
k_disp_to_disp: 0.1
diff --git a/test/yaml/params_problem_autosetup_no_map.yaml b/test/yaml/params_problem_autosetup_no_map.yaml
index 46c678eb2..840d40624 100644
--- a/test/yaml/params_problem_autosetup_no_map.yaml
+++ b/test/yaml/params_problem_autosetup_no_map.yaml
@@ -15,7 +15,7 @@ config:
type: "None"
sensors:
-
- type: "SensorOdom"
+ type: "SensorOdom3d"
name: "odom"
plugin: "core"
k_disp_to_disp: 0.1
diff --git a/test/yaml/processor_odom_3d.yaml b/test/yaml/processor_odom_3d.yaml
index ffd83cefc..93ec73af0 100644
--- a/test/yaml/processor_odom_3d.yaml
+++ b/test/yaml/processor_odom_3d.yaml
@@ -1,3 +1,5 @@
+name: a cool processor odom 3d
+
time_tolerance: 0.01 # seconds
keyframe_vote:
diff --git a/test/yaml/processor_tracker_feature_dummy.yaml b/test/yaml/processor_tracker_feature_dummy.yaml
index ea6f0af32..1ec67d427 100644
--- a/test/yaml/processor_tracker_feature_dummy.yaml
+++ b/test/yaml/processor_tracker_feature_dummy.yaml
@@ -1,4 +1,8 @@
name: cool dummy processor
-min_features_for_keyframe: 1
+time_tolerance: 0.1
+keyframe_vote:
+ voting_active: true
+ min_features_for_keyframe: 1
+apply_loss_function: false
max_new_features: 0
n_tracks_lost: 1
\ No newline at end of file
--
GitLab