void ElevationVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameElevation, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("color"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at(typeNameElevation, buffIndex); if(std::isnan(elevation)) continue; const float color = haveColor ? map.at(typeNameColor, buffIndex) : lowerValueBound; // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { std_msgs::ColorRGBA markerColor = grid_map_visualization::color_utils::interpolateBetweenColors( color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); marker_.colors.push_back(markerColor); } } } markerPublisher_.publish(marker_); }
void SurfaceVisualization::visualize( const grid_map::GridMap& map, const std::string& typeNameSurface, const std::string& typeNameColor, const float lowerValueBound, const float upperValueBound) { // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); marker_.scale.x = map.getResolution(); marker_.scale.y = map.getResolution(); // Clear points. marker_.points.clear(); marker_.colors.clear(); float markerHeightOffset = static_cast<float>(markerHeight_/2.0); const Eigen::Array2i buffSize = map.getBufferSize(); const Eigen::Array2i buffStartIndex = map.getBufferStartIndex(); const bool haveColor = map.isType("class"); for (unsigned int i = 0; i < buffSize(0); ++i) { for (unsigned int j = 0; j < buffSize(1); ++j) { // Getting map data. const Eigen::Array2i cellIndex(i, j); const Eigen::Array2i buffIndex = grid_map_lib::getBufferIndexFromIndex(cellIndex, buffSize, buffStartIndex); const float& elevation = map.at("mu", buffIndex); const float& class_pred = map.at("class", buffIndex); int class_number=0; if(!std::isnan(class_pred)) { class_number=(int)class_pred; } if(std::isnan(elevation)) { continue; } //const float color = haveColor ? 16711680/class_pred : lowerValueBound; //ROS_INFO_STREAM("class_pred: "<<class_pred <<" int: "<<(int)class_pred); std_msgs::ColorRGBA surface_color; surface_color.a = 1.0; switch (class_number) { case 1: //long_grass surface_color.r = 0.0; surface_color.g = 1.0; surface_color.b = 0.0; break; case 3: //cobblestone surface_color.r = 0.62; surface_color.g = 0.32; surface_color.b = 0.17; break; case 4: //gravel surface_color.r = 0.2; surface_color.g = 0.2; surface_color.b = 0.2; break; default: surface_color.r = 1.0; surface_color.g = 1.0; surface_color.b = 1.0; break; } // Add marker point. Eigen::Vector2d position; map.getPosition(buffIndex, position); geometry_msgs::Point point; point.x = position.x(); point.y = position.y(); point.z = elevation - markerHeightOffset; marker_.points.push_back(point); // Add marker color. if(haveColor) { // std_msgs::ColorRGBA markerColor = // grid_map_visualization::color_utils::interpolateBetweenColors( // color, lowerValueBound, upperValueBound, lowerColor_, upperColor_); //marker_.colors.push_back(markerColor); marker_.colors.push_back(surface_color); } } } markerPublisher_.publish(marker_); }