From 0dd12897a0b73fe733ef4907039f7dfb1237251b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 3 Aug 2019 14:38:37 +0200
Subject: [PATCH] Hello_wolf autoconf uses individual config files for sen/prc
---
hello_wolf/hello_wolf_autoconf.cpp | 5 +-
hello_wolf/hello_wolf_config.yaml | 55 --------------------
hello_wolf/yaml/hello_wolf_config.yaml | 44 ++++++++++++++++
hello_wolf/yaml/processor_odom_2D.yaml | 12 +++++
hello_wolf/yaml/processor_range_bearing.yaml | 7 +++
hello_wolf/{ => yaml}/sensor_odom_2D.yaml | 0
hello_wolf/yaml/sensor_range_bearing.yaml | 4 ++
include/core/processor/processor_odom_2D.h | 2 +-
include/core/yaml/parser_yaml.hpp | 6 ++-
9 files changed, 75 insertions(+), 60 deletions(-)
delete mode 100644 hello_wolf/hello_wolf_config.yaml
create mode 100644 hello_wolf/yaml/hello_wolf_config.yaml
create mode 100644 hello_wolf/yaml/processor_odom_2D.yaml
create mode 100644 hello_wolf/yaml/processor_range_bearing.yaml
rename hello_wolf/{ => yaml}/sensor_odom_2D.yaml (100%)
create mode 100644 hello_wolf/yaml/sensor_range_bearing.yaml
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 3c89914f7..bc53460f2 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -104,10 +104,11 @@ int main()
WOLF_TRACE("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
- std::string file = std::string(_WOLF_ROOT_DIR) + "/hello_wolf/hello_wolf_config.yaml";
+ std::string config_file = "hello_wolf/yaml/hello_wolf_config.yaml";
+ std::string wolf_path = std::string(_WOLF_ROOT_DIR);
// parse file into params server: each param will be retrievable from this params server:
- ParserYAML parser = ParserYAML(file, std::string(_WOLF_ROOT_DIR));
+ ParserYAML parser = ParserYAML(config_file, wolf_path);
parser.parse();
ParamsServer server = ParamsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
diff --git a/hello_wolf/hello_wolf_config.yaml b/hello_wolf/hello_wolf_config.yaml
deleted file mode 100644
index 673313d13..000000000
--- a/hello_wolf/hello_wolf_config.yaml
+++ /dev/null
@@ -1,55 +0,0 @@
-config:
-
- problem:
- frame_structure: "PO"
- dimension: 2
- prior:
- timestamp: 0.0
- state: [0,0,0]
- cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
- time_tolerance: 0.1
-
- sensors:
- -
- type: "ODOM 2D"
- name: "sen odom"
- extrinsic:
- pose: [0,0, 0]
- follow: "hello_wolf/sensor_odom_2D.yaml"
-
- -
- type: "RANGE BEARING"
- name: "sen rb"
- extrinsic:
- pose: [1,1, 0]
- noise_range_metres_std: 0.1
- noise_bearing_degrees_std: 0.5
-
- processors:
- -
- type: "ODOM 2D"
- name: "prc odom"
- sensor_name: "sen odom"
- time_tolerance: 0.1
- voting_active: true
- voting_aux_active: false
- max_time_span: 999
- dist_traveled: 0.95
- angle_turned: 999
- max_buff_length: 999
- cov_det: 999
- unmeasured_perturbation_std: 0.001
-
- -
- type: "RANGE BEARING"
- name: "prc rb"
- sensor_name: "sen rb"
- voting_active: false
- voting_aux_active: false
- time_tolerance: 0.1
-
-
-files:
-# - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
-
-
\ No newline at end of file
diff --git a/hello_wolf/yaml/hello_wolf_config.yaml b/hello_wolf/yaml/hello_wolf_config.yaml
new file mode 100644
index 000000000..9ca64d247
--- /dev/null
+++ b/hello_wolf/yaml/hello_wolf_config.yaml
@@ -0,0 +1,44 @@
+config:
+
+ problem:
+
+ frame_structure: "PO" # keyframes have position and orientation
+ dimension: 2 # space is 2D
+ prior:
+ timestamp: 0.0
+ state: [0,0,0]
+ cov: [[3,3],.01,0,0,0,.01,0,0,0,.01]
+ time_tolerance: 0.1
+
+ sensors:
+
+ - type: "ODOM 2D"
+ name: "sen odom"
+ extrinsic:
+ pose: [0,0, 0]
+ follow: "hello_wolf/yaml/sensor_odom_2D.yaml" # config parameters in this file
+
+ - type: "RANGE BEARING"
+ name: "sen rb"
+ extrinsic:
+ pose: [1,1, 0]
+ noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param
+ follow: hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file
+
+ processors:
+
+ - type: "ODOM 2D"
+ name: "prc odom"
+ sensor_name: "sen odom" # attach processor to this sensor
+ follow: hello_wolf/yaml/processor_odom_2D.yaml # config parameters in this file
+
+ - type: "RANGE BEARING"
+ name: "prc rb"
+ sensor_name: "sen rb" # attach processor to this sensor
+ follow: hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file
+
+
+files:
+# - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
+
+
\ No newline at end of file
diff --git a/hello_wolf/yaml/processor_odom_2D.yaml b/hello_wolf/yaml/processor_odom_2D.yaml
new file mode 100644
index 000000000..b620e4010
--- /dev/null
+++ b/hello_wolf/yaml/processor_odom_2D.yaml
@@ -0,0 +1,12 @@
+type: "ODOM 2D"
+
+time_tolerance: 0.1
+unmeasured_perturbation_std: 0.001
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
+ max_time_span: 999
+ dist_traveled: 0.95
+ angle_turned: 999
+ max_buff_length: 999
+ cov_det: 999
diff --git a/hello_wolf/yaml/processor_range_bearing.yaml b/hello_wolf/yaml/processor_range_bearing.yaml
new file mode 100644
index 000000000..fea427973
--- /dev/null
+++ b/hello_wolf/yaml/processor_range_bearing.yaml
@@ -0,0 +1,7 @@
+type: "RANGE BEARING"
+
+time_tolerance: 0.1
+
+keyframe_vote:
+ voting_active: true
+ voting_aux_active: false
\ No newline at end of file
diff --git a/hello_wolf/sensor_odom_2D.yaml b/hello_wolf/yaml/sensor_odom_2D.yaml
similarity index 100%
rename from hello_wolf/sensor_odom_2D.yaml
rename to hello_wolf/yaml/sensor_odom_2D.yaml
diff --git a/hello_wolf/yaml/sensor_range_bearing.yaml b/hello_wolf/yaml/sensor_range_bearing.yaml
new file mode 100644
index 000000000..80f2c75b0
--- /dev/null
+++ b/hello_wolf/yaml/sensor_range_bearing.yaml
@@ -0,0 +1,4 @@
+type: "RANGE BEARING"
+
+noise_range_metres_std: 0.1
+noise_bearing_degrees_std: 0.5
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 27d180d43..114f6f139 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -27,7 +27,7 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion
ProcessorParamsOdom2D(std::string _unique_name, const wolf::ParamsServer & _server) :
ProcessorParamsMotion(_unique_name, _server)
{
- cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det");
+ cov_det = _server.getParam<Scalar>(_unique_name + "/keyframe_vote/cov_det");
}
std::string print()
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index beb5a589a..42e1c3439 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -247,10 +247,8 @@ public:
std::string ParserYAML::generatePath(std::string path){
std::regex r("^/.*");
if(std::regex_match(path, r)){
- std::cout << "Generating " + path << std::endl;
return path;
}else{
- std::cout << "Generating " + _path_root + path << std::endl;
return _path_root + path;
}
}
@@ -263,17 +261,20 @@ std::string ParserYAML::tagsToString(std::vector<std::string> &tags){
}
void ParserYAML::walkTree(std::string file){
YAML::Node n;
+ std::cout << "Parsing " << generatePath(file) << std::endl;
n = YAML::LoadFile(generatePath(file));
std::vector<std::string> hdrs = std::vector<std::string>();
walkTreeR(n, hdrs, "");
}
void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags){
YAML::Node n;
+ std::cout << "Parsing " << generatePath(file) << std::endl;
n = YAML::LoadFile(generatePath(file));
walkTreeR(n, tags, "");
}
void ParserYAML::walkTree(std::string file, std::vector<std::string>& tags, std::string hdr){
YAML::Node n;
+ std::cout << "Parsing " << generatePath(file) << std::endl;
n = YAML::LoadFile(generatePath(file));
walkTreeR(n, tags, hdr);
}
@@ -363,6 +364,7 @@ void ParserYAML::updateActiveName(std::string tag){
* @param file is the path to the YAML file */
void ParserYAML::parseFirstLevel(std::string file){
YAML::Node n;
+ std::cout << "Parsing " << generatePath(file) << std::endl;
n = YAML::LoadFile(generatePath(file));
YAML::Node n_config = n["config"];
--
GitLab