Skip to content
Snippets Groups Projects
Commit 0561cadf authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

Fixed gtest_emplace

parent 75a05b04
No related branches found
No related tags found
No related merge requests found
Pipeline #3129 passed
......@@ -26,157 +26,178 @@ using namespace wolf;
using namespace Eigen;
int main(int argc, char** argv) {
string file = "";
if(argc > 1) file = argv[1];
parserYAML parser = parserYAML(file);
parser.parse();
paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
cout << "PRINTING SERVER MAP" << endl;
server.print();
cout << "-----------------------------------" << endl;
/**
It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
**/
// vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
// for(auto it : parser.getFiles()) {
// auto c = new ClassLoader(it);
// class_loaders.push_back(c);
// }
auto loaders = vector<Loader*>();
for(auto it : parser.getFiles()) {
auto l = new LoaderRaw(it);
loaders.push_back(l);
}
ProblemPtr problem = Problem::create("PO", 2);
auto sensorMap = map<string, SensorBasePtr>();
auto procesorMap = map<string, ProcessorBasePtr>();
for(auto s : server.getSensors()){
cout << s._name << " " << s._type << endl;
sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
}
for(auto s : server.getProcessors()){
cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
}
problem->print(4,1,1,1);
Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
// landmark observations data
VectorXi ids;
VectorXs ranges, bearings;
// SET OF EVENTS =======================================================
std::cout << std::endl;
WOLF_TRACE("======== BUILD PROBLEM =======");
// ceres::Solver::Options options;
// options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
auto ceres = SolverFactory::get().create("Solver", problem, server);
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
// initialize
TimeStamp t(0.0); // t : 0.0
Vector3s x(0,0,0);
Matrix3s P = Matrix3s::Identity() * 0.1;
problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
auto sensor_rb = sensorMap.find("rb")->second;
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI/2;
CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0
// motion
auto sensor_odometry = sensorMap.find("odom")->second;
CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odometry, motion_data, motion_cov);
sensor_odometry ->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0
// motion
cap_motion ->setTimeStamp(t);
sensor_odometry ->process(cap_motion); // KF3 : (2,0,0)
// observe lmks
ids.resize(2); ranges.resize(2); bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3*M_PI/4, M_PI/2;
cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
sensor_rb ->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1,0,1,0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(1,0,1,0);
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto sen : problem->getHardware()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMap()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1,0,1,0);
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList()){
if (kf->isKey())
{
Eigen::MatrixXs cov;
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
}
for (auto lmk : problem->getMap()->getLandmarkList()) {
Eigen::MatrixXs cov;
WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
}
}
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4,1,1,1);
return 0;
string file = "";
if (argc > 1)
file = argv[1];
parserYAML parser = parserYAML(file);
parser.parse();
paramsServer server = paramsServer(parser.getParams(),
parser.sensorsSerialization(), parser.processorsSerialization());
cout << "PRINTING SERVER MAP" << endl;
server.print();
cout << "-----------------------------------" << endl;
/**
It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
**/
// vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
// for(auto it : parser.getFiles()) {
// auto c = new ClassLoader(it);
// class_loaders.push_back(c);
// }
auto loaders = vector<Loader*>();
for (auto it : parser.getFiles()) {
auto l = new LoaderRaw(it);
loaders.push_back(l);
}
ProblemPtr problem = Problem::create("PO", 2);
auto sensorMap = map<string, SensorBasePtr>();
auto procesorMap = map<string, ProcessorBasePtr>();
for (auto s : server.getSensors()) {
cout << s._name << " " << s._type << endl;
sensorMap.insert(
pair<string, SensorBasePtr>(s._name,
problem->installSensor(s._type, s._name, server)));
}
for (auto s : server.getProcessors()) {
cout << s._name << " " << s._type << " " << s._name_assoc_sensor
<< endl;
procesorMap.insert(
pair<string, ProcessorBasePtr>(s._name,
problem->installProcessor(s._type, s._name,
s._name_assoc_sensor, server)));
}
problem->print(4, 1, 1, 1);
Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
// landmark observations data
VectorXi ids;
VectorXs ranges, bearings;
// SET OF EVENTS =======================================================
std::cout << std::endl;
WOLF_TRACE("======== BUILD PROBLEM =======");
// ceres::Solver::Options options;
// options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations
// CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options);
auto ceres = SolverFactory::get().create("Solver", problem, server);
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
// initialize
TimeStamp t(0.0); // t : 0.0
Vector3s x(0, 0, 0);
Matrix3s P = Matrix3s::Identity() * 0.1;
problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
auto sensor_rb = sensorMap.find("rb")->second;
// observe lmks
ids.resize(1);
ranges.resize(1);
bearings.resize(1);
ids << 1; // will observe Lmk 1
ranges << 1.0; // see drawing
bearings << M_PI / 2;
CaptureRangeBearingPtr cap_rb = std::make_shared < CaptureRangeBearing
> (t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2)
// STEP 2 --------------------------------------------------------------
t += 1.0; // t : 1.0
// motion
auto sensor_odometry = sensorMap.find("odom")->second;
CaptureOdom2DPtr cap_motion = std::make_shared < CaptureOdom2D
> (t, sensor_odometry, motion_data, motion_cov);
sensor_odometry->process(cap_motion); // KF2 : (1,0,0)
// observe lmks
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 1, 2; // will observe Lmks 1 and 2
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared < CaptureRangeBearing
> (t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
t += 1.0; // t : 2.0
// motion
cap_motion->setTimeStamp(t);
sensor_odometry->process(cap_motion); // KF3 : (2,0,0)
// observe lmks
ids.resize(2);
ranges.resize(2);
bearings.resize(2);
ids << 2, 3; // will observe Lmks 2 and 3
ranges << sqrt(2.0), 1.0; // see drawing
bearings << 3 * M_PI / 4, M_PI / 2;
cap_rb = std::make_shared < CaptureRangeBearing
> (t, sensor_rb, ids, ranges, bearings);
sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem->print(1, 0, 1, 0);
// SOLVE ================================================================
// SOLVE with exact initial guess
WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(
wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
problem->print(1, 0, 1, 0);
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
for (auto sen : problem->getHardware()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(
sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(
sb->getState()
+ VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto lmk : problem->getMap()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(
sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1, 0, 1, 0);
// SOLVE again
WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1, 0, 1, 0);
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(
SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto kf : problem->getTrajectory()->getFrameList()) {
if (kf->isKey()) {
Eigen::MatrixXs cov;
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
}
for (auto lmk : problem->getMap()->getLandmarkList()) {
Eigen::MatrixXs cov;
WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
}
}
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4, 1, 1, 1);
return 0;
}
#ifndef SOLVER_MANAGER_H_
#define SOLVER_MANAGER_H_
//wolf includes
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "core/factor/factor_base.h"
namespace wolf {
/** \brief Enumeration of covariance blocks to be computed
*
* Enumeration of covariance blocks to be computed
*
*/
typedef enum
{
ALL, ///< All blocks and all cross-covariances
ALL_MARGINALS, ///< All marginals
ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
} CovarianceBlocksToBeComputed;
WOLF_PTR_TYPEDEFS(SolverManager);
/** \brief Solver manager for WOLF
*
*/
class SolverManager
{
protected:
ProblemPtr wolf_problem_;
public:
SolverManager(ProblemPtr _wolf_problem);
virtual ~SolverManager();
virtual std::string solve(const unsigned int& _report_level) = 0;
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
virtual void update();
virtual ProblemPtr getProblem();
private:
virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
};
} // namespace wolf
#endif
#include "core/solver/solver_manager.h"
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
namespace wolf {
SolverManager::SolverManager(ProblemPtr _wolf_problem) :
wolf_problem_(_wolf_problem)
{
}
SolverManager::~SolverManager()
{
}
void SolverManager::update()
{
//std::cout << "SolverManager: updating... " << std::endl;
//std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
//std::cout << wolf_problem_->getFactorNotificationList().size() << " factor notifications" << std::endl;
// REMOVE CONSTRAINTS
auto fac_notification_it = wolf_problem_->getFactorNotificationList().begin();
while ( fac_notification_it != wolf_problem_->getFactorNotificationList().end() )
if (fac_notification_it->notification_ == REMOVE)
{
removeFactor(fac_notification_it->factor_ptr_);
fac_notification_it = wolf_problem_->getFactorNotificationList().erase(fac_notification_it);
}
else
fac_notification_it++;
// REMOVE STATE BLOCKS
auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() )
if (state_notification_it->notification_ == REMOVE)
{
removeStateBlock(state_notification_it->state_block_ptr_);
state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it);
}
else
state_notification_it++;
// ADD/UPDATE STATE BLOCKS
while (!wolf_problem_->getStateBlockNotificationList().empty())
{
switch (wolf_problem_->getStateBlockNotificationList().front().notification_)
{
case ADD:
{
addStateBlock(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
if (wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_->isFixed())
updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
break;
}
case UPDATE:
{
updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
break;
}
default:
throw std::runtime_error("SolverManager::update: State Block notification must be ADD, UPATE or REMOVE.");
}
wolf_problem_->getStateBlockNotificationList().pop_front();
}
// ADD CONSTRAINTS
while (!wolf_problem_->getFactorNotificationList().empty())
{
switch (wolf_problem_->getFactorNotificationList().front().notification_)
{
case ADD:
{
addFactor(wolf_problem_->getFactorNotificationList().front().factor_ptr_);
break;
}
default:
throw std::runtime_error("SolverManager::update: Factor notification must be ADD or REMOVE.");
}
wolf_problem_->getFactorNotificationList().pop_front();
}
assert(wolf_problem_->getFactorNotificationList().empty() && "wolf problem's factors notification list not empty after update");
assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
}
ProblemPtr SolverManager::getProblem()
{
return wolf_problem_;
}
} // namespace wolf
......@@ -62,8 +62,8 @@ target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
# Node Emplace test
# TODO: TO BE FIXED
# wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
# target_link_libraries(gtest_emplace ${PROJECT_NAME})
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)
......
......@@ -9,14 +9,12 @@
#include "core/utils/logging.h"
#include "core/problem/problem.h"
#include "core/capture/capture_IMU.h"
#include "core/sensor/sensor_base.h"
#include "core/sensor/sensor_odom_2D.h"
#include "core/sensor/sensor_odom_3D.h"
#include "core/sensor/sensor_IMU.h"
#include "core/processor/processor_odom_3D.h"
#include "core/processor/processor_odom_2D.h"
#include "core/feature/feature_odom_2D.h"
#include "core/feature/feature_IMU.h"
#include "core/processor/processor_tracker_feature_dummy.h"
#include <iostream>
......@@ -122,20 +120,18 @@ TEST(Emplace, EmplaceDerived)
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
// LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
auto sen = SensorBase::emplace<SensorIMU>(P->getHardware(), Eigen::VectorXs(7), IntrinsicsIMU());
auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
auto cov = Eigen::MatrixXs(2,2);
cov(0,0) = 1;
cov(1,1) = 1;
auto cpt = CaptureBase::emplace<CaptureIMU>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov,
Eigen::Vector6s(), frm);
auto cpt2 = std::static_pointer_cast<CaptureIMU>(cpt);
auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
auto m = Eigen::Matrix<Scalar,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
m(i,j) = 1;
auto ftr = FeatureBase::emplace<FeatureIMU>(cpt, Eigen::VectorXs(5), cov,
Eigen::Vector6s(), m, cpt2);
auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
......
......@@ -24,7 +24,7 @@ using namespace Eigen;
WOLF_PTR_TYPEDEFS(DummySolverManager);
struct DummySolverManager : public SolverManager
class DummySolverManager : public SolverManager
{
DummySolverManager(const ProblemPtr& _problem)
: SolverManager(_problem)
......@@ -239,11 +239,11 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/src/examples/sensor_odom_3D.yaml");
ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// 2 state blocks, fixed
SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, "");
ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (2/* + 3*/)); // consume empties the notification map, so only should contain notification since last call
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2/* + 3*/)); // consume empties the notification map, so only should contain notification since last call
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
......@@ -255,16 +255,23 @@ TEST(Problem, StateBlocks)
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY, xs3d, 0);
ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
auto KF = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
// Notifications
Notification notif;
ASSERT_TRUE(P->getStateBlockNotification(KF->getP(), notif));
ASSERT_EQ(notif, ADD);
ASSERT_TRUE(P->getStateBlockNotification(KF->getO(), notif));
ASSERT_EQ(notif, ADD);
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2));
// ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 2 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(0)); // consume empties the notification map, so only should contain notification since last call
// ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// ASSERT_EQ(P->getStateBlockPtrList().size(), (SizeStd)(2 + 2 + 2));
// ASSERT_EQ(P->getStateBlockNotificationMap().size(),(SizeStd)(2 + 2 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
// P->print(4,1,1,1);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment