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));
}
}
void ElevationVisualization::visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform)
{
int current_height_level;
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
map_marker_array_msg.markers[i].points.clear();
}
map_marker_array_msg.markers.clear();
// each array stores all cubes of one height level:
map_marker_array_msg.markers.resize(max_height_levels+1);
for (int index_y = 0; index_y < (int)elevation_map.info.height; ++index_y)
{
for (int index_x = 0; index_x < (int)elevation_map.info.width; ++index_x)
{
// visualize only known cells
if(elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)] != (int16_t)-elevation_map.info.zero_elevation)
{
geometry_msgs::Point cube_center;
Eigen::Vector2f index_map(index_x, index_y);
Eigen::Vector2f index_world = world_map_transform.getC1Coords(index_map);
cube_center.x = index_world(0);//+elevation_map.info.resolution_xy/2.0;
cube_center.y = index_world(1);//+elevation_map.info.resolution_xy/2.0;
cube_center.z = (elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)]-elevation_map.info.zero_elevation)*elevation_map.info.resolution_z;
current_height_level = max_height_levels/2+(int)round(std::min(std::max((double)cube_center.z+local_map_transform.getOrigin().z(), -(double)max_height), (double)max_height)*(double)max_height_levels/((double)max_height*2.0f));
map_marker_array_msg.markers[current_height_level].points.push_back(cube_center);
if(use_color_map)
{
double h = (1.0 - std::min(std::max((cube_center.z-min_height)/ (max_height - min_height), 0.0), 1.0)) *color_factor;
map_marker_array_msg.markers[current_height_level].colors.push_back(heightMapColor(h));
}
}
}
}
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
std::stringstream ss;
ss <<"Level "<<i;
map_marker_array_msg.markers[i].ns = ss.str();
map_marker_array_msg.markers[i].id = i;
map_marker_array_msg.markers[i].header.frame_id = "/map";
map_marker_array_msg.markers[i].header.stamp = elevation_map.header.stamp;
map_marker_array_msg.markers[i].lifetime = ros::Duration();
map_marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
map_marker_array_msg.markers[i].scale.x = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.y = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.z = elevation_map.info.resolution_z;
map_marker_array_msg.markers[i].color = marker_color;
if (map_marker_array_msg.markers[i].points.size() > 0)
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
else
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::DELETE;
}
}
