diff --git a/perception/traffic_light_visualization/CMakeLists.txt b/perception/traffic_light_visualization/CMakeLists.txt index 8b5f54c034c7b..08f6b48cabbd9 100644 --- a/perception/traffic_light_visualization/CMakeLists.txt +++ b/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 diff --git a/perception/traffic_light_visualization/images/circle.png b/perception/traffic_light_visualization/images/circle.png new file mode 100644 index 0000000000000..1f44f7bff39a2 Binary files /dev/null and b/perception/traffic_light_visualization/images/circle.png differ diff --git a/perception/traffic_light_visualization/images/cross.png b/perception/traffic_light_visualization/images/cross.png new file mode 100644 index 0000000000000..9848d3eeab14e Binary files /dev/null and b/perception/traffic_light_visualization/images/cross.png differ diff --git a/perception/traffic_light_visualization/images/down_left_arrow.png b/perception/traffic_light_visualization/images/down_left_arrow.png new file mode 100644 index 0000000000000..0444591a65121 Binary files /dev/null and b/perception/traffic_light_visualization/images/down_left_arrow.png differ diff --git a/perception/traffic_light_visualization/images/left_arrow.png b/perception/traffic_light_visualization/images/left_arrow.png new file mode 100644 index 0000000000000..e48efaa8092e7 Binary files /dev/null and b/perception/traffic_light_visualization/images/left_arrow.png differ diff --git a/perception/traffic_light_visualization/images/straight_arrow.png b/perception/traffic_light_visualization/images/straight_arrow.png new file mode 100644 index 0000000000000..d941d7acbc8f5 Binary files /dev/null and b/perception/traffic_light_visualization/images/straight_arrow.png differ diff --git a/perception/traffic_light_visualization/images/unknown.png b/perception/traffic_light_visualization/images/unknown.png new file mode 100644 index 0000000000000..52393aeeea1fe Binary files /dev/null and b/perception/traffic_light_visualization/images/unknown.png differ diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp index f89b8efa35892..f7ac7471e21ef 100644 --- a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/nodelet.hpp +++ b/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); diff --git a/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp b/perception/traffic_light_visualization/include/traffic_light_visualization/traffic_light_roi_visualizer/shape_draw.hpp new file mode 100644 index 0000000000000..7f83104edf48e --- /dev/null +++ b/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 +#include +#include + +#include +#include + +#include +#include +#include + +struct DrawFunctionParams +{ + cv::Mat & image; + cv::Point position; + cv::Scalar color; + int size; + float probability; +}; + +using DrawFunction = std::function; + +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); diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index 45d6759e6cb7d..df097e64ecd36 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/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 #include @@ -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; } diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/shape_draw.cpp new file mode 100644 index 0000000000000..91a00a940885c --- /dev/null +++ b/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(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(y, x); + if (pixel[3] != 0) { // Only non-transparent pixels + destinationROI.at(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 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; + } +}