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