Merge branch '9-adapt-to-state-block-map' into 'devel'

Resolve "Adapt to state block map"

Closes #9

See merge request !12

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

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()