From 9448feed00db7452e9df12909bd94435cd634a35 Mon Sep 17 00:00:00 2001 From: Joan Sola <jsola@iri.upc.edu> Date: Mon, 20 Jan 2020 16:27:25 +0100 Subject: [PATCH] Replace setStateBlockPtrStatic --> addStateBlock --- include/gnss/sensor/sensor_gnss.h | 8 ++++---- src/sensor/sensor_gnss.cpp | 18 ++++++++---------- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h index 66c410926..9332eb38c 100644 --- a/include/gnss/sensor/sensor_gnss.h +++ b/include/gnss/sensor/sensor_gnss.h @@ -102,22 +102,22 @@ inline bool SensorGnss::isEnuMapInitialized() const inline StateBlockPtr SensorGnss::getEnuMapTranslation() const { - return getStateBlockPtrStatic(3); + return getStateBlock("t"); } inline StateBlockPtr SensorGnss::getEnuMapRoll() const { - return getStateBlockPtrStatic(4); + return getStateBlock("r"); } inline StateBlockPtr SensorGnss::getEnuMapPitch() const { - return getStateBlockPtrStatic(5); + return getStateBlock("p"); } inline StateBlockPtr SensorGnss::getEnuMapYaw() const { - return getStateBlockPtrStatic(6); + return getStateBlock("y"); } inline const Eigen::Matrix3s& SensorGnss::getREnuEcef() const diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index 5beaf5097..6dc528f08 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -23,11 +23,10 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi assert(_p_ptr->getSize() == 3 && "Bad extrinsics size"); assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size"); - getStateBlockVec().resize(7); - setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, params_->translation_fixed_)); - setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, params_->roll_fixed_)); - setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, params_->pitch_fixed_)); - setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, params_->yaw_fixed_)); + addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed_)); + addStateBlock("r", std::make_shared<StateBlock>(1, params_->roll_fixed_)); + addStateBlock("p", std::make_shared<StateBlock>(1, params_->pitch_fixed_)); + addStateBlock("y", std::make_shared<StateBlock>(1, params_->yaw_fixed_)); } SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame) @@ -49,11 +48,10 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position assert(_pitch_ENU_MAP_ptr->getSize() == 1 && "Bad ENU_MAP pitch size"); assert(_yaw_ENU_MAP_ptr->getSize() == 1 && "Bad ENU_MAP yaw size"); - getStateBlockVec().resize(7); - setStateBlockPtrStatic(3, _t_ENU_MAP_ptr); - setStateBlockPtrStatic(4, _roll_ENU_MAP_ptr); - setStateBlockPtrStatic(5, _pitch_ENU_MAP_ptr); - setStateBlockPtrStatic(6, _yaw_ENU_MAP_ptr); + addStateBlock("t", _t_ENU_MAP_ptr); + addStateBlock("r", _roll_ENU_MAP_ptr); + addStateBlock("p", _pitch_ENU_MAP_ptr); + addStateBlock("y", _yaw_ENU_MAP_ptr); } SensorGnss::~SensorGnss() -- GitLab