PolygonRosConverter.cpp
Go to the documentation of this file.
1 /*
2  * PolygonRosConverter.cpp
3  *
4  * Created on: Nov 7, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
9 
10 // ROS
11 #include <geometry_msgs/Point32.h>
12 
13 namespace grid_map {
14 
16 
18 
19 void PolygonRosConverter::toMessage(const grid_map::Polygon& polygon, geometry_msgs::PolygonStamped& message)
20 {
21  message.header.stamp.fromNSec(polygon.getTimestamp());
22  message.header.frame_id = polygon.getFrameId();
23 
24  for (const auto& vertex : polygon.getVertices()) {
25  geometry_msgs::Point32 point;
26  point.x = vertex.x();
27  point.y = vertex.y();
28  point.z = 0.0;
29  message.polygon.points.push_back(point);
30  }
31 }
32 
33 void PolygonRosConverter::toLineMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color, const double lineWidth,
34  const double zCoordinate, visualization_msgs::Marker& marker)
35 {
36  marker.header.stamp.fromNSec(polygon.getTimestamp());
37  marker.header.frame_id = polygon.getFrameId();
38  marker.lifetime = ros::Duration(0.0);
39  marker.action = visualization_msgs::Marker::ADD;
40  marker.type = visualization_msgs::Marker::LINE_STRIP;
41  marker.color = color;
42  marker.scale.x = lineWidth;
43 
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);
49 
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;
55  }
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;
59 }
60 
61 void PolygonRosConverter::toTriangleListMarker(const grid_map::Polygon& polygon, const std_msgs::ColorRGBA& color,
62  const double zCoordinate, visualization_msgs::Marker& marker)
63 {
64  marker.header.stamp.fromNSec(polygon.getTimestamp());
65  marker.header.frame_id = polygon.getFrameId();
66  marker.lifetime = ros::Duration(0.0);
67  marker.action = visualization_msgs::Marker::ADD;
68  marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
69  marker.scale.x = 1.0;
70  marker.scale.y = 1.0;
71  marker.scale.z = 1.0;
72  marker.color = color;
73 
74  std::vector<Polygon> polygons = polygon.triangulate();
75  if (polygons.size() < 1) return;
76 
77  size_t nPoints = 3 * polygons.size();
78  marker.points.resize(nPoints);
79  marker.colors.resize(polygons.size(), color);
80 
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;
87  }
88  }
89 }
90 
91 } /* namespace grid_map */
const std::string & getFrameId() const
static void toTriangleListMarker(const grid_map::Polygon &polygon, const std_msgs::ColorRGBA &color, const double zCoordinate, visualization_msgs::Marker &marker)
size_t nVertices() const
static void toMessage(const grid_map::Polygon &polygon, geometry_msgs::PolygonStamped &message)
std::vector< Polygon > triangulate(const TriangulationMethods &method=TriangulationMethods::FAN) const
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


grid_map_ros
Author(s): Péter Fankhauser
autogenerated on Wed Jul 5 2023 02:23:44