Пример #1
0
void srs_env_model::CMarkerArrayPlugin::handleNode(const srs_env_model::tButServerOcTree::iterator & it, const SMapWithParameters & mp)
{
    unsigned idx = it.getDepth();
    assert(idx < m_data->markers.size());

    geometry_msgs::Point cubeCenter;



    if( m_bTransform )
    {
        // Transform input point
        Eigen::Vector3f point( it.getX(), it.getY(), it.getZ() );
        point = m_ocToMarkerArrayRot * point + m_ocToMarkerArrayTrans;
        cubeCenter.x = it.getX();
        cubeCenter.y = it.getY();
        cubeCenter.z = it.getZ();

    }else{
        cubeCenter.x = it.getX();
        cubeCenter.y = it.getY();
        cubeCenter.z = it.getZ();
    }

    m_data->markers[idx].points.push_back(cubeCenter);

    if (m_bHeightMap){
        double h = (1.0 - std::min(std::max((cubeCenter.z-m_minZ)/ (m_maxZ - m_minZ), 0.0), 1.0)) *m_colorFactor;
        m_data->markers[idx].colors.push_back(heightMapColor(h));
    }
}
Пример #2
0
/**
 * 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;
				}
			}
		}
	}
}
Пример #3
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;
			}
		}
	}
}