16 #ifndef PSEN_SCAN_V2_ZONESET_TO_MARKER_CONVERSION_H 17 #define PSEN_SCAN_V2_ZONESET_TO_MARKER_CONVERSION_H 22 #include <fmt/format.h> 24 #include <geometry_msgs/Point.h> 25 #include <geometry_msgs/Point32.h> 26 #include <std_msgs/ColorRGBA.h> 27 #include <visualization_msgs/Marker.h> 29 #include "psen_scan_v2/ZoneSet.h" 31 #define TO_MARKER(zoneset_obj, polygon_type, polygon_index) \ 32 createMarker(fmt::format("active zoneset {}{} {}", #polygon_type, #polygon_index, getRangeInfo(zoneset_obj)), \ 33 createRGBA(strcmp(#polygon_type, "muting") != 0, \ 34 strcmp(#polygon_type, "warn") == 0, \ 35 strcmp(#polygon_type, "muting") == 0, \ 37 zoneset_obj.header.frame_id, \ 38 zoneset_obj.polygon_type##polygon_index.points, \ 39 0.01 * (strcmp(#polygon_type, "warn") == 0) + 0.02 * (strcmp(#polygon_type, "muting") == 0)) 43 geometry_msgs::Point
createPoint(
const double& x,
const double& y,
const double& z)
45 geometry_msgs::Point p;
52 std_msgs::ColorRGBA
createRGBA(
const float& r,
const float& g,
const float& b,
const float& a)
54 std_msgs::ColorRGBA color;
63 const std_msgs::ColorRGBA& color,
64 const std::string& frame_id,
65 const std::vector<geometry_msgs::Point32>& points,
66 const double& z_offset = 0)
68 visualization_msgs::Marker marker;
69 marker.header.frame_id = frame_id;
72 marker.type = marker.TRIANGLE_LIST;
73 marker.action = marker.ADD;
84 marker.pose.orientation.x = 0.0;
85 marker.pose.orientation.y = 0.0;
86 marker.pose.orientation.z = 0.0;
87 marker.pose.orientation.w = 1.0;
88 marker.pose.position.x = 0.0;
89 marker.pose.position.y = 0.0;
90 marker.pose.position.z = z_offset;
92 marker.points.reserve(3 * (points.size() - 1));
93 marker.colors.reserve(points.size() - 1);
95 for (std::size_t i = 1; i < points.size(); ++i)
97 marker.points.push_back(
createPoint(0.0, 0.0, 0.0));
98 marker.points.push_back(
createPoint(points.at(i - 1).x, points.at(i - 1).y, 0.0));
99 marker.points.push_back(
createPoint(points.at(i).x, points.at(i).y, 0.0));
100 marker.colors.push_back(color);
107 std::string range_info =
"";
108 if (zoneset.speed_lower != 0 || zoneset.speed_upper != 0)
110 range_info = fmt::format(
"min:{:+} max:{:+}", zoneset.speed_lower, zoneset.speed_upper);
115 std::vector<visualization_msgs::Marker>
toMarkers(
const ZoneSet& zoneset)
117 std::vector<visualization_msgs::Marker> return_vec;
119 if (!zoneset.safety1.points.empty())
121 return_vec.push_back(
TO_MARKER(zoneset, safety, 1));
123 if (!zoneset.safety2.points.empty())
125 return_vec.push_back(
TO_MARKER(zoneset, safety, 2));
127 if (!zoneset.safety3.points.empty())
129 return_vec.push_back(
TO_MARKER(zoneset, safety, 3));
131 if (!zoneset.warn1.points.empty())
133 return_vec.push_back(
TO_MARKER(zoneset, warn, 1));
135 if (!zoneset.warn2.points.empty())
137 return_vec.push_back(
TO_MARKER(zoneset, warn, 2));
139 if (!zoneset.muting1.points.empty())
141 return_vec.push_back(
TO_MARKER(zoneset, muting, 1));
143 if (!zoneset.muting2.points.empty())
145 return_vec.push_back(
TO_MARKER(zoneset, muting, 2));
153 #endif // PSEN_SCAN_V2_ZONESET_TO_MARKER_CONVERSION_H std_msgs::ColorRGBA createRGBA(const float &r, const float &g, const float &b, const float &a)
std::string getRangeInfo(const ZoneSet &zoneset)
#define TO_MARKER(zoneset_obj, polygon_type, polygon_index)
geometry_msgs::Point createPoint(const double &x, const double &y, const double &z)
Root namespace for the ROS part.
std::vector< visualization_msgs::Marker > toMarkers(const ZoneSet &zoneset)
visualization_msgs::Marker createMarker(const std::string &ns, const std_msgs::ColorRGBA &color, const std::string &frame_id, const std::vector< geometry_msgs::Point32 > &points, const double &z_offset=0)