feat: update trafficlight roi styling (autowarefoundation#6985)

Signed-off-by: KhalilSelyan <[email protected]>
Signed-off-by: palas21 <[email protected]>

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
@@ -24,6 +25,12 @@ ament_auto_add_executable(traffic_light_map_visualizer_node
   src/traffic_light_map_visualizer/main.cpp
 )
 
+# Copy the assets directory to the installation directory
+install(
+    DIRECTORY images/
+    DESTINATION share/${PROJECT_NAME}/images
+)
+
 ament_auto_package(
   INSTALL_TO_SHARE
   launch

perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp

@@ -85,6 +85,20 @@ class TrafficLightRoiVisualizerNodelet : public rclcpp::Node
     {tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN, "unknown"},
   };
 
+  std::string extractShapeName(const std::string & label)
+  {
+    size_t start_pos = label.find('-');
+    if (start_pos != std::string::npos) {
+      start_pos++;                                  // Start after the hyphen
+      size_t end_pos = label.find(',', start_pos);  // Find the next comma after the hyphen
+      if (end_pos == std::string::npos) {  // If no comma is found, take the rest of the string
+        end_pos = label.length();
+      }
+      return label.substr(start_pos, end_pos - start_pos);
+    }
+    return "unknown";  // Return "unknown" if no hyphen is found
+  }
+
   bool createRect(
     cv::Mat & image, const tier4_perception_msgs::msg::TrafficLightRoi & tl_roi,
     const cv::Scalar & color);

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

@@ -0,0 +1,51 @@
+// 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 <ament_index_cpp/get_package_share_directory.hpp>
+#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>
+#include <string>
+
+struct DrawFunctionParams
+{
+  cv::Mat & image;
+  cv::Point position;
+  cv::Scalar color;
+  int size;
+  float probability;
+};
+
+using DrawFunction = std::function<void(const DrawFunctionParams & params)>;
+
+void drawShape(
+  const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally,
+  bool flipVertically, int x_offset, int y_offset, double scale_factor = 0.3);
+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 drawUnknown(const DrawFunctionParams & params);
+void drawTrafficLightShape(
+  cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color,
+  int size, float probability);

perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp

@@ -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>
 
@@ -93,29 +95,24 @@ bool TrafficLightRoiVisualizerNodelet::createRect(
 {
   cv::Scalar color;
   if (result.label.find("red") != std::string::npos) {
-    color = cv::Scalar{255, 0, 0};
+    color = cv::Scalar{254, 149, 149};
   } else if (result.label.find("yellow") != std::string::npos) {
-    color = cv::Scalar{0, 255, 0};
+    color = cv::Scalar{254, 250, 149};
   } else if (result.label.find("green") != std::string::npos) {
-    color = cv::Scalar{0, 0, 255};
+    color = cv::Scalar{149, 254, 161};
   } else {
-    color = cv::Scalar{255, 255, 255};
+    color = cv::Scalar{250, 250, 250};
   }
 
   cv::rectangle(
     image, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset),
     cv::Point(tl_roi.roi.x_offset + tl_roi.roi.width, tl_roi.roi.y_offset + tl_roi.roi.height),
-    color, 3);
+    color, 2);
 
-  int offset = 40;
-  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);
+  std::string shape_name = extractShapeName(result.label);
 
-  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);
+  drawTrafficLightShape(
+    image, shape_name, cv::Point(tl_roi.roi.x_offset, tl_roi.roi.y_offset), color, 16, result.prob);
 
   return true;
 }

perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp

