From b4f7b70c31cdacf000b2738425c7ab8b7bcd8d33 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sun, 1 Sep 2019 17:00:29 +0200
Subject: [PATCH] Add frame structure string
---
include/core/frame/frame_base.h | 1 +
src/frame/frame_base.cpp | 21 +++++++++++++++++++++
2 files changed, 22 insertions(+)
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index a27970fa4..aafa35a21 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -39,6 +39,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
FactorBasePtrList constrained_by_list_;
+ std::string structure_;
// std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
static unsigned int frame_id_count_;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 3269ff36b..6e0510363 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -14,17 +14,27 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
+ structure_(""),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(NON_ESTIMATED),
time_stamp_(_ts)
{
if (_p_ptr)
+ {
setStateBlock("P", _p_ptr);
+ structure_ += "P";
+ }
if (_o_ptr)
+ {
setStateBlock("O", _o_ptr);
+ structure_ += "O";
+ }
if (_v_ptr)
+ {
setStateBlock("V", _v_ptr);
+ structure_ += "V";
+ }
// state_block_vec_[0] = _p_ptr;
// state_block_vec_[1] = _o_ptr;
// state_block_vec_[2] = _v_ptr;
@@ -33,17 +43,27 @@ FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
+ structure_(""),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
{
if (_p_ptr)
+ {
setStateBlock("P", _p_ptr);
+ structure_ += "P";
+ }
if (_o_ptr)
+ {
setStateBlock("O", _o_ptr);
+ structure_ += "O";
+ }
if (_v_ptr)
+ {
setStateBlock("V", _v_ptr);
+ structure_ += "V";
+ }
// state_block_vec_[0] = _p_ptr;
// state_block_vec_[1] = _o_ptr;
// state_block_vec_[2] = _v_ptr;
@@ -52,6 +72,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
+ structure_(_frame_structure),
// state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
frame_id_(++frame_id_count_),
type_(_tp),
--
GitLab