fixed octomap ground cells removed when updating graph

This commit is contained in:
matlabbe
2016-07-17 15:37:08 -04:00
parent b5e5e9508c
commit d4e5cfb548

View File

@@ -114,7 +114,7 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::OcTreeKey key;
if(newOcTree->coordToKeyChecked(cvPt.x, cvPt.y, cvPt.z, key))
{
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, true);
octomap::ColorOcTreeNode * n = newOcTree->updateNode(key, iter->second.isObstacle_);
if(n)
{
++copied;