feat: update trafficlight roi styling

perception/traffic_light_visualization/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(OpenCV REQUIRED)
 
 ament_auto_add_library(traffic_light_roi_visualizer_nodelet SHARED
   src/traffic_light_roi_visualizer/nodelet.cpp
+  src/traffic_light_roi_visualizer/shape_draw.cpp
 )
 
 target_link_libraries(traffic_light_roi_visualizer_nodelet

perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp

@@ -0,0 +1,44 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#pragma once
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/imgproc/imgproc_c.h>
+
+#include <functional>
+#include <map>
+
+struct DrawFunctionParams
+{
+  cv::Mat & image;
+  cv::Point position;
+  cv::Scalar color;
+  int size;
+};
+
+using DrawFunction = std::function<void(const DrawFunctionParams & params)>;
+
+void drawCircle(const DrawFunctionParams & params);
+void drawLeftArrow(const DrawFunctionParams & params);
+void drawRightArrow(const DrawFunctionParams & params);
+void drawStraightArrow(const DrawFunctionParams & params);
+void drawDownArrow(const DrawFunctionParams & params);
+void drawDownLeftArrow(const DrawFunctionParams & params);
+void drawDownRightArrow(const DrawFunctionParams & params);
+void drawCross(const DrawFunctionParams & params);
+void drawTrafficLightShape(
+  cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color,
+  int size);

perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -14,6 +14,8 @@
 
 #include "traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp"  // NOLINT(whitespace/line_length)
 
+#include "traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp"  // NOLINT(whitespace/line_length)
+
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 
@@ -95,9 +97,9 @@
   if (result.label.find("red") != std::string::npos) {
     color = cv::Scalar{255, 0, 0};
   } else if (result.label.find("yellow") != std::string::npos) {
-    color = cv::Scalar{0, 255, 0};
+    color = cv::Scalar{255, 255, 0};
   } else if (result.label.find("green") != std::string::npos) {
-    color = cv::Scalar{0, 0, 255};
+    color = cv::Scalar{0, 255, 0};
   } else {
     color = cv::Scalar{255, 255, 255};
   }
@@ -107,15 +109,29 @@
     cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height),
     color, 3);
 
-  int offset = 40;
+  int y_offset = 10;
+  int x_offset = 15;
   cv::putText(
-    image, std::to_string(result.prob),
-    cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset - (offset * 0)), cv::FONT_HERSHEY_COMPLEX,
-    1.1, color, 3);
-
-  cv::putText(
-    image, result.label, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset - (offset * 1)),
-    cv::FONT_HERSHEY_COMPLEX, 1.1, color, 2);
+    image, std::to_string(static_cast<int>(round(result.prob * 100))),
+    cv::Point(
+      tl_roi.roi.x_offset + tl_roi.roi.width + (x_offset * 0.5),
+      tl_roi.roi.y_offset + (y_offset * 0.75)),
+    cv::FONT_HERSHEY_COMPLEX, 0.5, color, 2);
+
+  std::string shape_name = result.label.find('-') != std::string::npos
+                             ? result.label.substr(result.label.find('-') + 1)
+                             : "unknown";
+
+  if (shape_name != "unknown") {
+    drawTrafficLightShape(
+      image, shape_name, cv::Point(tl_roi.roi.x_offset + (x_offset * 1), tl_roi.roi.y_offset),
+      color, 16);
+  } else {
+    cv::putText(
+      image, shape_name,
+      cv::Point(tl_roi.roi.x_offset + (x_offset * 0), tl_roi.roi.y_offset - (y_offset * 0.75)),
+      cv::FONT_HERSHEY_COMPLEX, 0.5, color, 2);
+  }
 
   return true;
 }

perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp

@@ -0,0 +1,124 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp"
+
+#include "opencv2/core/types.hpp"
+
+void drawCircle(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;
+  cv::circle(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset), params.size / 2.0,
+    params.color, -1);  // Filled circle
+}
+
+void drawLeftArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;
+  // Start from the right, go to the left (start x > end x)
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),  // Start point
+    cv::Point(
+      params.position.x - params.size, params.position.y - y_offset),  // End point to the left
+    params.color, 2, 8, 0, 0.3);
+}
+
+void drawRightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;
+  // Start from the left, go to the right (start x < end x)
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),  // Start point
+    cv::Point(
+      params.position.x + params.size, params.position.y - y_offset),  // End point to the right
+    params.color, 2, 8, 0, 0.3);
+}
+
+void drawStraightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;  // This adjusts the base position upwards
+  // Start point is lower, and it goes upwards to a higher (smaller y-value) point
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),  // Start point
+    cv::Point(params.position.x, params.position.y - params.size - y_offset),  // End point
+    params.color, 2, 8, 0, 0.3);
+}
+void drawDownArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;  // This adjusts the base position upwards
+  // Start point is higher, and it goes downwards to a lower (larger y-value) point
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),  // Start point
+    cv::Point(params.position.x, params.position.y + params.size - y_offset),  // End point
+    params.color, 2, 8, 0, 0.3);
+}
+
+void drawDownLeftArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 15;
+  // Down-left arrow
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),
+    cv::Point(params.position.x - params.size, params.position.y + params.size - y_offset),
+    params.color, 2, 8, 0, 0.3);
+}
+
+void drawDownRightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 15;
+  // Down-right arrow
+  cv::arrowedLine(
+    params.image, cv::Point(params.position.x, params.position.y - y_offset),
+    cv::Point(params.position.x + params.size, params.position.y + params.size - y_offset),
+    params.color, 2, 8, 0, 0.3);
+}
+
+void drawCross(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 10;
+  // Cross (two lines intersecting)
+  int half_size = params.size / 2;
+  cv::line(
+    params.image,
+    cv::Point(params.position.x - half_size, params.position.y - half_size - y_offset),
+    cv::Point(params.position.x + half_size, params.position.y + half_size - y_offset),
+    params.color, 2);
+  cv::line(
+    params.image,
+    cv::Point(params.position.x + half_size, params.position.y - half_size - y_offset),
+    cv::Point(params.position.x - half_size, params.position.y + half_size - y_offset),
+    params.color, 2);
+}
+
+void drawTrafficLightShape(
+  cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color,
+  int size)
+{
+  static std::map<std::string, DrawFunction> shapeToFunction = {
+    {"circle", drawCircle},
+    {"left", drawLeftArrow},
+    {"right", drawRightArrow},
+    {"straight", drawStraightArrow},
+    {"down", drawDownArrow},
+    {"down_left", drawDownLeftArrow},
+    {"down_right", drawDownRightArrow},
+    {"cross", drawCross}};
+  auto it = shapeToFunction.find(shape);
+  if (it != shapeToFunction.end()) {
+    DrawFunctionParams params{image, position, color, size};
+    it->second(params);
+  } else {
+    std::cerr << "Unknown shape: " << shape << std::endl;
+  }
+}