From a07758d6b2c59db0bbe37ec8af4d59a4f1491a7a Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 10 Oct 2022 14:23:06 +0200
Subject: [PATCH] [skip ci] adding local reference state blocks
---
include/core/problem/problem.h | 12 ++++++++++--
src/problem/problem.cpp | 10 +++++++---
2 files changed, 17 insertions(+), 5 deletions(-)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index d2de44d0b..e44670b04 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -99,11 +99,19 @@ class Problem : public std::enable_shared_from_this<Problem>
Reference local_reference_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
- Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map); // USE create() below !!
+ Problem(const std::string& _frame_structure,
+ SizeEigen _dim,
+ MapBasePtr _map,
+ StateBlockPtr _local_reference_p = nullptr,
+ StateBlockPtr _local_reference_o = nullptr); // USE create() below !!
void setup();
public:
- static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map = std::make_shared<MapBase>()); // USE THIS AS A CONSTRUCTOR!
+ static ProblemPtr create(const std::string& _frame_structure,
+ SizeEigen _dim,
+ MapBasePtr _map = std::make_shared<MapBase>(),
+ StateBlockPtr _local_reference_p = nullptr,
+ StateBlockPtr _local_reference_o = nullptr); // USE THIS AS A CONSTRUCTOR!
static ProblemPtr autoSetup(ParamsServer &_server);
virtual ~Problem();
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index fa168d5d5..5af0122e5 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -29,6 +29,7 @@
#include "core/factor/factor_quaternion_absolute.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
+#include "core/state_block/state_block_derived.h"
#include "core/tree_manager/factory_tree_manager.h"
#include "core/tree_manager/tree_manager_base.h"
#include "core/utils/loader.h"
@@ -37,7 +38,7 @@
namespace wolf
{
-Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map) :
+Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map, StateBlockPtr _local_reference_p, StateBlockPtr _local_reference_o) :
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
@@ -67,6 +68,9 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
+
+ local_reference_ = Reference{_local_reference_p ? _local_reference_p : std::make_shared<StatePoint3d>(Eigen::Vector3d::Zero()),
+ _local_reference_o ? _local_reference_o : std::make_shared<StateQuaternion>(Eigen::Quaterniond::Identity())};
}
void Problem::setup()
@@ -77,9 +81,9 @@ void Problem::setup()
map_ptr_ -> setProblem(shared_from_this());
}
-ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map)
+ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr _map, StateBlockPtr _local_reference_p, StateBlockPtr _local_reference_o)
{
- 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`.
+ ProblemPtr p(new Problem(_frame_structure, _dim, _map, _local_reference_p, _local_reference_o)); // 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();
}
--
GitLab