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