diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 696061983077c6da19fe292206bff168bacc4b95..b1742c5aea60265d255aa142733ae45961d16e94 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -198,7 +198,7 @@ void SensorBase::registerNewStateBlocks() const auto key = pair_key_sbp.first; auto sbp = pair_key_sbp.second; - if (sbp && !isStateBlockInCapture(key)) + if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key)) getProblem()->notifyStateBlock(sbp, ADD); } } @@ -481,6 +481,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st auto sb = getStateBlockDynamic(key); if (sb) _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); "; + else + _stream << "NULLPTR "; } _stream << std::endl; } @@ -492,6 +494,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st auto sb = getStateBlockDynamic(key); if (sb) _stream << sb->getState().transpose() << " "; + else + _stream << "NULLPTR "; } _stream << ")" << std::endl; } @@ -503,6 +507,8 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st auto sb = getStateBlockDynamic(key); if (sb) _stream << key << "[" << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; "; + else + _stream << "NULLPTR "; } _stream << std::endl; }