C++ (Cpp) iterator::getIndexKey Exemples

Exemple #1

Fichier : collision_grid_plugin.cpp Projet : ipa-jsf/srs_public

/**
 * 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;
				}
			}
		}
	}
}

Exemple #2

Fichier : map2d_plugin.cpp Projet : ipa-jsf/srs_public

/**
 * 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;
			}
		}
	}
}