From 9bacc7fd0cbe8e616ec3f0f35594dbbabc7e455a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 14 Aug 2019 00:42:22 +0200
Subject: [PATCH] hotfix state block vector size assert fail
---
include/core/landmark/landmark_base.h | 7 +++++--
src/landmark/landmark_base.cpp | 2 +-
2 files changed, 6 insertions(+), 3 deletions(-)
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index e33351c3b..853ffc3c1 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -168,8 +168,11 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
{
- assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
- return state_block_vec_[_i];
+ // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
+ if (_i < state_block_vec_.size())
+ return state_block_vec_[_i];
+ else
+ return nullptr;
}
inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5f245efc3..20e32da0a 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -156,7 +156,7 @@ YAML::Node LandmarkBase::saveToYaml() const
node["position"] = getP()->getState();
node["position fixed"] = getP()->isFixed();
}
- if (getO() != nullptr)
+ if (state_block_vec_.size() > 1 && getO() != nullptr)
{
node["orientation"] = getO()->getState();
node["orientation fixed"] = getO()->isFixed();
--
GitLab