Go to the documentation of this file.
15 #ifndef HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_
16 #define HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_
18 #include <visualization_msgs/msg/marker.hpp>
19 #include <visualization_msgs/msg/marker_array.hpp>
20 #include <geometry_msgs/msg/point.hpp>
21 #include <geometry_msgs/msg/point32.hpp>
22 #include <geometry_msgs/msg/polygon.hpp>
23 #include <rclcpp/rclcpp.hpp>
26 #include <lanelet2_core/LaneletMap.h>
29 #include <unordered_set>
45 namespace had_map_utils
56 std_msgs::msg::ColorRGBA * cl,
74 const int32_t &
id,
const rclcpp::Time & t,
75 const std::string & frame_id,
const std::string & ns,
76 const std_msgs::msg::ColorRGBA & c,
const int32_t & action,
const int32_t & type,
90 const rclcpp::Time & t,
91 const lanelet::LineString3d & ls,
92 const std::string & frame_id,
const std::string & ns,
const std_msgs::msg::ColorRGBA & c,
106 const rclcpp::Time & t,
107 const lanelet::ConstLineString3d & ls,
108 const std::string & frame_id,
const std::string & ns,
const std_msgs::msg::ColorRGBA & c,
121 const rclcpp::Time & t,
122 const std::string & ns,
123 const lanelet::LineStrings3d & linestrings,
124 const std_msgs::msg::ColorRGBA & c);
135 const rclcpp::Time & t,
136 const lanelet::ConstLanelets & lanelets,
137 const std_msgs::msg::ColorRGBA & c,
138 const bool8_t & viz_centerline);
152 const rclcpp::Time & t,
153 const int32_t & line_id,
const lanelet::BasicPolygon3d & pg,
154 const std::string & frame_id,
const std::string & ns,
155 const std_msgs::msg::ColorRGBA & c,
const float32_t & lss);
166 const rclcpp::Time & t,
167 const std::string & ns,
const lanelet::Areas & areas,
168 const std_msgs::msg::ColorRGBA & c);
179 const rclcpp::Time & t,
180 const std::string & ns,
const lanelet::Polygons3d & polygons,
181 const std_msgs::msg::ColorRGBA & c);
196 const rclcpp::Time & t,
const int32_t & line_id,
198 const std::string & frame_id,
const std::string & ns,
199 const std_msgs::msg::ColorRGBA & c,
const float32_t & lss);
211 const rclcpp::Time & t,
213 const std_msgs::msg::ColorRGBA & c);
220 std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC
lanelet2Triangle(
221 const lanelet::ConstLanelet & ll);
228 std::vector<geometry_msgs::msg::Polygon> HAD_MAP_UTILS_PUBLIC
polygon2Triangle(
237 const lanelet::ConstArea & area);
245 const lanelet::ConstLanelet & ll);
256 const rclcpp::Time & t,
const std::string & ns,
const lanelet::ConstLanelets & lanelets,
257 const std_msgs::msg::ColorRGBA & c);
268 const rclcpp::Time & t,
const std::string & ns,
const lanelet::Areas & areas,
269 const std_msgs::msg::ColorRGBA & c);
275 #endif // HAD_MAP_UTILS__HAD_MAP_VISUALIZATION_HPP_
boost::geometry::model::polygon< boost::geometry::model::d2::point_xy< float64_t > > Polygon
Definition: evaluate_object.cpp:58
visualization_msgs::msg::Marker Marker
Definition: object_polygon_detail.cpp:31
visualization_msgs::msg::MarkerArray MarkerArray
Definition: off_map_obstacles_filter_node.cpp:39
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC lineString2Marker(const rclcpp::Time &t, const lanelet::LineString3d &ls, const std::string &frame_id, const std::string &ns, const std_msgs::msg::ColorRGBA &c, const float32_t &lss)
creates marker with type LINE_STRIP from a lanelet::LineString3d object
Definition: had_map_visualization.cpp:90
std::vector< geometry_msgs::msg::Polygon > HAD_MAP_UTILS_PUBLIC lanelet2Triangle(const lanelet::ConstLanelet &ll)
converts area enclosed by lanelet::Lanelet into list of triangles.
Definition: had_map_visualization.cpp:339
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasBoundaryAsMarkerArray(const rclcpp::Time &t, const std::string &ns, const lanelet::Areas &areas, const std_msgs::msg::ColorRGBA &c)
converts outer bound of lanelet::Area into markers with type LINE_STRIP
Definition: had_map_visualization.cpp:224
This file includes common type definition.
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsAsTriangleMarkerArray(const rclcpp::Time &t, const std::string &ns, const lanelet::ConstLanelets &lanelets, const std_msgs::msg::ColorRGBA &c)
converts bounded area by lanelet::Lanelet into triangle markers
Definition: had_map_visualization.cpp:483
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC areasAsTriangleMarkerArray(const rclcpp::Time &t, const std::string &ns, const lanelet::Areas &areas, const std_msgs::msg::ColorRGBA &c)
converts bounded area by lanelet::Area into triangle markers
Definition: had_map_visualization.cpp:523
geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC area2Polygon(const lanelet::ConstArea &area)
converts lanelet::Area into geometry_msgs::msg::Polygon type
Definition: had_map_visualization.cpp:451
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC laneletsBoundaryAsMarkerArray(const rclcpp::Time &t, const lanelet::ConstLanelets &lanelets, const std_msgs::msg::ColorRGBA &c, const bool8_t &viz_centerline)
converts outer bound of lanelet::Lanelet into markers with type LINE_STRIP
Definition: had_map_visualization.cpp:153
bool bool8_t
Definition: types.hpp:39
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
OnlineData m
Definition: linear_tire.snippet.hpp:36
float float32_t
Definition: types.hpp:45
std::vector< geometry_msgs::msg::Polygon > HAD_MAP_UTILS_PUBLIC polygon2Triangle(const geometry_msgs::msg::Polygon &polygon)
converts area enclosed by geometry_msg::msg::Polygon into list of triangles.
Definition: had_map_visualization.cpp:381
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC boundingBoxAsMarkerArray(const rclcpp::Time &t, const std::string &ns, const float64_t upper[], const float64_t lower[], const std_msgs::msg::ColorRGBA &c)
creates marker array from bounding box
Definition: had_map_visualization.cpp:290
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC basicPolygon2Marker(const rclcpp::Time &t, const int32_t &line_id, const lanelet::BasicPolygon3d &pg, const std::string &frame_id, const std::string &ns, const std_msgs::msg::ColorRGBA &c, const float32_t &lss)
creates marker with type LINE_STRIP from a lanelet::BasicPolygon object
Definition: had_map_visualization.cpp:192
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC lineStringsAsMarkerArray(const rclcpp::Time &t, const std::string &ns, const lanelet::LineStrings3d &linestrings, const std_msgs::msg::ColorRGBA &c)
converts lanelet::LineString into markers with type LINE_STRIP
Definition: had_map_visualization.cpp:136
void HAD_MAP_UTILS_PUBLIC setMarkerHeader(visualization_msgs::msg::Marker *m, const int32_t &id, const rclcpp::Time &t, const std::string &frame_id, const std::string &ns, const std_msgs::msg::ColorRGBA &c, const int32_t &action, const int32_t &type, const float32_t &scale)
Set the header information to a marker object.
Definition: had_map_visualization.cpp:59
void HAD_MAP_UTILS_PUBLIC setColor(std_msgs::msg::ColorRGBA *cl, const float32_t &r, const float32_t &g, const float32_t &b, const float32_t &a)
Set set rgba information to a Color Object.
Definition: had_map_visualization.cpp:49
visualization_msgs::msg::MarkerArray HAD_MAP_UTILS_PUBLIC polygonsBoundaryAsMarkerArray(const rclcpp::Time &t, const std::string &ns, const lanelet::Polygons3d &polygons, const std_msgs::msg::ColorRGBA &c)
converts outer bound of lanelet::Polygon into markers with type LINE_STRIP
Definition: had_map_visualization.cpp:246
double float64_t
Definition: types.hpp:47
visualization_msgs::msg::Marker HAD_MAP_UTILS_PUBLIC bbox2Marker(const rclcpp::Time &t, const int32_t &line_id, const float64_t lower[], const float64_t upper[], const std::string &frame_id, const std::string &ns, const std_msgs::msg::ColorRGBA &c, const float32_t &lss)
creates marker with type LINE_STRIP from a bounding box
Definition: had_map_visualization.cpp:264
geometry_msgs::msg::Polygon HAD_MAP_UTILS_PUBLIC lanelet2Polygon(const lanelet::ConstLanelet &ll)
converts lanelet::Lanelet into geometry_msgs::msg::Polygon type
Definition: had_map_visualization.cpp:466