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