What: Added different color feature for detection area, resolves issu…

…e 1121 Signed-off-by: sujithvemi <sujith.3920@gmail.com>
sujithvemi · Nov 29, 2022 · 9e0b7d5 · 9e0b7d5
1 parent a829edd
commit 9e0b7d5
Showing 1 changed file with 29 additions and 5 deletions.
diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp
@@ -36,6 +36,14 @@ namespace
 {
 using DebugData = DetectionAreaModule::DebugData;
 
+struct MarkerColor
+{
+  float r;
+  float g;
+  float b;
+  float a;
+};
+
 lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly)
 {
   lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0};
@@ -55,7 +63,8 @@ geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point)
 }
 
 visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
-  const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now)
+  const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now,
+  MarkerColor marker_color)
 {
   visualization_msgs::msg::MarkerArray msg;
 
@@ -87,7 +96,7 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
     auto marker = createDefaultMarker(
       "map", now, "detection_area_polygon", detection_area_reg_elem.id(),
       visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0),
-      createMarkerColor(0.1, 0.1, 1.0, 0.500));
+      createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a));
     marker.lifetime = rclcpp::Duration::from_seconds(0.5);
 
     for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) {
@@ -113,7 +122,7 @@ visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray(
     auto marker = createDefaultMarker(
       "map", now, "detection_area_correspondence", detection_area_reg_elem.id(),
       visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0),
-      createMarkerColor(0.1, 0.1, 1.0, 0.500));
+      createMarkerColor(marker_color.r, marker_color.g, marker_color.b, marker_color.a));
     marker.lifetime = rclcpp::Duration::from_seconds(0.5);
 
     for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) {
@@ -138,13 +147,28 @@ visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray
   const rclcpp::Time now = clock_->now();
 
   if (!debug_data_.stop_poses.empty()) {
+    MarkerColor marker_color;
+    marker_color.r = 1.0;
+    marker_color.g = 0.0;
+    marker_color.b = 0.0;
+    marker_color.a = 1.0;
+
     appendMarkerArray(
-      createCorrespondenceMarkerArray(detection_area_reg_elem_, now), &wall_marker, now);
+      createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker);
 
     appendMarkerArray(
       debug::createPointsMarkerArray(
-        debug_data_.obstacle_points, "obstalces", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0),
+        debug_data_.obstacle_points, "obstacles", module_id_, now, 0.6, 0.6, 0.6, 1.0, 0.0, 0.0),
       &wall_marker, now);
+  } else {
+    MarkerColor marker_color;
+    marker_color.r = 0.0;
+    marker_color.g = 1.0;
+    marker_color.b = 0.0;
+    marker_color.a = 1.0;
+
+    appendMarkerArray(
+      createCorrespondenceMarkerArray(detection_area_reg_elem_, now, marker_color), &wall_marker);
   }
 
   return wall_marker;