@@ -0,0 +1,173 @@
+// 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"
+#include "opencv2/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+
+void drawShape(
+  const DrawFunctionParams & params, const std::string & filename, bool flipHorizontally,
+  bool flipVertically, int x_offset, int y_offset, double scale_factor)
+{
+  std::string filepath =
+    ament_index_cpp::get_package_share_directory("traffic_light_visualization") + "/images/" +
+    filename;
+  cv::Mat shapeImg = cv::imread(filepath, cv::IMREAD_UNCHANGED);
+  if (shapeImg.empty()) {
+    std::cerr << "Failed to load image: " << filepath << std::endl;
+    return;
+  }
+
+  if (flipHorizontally) {
+    cv::flip(shapeImg, shapeImg, 1);  // Flip horizontally
+  }
+
+  if (flipVertically) {
+    cv::flip(shapeImg, shapeImg, 0);  // Flip vertically
+  }
+
+  cv::resize(
+    shapeImg, shapeImg, cv::Size(params.size, params.size), scale_factor, scale_factor,
+    cv::INTER_AREA);
+
+  // Calculate the center position including offsets
+  cv::Point position(
+    params.position.x + x_offset, params.position.y - shapeImg.rows / 2 + y_offset);
+
+  // Check for image boundaries
+  if (
+    position.x < 0 || position.y < 0 || position.x + shapeImg.cols > params.image.cols ||
+    position.y + shapeImg.rows > params.image.rows) {
+    std::cerr << "Adjusted position is out of image bounds." << std::endl;
+    return;
+  }
+
+  // Calculate the width of the text
+  std::string probabilityText =
+    std::to_string(static_cast<int>(round(params.probability * 100))) + "%";
+  int baseline = 0;
+  cv::Size textSize =
+    cv::getTextSize(probabilityText, cv::FONT_HERSHEY_SIMPLEX, 0.75, 2, &baseline);
+
+  // Adjust the filled rectangle to be at the top edge and the correct width
+  int filledRectWidth =
+    shapeImg.cols + (filename != "unknown.png" ? textSize.width + 10 : 5);  // Add some padding
+  int filledRectHeight = shapeImg.rows + 10;                                // Add some padding
+
+  cv::rectangle(
+    params.image, cv::Rect(position.x - 2, position.y - 5, filledRectWidth, filledRectHeight),
+    params.color,
+    -1);  // Filled rectangle
+
+  // Create ROI on the destination image
+  cv::Mat destinationROI = params.image(cv::Rect(position, cv::Size(shapeImg.cols, shapeImg.rows)));
+
+  // Overlay the image onto the main image
+  for (int y = 0; y < shapeImg.rows; ++y) {
+    for (int x = 0; x < shapeImg.cols; ++x) {
+      cv::Vec4b & pixel = shapeImg.at<cv::Vec4b>(y, x);
+      if (pixel[3] != 0) {  // Only non-transparent pixels
+        destinationROI.at<cv::Vec3b>(y, x) = cv::Vec3b(pixel[0], pixel[1], pixel[2]);
+      }
+    }
+  }
+
+  // Position the probability text right next to the shape
+  if (filename != "unknown.png") {
+    cv::putText(
+      params.image, probabilityText,
+      cv::Point(
+        position.x + shapeImg.cols + 5, position.y + shapeImg.rows / 2 + textSize.height / 2),
+      cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);
+  }
+}
+
+void drawCircle(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "circle.png", false, false, 0, -y_offset);
+}
+
+void drawLeftArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "left_arrow.png", false, false, 0, -y_offset);
+}
+
+void drawRightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "left_arrow.png", true, false, 0, -y_offset);
+}
+
+void drawStraightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;  // This adjusts the base position upwards
+
+  drawShape(params, "straight_arrow.png", false, false, 0, -y_offset);
+}
+void drawDownArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;  // This adjusts the base position upwards
+  drawShape(params, "straight_arrow.png", false, true, 0, -y_offset);
+}
+
+void drawDownLeftArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "down_left_arrow.png", false, false, 0, -y_offset);
+}
+
+void drawDownRightArrow(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "down_left_arrow.png", true, false, 0, -y_offset);
+}
+
+void drawCross(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+
+  drawShape(params, "cross.png", false, false, 0, -y_offset);
+}
+
+void drawUnknown(const DrawFunctionParams & params)
+{
+  int y_offset = params.size / 2 + 5;
+  drawShape(params, "unknown.png", false, false, 0, -y_offset);
+}
+
+void drawTrafficLightShape(
+  cv::Mat & image, const std::string & shape, const cv::Point & position, const cv::Scalar & color,
+  int size, float probability)
+{
+  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},
+    {"unknown", drawUnknown}};
+  auto it = shapeToFunction.find(shape);
+  if (it != shapeToFunction.end()) {
+    DrawFunctionParams params{image, position, color, size, probability};
+    it->second(params);
+  } else {
+    std::cerr << "Unknown shape: " << shape << std::endl;
+  }
+}