From c7766f57441d2972e66f0916818a2d9fc1d9fdde Mon Sep 17 00:00:00 2001
From: Hiroki OTA <hiroki.ota@tier4.jp>
Date: Fri, 20 May 2022 15:36:48 +0900
Subject: [PATCH] feat(map_loader): visualize center line by points (#931)

* feat: visualize center line points

* fix: delete space

* feat: visualize center line by arrow

* revert insertMarkerArray

* fix: delete space
---
 .../visualization/visualization.hpp           | 10 ++++
 map/lanelet2_extension/lib/visualization.cpp  | 56 +++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
index 9d21ac69bd2f3..026a78ea1521c 100644
--- a/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
+++ b/map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp
@@ -83,6 +83,16 @@ void pushLineStringMarker(
   visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls,
   const std_msgs::msg::ColorRGBA c, const float lss = 0.1);
 
+/**
+ * [pushArrowMarkerArray pushes marker to visualize arrows]
+ * @param marker_array     [output marker array message]
+ * @param ls         [input linestring]
+ * @param c          [color of the marker]
+ */
+void pushArrowMarkerArray(
+  visualization_msgs::msg::MarkerArray * marker_array, const lanelet::ConstLineString3d & ls,
+  const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c);
+
 /**
  * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic
  * lights]
diff --git a/map/lanelet2_extension/lib/visualization.cpp b/map/lanelet2_extension/lib/visualization.cpp
index a95a9aa1e49e0..b846382c99b87 100644
--- a/map/lanelet2_extension/lib/visualization.cpp
+++ b/map/lanelet2_extension/lib/visualization.cpp
@@ -21,6 +21,8 @@
 #include "lanelet2_extension/utility/utilities.hpp"
 
 #include <Eigen/Eigen>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/marker_helper.hpp>
 
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -880,6 +882,7 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
   std::unordered_set<lanelet::Id> added;
   visualization_msgs::msg::Marker left_line_strip, right_line_strip, start_bound_line_strip,
     center_line_strip;
+  visualization_msgs::msg::MarkerArray center_line_arrows;
   visualization::initLineStringMarker(
     &left_line_strip, "map", additional_namespace + "left_lane_bound", c);
   visualization::initLineStringMarker(
@@ -915,6 +918,8 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
     }
     if (viz_centerline && !exists(added, center_ls.id())) {
       visualization::pushLineStringMarker(&center_line_strip, center_ls, c, lss_center);
+      visualization::pushArrowMarkerArray(
+        &center_line_arrows, center_ls, "map", additional_namespace + "center_line_arrows", c);
       added.insert(center_ls.id());
     }
   }
@@ -932,6 +937,9 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra
   if (!start_bound_line_strip.points.empty()) {
     marker_array.markers.push_back(start_bound_line_strip);
   }
+  marker_array.markers.insert(
+    marker_array.markers.end(), center_line_arrows.markers.begin(),
+    center_line_arrows.markers.end());
   return marker_array;
 }
 
@@ -1183,4 +1191,52 @@ void visualization::pushLineStringMarker(
   }
 }
 
+void visualization::pushArrowMarkerArray(
+  visualization_msgs::msg::MarkerArray * marker_array, const lanelet::ConstLineString3d & ls,
+  const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c)
+{
+  if (marker_array == nullptr) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker_array is null pointer!");
+    return;
+  }
+
+  // fill out lane line
+  if (ls.size() < 2) {
+    RCLCPP_ERROR_STREAM(
+      rclcpp::get_logger("lanelet2_extension.visualization"),
+      __FUNCTION__ << ": marker line size is 1 or 0!");
+    return;
+  }
+
+  visualization_msgs::msg::Marker marker;
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = rclcpp::Time();
+  marker.frame_locked = true;
+  marker.ns = ns;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.type = visualization_msgs::msg::Marker::ARROW;
+
+  int32_t start_index = !marker_array->markers.empty() ? marker_array->markers.back().id : 0;
+  for (auto i = ls.begin(); i + 1 != ls.end(); i++) {
+    const auto index = i - ls.begin();
+    marker.id = start_index + index + 1;
+    const auto curr_point =
+      geometry_msgs::build<geometry_msgs::msg::Point>().x((*i).x()).y((*i).y()).z((*i).z());
+    const auto next_point = geometry_msgs::build<geometry_msgs::msg::Point>()
+                              .x((*(i + 1)).x())
+                              .y((*(i + 1)).y())
+                              .z((*(i + 1)).z());
+    marker.pose.position = curr_point;
+
+    const double yaw = tier4_autoware_utils::calcAzimuthAngle(curr_point, next_point);
+    marker.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
+    marker.scale = tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1);
+    marker.color = c;
+
+    marker_array->markers.push_back(marker);
+  }
+}
+
 }  // namespace lanelet