11 #include <geometry_msgs/Point32.h> 22 message.header.frame_id = polygon.
getFrameId();
25 geometry_msgs::Point32 point;
29 message.polygon.points.push_back(point);
34 const double zCoordinate, visualization_msgs::Marker& marker)
39 marker.action = visualization_msgs::Marker::ADD;
40 marker.type = visualization_msgs::Marker::LINE_STRIP;
42 marker.scale.x = lineWidth;
44 unsigned int startIndex = marker.points.size();
45 unsigned int nVertices = polygon.
nVertices() + 1;
46 unsigned int nTotalVertices = marker.points.size() + nVertices;
47 marker.points.resize(nTotalVertices);
48 marker.colors.resize(nTotalVertices, color);
50 unsigned int i = startIndex;
51 for( ; i < nTotalVertices - 1; i++) {
52 marker.points[i].x = polygon[i].x();
53 marker.points[i].y = polygon[i].y();
54 marker.points[i].z = zCoordinate;
56 marker.points[i].x = marker.points[startIndex].x;
57 marker.points[i].y = marker.points[startIndex].y;
58 marker.points[i].z = marker.points[startIndex].z;
62 const double zCoordinate, visualization_msgs::Marker& marker)
67 marker.action = visualization_msgs::Marker::ADD;
68 marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
74 std::vector<Polygon> polygons = polygon.
triangulate();
75 if (polygons.size() < 1)
return;
77 size_t nPoints = 3 * polygons.size();
78 marker.points.resize(nPoints);
79 marker.colors.resize(polygons.size(), color);
81 for (
size_t i = 0; i < polygons.size(); ++i) {
82 for (
size_t j = 0; j < 3; ++j) {
83 const size_t pointIndex = 3 * i + j;
84 marker.points[pointIndex].x = polygons[i].getVertex(j).x();
85 marker.points[pointIndex].y = polygons[i].getVertex(j).y();
86 marker.points[pointIndex].z = zCoordinate;
static void toTriangleListMarker(const grid_map::Polygon &polygon, const std_msgs::ColorRGBA &color, const double zCoordinate, visualization_msgs::Marker &marker)
std::vector< Polygon > triangulate(const TriangulationMethods &method=TriangulationMethods::FAN) const
const std::string & getFrameId() const
virtual ~PolygonRosConverter()
static void toMessage(const grid_map::Polygon &polygon, geometry_msgs::PolygonStamped &message)
static void toLineMarker(const grid_map::Polygon &polygon, const std_msgs::ColorRGBA &color, const double lineWidth, const double zCoordinate, visualization_msgs::Marker &marker)
uint64_t getTimestamp() const
const std::vector< Position > & getVertices() const