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