void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points, const std::string &in_grid_layer_name, const int in_layer_background_value, const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value, const std::string &in_tf_target_frame, const std::string &in_tf_source_frame, const tf::TransformListener &in_tf_listener) { if(!out_grid_map.exists(in_grid_layer_name)) { out_grid_map.add(in_grid_layer_name); } out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value); cv::Mat original_image; grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map, in_grid_layer_name, CV_8UC1, in_layer_min_value, in_layer_max_value, original_image); cv::Mat filled_image = original_image.clone(); tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener); // calculate out_grid_map position grid_map::Position map_pos = out_grid_map.getPosition(); double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x(); double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y(); for (const auto &points : in_area_points) { std::vector<cv::Point> cv_points; for (const auto &p : points) { // transform to GridMap coordinate geometry_msgs::Point tf_point = TransformPoint(p, tf); // coordinate conversion for cv image double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution(); double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution(); cv_points.emplace_back(cv::Point(cv_x, cv_y)); } cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color)); } // convert to ROS msg grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image, in_grid_layer_name, out_grid_map, in_layer_min_value, in_layer_max_value); }
bool GridCellsVisualization::visualize(const grid_map::GridMap& map) { if (!isActive()) return true; if (!map.exists(layer_)) { ROS_WARN_STREAM("GridCellsVisualization::visualize: No grid map layer with name '" << layer_ << "' found."); return false; } nav_msgs::GridCells gridCells; grid_map::GridMapRosConverter::toGridCells(map, layer_, lowerThreshold_, upperThreshold_, gridCells); publisher_.publish(gridCells); return true; }
bool VectorVisualization::visualize(const grid_map::GridMap& map) { if (!isActive()) return true; for (const auto& type : types_) { if (!map.exists(type)) { ROS_WARN_STREAM( "VectorVisualization::visualize: No grid map layer with name '" << type << "' found."); return false; } } // Set marker info. marker_.header.frame_id = map.getFrameId(); marker_.header.stamp.fromNSec(map.getTimestamp()); // Clear points. marker_.points.clear(); marker_.colors.clear(); for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue; geometry_msgs::Vector3 vector; vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set vector.y = map.at(types_[1], *iterator); vector.z = map.at(types_[2], *iterator); Eigen::Vector3d position; map.getPosition3(positionLayer_, *iterator, position); geometry_msgs::Point startPoint; startPoint.x = position.x(); startPoint.y = position.y(); startPoint.z = position.z(); marker_.points.push_back(startPoint); geometry_msgs::Point endPoint; endPoint.x = startPoint.x + scale_ * vector.x; endPoint.y = startPoint.y + scale_ * vector.y; endPoint.z = startPoint.z + scale_ * vector.z; marker_.points.push_back(endPoint); marker_.colors.push_back(color_); // Each vertex needs a color. marker_.colors.push_back(color_); } publisher_.publish(marker_); return true; }