From 9ae41f4c91cad2d3924c37905c04f9d8e1cddff7 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 28 Jul 2021 11:35:06 +0200
Subject: [PATCH] compiling and tests OK
---
include/core/map/map_base.h | 1 +
include/core/problem/problem.h | 4 +--
src/map/map_base.cpp | 6 +++++
src/problem/problem.cpp | 26 ++++++++++---------
test/yaml/params_problem_odom_3d.yaml | 2 ++
test/yaml/params_tree_manager1.yaml | 2 ++
test/yaml/params_tree_manager2.yaml | 2 ++
.../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 | 2 ++
13 files changed, 41 insertions(+), 14 deletions(-)
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 622efcdf1..f981f498a 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -72,6 +72,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
LandmarkBasePtrList landmark_list_;
public:
+ MapBase();
MapBase(ParamsMapBasePtr _params, const std::string& _type = "Base");
WOLF_MAP_CREATE(MapBase, ParamsMapBase);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 083e6a932..7004a205c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -69,11 +69,11 @@ class Problem : public std::enable_shared_from_this<Problem>
PriorOptionsPtr prior_options_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !!
+ Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
void setup();
public:
- static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 5d5433fc4..e263f9ad0 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -15,6 +15,12 @@
namespace wolf {
+MapBase::MapBase() :
+ NodeBase("MAP", "Base")
+{
+// std::cout << "constructed M"<< std::endl;
+}
+
MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
NodeBase("MAP", _type)
{
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index f4cdc9dfb..8258db880 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -41,11 +41,11 @@
namespace wolf
{
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
- map_ptr_(nullptr),
+ map_ptr_(_map),
processor_is_motion_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
@@ -76,13 +76,13 @@ void Problem::setup()
{
hardware_ptr_ -> setProblem(shared_from_this());
trajectory_ptr_-> setProblem(shared_from_this());
- map_ptr_ = std::make_shared<MapBase>(std::make_shared<ParamsMapBase>());
- map_ptr_ -> setProblem(shared_from_this());
+ if (map_ptr_)
+ map_ptr_ -> setProblem(shared_from_this());
}
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
{
- ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
+ ProblemPtr p(new Problem(_frame_structure, _dim, _map)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
p->setup();
return p->shared_from_this();
}
@@ -97,8 +97,8 @@ 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);
+
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
@@ -190,9 +190,6 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
_server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
-
-
-
}
else
{
@@ -203,11 +200,16 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
// Map
- std::string map_type = _server.getParam<std::string>("problem/map/type");
+ 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 6c5ed47c2..387924859 100644
--- a/test/yaml/params_problem_odom_3d.yaml
+++ b/test/yaml/params_problem_odom_3d.yaml
@@ -13,6 +13,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager1.yaml b/test/yaml/params_tree_manager1.yaml
index fa43fecb3..a27f60df2 100644
--- a/test/yaml/params_tree_manager1.yaml
+++ b/test/yaml/params_tree_manager1.yaml
@@ -18,6 +18,8 @@ config:
tree_manager:
type: "TreeManagerDummy"
toy_param: 0
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager2.yaml b/test/yaml/params_tree_manager2.yaml
index f37e31459..10759a6ed 100644
--- a/test/yaml/params_tree_manager2.yaml
+++ b/test/yaml/params_tree_manager2.yaml
@@ -17,6 +17,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window1.yaml b/test/yaml/params_tree_manager_sliding_window1.yaml
index 277810464..d0c3d81dd 100644
--- a/test/yaml/params_tree_manager_sliding_window1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window1.yaml
@@ -16,6 +16,8 @@ config:
n_frames: 3
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
diff --git a/test/yaml/params_tree_manager_sliding_window2.yaml b/test/yaml/params_tree_manager_sliding_window2.yaml
index f22fdde12..e7a7bc9b9 100644
--- a/test/yaml/params_tree_manager_sliding_window2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window2.yaml
@@ -16,6 +16,8 @@ config:
n_frames: 3
n_fix_first_frames: 0
viral_remove_empty_parent: false
+ map:
+ type: "MapBase"
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 a7f0f7434..299fa63e0 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml
@@ -18,3 +18,5 @@ config:
rate_old_frames: 2
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
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 cae3df67f..928408fcb 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml
@@ -18,3 +18,5 @@ config:
rate_old_frames: 2
n_fix_first_frames: 0
viral_remove_empty_parent: false
+ map:
+ type: "MapBase"
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 8f00f6499..9e64db064 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -24,6 +24,8 @@ config:
rate_old_frames: 2
n_fix_first_frames: 2
viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
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 114e865bd..16e421556 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
@@ -18,6 +18,8 @@ config:
time_tolerance: 0.1
tree_manager:
type: "None"
+ map:
+ type: "MapBase"
sensors:
-
type: "SensorOdom3d"
--
GitLab