/** * Free node handler */ void srs_env_model::CCollisionGridPlugin::handleFreeNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp ) { if (it.getDepth() == m_crawlDepth) { octomap::OcTreeKey nKey = it.getKey(); //TODO: remove intermedate obj (1.4) int i = (nKey[0] - m_paddedMinKey[0])/m_multires2DScale;; int j = (nKey[1] - m_paddedMinKey[1])/m_multires2DScale; if (m_data->data[m_data->info.width * j + i] == -1) { m_data->data[m_data->info.width * j + i] = 0; } } else { int intSize = 1 << (m_crawlDepth - it.getDepth()); octomap::OcTreeKey minKey = it.getIndexKey(); for (int dx = 0; dx < intSize; dx++) { int i = (minKey[0] + dx - m_paddedMinKey[0])/m_multires2DScale; for (int dy = 0; dy < intSize; dy++) { int j = (minKey[1] + dy - m_paddedMinKey[1])/m_multires2DScale; if (m_data->data[m_data->info.width * j + i] == -1) { m_data->data[m_data->info.width * j + i] = 0; } } } } }
/** * Occupied node handler */ void srs_env_model::CMap2DPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp) { if (it.getDepth() == mp.treeDepth) { octomap::OcTreeKey nKey = it.getKey(); // TODO: remove intermedate obj (1.4) int i = nKey[0] - m_paddedMinKey[0]; int j = nKey[1] - m_paddedMinKey[1]; m_data->data[m_data->info.width * j + i] = 100; } else { int intSize = 1 << (mp.treeDepth - it.getDepth()); octomap::OcTreeKey minKey = it.getIndexKey(); for (int dx = 0; dx < intSize; dx++) { int i = minKey[0] + dx - m_paddedMinKey[0]; for (int dy = 0; dy < intSize; dy++) { int j = minKey[1] + dy - m_paddedMinKey[1]; m_data->data[m_data->info.width * j + i] = 100; } } } }