Fix rviz2 low FPS (autowarefoundation#390)

* add nullptr check when publish concatenate data (autowarefoundation#369)

* Add warning msg when concat pointcloud is not published (autowarefoundation#388)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Change lineString2Marker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Change trafficLight2TriangleMarker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Change laneletDirectionAsMarker

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Remove debug code

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix linter problems

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp

@@ -63,32 +63,49 @@ void polygon2Triangle(
 void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon);
 
 /**
- * [lineString2Marker creates marker to visualize shape of linestring]
- * @param ls         [input linestring]
- * @param marker [output marker message]
+ * [initLineStringMarker initializes marker to visualize shape of linestring]
+ * @param marker     [output marker message]
  * @param frame_id   [frame id of the marker]
  * @param ns         [namespace of the marker]
  * @param c          [color of the marker]
+ */
+void initLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const std::string frame_id, const std::string ns,
+  const std_msgs::msg::ColorRGBA c);
+
+/**
+ * [pushLineStringMarker pushes marker vertices to visualize shape of linestring]
+ * @param marker     [output marker message]
+ * @param ls         [input linestring]
+ * @param c          [color of the marker]
  * @param lss        [thickness of the marker]
  */
-void lineString2Marker(
-  const lanelet::ConstLineString3d ls, visualization_msgs::msg::Marker * marker,
-  const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c,
-  const float lss = 0.1);
+void pushLineStringMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
+  const std_msgs::msg::ColorRGBA c, const float lss = 0.1);
+
 /**
- * [trafficLight2TriangleMarker creates marker to visualize shape of traffic
+ * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic
  * lights]
- * @param ls       [linestring that represents traffic light shape]
  * @param marker   [created marker]
  * @param ns       [namespace of the marker]
- * @param cl       [color of the marker]
  * @param duration [lifetime of the marker]
  */
-void trafficLight2TriangleMarker(
-  const lanelet::ConstLineString3d ls, visualization_msgs::msg::Marker * marker,
-  const std::string ns,
-  const std_msgs::msg::ColorRGBA cl, const rclcpp::Duration duration = rclcpp::Duration(0, 0),
-  const double scale = 1.0);
+void initTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker,
+  const std::string ns, const rclcpp::Duration duration = rclcpp::Duration(0, 0));
+
+/**
+ * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic
+ * lights]
+ * @param marker   [created marker]
+ * @param ls       [linestring that represents traffic light shape]
+ * @param cl       [color of the marker]
+ * @param scale    [scale of the marker]
+ */
+void pushTrafficLightTriangleMarker(
+  visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d ls,
+  const std_msgs::msg::ColorRGBA cl, const double scale = 1.0);
 
 /**
  * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of