diff --git a/perception/traffic_light_classifier/.gitignore b/perception/traffic_light_classifier/.gitignore new file mode 100644 index 000000000000..60baa9cb833f --- /dev/null +++ b/perception/traffic_light_classifier/.gitignore @@ -0,0 +1 @@ +data/* diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt new file mode 100644 index 000000000000..4e591fa478c3 --- /dev/null +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -0,0 +1,184 @@ +cmake_minimum_required(VERSION 3.5) +project(traffic_light_classifier) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Werror) +endif() + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if (CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if (CUDA_VERBOSE) + message(STATUS "CUDA is available!") + message(STATUS "CUDA Libs: ${CUDA_LIBRARIES}") + message(STATUS "CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif () + set(CUDA_AVAIL ON) +else() + message(STATUS "CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif (CUDA_FOUND) + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER NAMES nvinfer) +find_library(NVONNXPARSER nvonnxparser) +find_library(NVINFER_PLUGIN NAMES nvinfer_plugin) +if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) + if (CUDA_VERBOSE) + message(STATUS "TensorRT is available!") + message(STATUS "NVINFER: ${NVINFER}") + message(STATUS "NVPARSERS: ${NVPARSERS}") + message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") + message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") + endif () + set(TRT_AVAIL ON) +else() + message(STATUS "TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY + NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} + PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} + PATH_SUFFIXES lib lib64 bin + DOC "CUDNN library." ) +if(CUDNN_LIBRARY) + if (CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif () + set(CUDNN_AVAIL ON) + +else() + message(STATUS "CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +# Download caffemodel and prototxt +set(PRETRAINED_MODEL_LINK "https://drive.google.com/uc?id=15CQceCn9TZDU6huKJacQvUnDiLHionb3") +set(PRETRAINED_MODEL_HASH 7dc31c696b0400ddfc2cc5521586fa51) +set(LAMP_LABEL_LINK "https://drive.google.com/uc?id=1D7n3oGSWLkWgxET6PcWqEzOhmmPcqM52") +set(LAMP_LABEL_HASH 20167c8e9a1f9d2ec7b0b0088c4100f0) + +find_program(GDOWN_AVAIL "gdown") +if (NOT GDOWN_AVAIL) + message(STATUS "gdown: command not found. External files could not be downloaded.") +endif() +set(PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") +if (NOT EXISTS "${PATH}") + execute_process(COMMAND mkdir -p ${PATH}) +endif() +set(FILE "${PATH}/traffic_light_classifier_mobilenetv2.onnx") +message(STATUS "Checking and downloading traffic_light_classifier_mobilenetv2.onnx") +if (EXISTS "${FILE}") + file(MD5 "${FILE}" EXISTING_FILE_HASH) + if (NOT "${PRETRAINED_MODEL_HASH}" EQUAL "${EXISTING_FILE_HASH}") + message(STATUS "... file hash changed. Downloading now ...") + execute_process(COMMAND gdown --quiet "${PRETRAINED_MODEL_LINK}" -O ${PATH}/traffic_light_classifier_mobilenetv2.onnx) + endif() +else() + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${PRETRAINED_MODEL_LINK}" -O ${PATH}/traffic_light_classifier_mobilenetv2.onnx) +endif() + +set(FILE "${PATH}/lamp_labels.txt") +message(STATUS "Checking and downloading lamp_labels.txt") +if (EXISTS "${FILE}") + file(MD5 "${FILE}" EXISTING_FILE_HASH) + if (NOT "${LAMP_LABEL_HASH}" EQUAL "${EXISTING_FILE_HASH}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${LAMP_LABEL_LINK}" -O ${PATH}/lamp_labels.txt) + endif() +else() + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${LAMP_LABEL_LINK}" -O ${PATH}/lamp_labels.txt) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() +find_package(OpenCV REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem) + +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + add_definitions(-DENABLE_GPU) + + include_directories( + utils + ${OpenCV_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + ) + + ament_auto_add_library(libutils SHARED + utils/trt_common.cpp + ) + target_link_libraries(libutils + ${OpenCV_LIBRARIES} + ${NVINFER} + ${NVONNXPARSER} + ${NVINFER_PLUGIN} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + ${Boost_LIBRARIES} + ) + + ament_auto_add_library(traffic_light_classifier_nodelet SHARED + src/color_classifier.cpp + src/cnn_classifier.cpp + src/nodelet.cpp + ) + target_link_libraries(traffic_light_classifier_nodelet + libutils + ${OpenCV_LIBRARIES} + ) + rclcpp_components_register_node(traffic_light_classifier_nodelet + PLUGIN "traffic_light::TrafficLightClassifierNodelet" + EXECUTABLE traffic_light_classifier_node + ) + + ament_auto_package(INSTALL_TO_SHARE + data + launch + ) + +else() + message(STATUS "CUDA and/or TensorRT were not found. build only color classifier") + + include_directories( + ${OpenCV_INCLUDE_DIRS} + ) + + ament_auto_add_library(traffic_light_classifier_nodelet SHARED + src/color_classifier.cpp + src/nodelet.cpp + ) + target_link_libraries(traffic_light_classifier_nodelet + ${OpenCV_LIBRARIES} + ) + + rclcpp_components_register_node(traffic_light_classifier_nodelet + PLUGIN "traffic_light::TrafficLightClassifierNodelet" + EXECUTABLE traffic_light_classifier_node + ) + + ament_auto_package(INSTALL_TO_SHARE + data + launch + ) + +endif() diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md new file mode 100644 index 000000000000..6b07c73e93b3 --- /dev/null +++ b/perception/traffic_light_classifier/README.md @@ -0,0 +1,131 @@ +# traffic_light_classifier + +## Purpose + +traffic_light_classifier is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. + +## Inner-workings / Algorithms + +### cnn_classifier + +Traffic light labels are classified by MobileNetV2. + +### hsv_classifier + +Traffic light colors (green, yellow and red) are classified in HSV model. + +### About Label + +The message type is designed to comply with the unified road signs proposed at the [Vienna Convention](https://en.wikipedia.org/wiki/Vienna_Convention_on_Road_Signs_and_Signals#Traffic_lights). This idea has been also proposed in [Autoware.Auto](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/merge_requests/16). + +There are rules for naming labels that nodes receive. One traffic light is represented by the following character string separated by commas. `color1-shape1, color2-shape2` . + +For example, the simple red and red cross traffic light label must be expressed as "red-circle, red-cross". + +These colors and shapes are assigned to the message as follows: +![TrafficLightDataStructure.jpg](./image/TrafficLightDataStructure.jpg) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------- | ---------------------------------------------------------- | ---------------------- | +| `~/input/image` | `sensor_msgs::msg::Image` | input image | +| `~/input/rois` | `autoware_auto_perception_msgs::msg::TrafficLightRoiArray` | rois of traffic lights | + +### Output + +| Name | Type | Description | +| -------------------------- | -------------------------------------------------------- | ------------------- | +| `~/output/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | classified signals | +| `~/output/debug/image` | `sensor_msgs::msg::Image` | image for debugging | + +## Parameters + +### Node Parameters + +| Name | Type | Description | +| ----------------- | ---- | ------------------------------------------- | +| `classifier_type` | int | if the value is `1`, cnn_classifier is used | + +### Core Parameters + +#### cnn_classifier + +| Name | Type | Description | +| ----------------- | ---- | ------------------------------------ | +| `model_file_path` | str | path to the model file | +| `label_file_path` | str | path to the label file | +| `precision` | str | TensorRT precision, `fp16` or `int8` | +| `input_c` | str | the channel size of an input image | +| `input_h` | str | the height of an input image | +| `input_w` | str | the width of an input image | + +#### hsv_classifier + +| Name | Type | Description | +| -------------- | ---- | ---------------------------------------------- | +| `green_min_h` | int | the minimum hue of green color | +| `green_min_s` | int | the minimum saturation of green color | +| `green_min_v` | int | the minimum value (brightness) of green color | +| `green_max_h` | int | the maximum hue of green color | +| `green_max_s` | int | the maximum saturation of green color | +| `green_max_v` | int | the maximum value (brightness) of green color | +| `yellow_min_h` | int | the minimum hue of yellow color | +| `yellow_min_s` | int | the minimum saturation of yellow color | +| `yellow_min_v` | int | the minimum value (brightness) of yellow color | +| `yellow_max_h` | int | the maximum hue of yellow color | +| `yellow_max_s` | int | the maximum saturation of yellow color | +| `yellow_max_v` | int | the maximum value (brightness) of yellow color | +| `red_min_h` | int | the minimum hue of red color | +| `red_min_s` | int | the minimum saturation of red color | +| `red_min_v` | int | the minimum value (brightness) of red color | +| `red_max_h` | int | the maximum hue of red color | +| `red_max_s` | int | the maximum saturation of red color | +| `red_max_v` | int | the maximum value (brightness) of red color | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## References/External links + +[1] M. Sandler, A. Howard, M. Zhu, A. Zhmoginov and L. Chen, "MobileNetV2: Inverted Residuals and Linear Bottlenecks," 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition, Salt Lake City, UT, 2018, pp. 4510-4520, doi: 10.1109/CVPR.2018.00474. + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/traffic_light_classifier/cfg/HSVFilter.cfg b/perception/traffic_light_classifier/cfg/HSVFilter.cfg new file mode 100755 index 000000000000..a662704741d2 --- /dev/null +++ b/perception/traffic_light_classifier/cfg/HSVFilter.cfg @@ -0,0 +1,29 @@ +#! /usr/bin/env python + +# set up parameters that we care about +PACKAGE = 'traffic_light_classifier' + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator () +# def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): +gen.add ("green_min_h", int_t, 0, "min h green", 50, 0, 180) +gen.add ("green_max_h", int_t, 0, "max h green", 120, 0, 180) +gen.add ("green_min_s", int_t, 0, "min s green", 100, 0, 255) +gen.add ("green_max_s", int_t, 0, "max s green", 200, 0, 255) +gen.add ("green_min_v", int_t, 0, "min v green", 150, 0, 255) +gen.add ("green_max_v", int_t, 0, "max v green", 255, 0, 255) +gen.add ("yellow_min_h", int_t, 0, "min h yellow", 0, 0, 180) +gen.add ("yellow_max_h", int_t, 0, "max h yellow", 50, 0, 180) +gen.add ("yellow_min_s", int_t, 0, "min s yellow", 80, 0, 255) +gen.add ("yellow_max_s", int_t, 0, "max s yellow", 200, 0, 255) +gen.add ("yellow_min_v", int_t, 0, "min v yellow", 150, 0, 255) +gen.add ("yellow_max_v", int_t, 0, "max v yellow", 255, 0, 255) +gen.add ("red_min_h", int_t, 0, "min h red", 160, 0, 180) +gen.add ("red_max_h", int_t, 0, "max h red", 180, 0, 180) +gen.add ("red_min_s", int_t, 0, "min s red", 100, 0, 255) +gen.add ("red_max_s", int_t, 0, "max s red", 255, 0, 255) +gen.add ("red_min_v", int_t, 0, "min v red", 150, 0, 255) +gen.add ("red_max_v", int_t, 0, "max v red", 255, 0, 255) + +exit (gen.generate (PACKAGE, "traffic_light_classifier", "HSVFilter")) diff --git a/perception/traffic_light_classifier/image/TrafficLightDataStructure.jpg b/perception/traffic_light_classifier/image/TrafficLightDataStructure.jpg new file mode 100644 index 000000000000..52bac72b28d5 Binary files /dev/null and b/perception/traffic_light_classifier/image/TrafficLightDataStructure.jpg differ diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp new file mode 100644 index 000000000000..e60042d2257a --- /dev/null +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/classifier_interface.hpp @@ -0,0 +1,36 @@ +// 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. +// 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. + +#ifndef TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ + +#include +#include + +#include + +#include + +namespace traffic_light +{ +class ClassifierInterface +{ +public: + virtual bool getTrafficSignal( + const cv::Mat & input_image, + autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) = 0; +}; +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_CLASSIFIER__CLASSIFIER_INTERFACE_HPP_ diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp new file mode 100644 index 000000000000..ef91f58310ca --- /dev/null +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/cnn_classifier.hpp @@ -0,0 +1,113 @@ +// 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. +// 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. + +#ifndef TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ + +#include "traffic_light_classifier/classifier_interface.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace traffic_light +{ +class CNNClassifier : public ClassifierInterface +{ +public: + explicit CNNClassifier(rclcpp::Node * node_ptr); + virtual ~CNNClassifier() = default; + + bool getTrafficSignal( + const cv::Mat & input_image, + autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) override; + +private: + void preProcess(cv::Mat & image, std::vector & tensor, bool normalize = true); + bool postProcess( + std::vector & output_data_host, + autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal); + bool readLabelfile(std::string filepath, std::vector & labels); + bool isColorLabel(const std::string label); + void calcSoftmax(std::vector & data, std::vector & probs, int num_output); + std::vector argsort(std::vector & tensor, int num_output); + void outputDebugImage( + cv::Mat & debug_image, + const autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal); + +private: + std::map state2label_{ + // color + {autoware_auto_perception_msgs::msg::TrafficLight::RED, "red"}, + {autoware_auto_perception_msgs::msg::TrafficLight::AMBER, "yellow"}, + {autoware_auto_perception_msgs::msg::TrafficLight::GREEN, "green"}, + {autoware_auto_perception_msgs::msg::TrafficLight::WHITE, "white"}, + // shape + {autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE, "circle"}, + {autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW, "left"}, + {autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW, "right"}, + {autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW, "straight"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW, "down"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW, "down_left"}, + {autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW, "down_right"}, + {autoware_auto_perception_msgs::msg::TrafficLight::CROSS, "cross"}, + // other + {autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN, "unknown"}, + }; + + std::map label2state_{ + // color + {"red", autoware_auto_perception_msgs::msg::TrafficLight::RED}, + {"yellow", autoware_auto_perception_msgs::msg::TrafficLight::AMBER}, + {"green", autoware_auto_perception_msgs::msg::TrafficLight::GREEN}, + {"white", autoware_auto_perception_msgs::msg::TrafficLight::WHITE}, + // shape + {"circle", autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE}, + {"left", autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW}, + {"right", autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW}, + {"straight", autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW}, + {"down", autoware_auto_perception_msgs::msg::TrafficLight::DOWN_ARROW}, + {"down_left", autoware_auto_perception_msgs::msg::TrafficLight::DOWN_LEFT_ARROW}, + {"down_right", autoware_auto_perception_msgs::msg::TrafficLight::DOWN_RIGHT_ARROW}, + {"cross", autoware_auto_perception_msgs::msg::TrafficLight::CROSS}, + // other + {"unknown", autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN}, + }; + + rclcpp::Node * node_ptr_; + + std::shared_ptr trt_; + image_transport::Publisher image_pub_; + std::vector labels_; + std::vector mean_{0.242, 0.193, 0.201}; + std::vector std_{1.0, 1.0, 1.0}; + int input_c_; + int input_h_; + int input_w_; +}; + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_CLASSIFIER__CNN_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp new file mode 100644 index 000000000000..27548c9bd92c --- /dev/null +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/color_classifier.hpp @@ -0,0 +1,94 @@ +// 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. +// 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. + +#ifndef TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ + +#include "traffic_light_classifier/classifier_interface.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace traffic_light +{ +struct HSVConfig +{ + int green_min_h; + int green_min_s; + int green_min_v; + int green_max_h; + int green_max_s; + int green_max_v; + int yellow_min_h; + int yellow_min_s; + int yellow_min_v; + int yellow_max_h; + int yellow_max_s; + int yellow_max_v; + int red_min_h; + int red_min_s; + int red_min_v; + int red_max_h; + int red_max_s; + int red_max_v; +}; + +class ColorClassifier : public ClassifierInterface +{ +public: + explicit ColorClassifier(rclcpp::Node * node_ptr); + virtual ~ColorClassifier() = default; + + bool getTrafficSignal( + const cv::Mat & input_image, + autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) override; + +private: + bool filterHSV( + const cv::Mat & input_image, cv::Mat & green_image, cv::Mat & yellow_image, + cv::Mat & red_image); + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector & parameters); + +private: + enum HSV { + Hue = 0, + Sat = 1, + Val = 2, + }; + image_transport::Publisher image_pub_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rclcpp::Node * node_ptr_; + + HSVConfig hsv_config_; + cv::Scalar min_hsv_green_; + cv::Scalar max_hsv_green_; + cv::Scalar min_hsv_yellow_; + cv::Scalar max_hsv_yellow_; + cv::Scalar min_hsv_red_; + cv::Scalar max_hsv_red_; +}; + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_CLASSIFIER__COLOR_CLASSIFIER_HPP_ diff --git a/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp new file mode 100644 index 000000000000..c04e610eb939 --- /dev/null +++ b/perception/traffic_light_classifier/include/traffic_light_classifier/nodelet.hpp @@ -0,0 +1,89 @@ +// 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. +// 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. + +#ifndef TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ +#define TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ + +#include "traffic_light_classifier/classifier_interface.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#if ENABLE_GPU +#include "traffic_light_classifier/cnn_classifier.hpp" +#endif + +#include "traffic_light_classifier/color_classifier.hpp" + +#include +#include + +namespace traffic_light +{ +class TrafficLightClassifierNodelet : public rclcpp::Node +{ +public: + explicit TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options); + void imageRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & + input_rois_msg); + +private: + enum ClassifierType { + HSVFilter = 0, + CNN = 1, + }; + void connectCb(); + + rclcpp::TimerBase::SharedPtr timer_; + image_transport::SubscriberFilter image_sub_; + message_filters::Subscriber roi_sub_; + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray> + ApproximateSyncPolicy; + typedef message_filters::Synchronizer ApproximateSync; + std::shared_ptr approximate_sync_; + bool is_approximate_sync_; + rclcpp::Publisher::SharedPtr + traffic_signal_array_pub_; + std::shared_ptr classifier_ptr_; +}; + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_CLASSIFIER__NODELET_HPP_ diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml new file mode 100644 index 000000000000..eb2d2951cf3d --- /dev/null +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/traffic_light_classifier/package.xml b/perception/traffic_light_classifier/package.xml new file mode 100644 index 000000000000..6f5bf6146b99 --- /dev/null +++ b/perception/traffic_light_classifier/package.xml @@ -0,0 +1,25 @@ + + + traffic_light_classifier + 0.1.0 + The traffic_light_classifier package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + wget + + autoware_auto_perception_msgs + cv_bridge + image_transport + libboost-filesystem-dev + message_filters + rclcpp + rclcpp_components + sensor_msgs + + + ament_cmake + + diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/cnn_classifier.cpp new file mode 100644 index 000000000000..40f1757e6329 --- /dev/null +++ b/perception/traffic_light_classifier/src/cnn_classifier.cpp @@ -0,0 +1,265 @@ +// 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. +// 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_classifier/cnn_classifier.hpp" + +#include + +#include +#include + +#include +#include +#include + +namespace traffic_light +{ +CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) +{ + image_pub_ = image_transport::create_publisher( + node_ptr_, "~/output/debug/image", rclcpp::QoS{1}.get_rmw_qos_profile()); + + std::string precision; + std::string label_file_path; + std::string model_file_path; + precision = node_ptr_->declare_parameter("precision", "fp16"); + label_file_path = node_ptr_->declare_parameter("label_file_path", "labels.txt"); + model_file_path = node_ptr_->declare_parameter("model_file_path", "model.onnx"); + input_c_ = node_ptr_->declare_parameter("input_c", 3); + input_h_ = node_ptr_->declare_parameter("input_h", 224); + input_w_ = node_ptr_->declare_parameter("input_w", 224); + + readLabelfile(label_file_path, labels_); + + std::string cache_dir = + ament_index_cpp::get_package_share_directory("traffic_light_classifier") + "/data"; + trt_ = std::make_shared(model_file_path, cache_dir, precision); + trt_->setup(); +} + +bool CNNClassifier::getTrafficSignal( + const cv::Mat & input_image, autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) +{ + if (!trt_->isInitialized()) { + RCLCPP_WARN(node_ptr_->get_logger(), "failed to init tensorrt"); + return false; + } + + int num_input = trt_->getNumInput(); + int num_output = trt_->getNumOutput(); + + std::vector input_data_host(num_input); + + cv::Mat image = input_image.clone(); + preProcess(image, input_data_host, true); + + auto input_data_device = Tn::make_unique(num_input); + cudaMemcpy( + input_data_device.get(), input_data_host.data(), num_input * sizeof(float), + cudaMemcpyHostToDevice); + + auto output_data_device = Tn::make_unique(num_output); + + // do inference + std::vector bindings = {input_data_device.get(), output_data_device.get()}; + + trt_->context_->executeV2(bindings.data()); + + std::vector output_data_host(num_output); + cudaMemcpy( + output_data_host.data(), output_data_device.get(), num_output * sizeof(float), + cudaMemcpyDeviceToHost); + + postProcess(output_data_host, traffic_signal); + + /* debug */ + if (0 < image_pub_.getNumSubscribers()) { + cv::Mat debug_image = input_image.clone(); + outputDebugImage(debug_image, traffic_signal); + } + + return true; +} + +void CNNClassifier::outputDebugImage( + cv::Mat & debug_image, const autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) +{ + float probability; + std::string label; + for (std::size_t i = 0; i < traffic_signal.lights.size(); i++) { + auto light = traffic_signal.lights.at(i); + const auto light_label = state2label_[light.color] + "-" + state2label_[light.shape]; + label += light_label; + // all lamp confidence are the same + probability = light.confidence; + if (i < traffic_signal.lights.size() - 1) { + label += ","; + } + } + + const int expand_w = 200; + const int expand_h = + std::max(static_cast((expand_w * debug_image.rows) / debug_image.cols), 1); + + cv::resize(debug_image, debug_image, cv::Size(expand_w, expand_h)); + cv::Mat text_img(cv::Size(expand_w, 50), CV_8UC3, cv::Scalar(0, 0, 0)); + std::string text = label + " " + std::to_string(probability); + cv::putText( + text_img, text, cv::Point(5, 25), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(0, 255, 0), 1); + cv::vconcat(debug_image, text_img, debug_image); + + const auto debug_image_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "rgb8", debug_image).toImageMsg(); + image_pub_.publish(debug_image_msg); +} + +void CNNClassifier::preProcess(cv::Mat & image, std::vector & input_tensor, bool normalize) +{ + /* normalize */ + /* ((channel[0] / 255) - mean[0]) / std[0] */ + + // cv::cvtColor(image, image, cv::COLOR_BGR2RGB, 3); + cv::resize(image, image, cv::Size(input_w_, input_h_)); + + const size_t strides_cv[3] = { + static_cast(input_w_ * input_c_), static_cast(input_c_), 1}; + const size_t strides[3] = { + static_cast(input_h_ * input_w_), static_cast(input_w_), 1}; + + for (int i = 0; i < input_h_; i++) { + for (int j = 0; j < input_w_; j++) { + for (int k = 0; k < input_c_; k++) { + const size_t offset_cv = i * strides_cv[0] + j * strides_cv[1] + k * strides_cv[2]; + const size_t offset = k * strides[0] + i * strides[1] + j * strides[2]; + if (normalize) { + input_tensor[offset] = + ((static_cast(image.data[offset_cv]) / 255) - mean_[k]) / std_[k]; + } else { + input_tensor[offset] = static_cast(image.data[offset_cv]); + } + } + } + } +} + +bool CNNClassifier::postProcess( + std::vector & output_tensor, + autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) +{ + std::vector probs; + int num_output = trt_->getNumOutput(); + calcSoftmax(output_tensor, probs, num_output); + std::vector sorted_indices = argsort(output_tensor, num_output); + + // ROS_INFO("label: %s, score: %.2f\%", + // labels_[sorted_indices[0]].c_str(), + // probs[sorted_indices[0]] * 100); + + std::string match_label = labels_[sorted_indices[0]]; + float probability = probs[sorted_indices[0]]; + + // label names are assumed to be comma-separated to represent each lamp + // e.g. + // match_label: "red","red-cross","right" + // split_label: ["red","red-cross","right"] + // if shape doesn't have color suffix, set GREEN to color state. + // if color doesn't have shape suffix, set CIRCLE to shape state. + std::vector split_label; + boost::algorithm::split(split_label, match_label, boost::is_any_of(",")); + for (auto label : split_label) { + if (label2state_.find(label) == label2state_.end()) { + RCLCPP_DEBUG( + node_ptr_->get_logger(), "cnn_classifier does not have a key [%s]", label.c_str()); + continue; + } + autoware_auto_perception_msgs::msg::TrafficLight light; + if (label.find("-") != std::string::npos) { + // found "-" delimiter in label string + std::vector color_and_shape; + boost::algorithm::split(color_and_shape, label, boost::is_any_of("-")); + light.color = label2state_[color_and_shape.at(0)]; + light.shape = label2state_[color_and_shape.at(1)]; + } else { + if (label == state2label_[autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN]) { + light.color = autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + light.shape = autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + } else if (isColorLabel(label)) { + light.color = label2state_[label]; + light.shape = autoware_auto_perception_msgs::msg::TrafficLight::CIRCLE; + } else { + light.color = autoware_auto_perception_msgs::msg::TrafficLight::GREEN; + light.shape = label2state_[label]; + } + } + light.confidence = probability; + traffic_signal.lights.push_back(light); + } + + return true; +} + +bool CNNClassifier::readLabelfile(std::string filepath, std::vector & labels) +{ + std::ifstream labelsFile(filepath); + if (!labelsFile.is_open()) { + RCLCPP_ERROR(node_ptr_->get_logger(), "Could not open label file. [%s]", filepath.c_str()); + return false; + } + std::string label; + while (getline(labelsFile, label)) { + labels.push_back(label); + } + return true; +} + +void CNNClassifier::calcSoftmax( + std::vector & data, std::vector & probs, int num_output) +{ + float exp_sum = 0.0; + for (int i = 0; i < num_output; ++i) { + exp_sum += exp(data[i]); + } + + for (int i = 0; i < num_output; ++i) { + probs.push_back(exp(data[i]) / exp_sum); + } +} + +std::vector CNNClassifier::argsort(std::vector & tensor, int num_output) +{ + std::vector indices(num_output); + for (int i = 0; i < num_output; i++) { + indices[i] = i; + } + std::sort(indices.begin(), indices.begin() + num_output, [tensor](size_t idx1, size_t idx2) { + return tensor[idx1] > tensor[idx2]; + }); + + return indices; +} + +bool CNNClassifier::isColorLabel(const std::string label) +{ + using autoware_auto_perception_msgs::msg::TrafficSignal; + if ( + label == state2label_[autoware_auto_perception_msgs::msg::TrafficLight::GREEN] || + label == state2label_[autoware_auto_perception_msgs::msg::TrafficLight::AMBER] || + label == state2label_[autoware_auto_perception_msgs::msg::TrafficLight::RED] || + label == state2label_[autoware_auto_perception_msgs::msg::TrafficLight::WHITE]) { + return true; + } + return false; +} + +} // namespace traffic_light diff --git a/perception/traffic_light_classifier/src/color_classifier.cpp b/perception/traffic_light_classifier/src/color_classifier.cpp new file mode 100644 index 000000000000..0267b3cd3c3a --- /dev/null +++ b/perception/traffic_light_classifier/src/color_classifier.cpp @@ -0,0 +1,248 @@ +// 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. +// 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_classifier/color_classifier.hpp" + +#include + +#include +#include +#include + +namespace traffic_light +{ +ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) +{ + using std::placeholders::_1; + image_pub_ = image_transport::create_publisher( + node_ptr_, "~/debug/image", rclcpp::QoS{1}.get_rmw_qos_profile()); + + hsv_config_.green_min_h = node_ptr_->declare_parameter("green_min_h", 50); + hsv_config_.green_min_s = node_ptr_->declare_parameter("green_min_s", 100); + hsv_config_.green_min_v = node_ptr_->declare_parameter("green_min_v", 150); + hsv_config_.green_max_h = node_ptr_->declare_parameter("green_max_h", 120); + hsv_config_.green_max_s = node_ptr_->declare_parameter("green_max_s", 200); + hsv_config_.green_max_v = node_ptr_->declare_parameter("green_max_v", 255); + hsv_config_.yellow_min_h = node_ptr_->declare_parameter("yellow_min_h", 0); + hsv_config_.yellow_min_s = node_ptr_->declare_parameter("yellow_min_s", 80); + hsv_config_.yellow_min_v = node_ptr_->declare_parameter("yellow_min_v", 150); + hsv_config_.yellow_max_h = node_ptr_->declare_parameter("yellow_max_h", 50); + hsv_config_.yellow_max_s = node_ptr_->declare_parameter("yellow_max_s", 200); + hsv_config_.yellow_max_v = node_ptr_->declare_parameter("yellow_max_v", 255); + hsv_config_.red_min_h = node_ptr_->declare_parameter("red_min_h", 160); + hsv_config_.red_min_s = node_ptr_->declare_parameter("red_min_s", 100); + hsv_config_.red_min_v = node_ptr_->declare_parameter("red_min_v", 150); + hsv_config_.red_max_h = node_ptr_->declare_parameter("red_max_h", 180); + hsv_config_.red_max_s = node_ptr_->declare_parameter("red_max_s", 255); + hsv_config_.red_max_v = node_ptr_->declare_parameter("red_max_v", 255); + + // set parameter callback + set_param_res_ = node_ptr_->add_on_set_parameters_callback( + std::bind(&ColorClassifier::parametersCallback, this, _1)); +} + +bool ColorClassifier::getTrafficSignal( + const cv::Mat & input_image, autoware_auto_perception_msgs::msg::TrafficSignal & traffic_signal) +{ + cv::Mat green_image; + cv::Mat yellow_image; + cv::Mat red_image; + filterHSV(input_image, green_image, yellow_image, red_image); + // binarize + cv::Mat green_bin_image; + cv::Mat yellow_bin_image; + cv::Mat red_bin_image; + const int bin_threshold = 127; + cv::threshold(green_image, green_bin_image, bin_threshold, 255, cv::THRESH_BINARY); + cv::threshold(yellow_image, yellow_bin_image, bin_threshold, 255, cv::THRESH_BINARY); + cv::threshold(red_image, red_bin_image, bin_threshold, 255, cv::THRESH_BINARY); + // filter noise + cv::Mat green_filtered_bin_image; + cv::Mat yellow_filtered_bin_image; + cv::Mat red_filtered_bin_image; + cv::Mat element4 = (cv::Mat_(3, 3) << 0, 1, 0, 1, 1, 1, 0, 1, 0); + cv::erode(green_bin_image, green_filtered_bin_image, element4, cv::Point(-1, -1), 1); + cv::erode(yellow_bin_image, yellow_filtered_bin_image, element4, cv::Point(-1, -1), 1); + cv::erode(red_bin_image, red_filtered_bin_image, element4, cv::Point(-1, -1), 1); + cv::dilate(green_filtered_bin_image, green_filtered_bin_image, cv::Mat(), cv::Point(-1, -1), 1); + cv::dilate(yellow_filtered_bin_image, yellow_filtered_bin_image, cv::Mat(), cv::Point(-1, -1), 1); + cv::dilate(red_filtered_bin_image, red_filtered_bin_image, cv::Mat(), cv::Point(-1, -1), 1); + + /* debug */ +#if 1 + if (0 < image_pub_.getNumSubscribers()) { + cv::Mat debug_raw_image; + cv::Mat debug_green_image; + cv::Mat debug_yellow_image; + cv::Mat debug_red_image; + cv::hconcat(input_image, input_image, debug_raw_image); + cv::hconcat(debug_raw_image, input_image, debug_raw_image); + cv::hconcat(green_image, green_bin_image, debug_green_image); + cv::hconcat(debug_green_image, green_filtered_bin_image, debug_green_image); + cv::hconcat(yellow_image, yellow_bin_image, debug_yellow_image); + cv::hconcat(debug_yellow_image, yellow_filtered_bin_image, debug_yellow_image); + cv::hconcat(red_image, red_bin_image, debug_red_image); + cv::hconcat(debug_red_image, red_filtered_bin_image, debug_red_image); + + cv::Mat debug_image; + cv::vconcat(debug_green_image, debug_yellow_image, debug_image); + cv::vconcat(debug_image, debug_red_image, debug_image); + cv::cvtColor(debug_image, debug_image, cv::COLOR_GRAY2RGB); + cv::vconcat(debug_raw_image, debug_image, debug_image); + const int width = input_image.cols; + const int height = input_image.rows; + cv::line( + debug_image, cv::Point(0, 0), cv::Point(debug_image.cols, 0), cv::Scalar(255, 255, 255), 1, + CV_AA, 0); + cv::line( + debug_image, cv::Point(0, height), cv::Point(debug_image.cols, height), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + cv::line( + debug_image, cv::Point(0, height * 2), cv::Point(debug_image.cols, height * 2), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + cv::line( + debug_image, cv::Point(0, height * 3), cv::Point(debug_image.cols, height * 3), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + + cv::line( + debug_image, cv::Point(0, 0), cv::Point(0, debug_image.rows), cv::Scalar(255, 255, 255), 1, + CV_AA, 0); + cv::line( + debug_image, cv::Point(width, 0), cv::Point(width, debug_image.rows), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + cv::line( + debug_image, cv::Point(width * 2, 0), cv::Point(width * 2, debug_image.rows), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + cv::line( + debug_image, cv::Point(width * 3, 0), cv::Point(width * 3, debug_image.rows), + cv::Scalar(255, 255, 255), 1, CV_AA, 0); + + cv::putText( + debug_image, "green", cv::Point(0, height * 1.5), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(255, 255, 255), 1, CV_AA); + cv::putText( + debug_image, "yellow", cv::Point(0, height * 2.5), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(255, 255, 255), 1, CV_AA); + cv::putText( + debug_image, "red", cv::Point(0, height * 3.5), cv::FONT_HERSHEY_SIMPLEX, 1.0, + cv::Scalar(255, 255, 255), 1, CV_AA); + const auto debug_image_msg = + cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", debug_image).toImageMsg(); + image_pub_.publish(debug_image_msg); + } +#endif + /* --- */ + + const int green_pixel_num = cv::countNonZero(green_filtered_bin_image); + const int yellow_pixel_num = cv::countNonZero(yellow_filtered_bin_image); + const int red_pixel_num = cv::countNonZero(red_filtered_bin_image); + const double green_ratio = + static_cast(green_pixel_num) / + static_cast(green_filtered_bin_image.rows * green_filtered_bin_image.cols); + const double yellow_ratio = + static_cast(yellow_pixel_num) / + static_cast(yellow_filtered_bin_image.rows * yellow_filtered_bin_image.cols); + const double red_ratio = + static_cast(red_pixel_num) / + static_cast(red_filtered_bin_image.rows * red_filtered_bin_image.cols); + + if (yellow_ratio < green_ratio && red_ratio < green_ratio) { + autoware_auto_perception_msgs::msg::TrafficLight light; + light.color = autoware_auto_perception_msgs::msg::TrafficLight::GREEN; + light.confidence = std::min(1.0, static_cast(green_pixel_num) / (20.0 * 20.0)); + traffic_signal.lights.push_back(light); + } else if (green_ratio < yellow_ratio && red_ratio < yellow_ratio) { + autoware_auto_perception_msgs::msg::TrafficLight light; + light.color = autoware_auto_perception_msgs::msg::TrafficLight::AMBER; + light.confidence = std::min(1.0, static_cast(yellow_pixel_num) / (20.0 * 20.0)); + traffic_signal.lights.push_back(light); + } else if (green_ratio < red_ratio && yellow_ratio < red_ratio) { + autoware_auto_perception_msgs::msg::TrafficLight light; + light.color = ::autoware_auto_perception_msgs::msg::TrafficLight::RED; + light.confidence = std::min(1.0, static_cast(red_pixel_num) / (20.0 * 20.0)); + traffic_signal.lights.push_back(light); + } else { + autoware_auto_perception_msgs::msg::TrafficLight light; + light.color = ::autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN; + light.confidence = 0.0; + traffic_signal.lights.push_back(light); + } + return true; +} + +bool ColorClassifier::filterHSV( + const cv::Mat & input_image, cv::Mat & green_image, cv::Mat & yellow_image, cv::Mat & red_image) +{ + cv::Mat hsv_image; + cv::cvtColor(input_image, hsv_image, cv::COLOR_BGR2HSV); + try { + cv::inRange(hsv_image, min_hsv_green_, max_hsv_green_, green_image); + cv::inRange(hsv_image, min_hsv_yellow_, max_hsv_yellow_, yellow_image); + cv::inRange(hsv_image, min_hsv_red_, max_hsv_red_, red_image); + } catch (cv::Exception & e) { + RCLCPP_ERROR(node_ptr_->get_logger(), "failed to filter image by hsv value : %s", e.what()); + return false; + } + return true; +} +rcl_interfaces::msg::SetParametersResult ColorClassifier::parametersCallback( + const std::vector & parameters) +{ + auto update_param = [&](const std::string & name, int & v) { + auto it = std::find_if( + parameters.cbegin(), parameters.cend(), + [&name](const rclcpp::Parameter & parameter) { return parameter.get_name() == name; }); + if (it != parameters.cend()) { + v = it->as_int(); + return true; + } + return false; + }; + + update_param("green_min_h", hsv_config_.green_min_h); + update_param("green_min_s", hsv_config_.green_min_s); + update_param("green_min_v", hsv_config_.green_min_v); + update_param("green_max_h", hsv_config_.green_max_h); + update_param("green_max_s", hsv_config_.green_max_s); + update_param("green_max_v", hsv_config_.green_max_v); + update_param("yellow_min_h", hsv_config_.yellow_min_h); + update_param("yellow_min_s", hsv_config_.yellow_min_s); + update_param("yellow_min_v", hsv_config_.yellow_min_v); + update_param("yellow_max_h", hsv_config_.yellow_max_h); + update_param("yellow_max_s", hsv_config_.yellow_max_s); + update_param("yellow_max_v", hsv_config_.yellow_max_v); + update_param("red_min_h", hsv_config_.red_min_h); + update_param("red_min_s", hsv_config_.red_min_s); + update_param("red_min_v", hsv_config_.red_min_v); + update_param("red_max_h", hsv_config_.red_max_h); + update_param("red_max_s", hsv_config_.red_max_s); + update_param("red_max_v", hsv_config_.red_max_v); + + min_hsv_green_ = + cv::Scalar(hsv_config_.green_min_h, hsv_config_.green_min_s, hsv_config_.green_min_v); + max_hsv_green_ = + cv::Scalar(hsv_config_.green_max_h, hsv_config_.green_max_s, hsv_config_.green_max_v); + min_hsv_yellow_ = + cv::Scalar(hsv_config_.yellow_min_h, hsv_config_.yellow_min_s, hsv_config_.yellow_min_v); + max_hsv_yellow_ = + cv::Scalar(hsv_config_.yellow_max_h, hsv_config_.yellow_max_s, hsv_config_.yellow_max_v); + min_hsv_red_ = cv::Scalar(hsv_config_.red_min_h, hsv_config_.red_min_s, hsv_config_.red_min_v); + max_hsv_red_ = cv::Scalar(hsv_config_.red_max_h, hsv_config_.red_max_s, hsv_config_.red_max_v); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +} // namespace traffic_light diff --git a/perception/traffic_light_classifier/src/nodelet.cpp b/perception/traffic_light_classifier/src/nodelet.cpp new file mode 100644 index 000000000000..768407be3964 --- /dev/null +++ b/perception/traffic_light_classifier/src/nodelet.cpp @@ -0,0 +1,122 @@ +// 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. +// 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_classifier/nodelet.hpp" + +#include +#include +#include +#include + +namespace traffic_light +{ +TrafficLightClassifierNodelet::TrafficLightClassifierNodelet(const rclcpp::NodeOptions & options) +: Node("traffic_light_classifier_node", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + if (is_approximate_sync_) { + approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); + approximate_sync_->registerCallback( + std::bind(&TrafficLightClassifierNodelet::imageRoiCallback, this, _1, _2)); + } else { + sync_.reset(new Sync(SyncPolicy(10), image_sub_, roi_sub_)); + sync_->registerCallback( + std::bind(&TrafficLightClassifierNodelet::imageRoiCallback, this, _1, _2)); + } + + traffic_signal_array_pub_ = + this->create_publisher( + "~/output/traffic_signals", rclcpp::QoS{1}); + + // + auto timer_callback = std::bind(&TrafficLightClassifierNodelet::connectCb, this); + const auto period_s = 0.1; + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + int classifier_type = this->declare_parameter( + "classifier_type", static_cast(TrafficLightClassifierNodelet::ClassifierType::HSVFilter)); + if (classifier_type == TrafficLightClassifierNodelet::ClassifierType::HSVFilter) { + classifier_ptr_ = std::make_shared(this); + } else if (classifier_type == TrafficLightClassifierNodelet::ClassifierType::CNN) { +#if ENABLE_GPU + classifier_ptr_ = std::make_shared(this); +#else + RCLCPP_ERROR( + this->get_logger(), "please install CUDA, CUDNN and TensorRT to use cnn classifier"); +#endif + } +} + +void TrafficLightClassifierNodelet::connectCb() +{ + // set callbacks only when there are subscribers to this node + if ( + traffic_signal_array_pub_->get_subscription_count() == 0 && + traffic_signal_array_pub_->get_intra_process_subscription_count() == 0) { + image_sub_.unsubscribe(); + roi_sub_.unsubscribe(); + } else if (!image_sub_.getSubscriber()) { + image_sub_.subscribe(this, "~/input/image", "raw", rmw_qos_profile_sensor_data); + roi_sub_.subscribe(this, "~/input/rois", rclcpp::QoS{1}.get_rmw_qos_profile()); + } +} + +void TrafficLightClassifierNodelet::imageRoiCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr & input_rois_msg) +{ + if (classifier_ptr_.use_count() == 0) { + return; + } + + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR( + this->get_logger(), "Could not convert from '%s' to 'rgb8'.", + input_image_msg->encoding.c_str()); + } + + autoware_auto_perception_msgs::msg::TrafficSignalArray output_msg; + + for (size_t i = 0; i < input_rois_msg->rois.size(); ++i) { + const sensor_msgs::msg::RegionOfInterest & roi = input_rois_msg->rois.at(i).roi; + cv::Mat clipped_image( + cv_ptr->image, cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height)); + + autoware_auto_perception_msgs::msg::TrafficSignal traffic_signal; + traffic_signal.map_primitive_id = input_rois_msg->rois.at(i).id; + if (!classifier_ptr_->getTrafficSignal(clipped_image, traffic_signal)) { + RCLCPP_ERROR(this->get_logger(), "failed classify image, abort callback"); + return; + } + output_msg.signals.push_back(traffic_signal); + } + + output_msg.header = input_image_msg->header; + traffic_signal_array_pub_->publish(output_msg); +} + +} // namespace traffic_light + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightClassifierNodelet) diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp new file mode 100644 index 000000000000..6e10d7d71ab5 --- /dev/null +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -0,0 +1,149 @@ +// 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. +// 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 + +#include +#include + +namespace Tn +{ +void check_error(const ::cudaError_t e, decltype(__FILE__) f, decltype(__LINE__) n) +{ + if (e != ::cudaSuccess) { + std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw std::runtime_error{s.str()}; + } +} + +TrtCommon::TrtCommon(std::string model_path, std::string cache_dir, std::string precision) +: model_file_path_(model_path), + cache_dir_(cache_dir), + precision_(precision), + input_name_("input_0"), + output_name_("output_0"), + is_initialized_(false), + max_batch_size_(1) +{ +} + +void TrtCommon::setup() +{ + const boost::filesystem::path path(model_file_path_); + std::string extension = path.extension().string(); + + if (boost::filesystem::exists(path)) { + if (extension == ".engine") { + loadEngine(model_file_path_); + } else if (extension == ".onnx") { + std::string cache_engine_path = cache_dir_ + "/" + path.stem().string() + ".engine"; + const boost::filesystem::path cache_path(cache_engine_path); + if (boost::filesystem::exists(cache_path)) { + loadEngine(cache_engine_path); + } else { + logger_.log(nvinfer1::ILogger::Severity::kINFO, "start build engine"); + buildEngineFromOnnx(model_file_path_, cache_engine_path); + logger_.log(nvinfer1::ILogger::Severity::kINFO, "end build engine"); + } + } else { + is_initialized_ = false; + return; + } + } else { + is_initialized_ = false; + return; + } + + context_ = UniquePtr(engine_->createExecutionContext()); + input_dims_ = engine_->getBindingDimensions(getInputBindingIndex()); + output_dims_ = engine_->getBindingDimensions(getOutputBindingIndex()); + is_initialized_ = true; +} + +bool TrtCommon::loadEngine(std::string engine_file_path) +{ + std::ifstream engine_file(engine_file_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); + runtime_ = UniquePtr(nvinfer1::createInferRuntime(logger_)); + engine_ = UniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast(engine_str.data()), engine_str.size(), nullptr)); + return true; +} + +bool TrtCommon::buildEngineFromOnnx(std::string onnx_file_path, std::string output_engine_file_path) +{ + auto builder = UniquePtr(nvinfer1::createInferBuilder(logger_)); + const auto explicitBatch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = UniquePtr(builder->createNetworkV2(explicitBatch)); + auto config = UniquePtr(builder->createBuilderConfig()); + + auto parser = UniquePtr(nvonnxparser::createParser(*network, logger_)); + if (!parser->parseFromFile( + onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { + return false; + } + + builder->setMaxBatchSize(max_batch_size_); + config->setMaxWorkspaceSize(16 << 20); + + if (precision_ == "fp16") { + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } else if (precision_ == "int8") { + config->setFlag(nvinfer1::BuilderFlag::kINT8); + } else { + return false; + } + + engine_ = UniquePtr(builder->buildEngineWithConfig(*network, *config)); + if (!engine_) { + return false; + } + + // save engine + nvinfer1::IHostMemory * data = engine_->serialize(); + std::ofstream file; + file.open(output_engine_file_path, std::ios::binary | std::ios::out); + if (!file.is_open()) { + return false; + } + file.write((const char *)data->data(), data->size()); + file.close(); + + return true; +} + +bool TrtCommon::isInitialized() { return is_initialized_; } + +int TrtCommon::getNumInput() +{ + return std::accumulate( + input_dims_.d, input_dims_.d + input_dims_.nbDims, 1, std::multiplies()); +} + +int TrtCommon::getNumOutput() +{ + return std::accumulate( + output_dims_.d, output_dims_.d + output_dims_.nbDims, 1, std::multiplies()); +} + +int TrtCommon::getInputBindingIndex() { return engine_->getBindingIndex(input_name_.c_str()); } + +int TrtCommon::getOutputBindingIndex() { return engine_->getBindingIndex(output_name_.c_str()); } + +} // namespace Tn diff --git a/perception/traffic_light_classifier/utils/trt_common.hpp b/perception/traffic_light_classifier/utils/trt_common.hpp new file mode 100644 index 000000000000..269f7b11c671 --- /dev/null +++ b/perception/traffic_light_classifier/utils/trt_common.hpp @@ -0,0 +1,146 @@ +// 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. +// 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. + +#ifndef PERCEPTION__TRAFFIC_LIGHT_RECOGNITION__TRAFFIC_LIGHT_CLASSIFIER__UTILS__TRT_COMMON_HPP_ +#define PERCEPTION__TRAFFIC_LIGHT_RECOGNITION__TRAFFIC_LIGHT_CLASSIFIER__UTILS__TRT_COMMON_HPP_ + +#include +#include + +#include + +#include <./cudnn.h> +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (Tn::check_error(e, __FILE__, __LINE__)) + +namespace Tn +{ +class Logger : public nvinfer1::ILogger +{ +public: + Logger() : Logger(Severity::kINFO) {} + + explicit Logger(Severity severity) : reportableSeverity(severity) {} + + void log(Severity severity, const char * msg) noexcept override + { + // suppress messages with severity enum value greater than the reportable + if (severity > reportableSeverity) { + return; + } + + switch (severity) { + case Severity::kINTERNAL_ERROR: + std::cerr << "[TRT_COMMON][INTERNAL_ERROR]: "; + break; + case Severity::kERROR: + std::cerr << "[TRT_COMMON][ERROR]: "; + break; + case Severity::kWARNING: + std::cerr << "[TRT_COMMON][WARNING]: "; + break; + case Severity::kINFO: + std::cerr << "[TRT_COMMON][INFO]: "; + break; + default: + std::cerr << "[TRT_COMMON][UNKNOWN]: "; + break; + } + std::cerr << msg << std::endl; + } + + Severity reportableSeverity{Severity::kWARNING}; +}; + +void check_error(const ::cudaError_t e, decltype(__FILE__) f, decltype(__LINE__) n); + +struct InferDeleter +{ + void operator()(void * p) const { ::cudaFree(p); } +}; + +template +using UniquePtr = std::unique_ptr; + +// auto array = Tn::make_unique(n); +// ::cudaMemcpy(array.get(), src_array, sizeof(float)*n, ::cudaMemcpyHostToDevice); +template +typename std::enable_if::value, Tn::UniquePtr>::type make_unique( + const std::size_t n) +{ + using U = typename std::remove_extent::type; + U * p; + ::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n); + return Tn::UniquePtr{p}; +} + +// auto value = Tn::make_unique(); +// ::cudaMemcpy(value.get(), src_value, sizeof(my_class), ::cudaMemcpyHostToDevice); +template +Tn::UniquePtr make_unique() +{ + T * p; + ::cudaMalloc(reinterpret_cast(&p), sizeof(T)); + return Tn::UniquePtr{p}; +} + +class TrtCommon +{ +public: + TrtCommon(std::string model_path, std::string cache_dir, std::string precision); + ~TrtCommon() {} + + bool loadEngine(std::string engine_file_path); + bool buildEngineFromOnnx(std::string onnx_file_path, std::string output_engine_file_path); + void setup(); + + bool isInitialized(); + int getNumInput(); + int getNumOutput(); + int getInputBindingIndex(); + int getOutputBindingIndex(); + + UniquePtr context_; + +private: + Logger logger_; + std::string model_file_path_; + UniquePtr runtime_; + UniquePtr engine_; + + nvinfer1::Dims input_dims_; + nvinfer1::Dims output_dims_; + std::string cache_dir_; + std::string precision_; + std::string input_name_; + std::string output_name_; + bool is_initialized_; + size_t max_batch_size_; +}; + +} // namespace Tn + +#endif // PERCEPTION__TRAFFIC_LIGHT_RECOGNITION__TRAFFIC_LIGHT_CLASSIFIER__UTILS__TRT_COMMON_HPP_ diff --git a/perception/traffic_light_map_based_detector/CMakeLists.txt b/perception/traffic_light_map_based_detector/CMakeLists.txt new file mode 100644 index 000000000000..054b75f7b5c9 --- /dev/null +++ b/perception/traffic_light_map_based_detector/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.5) +project(traffic_light_map_based_detector) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(Eigen3 REQUIRED) + +########### +## Build ## +########### + +include_directories( + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(traffic_light_map_based_detector SHARED + src/node.cpp +) + +rclcpp_components_register_node(traffic_light_map_based_detector + PLUGIN "traffic_light::MapBasedDetector" + EXECUTABLE traffic_light_map_based_detector_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/traffic_light_map_based_detector/README.md b/perception/traffic_light_map_based_detector/README.md new file mode 100644 index 000000000000..19e8dddf60e0 --- /dev/null +++ b/perception/traffic_light_map_based_detector/README.md @@ -0,0 +1,37 @@ +# The `traffic_light_map_based_detector` Package + +## Overview + +`traffic_light_map_based_detector` calculates where the traffic lights will appear in the image based on the HD map. + +Calibration and vibration errors can be entered as parameters, and the size of the detected RegionOfInterest will change according to the error. + +![traffic_light_map_based_detector_result](./docs/traffic_light_map_based_detector_result.svg) + +If the node receives route information, it only looks at traffic lights on that route. +If the node receives no route information, it looks at a radius of 200 meters and the angle between the traffic light and the camera is less than 40 degrees. + +## Input topics + +| Name | Type | Description | +| -------------------- | ---------------------------------------- | ----------------------- | +| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | +| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | +| `~input/route` | autoware_auto_planning_msgs::HADMapRoute | optional: route | + +## Output topics + +| Name | Type | Description | +| ---------------- | --------------------------------------------------- | -------------------------------------------------------------------- | +| `~output/rois` | autoware_auto_perception_msgs::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `~debug/markers` | visualization_msgs::MarkerArray | visualization to debug | + +## Node parameters + +| Parameter | Type | Description | +| ---------------------- | ------ | ----------------------------------------------------------- | +| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | +| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | +| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | +| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | +| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | diff --git a/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml new file mode 100644 index 000000000000..38fc29046485 --- /dev/null +++ b/perception/traffic_light_map_based_detector/config/traffic_light_map_based_detector.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + max_vibration_pitch: 0.01745329251 # -0.5 ~ 0.5 deg + max_vibration_yaw: 0.01745329251 # -0.5 ~ 0.5 deg + max_vibration_height: 0.1 # -0.05 ~ 0.05 m + max_vibration_width: 0.1 # -0.05 ~ 0.05 m + max_vibration_depth: 0.1 # -0.05 ~ 0.05 m diff --git a/perception/traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg b/perception/traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg new file mode 100644 index 000000000000..b3f5c95df415 --- /dev/null +++ b/perception/traffic_light_map_based_detector/docs/traffic_light_map_based_detector_result.svg @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp new file mode 100644 index 000000000000..860688b1b94d --- /dev/null +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -0,0 +1,131 @@ +// 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. +// 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. +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * 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. + * + * Authors: Yukihiro Saito + * + */ + +#ifndef TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ +#define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace traffic_light +{ +class MapBasedDetector : public rclcpp::Node +{ +public: + explicit MapBasedDetector(const rclcpp::NodeOptions & node_options); + +private: + struct Config + { + double max_vibration_pitch; + double max_vibration_yaw; + double max_vibration_height; + double max_vibration_width; + double max_vibration_depth; + }; + + struct IdLessThan + { + bool operator()( + const lanelet::ConstLineString3d & left, const lanelet::ConstLineString3d & right) const + { + return left.id() < right.id(); + } + }; + +private: + rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr camera_info_sub_; + rclcpp::Subscription::SharedPtr route_sub_; + + rclcpp::Publisher::SharedPtr roi_pub_; + rclcpp::Publisher::SharedPtr viz_pub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + using TrafficLightSet = std::set; + + std::shared_ptr all_traffic_lights_ptr_; + std::shared_ptr route_traffic_lights_ptr_; + + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + Config config_; + + void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_msg); + void cameraInfoCallback(const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg); + void routeCallback(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr input_msg); + void getVisibleTrafficLights( + const TrafficLightSet & all_traffic_lights, const geometry_msgs::msg::Pose & camera_pose, + const image_geometry::PinholeCameraModel & pinhole_camera_model, + std::vector & visible_traffic_lights); + bool isInDistanceRange( + const geometry_msgs::msg::Point & tl_point, const geometry_msgs::msg::Point & camera_point, + const double max_distance_range) const; + bool isInAngleRange( + const double & tl_yaw, const double & camera_yaw, const double max_angle_range) const; + bool isInImageFrame( + const image_geometry::PinholeCameraModel & pinhole_camera_model, + const geometry_msgs::msg::Point & point) const; + bool getTrafficLightRoi( + const geometry_msgs::msg::Pose & camera_pose, + const image_geometry::PinholeCameraModel & pinhole_camera_model, + const lanelet::ConstLineString3d traffic_light, const Config & config, + autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi); + void publishVisibleTrafficLights( + const geometry_msgs::msg::PoseStamped camera_pose_stamped, + const std::vector & visible_traffic_lights, + const rclcpp::Publisher::SharedPtr pub); +}; +} // namespace traffic_light +#endif // TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ diff --git a/perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml b/perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml new file mode 100644 index 000000000000..bb8804d2f006 --- /dev/null +++ b/perception/traffic_light_map_based_detector/launch/traffic_light_map_based_detector.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml new file mode 100644 index 000000000000..268f9f19a9e5 --- /dev/null +++ b/perception/traffic_light_map_based_detector/package.xml @@ -0,0 +1,32 @@ + + + traffic_light_map_based_detector + 0.1.0 + The traffic_light_map_based_detector package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_utils + geometry_msgs + image_geometry + lanelet2_extension + rclcpp + sensor_msgs + tf2 + tf2_ros + + + ament_lint_auto + autoware_lint_common + + + + ament_cmake + + diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp new file mode 100644 index 000000000000..39e8d8747107 --- /dev/null +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -0,0 +1,494 @@ +// 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. +// 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. +/* + * Copyright 2019 Autoware Foundation. All rights reserved. + * + * 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. + * + * Authors: Yukihiro Saito + * + */ + +#include "traffic_light_map_based_detector/node.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace +{ +cv::Point2d calcRawImagePointFromPoint3D( + const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d) +{ + cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); + return pinhole_camera_model.unrectifyPoint(rectified_image_point); +} + +cv::Point2d calcRawImagePointFromPoint3D( + const image_geometry::PinholeCameraModel & pinhole_camera_model, + const geometry_msgs::msg::Point & point3d) +{ + return calcRawImagePointFromPoint3D( + pinhole_camera_model, cv::Point3d(point3d.x, point3d.y, point3d.z)); +} + +void roundInImageFrame( + const image_geometry::PinholeCameraModel & pinhole_camera_model, cv::Point2d & point) +{ + const sensor_msgs::msg::CameraInfo camera_info = pinhole_camera_model.cameraInfo(); + point.x = + std::max(std::min(point.x, static_cast(static_cast(camera_info.width) - 1)), 0.0); + point.y = + std::max(std::min(point.y, static_cast(static_cast(camera_info.height) - 1)), 0.0); +} +} // namespace + +namespace traffic_light +{ +MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options) +: Node("traffic_light_map_based_detector", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + using std::placeholders::_1; + + // subscribers + map_sub_ = create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&MapBasedDetector::mapCallback, this, _1)); + camera_info_sub_ = create_subscription( + "~/input/camera_info", rclcpp::SensorDataQoS(), + std::bind(&MapBasedDetector::cameraInfoCallback, this, _1)); + route_sub_ = create_subscription( + "~/input/route", 1, std::bind(&MapBasedDetector::routeCallback, this, _1)); + + // publishers + roi_pub_ = this->create_publisher( + "~/output/rois", 1); + viz_pub_ = this->create_publisher("~/debug/markers", 1); + + // parameter declaration needs default values: are 0.0 goof defaults for this? + config_.max_vibration_pitch = declare_parameter("max_vibration_pitch", 0.0); + config_.max_vibration_yaw = declare_parameter("max_vibration_yaw", 0.0); + config_.max_vibration_height = declare_parameter("max_vibration_height", 0.0); + config_.max_vibration_width = declare_parameter("max_vibration_width", 0.0); + config_.max_vibration_depth = declare_parameter("max_vibration_depth", 0.0); +} + +void MapBasedDetector::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_msg) +{ + if (all_traffic_lights_ptr_ == nullptr && route_traffic_lights_ptr_ == nullptr) { + return; + } + + image_geometry::PinholeCameraModel pinhole_camera_model; + pinhole_camera_model.fromCameraInfo(*input_msg); + + autoware_auto_perception_msgs::msg::TrafficLightRoiArray output_msg; + output_msg.header = input_msg->header; + + /* Camera pose */ + geometry_msgs::msg::PoseStamped camera_pose_stamped; + try { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer_.lookupTransform( + "map", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.2)); + camera_pose_stamped.header = input_msg->header; + camera_pose_stamped.pose = autoware_utils::transform2pose(transform.transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "cannot get transform from map frame to camera frame"); + return; + } + + /* + * visible_traffic_lights : for each traffic light in map check if in range and in view angle of + * camera + */ + std::vector visible_traffic_lights; + // If get a route, use only traffic lights on the route. + if (route_traffic_lights_ptr_ != nullptr) { + getVisibleTrafficLights( + *route_traffic_lights_ptr_, camera_pose_stamped.pose, pinhole_camera_model, + visible_traffic_lights); + // If don't get a route, use the traffic lights around ego vehicle. + } else if (all_traffic_lights_ptr_ != nullptr) { + getVisibleTrafficLights( + *all_traffic_lights_ptr_, camera_pose_stamped.pose, pinhole_camera_model, + visible_traffic_lights); + // This shouldn't run. + } else { + return; + } + + /* + * Get the ROI from the lanelet and the intrinsic matrix of camera to determine where it appears + * in image. + */ + for (const auto & traffic_light : visible_traffic_lights) { + autoware_auto_perception_msgs::msg::TrafficLightRoi tl_roi; + if (!getTrafficLightRoi( + camera_pose_stamped.pose, pinhole_camera_model, traffic_light, config_, tl_roi)) { + continue; + } + output_msg.rois.push_back(tl_roi); + } + roi_pub_->publish(output_msg); + publishVisibleTrafficLights(camera_pose_stamped, visible_traffic_lights, viz_pub_); +} + +bool MapBasedDetector::getTrafficLightRoi( + const geometry_msgs::msg::Pose & camera_pose, + const image_geometry::PinholeCameraModel & pinhole_camera_model, + const lanelet::ConstLineString3d traffic_light, const Config & config, + autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi) +{ + const double tl_height = traffic_light.attributeOr("height", 0.0); + const auto & tl_left_down_point = traffic_light.front(); + const auto & tl_right_down_point = traffic_light.back(); + + tf2::Transform tf_map2camera( + tf2::Quaternion( + camera_pose.orientation.x, camera_pose.orientation.y, camera_pose.orientation.z, + camera_pose.orientation.w), + tf2::Vector3(camera_pose.position.x, camera_pose.position.y, camera_pose.position.z)); + // id + tl_roi.id = traffic_light.id(); + + // for roi.x_offset and roi.y_offset + { + tf2::Transform tf_map2tl( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3( + tl_left_down_point.x(), tl_left_down_point.y(), tl_left_down_point.z() + tl_height)); + tf2::Transform tf_camera2tl; + tf_camera2tl = tf_map2camera.inverse() * tf_map2tl; + // max vibration + const double max_vibration_x = + std::sin(config.max_vibration_yaw * 0.5) * tf_camera2tl.getOrigin().z() + + config.max_vibration_width * 0.5; + const double max_vibration_y = + std::sin(config.max_vibration_pitch * 0.5) * tf_camera2tl.getOrigin().z() + + config.max_vibration_height * 0.5; + const double max_vibration_z = config.max_vibration_depth * 0.5; + // target position in camera coordinate + geometry_msgs::msg::Point point3d; + point3d.x = tf_camera2tl.getOrigin().x() - max_vibration_x; + point3d.y = tf_camera2tl.getOrigin().y() - max_vibration_y; + point3d.z = tf_camera2tl.getOrigin().z() - max_vibration_z; + if (point3d.z <= 0.0) { + return false; + } + cv::Point2d point2d = calcRawImagePointFromPoint3D(pinhole_camera_model, point3d); + roundInImageFrame(pinhole_camera_model, point2d); + tl_roi.roi.x_offset = point2d.x; + tl_roi.roi.y_offset = point2d.y; + } + + // for roi.width and roi.height + { + tf2::Transform tf_map2tl( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(tl_right_down_point.x(), tl_right_down_point.y(), tl_right_down_point.z())); + tf2::Transform tf_camera2tl; + tf_camera2tl = tf_map2camera.inverse() * tf_map2tl; + // max vibration + const double max_vibration_x = + std::sin(config.max_vibration_yaw * 0.5) * tf_camera2tl.getOrigin().z() + + config.max_vibration_width * 0.5; + const double max_vibration_y = + std::sin(config.max_vibration_pitch * 0.5) * tf_camera2tl.getOrigin().z() + + config.max_vibration_height * 0.5; + const double max_vibration_z = config.max_vibration_depth * 0.5; + // target position in camera coordinate + geometry_msgs::msg::Point point3d; + point3d.x = tf_camera2tl.getOrigin().x() + max_vibration_x; + point3d.y = tf_camera2tl.getOrigin().y() + max_vibration_y; + point3d.z = tf_camera2tl.getOrigin().z() - max_vibration_z; + if (point3d.z <= 0.0) { + return false; + } + cv::Point2d point2d = calcRawImagePointFromPoint3D(pinhole_camera_model, point3d); + roundInImageFrame(pinhole_camera_model, point2d); + tl_roi.roi.width = point2d.x - tl_roi.roi.x_offset; + tl_roi.roi.height = point2d.y - tl_roi.roi.y_offset; + if (tl_roi.roi.width < 1 || tl_roi.roi.height < 1) { + return false; + } + } + return true; +} + +void MapBasedDetector::mapCallback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr input_msg) +{ + lanelet_map_ptr_ = std::make_shared(); + + lanelet::utils::conversion::fromBinMsg(*input_msg, lanelet_map_ptr_); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + std::vector all_lanelet_traffic_lights = + lanelet::utils::query::autowareTrafficLights(all_lanelets); + all_traffic_lights_ptr_ = std::make_shared(); + for (auto tl_itr = all_lanelet_traffic_lights.begin(); tl_itr != all_lanelet_traffic_lights.end(); + ++tl_itr) { + lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (!lsp.isLineString()) { // traffic lights must be linestrings + continue; + } + all_traffic_lights_ptr_->insert(static_cast(lsp)); + } + } +} + +void MapBasedDetector::routeCallback( + const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr input_msg) +{ + if (lanelet_map_ptr_ == nullptr) { + RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map"); + return; + } + lanelet::ConstLanelets route_lanelets; + for (const auto & segment : input_msg->segments) { + for (const auto & primitive : segment.primitives) { + try { + route_lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(primitive.id)); + } catch (const lanelet::NoSuchPrimitiveError & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return; + } + } + } + std::vector route_lanelet_traffic_lights = + lanelet::utils::query::autowareTrafficLights(route_lanelets); + route_traffic_lights_ptr_ = std::make_shared(); + for (auto tl_itr = route_lanelet_traffic_lights.begin(); + tl_itr != route_lanelet_traffic_lights.end(); ++tl_itr) { + lanelet::AutowareTrafficLightConstPtr tl = *tl_itr; + + auto lights = tl->trafficLights(); + for (auto lsp : lights) { + if (!lsp.isLineString()) { // traffic lights must be linestrings + continue; + } + route_traffic_lights_ptr_->insert(static_cast(lsp)); + } + } +} + +void MapBasedDetector::getVisibleTrafficLights( + const MapBasedDetector::TrafficLightSet & all_traffic_lights, + const geometry_msgs::msg::Pose & camera_pose, + const image_geometry::PinholeCameraModel & pinhole_camera_model, + std::vector & visible_traffic_lights) +{ + for (const auto & traffic_light : all_traffic_lights) { + const auto & tl_left_down_point = traffic_light.front(); + const auto & tl_right_down_point = traffic_light.back(); + const double tl_height = traffic_light.attributeOr("height", 0.0); + + // check distance range + geometry_msgs::msg::Point tl_central_point; + tl_central_point.x = (tl_right_down_point.x() + tl_left_down_point.x()) / 2.0; + tl_central_point.y = (tl_right_down_point.y() + tl_left_down_point.y()) / 2.0; + tl_central_point.z = (tl_right_down_point.z() + tl_left_down_point.z() + tl_height) / 2.0; + constexpr double max_distance_range = 200.0; + if (!isInDistanceRange(tl_central_point, camera_pose.position, max_distance_range)) { + continue; + } + + // check angle range + const double tl_yaw = autoware_utils::normalizeRadian( + std::atan2( + tl_right_down_point.y() - tl_left_down_point.y(), + tl_right_down_point.x() - tl_left_down_point.x()) + + M_PI_2); + constexpr double max_angle_range = autoware_utils::deg2rad(40.0); + + // get direction of z axis + tf2::Vector3 camera_z_dir(0, 0, 1); + tf2::Matrix3x3 camera_rotation_matrix(tf2::Quaternion( + camera_pose.orientation.x, camera_pose.orientation.y, camera_pose.orientation.z, + camera_pose.orientation.w)); + camera_z_dir = camera_rotation_matrix * camera_z_dir; + double camera_yaw = std::atan2(camera_z_dir.y(), camera_z_dir.x()); + camera_yaw = autoware_utils::normalizeRadian(camera_yaw); + if (!isInAngleRange(tl_yaw, camera_yaw, max_angle_range)) { + continue; + } + + // check within image frame + tf2::Transform tf_map2camera( + tf2::Quaternion( + camera_pose.orientation.x, camera_pose.orientation.y, camera_pose.orientation.z, + camera_pose.orientation.w), + tf2::Vector3(camera_pose.position.x, camera_pose.position.y, camera_pose.position.z)); + tf2::Transform tf_map2tl( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(tl_central_point.x, tl_central_point.y, tl_central_point.z)); + tf2::Transform tf_camera2tl; + tf_camera2tl = tf_map2camera.inverse() * tf_map2tl; + + geometry_msgs::msg::Point camera2tl_point; + camera2tl_point.x = tf_camera2tl.getOrigin().x(); + camera2tl_point.y = tf_camera2tl.getOrigin().y(); + camera2tl_point.z = tf_camera2tl.getOrigin().z(); + if (!isInImageFrame(pinhole_camera_model, camera2tl_point)) { + continue; + } + visible_traffic_lights.push_back(traffic_light); + } +} + +bool MapBasedDetector::isInDistanceRange( + const geometry_msgs::msg::Point & tl_point, const geometry_msgs::msg::Point & camera_point, + const double max_distance_range) const +{ + const double sq_dist = (tl_point.x - camera_point.x) * (tl_point.x - camera_point.x) + + (tl_point.y - camera_point.y) * (tl_point.y - camera_point.y); + return sq_dist < (max_distance_range * max_distance_range); +} + +bool MapBasedDetector::isInAngleRange( + const double & tl_yaw, const double & camera_yaw, const double max_angle_range) const +{ + Eigen::Vector2d vec1, vec2; + vec1 << std::cos(tl_yaw), std::sin(tl_yaw); + vec2 << std::cos(camera_yaw), std::sin(camera_yaw); + const double diff_angle = std::acos(vec1.dot(vec2)); + return std::fabs(diff_angle) < max_angle_range; +} + +bool MapBasedDetector::isInImageFrame( + const image_geometry::PinholeCameraModel & pinhole_camera_model, + const geometry_msgs::msg::Point & point) const +{ + if (point.z <= 0.0) { + return false; + } + + cv::Point2d point2d = calcRawImagePointFromPoint3D(pinhole_camera_model, point); + if (0 <= point2d.x && point2d.x < pinhole_camera_model.cameraInfo().width) { + if (0 <= point2d.y && point2d.y < pinhole_camera_model.cameraInfo().height) { + return true; + } + } + return false; +} + +void MapBasedDetector::publishVisibleTrafficLights( + const geometry_msgs::msg::PoseStamped camera_pose_stamped, + const std::vector & visible_traffic_lights, + const rclcpp::Publisher::SharedPtr pub) +{ + visualization_msgs::msg::MarkerArray output_msg; + for (const auto & traffic_light : visible_traffic_lights) { + const auto & tl_left_down_point = traffic_light.front(); + const auto & tl_right_down_point = traffic_light.back(); + const double tl_height = traffic_light.attributeOr("height", 0.0); + const int id = traffic_light.id(); + + geometry_msgs::msg::Point tl_central_point; + tl_central_point.x = (tl_right_down_point.x() + tl_left_down_point.x()) / 2.0; + tl_central_point.y = (tl_right_down_point.y() + tl_left_down_point.y()) / 2.0; + tl_central_point.z = (tl_right_down_point.z() + tl_left_down_point.z() + tl_height) / 2.0; + + visualization_msgs::msg::Marker marker; + + tf2::Transform tf_map2camera( + tf2::Quaternion( + camera_pose_stamped.pose.orientation.x, camera_pose_stamped.pose.orientation.y, + camera_pose_stamped.pose.orientation.z, camera_pose_stamped.pose.orientation.w), + tf2::Vector3( + camera_pose_stamped.pose.position.x, camera_pose_stamped.pose.position.y, + camera_pose_stamped.pose.position.z)); + tf2::Transform tf_map2tl( + tf2::Quaternion(0, 0, 0, 1), + tf2::Vector3(tl_central_point.x, tl_central_point.y, tl_central_point.z)); + tf2::Transform tf_camera2tl; + tf_camera2tl = tf_map2camera.inverse() * tf_map2tl; + + marker.header = camera_pose_stamped.header; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + marker.ns = std::string("beam"); + marker.scale.x = 0.05; + marker.action = visualization_msgs::msg::Marker::MODIFY; + marker.pose.position.x = 0.0; + marker.pose.position.y = 0.0; + marker.pose.position.z = 0.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + point.z = 0.0; + marker.points.push_back(point); + point.x = tf_camera2tl.getOrigin().x(); + point.y = tf_camera2tl.getOrigin().y(); + point.z = tf_camera2tl.getOrigin().z(); + marker.points.push_back(point); + + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + output_msg.markers.push_back(marker); + } + pub->publish(output_msg); +} +} // namespace traffic_light + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::MapBasedDetector) diff --git a/perception/traffic_light_ssd_fine_detector/.gitignore b/perception/traffic_light_ssd_fine_detector/.gitignore new file mode 100644 index 000000000000..8fce603003c1 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/.gitignore @@ -0,0 +1 @@ +data/ diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt new file mode 100644 index 000000000000..7fc6ad12675f --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -0,0 +1,168 @@ +cmake_minimum_required(VERSION 3.5) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +project(traffic_light_ssd_fine_detector) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if (CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if (CUDA_VERBOSE) + message(STATUS "CUDA is available!") + message(STATUS "CUDA Libs: ${CUDA_LIBRARIES}") + message(STATUS "CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif () + set(CUDA_AVAIL ON) +else() + message(STATUS "CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif (CUDA_FOUND) + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER NAMES nvinfer) +find_library(NVONNXPARSER nvonnxparser) +find_library(NVINFER_PLUGIN NAMES nvinfer_plugin) +if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) + if (CUDA_VERBOSE) + message(STATUS "TensorRT is available!") + message(STATUS "NVINFER: ${NVINFER}") + message(STATUS "NVPARSERS: ${NVPARSERS}") + message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") + message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") + endif () + set(TRT_AVAIL ON) +else() + message(STATUS "TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." ) +if(CUDNN_LIBRARY) + if (CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif () + set(CUDNN_AVAIL ON) +else() + message(STATUS "CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +# Download caffemodel and prototxt +set(PRETRAINED_MODEL_LINK "https://drive.google.com/uc?id=1USFDPRH9JrVdGoqt27qHjRgittwc0kcO") +set(PRETRAINED_MODEL_HASH 34ce7f2cbacbf6da8bc35769f027b73f) +set(LAMP_LABEL_LINK "https://drive.google.com/uc?id=1hPcKvKgKz0fqEo0cNAXH7roEletqZErL") +set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) + +find_program(GDOWN_AVAIL "gdown") +if (NOT GDOWN_AVAIL) + message(STATUS "gdown: command not found. External files could not be downloaded.") +endif() +set(PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") +if (NOT EXISTS "${PATH}") + execute_process(COMMAND mkdir -p ${PATH}) +endif() + +set(FILE "${PATH}/mb2-ssd-lite-tlr.onnx") +message(STATUS "Checking and downloading mb2-ssd-lite-tlr.onnx") +if (EXISTS "${FILE}") + file(MD5 "${FILE}" EXISTING_FILE_HASH) + if (NOT "${PRETRAINED_MODEL_HASH}" EQUAL "${EXISTING_FILE_HASH}") + message(STATUS "... file hash changed. Downloading now ...") + execute_process(COMMAND gdown --quiet "${PRETRAINED_MODEL_LINK}" -O ${PATH}/mb2-ssd-lite-tlr.onnx) + endif() +else() + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${PRETRAINED_MODEL_LINK}" -O ${PATH}/mb2-ssd-lite-tlr.onnx) +endif() + +set(FILE "${PATH}/voc_labels_tl.txt") +message(STATUS "Checking and downloading voc_labels_tl.txt") +if (EXISTS "${FILE}") + file(MD5 "${FILE}" EXISTING_FILE_HASH) + if (NOT "${LAMP_LABEL_HASH}" EQUAL "${EXISTING_FILE_HASH}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${LAMP_LABEL_LINK}" -O ${PATH}/voc_labels_tl.txt) + endif() +else() + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown --quiet "${LAMP_LABEL_LINK}" -O ${PATH}/voc_labels_tl.txt) +endif() + +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + include_directories( + ${OpenCV_INCLUDE_DIRS} + ${CUDA_INCLUDE_DIRS} + ) + + set(CMAKE_CXX_FLAGS "-O2 -Wall -Wunused-variable ${CMAKE_CXX_FLAGS} -fpic -std=c++11 -pthread") + + ### ssd ### + ament_auto_add_library(ssd SHARED + lib/src/trt_ssd.cpp + ) + + target_include_directories(ssd PUBLIC + lib/include + ) + + target_link_libraries(ssd + ${NVINFER} + ${NVONNXPARSER} + ${NVINFER_PLUGIN} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + ) + + ament_auto_add_library(traffic_light_ssd_fine_detector_nodelet SHARED + src/nodelet.cpp + ) + + target_link_libraries(traffic_light_ssd_fine_detector_nodelet + ${OpenCV_LIB} + ssd + ) + + rclcpp_components_register_node(traffic_light_ssd_fine_detector_nodelet + PLUGIN "traffic_light::TrafficLightSSDFineDetectorNodelet" + EXECUTABLE traffic_light_ssd_fine_detector_node + ) + + ament_auto_package(INSTALL_TO_SHARE + data + launch + ) + +else() + message(STATUS "TrafficLightSSDFineDetector won't be built, CUDA and/or TensorRT were not found.") +endif() diff --git a/perception/traffic_light_ssd_fine_detector/README.md b/perception/traffic_light_ssd_fine_detector/README.md new file mode 100644 index 000000000000..91fe13f3b689 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/README.md @@ -0,0 +1,63 @@ +# traffic_light_ssd_fine_detector + +## Purpose + +It is a package for traffic light detection using MobileNetV2 and SSDLite. + +The trained model is based on [pytorch-ssd](https://github.com/qfgaohao/pytorch-ssd). + +## Inner-workings / Algorithms + +Based on the camera image and the global ROI array detected by `map_based_detection` node, a CNN-based detection method enables highly accurate traffic light detection. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------- | ---------------------------------------------------------- | ------------------------------------------------ | +| `~/input/image` | `sensor_msgs/Image` | The full size camera image | +| `~/input/rois` | `autoware_auto_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | + +### Output + +| Name | Type | Description | +| --------------------- | ---------------------------------------------------------- | ---------------------------- | +| `~/output/rois` | `autoware_auto_perception_msgs::msg::TrafficLightRoiArray` | The detected accurate rois | +| `~/debug/exe_time_ms` | `autoware_debug_msgs::msg::Float32Stamped` | The time taken for inference | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| -------------- | ------------------- | ------------- | ------------------------------------------------------------------------------- | +| `score_thresh` | double | 0.7 | If the objectness score is less than this value, the object is ignored | +| `mean` | std::vector | [0.5,0.5,0.5] | Average value of the normalized values of the image data used for training | +| `std` | std::vector | [0.5,0.5,0.5] | Standard deviation of the normalized values of the image data used for training | + +### Node Parameters + +| Name | Type | Default Value | Description | +| ------------------ | ------ | ------------------------------ | -------------------------------------------------------------------- | +| `onnx_file` | string | "./data/mb2-ssd-lite-tlr.onnx" | The onnx file name for yolo model | +| `label_file` | string | "./data/voc_labels_tl.txt" | The label file with label names for detected objects written on it | +| `mode` | string | "FP32" | The inference mode: "FP32", "FP16", "INT8" | +| `max_batch_size` | int | 8 | The size of the batch processed at one time by inference by TensorRT | +| `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | + +## Assumptions / Known limits + +## Onnx model + +- + +## Reference repositories + +pytorch-ssd github repository + +- + +MobileNetV2 + +- M. Sandler, A. Howard, M. Zhu, A. Zhmoginov and L. Chen, "MobileNetV2: Inverted Residuals and Linear Bottlenecks," 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition, Salt Lake City, UT, 2018, pp. 4510-4520, doi: 10.1109/CVPR.2018.00474. diff --git a/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp b/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp new file mode 100644 index 000000000000..73065cb92455 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/include/traffic_light_ssd_fine_detector/nodelet.hpp @@ -0,0 +1,114 @@ +// 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. +// 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. + +#ifndef TRAFFIC_LIGHT_SSD_FINE_DETECTOR__NODELET_HPP_ +#define TRAFFIC_LIGHT_SSD_FINE_DETECTOR__NODELET_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +typedef struct Detection +{ + float x, y, w, h, prob; +} Detection; + +namespace traffic_light +{ +class TrafficLightSSDFineDetectorNodelet : public rclcpp::Node +{ +public: + explicit TrafficLightSSDFineDetectorNodelet(const rclcpp::NodeOptions & options); + void connectCb(); + void callback( + const sensor_msgs::msg::Image::ConstSharedPtr image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr + traffic_light_roi_msg); + +private: + bool cvMat2CnnInput( + const std::vector & in_imgs, const int num_rois, std::vector & data); + bool cnnOutput2BoxDetection( + const float * scores, const float * boxes, const int tlr_id, + const std::vector & in_imgs, const int num_rois, std::vector & detections); + bool rosMsg2CvMat(const sensor_msgs::msg::Image::ConstSharedPtr image_msg, cv::Mat & image); + bool fitInFrame(cv::Point & lt, cv::Point & rb, const cv::Size & size); + void cvRect2TlRoiMsg( + const cv::Rect & rect, const int32_t id, + autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi); + bool readLabelFile(std::string filepath, std::vector & labels); + bool getTlrIdFromLabel(const std::vector & labels, int & tlr_id); + + // variables + std::shared_ptr image_transport_; + image_transport::SubscriberFilter image_sub_; + message_filters::Subscriber roi_sub_; + std::mutex connect_mutex_; + rclcpp::Publisher::SharedPtr + output_roi_pub_; + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + typedef message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + std::shared_ptr sync_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, autoware_auto_perception_msgs::msg::TrafficLightRoiArray> + ApproximateSyncPolicy; + typedef message_filters::Synchronizer ApproximateSync; + std::shared_ptr approximate_sync_; + + bool is_approximate_sync_; + double score_thresh_; + + int tlr_id_; + int channel_; + int width_; + int height_; + int class_num_; + int detection_per_class_; + + std::vector mean_; + std::vector std_; + + std::unique_ptr net_ptr_; +}; // TrafficLightSSDFineDetectorNodelet + +} // namespace traffic_light + +#endif // TRAFFIC_LIGHT_SSD_FINE_DETECTOR__NODELET_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml new file mode 100644 index 000000000000..03180319f7aa --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp new file mode 100644 index 000000000000..79984ecba8e3 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/cuda_utils.hpp @@ -0,0 +1,75 @@ +// 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. +// 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. + +/* + * This code is licensed under CC0 1.0 Universal (Public Domain). + * You can use this without any limitation. + * https://creativecommons.org/publicdomain/zero/1.0/deed.en + */ + +#ifndef CUDA_UTILS_HPP_ +#define CUDA_UTILS_HPP_ + +#include <./cuda_runtime_api.h> + +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) + +namespace cuda +{ +void check_error(const ::cudaError_t e, decltype(__FILE__) f, decltype(__LINE__) n) +{ + if (e != ::cudaSuccess) { + std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw std::runtime_error{s.str()}; + } +} + +struct deleter +{ + void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } +}; +template +using unique_ptr = std::unique_ptr; + +// auto array = cuda::make_unique(n); +// ::cudaMemcpy(array.get(), src_array, sizeof(float)*n, ::cudaMemcpyHostToDevice); +template +typename std::enable_if::value, cuda::unique_ptr>::type make_unique( + const std::size_t n) +{ + using U = typename std::remove_extent::type; + U * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); + return cuda::unique_ptr{p}; +} + +// auto value = cuda::make_unique(); +// ::cudaMemcpy(value.get(), src_value, sizeof(my_class), ::cudaMemcpyHostToDevice); +template +cuda::unique_ptr make_unique() +{ + T * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); + return cuda::unique_ptr{p}; +} +} // namespace cuda + +#endif // CUDA_UTILS_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp new file mode 100644 index 000000000000..bbe5ae8da513 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -0,0 +1,100 @@ +// 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. +// 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. + +#ifndef TRT_SSD_HPP_ +#define TRT_SSD_HPP_ + +#include <./cuda_runtime.h> +#include + +#include +#include +#include +#include + +namespace ssd +{ +struct Deleter +{ + template + void operator()(T * obj) const + { + if (obj) { + obj->destroy(); + } + } +}; + +template +using unique_ptr = std::unique_ptr; + +class Logger : public nvinfer1::ILogger +{ +public: + explicit Logger(bool verbose) : verbose_(verbose) {} + + void log(Severity severity, const char * msg) noexcept override + { + if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { + std::cout << msg << std::endl; + } + } + +private: + bool verbose_{false}; +}; + +class Net +{ +public: + // Create engine from engine path + explicit Net(const std::string & engine_path, bool verbose = false); + + // Create engine from serialized onnx model + Net( + const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, + bool verbose = false, size_t workspace_size = (1ULL << 30)); + + ~Net(); + + // Save model to path + void save(const std::string & path); + + // Infer using pre-allocated GPU buffers {data, scores, boxes} + void infer(std::vector & buffers, const int batch_size); + + // Get (c, h, w) size of the fixed input + std::vector getInputSize(); + + std::vector getOutputScoreSize(); + + // Get max allowed batch size + int getMaxBatchSize(); + + // Get max number of detections + int getMaxDetections(); + +private: + unique_ptr runtime_ = nullptr; + unique_ptr engine_ = nullptr; + unique_ptr context_ = nullptr; + cudaStream_t stream_ = nullptr; + + void load(const std::string & path); + void prepare(); +}; + +} // namespace ssd + +#endif // TRT_SSD_HPP_ diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp new file mode 100644 index 000000000000..766de7eb4cde --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -0,0 +1,190 @@ +// 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. +// 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 + +#include +#include + +#include +#include +#include +#include +#include + +namespace ssd +{ +void Net::load(const std::string & path) +{ + std::ifstream file(path, std::ios::in | std::ios::binary); + file.seekg(0, file.end); + size_t size = file.tellg(); + file.seekg(0, file.beg); + + char * buffer = new char[size]; + file.read(buffer, size); + file.close(); + if (runtime_) { + engine_ = + unique_ptr(runtime_->deserializeCudaEngine(buffer, size, nullptr)); + } + delete[] buffer; +} + +void Net::prepare() +{ + if (engine_) { + context_ = unique_ptr(engine_->createExecutionContext()); + } + cudaStreamCreate(&stream_); +} + +Net::Net(const std::string & path, bool verbose) +{ + Logger logger(verbose); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + load(path); + prepare(); +} + +Net::~Net() +{ + if (stream_) { + cudaStreamDestroy(stream_); + } +} + +Net::Net( + const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, + bool verbose, size_t workspace_size) +{ + Logger logger(verbose); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + if (!runtime_) { + std::cout << "Fail to create runtime" << std::endl; + return; + } + bool fp16 = precision.compare("FP16") == 0; + bool int8 = precision.compare("INT8") == 0; + + // Create builder + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + if (!builder) { + std::cout << "Fail to create builder" << std::endl; + return; + } + auto config = unique_ptr(builder->createBuilderConfig()); + if (!config) { + std::cout << "Fail to create builder config" << std::endl; + return; + } + // Allow use of FP16 layers when running in INT8 + if (fp16 || int8) { + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } + config->setMaxWorkspaceSize(workspace_size); + + // Parse ONNX FCN + std::cout << "Building " << precision << " core model..." << std::endl; + const auto flag = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = unique_ptr(builder->createNetworkV2(flag)); + if (!network) { + std::cout << "Fail to create network" << std::endl; + return; + } + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + if (!parser) { + std::cout << "Fail to create parser" << std::endl; + return; + } + parser->parseFromFile( + onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); + + // TODO(someone): int8 calibrator + /* std::unique_ptr calib; + if (int8) { + config->setFlag(BuilderFlag::kINT8); + ImageStream stream(batch, inputDims, calibration_images); + calib = std::unique_ptr(new Int8EntropyCalibrator(stream, model_name, + calibration_table)); config->setInt8Calibrator(calib.get()); + }*/ + + // create profile + auto profile = builder->createOptimizationProfile(); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, + nvinfer1::Dims4{1, 3, 300, 300}); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, + nvinfer1::Dims4{max_batch_size, 3, 300, 300}); + profile->setDimensions( + network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, + nvinfer1::Dims4{max_batch_size, 3, 300, 300}); + config->addOptimizationProfile(profile); + + // Build engine + std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; + engine_ = unique_ptr(builder->buildEngineWithConfig(*network, *config)); + if (!engine_) { + std::cout << "Fail to create engine" << std::endl; + return; + } + context_ = unique_ptr(engine_->createExecutionContext()); + if (!context_) { + std::cout << "Fail to create context" << std::endl; + return; + } +} + +void Net::save(const std::string & path) +{ + std::cout << "Writing to " << path << "..." << std::endl; + auto serialized = unique_ptr(engine_->serialize()); + std::ofstream file(path, std::ios::out | std::ios::binary); + file.write(reinterpret_cast(serialized->data()), serialized->size()); +} + +void Net::infer(std::vector & buffers, const int batch_size) +{ + if (!context_) { + throw std::runtime_error("Fail to create context"); + } + auto input_dims = engine_->getBindingDimensions(0); + context_->setBindingDimensions( + 0, nvinfer1::Dims4(batch_size, input_dims.d[1], input_dims.d[2], input_dims.d[3])); + context_->enqueueV2(buffers.data(), stream_, nullptr); + cudaStreamSynchronize(stream_); +} + +std::vector Net::getInputSize() +{ + auto dims = engine_->getBindingDimensions(0); + return {dims.d[1], dims.d[2], dims.d[3]}; +} + +std::vector Net::getOutputScoreSize() +{ + auto dims = engine_->getBindingDimensions(1); + return {dims.d[1], dims.d[2]}; +} + +int Net::getMaxBatchSize() +{ + return engine_->getProfileDimensions(0, 0, nvinfer1::OptProfileSelector::kMAX).d[0]; +} + +int Net::getMaxDetections() { return engine_->getBindingDimensions(1).d[1]; } + +} // namespace ssd diff --git a/perception/traffic_light_ssd_fine_detector/package.xml b/perception/traffic_light_ssd_fine_detector/package.xml new file mode 100644 index 000000000000..d78e59e77916 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/package.xml @@ -0,0 +1,36 @@ + + + + traffic_light_ssd_fine_detector + 0.1.0 + The traffic_light_ssd_fine_detector package + + + + + Daisuke Nishimatsu + + + + + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_debug_msgs + cv_bridge + image_transport + message_filters + rclcpp + rclcpp_components + sensor_msgs + + autoware_lint_common + + + + ament_cmake + + diff --git a/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp new file mode 100644 index 000000000000..8578d3ebbc09 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/src/nodelet.cpp @@ -0,0 +1,361 @@ +// 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. +// 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_ssd_fine_detector/nodelet.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace traffic_light +{ +inline std::vector toFloatVector(const std::vector double_vector) +{ + return std::vector(double_vector.begin(), double_vector.end()); +} + +TrafficLightSSDFineDetectorNodelet::TrafficLightSSDFineDetectorNodelet( + const rclcpp::NodeOptions & options) +: Node("traffic_light_ssd_fine_detector_node", options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + std::string package_path = + ament_index_cpp::get_package_share_directory("traffic_light_ssd_fine_detector"); + std::string data_path = package_path + "/data/"; + std::string engine_path = package_path + "/data/mb2-ssd-lite.engine"; + std::ifstream fs(engine_path); + std::vector labels; + const int max_batch_size = this->declare_parameter("max_batch_size", 8); + const std::string onnx_file = this->declare_parameter("onnx_file"); + const std::string label_file = this->declare_parameter("label_file"); + const std::string mode = this->declare_parameter("mode", "FP32"); + + if (readLabelFile(label_file, labels)) { + if (!getTlrIdFromLabel(labels, tlr_id_)) { + RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); + } + } + + if (fs.is_open()) { + RCLCPP_INFO(this->get_logger(), "Found %s", engine_path.c_str()); + net_ptr_.reset(new ssd::Net(engine_path, false)); + if (max_batch_size != net_ptr_->getMaxBatchSize()) { + RCLCPP_INFO( + this->get_logger(), + "Required max batch size %d does not correspond to Profile max batch size %d. Rebuild " + "engine " + "from onnx", + max_batch_size, net_ptr_->getMaxBatchSize()); + net_ptr_.reset(new ssd::Net(onnx_file, mode, max_batch_size)); + net_ptr_->save(engine_path); + } + } else { + RCLCPP_INFO( + this->get_logger(), "Could not find %s, try making TensorRT engine from onnx", + engine_path.c_str()); + net_ptr_.reset(new ssd::Net(onnx_file, mode, max_batch_size)); + net_ptr_->save(engine_path); + } + channel_ = net_ptr_->getInputSize()[0]; + width_ = net_ptr_->getInputSize()[1]; + height_ = net_ptr_->getInputSize()[2]; + detection_per_class_ = net_ptr_->getOutputScoreSize()[0]; + class_num_ = net_ptr_->getOutputScoreSize()[1]; + + is_approximate_sync_ = this->declare_parameter("approximate_sync", false); + score_thresh_ = this->declare_parameter("score_thresh", 0.7); + mean_ = toFloatVector(this->declare_parameter("mean", std::vector({0.5, 0.5, 0.5}))); + std_ = toFloatVector(this->declare_parameter("std", std::vector({0.5, 0.5, 0.5}))); + + auto timer_callback = std::bind(&TrafficLightSSDFineDetectorNodelet::connectCb, this); + const auto period_s = 0.1; + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + std::lock_guard lock(connect_mutex_); + output_roi_pub_ = + this->create_publisher( + "~/output/rois", 1); + exe_time_pub_ = + this->create_publisher("~/debug/exe_time_ms", 1); + if (is_approximate_sync_) { + approximate_sync_.reset(new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, roi_sub_)); + approximate_sync_->registerCallback( + std::bind(&TrafficLightSSDFineDetectorNodelet::callback, this, _1, _2)); + } else { + sync_.reset(new Sync(SyncPolicy(10), image_sub_, roi_sub_)); + sync_->registerCallback(std::bind(&TrafficLightSSDFineDetectorNodelet::callback, this, _1, _2)); + } +} + +void TrafficLightSSDFineDetectorNodelet::connectCb() +{ + std::lock_guard lock(connect_mutex_); + if (output_roi_pub_->get_subscription_count() == 0) { + image_sub_.unsubscribe(); + roi_sub_.unsubscribe(); + } else if (!image_sub_.getSubscriber()) { + image_sub_.subscribe(this, "~/input/image", "raw", rmw_qos_profile_sensor_data); + roi_sub_.subscribe(this, "~/input/rois", rclcpp::QoS{1}.get_rmw_qos_profile()); + } +} + +void TrafficLightSSDFineDetectorNodelet::callback( + const sensor_msgs::msg::Image::ConstSharedPtr in_image_msg, + const autoware_auto_perception_msgs::msg::TrafficLightRoiArray::ConstSharedPtr in_roi_msg) +{ + if (in_image_msg->width < 2 || in_image_msg->height < 2) { + return; + } + + using std::chrono::high_resolution_clock; + using std::chrono::milliseconds; + const auto exe_start_time = high_resolution_clock::now(); + cv::Mat original_image; + autoware_auto_perception_msgs::msg::TrafficLightRoiArray out_rois; + + rosMsg2CvMat(in_image_msg, original_image); + int num_rois = in_roi_msg->rois.size(); + int batch_count = 0; + const int batch_size = net_ptr_->getMaxBatchSize(); + while (num_rois != 0) { + const int num_infer = (num_rois / batch_size > 0) ? batch_size : num_rois % batch_size; + auto data_d = cuda::make_unique(num_infer * channel_ * width_ * height_); + auto scores_d = cuda::make_unique(num_infer * detection_per_class_ * class_num_); + auto boxes_d = cuda::make_unique(num_infer * detection_per_class_ * 4); + std::vector buffers = {data_d.get(), scores_d.get(), boxes_d.get()}; + std::vector lts, rbs; + std::vector cropped_imgs; + + for (int i = 0; i < num_infer; ++i) { + int roi_index = i + batch_count * batch_size; + lts.push_back(cv::Point( + in_roi_msg->rois.at(roi_index).roi.x_offset, in_roi_msg->rois.at(roi_index).roi.y_offset)); + rbs.push_back(cv::Point( + in_roi_msg->rois.at(roi_index).roi.x_offset + in_roi_msg->rois.at(roi_index).roi.width, + in_roi_msg->rois.at(roi_index).roi.y_offset + in_roi_msg->rois.at(roi_index).roi.height)); + fitInFrame(lts.at(i), rbs.at(i), cv::Size(original_image.size())); + cropped_imgs.push_back(cv::Mat(original_image, cv::Rect(lts.at(i), rbs.at(i)))); + } + + std::vector data(num_infer * channel_ * width_ * height_); + if (!cvMat2CnnInput(cropped_imgs, num_infer, data)) { + RCLCPP_ERROR(this->get_logger(), "Fail to preprocess image"); + return; + } + + cudaMemcpy(data_d.get(), data.data(), data.size() * sizeof(float), cudaMemcpyHostToDevice); + + try { + net_ptr_->infer(buffers, num_infer); + } catch (std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + return; + } + + auto scores = std::make_unique(num_infer * detection_per_class_ * class_num_); + auto boxes = std::make_unique(num_infer * detection_per_class_ * 4); + cudaMemcpy( + scores.get(), scores_d.get(), sizeof(float) * num_infer * detection_per_class_ * class_num_, + cudaMemcpyDeviceToHost); + cudaMemcpy( + boxes.get(), boxes_d.get(), sizeof(float) * num_infer * detection_per_class_ * 4, + cudaMemcpyDeviceToHost); + // Get Output + std::vector detections; + if (!cnnOutput2BoxDetection( + scores.get(), boxes.get(), tlr_id_, cropped_imgs, num_infer, detections)) { + RCLCPP_ERROR(this->get_logger(), "Fail to postprocess image"); + return; + } + + for (int i = 0; i < num_infer; ++i) { + if (detections.at(i).prob > score_thresh_) { + cv::Point lt_roi = + cv::Point(lts.at(i).x + detections.at(i).x, lts.at(i).y + detections.at(i).y); + cv::Point rb_roi = cv::Point( + lts.at(i).x + detections.at(i).x + detections.at(i).w, + lts.at(i).y + detections.at(i).y + detections.at(i).h); + fitInFrame(lt_roi, rb_roi, cv::Size(original_image.size())); + autoware_auto_perception_msgs::msg::TrafficLightRoi tl_roi; + cvRect2TlRoiMsg( + cv::Rect(lt_roi, rb_roi), in_roi_msg->rois.at(i + batch_count * batch_size).id, tl_roi); + out_rois.rois.push_back(tl_roi); + } + } + num_rois -= num_infer; + ++batch_count; + } + out_rois.header = in_roi_msg->header; + output_roi_pub_->publish(out_rois); + const auto exe_end_time = high_resolution_clock::now(); + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count(); + autoware_debug_msgs::msg::Float32Stamped exe_time_msg; + exe_time_msg.data = exe_time; + exe_time_msg.stamp = this->now(); + exe_time_pub_->publish(exe_time_msg); +} + +bool TrafficLightSSDFineDetectorNodelet::cvMat2CnnInput( + const std::vector & in_imgs, const int num_rois, std::vector & data) +{ + for (int i = 0; i < num_rois; ++i) { + // cv::Mat rgb; + // cv::cvtColor(in_imgs.at(i), rgb, CV_BGR2RGB); + cv::Mat resized; + cv::resize(in_imgs.at(i), resized, cv::Size(width_, height_)); + + cv::Mat pixels; + resized.convertTo(pixels, CV_32FC3, 1.0 / 255, 0); + std::vector img; + if (pixels.isContinuous()) { + img.assign( + reinterpret_cast(pixels.datastart), + reinterpret_cast(pixels.dataend)); + } else { + return false; + } + + for (int c = 0; c < channel_; ++c) { + for (int j = 0, hw = width_ * height_; j < hw; ++j) { + data[i * channel_ * width_ * height_ + c * hw + j] = + (img[channel_ * j + 2 - c] - mean_[c]) / std_[c]; + } + } + } + return true; +} + +bool TrafficLightSSDFineDetectorNodelet::cnnOutput2BoxDetection( + const float * scores, const float * boxes, const int tlr_id, const std::vector & in_imgs, + const int num_rois, std::vector & detections) +{ + if (tlr_id > class_num_ - 1) { + return false; + } + for (int i = 0; i < num_rois; ++i) { + std::vector tlr_scores; + Detection det; + for (int j = 0; j < detection_per_class_; ++j) { + tlr_scores.push_back(scores[i * detection_per_class_ * class_num_ + tlr_id + j * class_num_]); + } + std::vector::iterator iter = std::max_element(tlr_scores.begin(), tlr_scores.end()); + size_t index = std::distance(tlr_scores.begin(), iter); + size_t box_index = i * detection_per_class_ * 4 + index * 4; + cv::Point lt, rb; + lt.x = boxes[box_index] * in_imgs.at(i).cols; + lt.y = boxes[box_index + 1] * in_imgs.at(i).rows; + rb.x = boxes[box_index + 2] * in_imgs.at(i).cols; + rb.y = boxes[box_index + 3] * in_imgs.at(i).rows; + fitInFrame(lt, rb, cv::Size(in_imgs.at(i).cols, in_imgs.at(i).rows)); + det.x = lt.x; + det.y = lt.y; + det.w = rb.x - lt.x; + det.h = rb.y - lt.y; + + det.prob = tlr_scores[index]; + detections.push_back(det); + } + return true; +} + +bool TrafficLightSSDFineDetectorNodelet::rosMsg2CvMat( + const sensor_msgs::msg::Image::ConstSharedPtr image_msg, cv::Mat & image) +{ + try { + cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_msg, "rgb8"); + image = cv_image->image; + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR( + this->get_logger(), "Failed to convert sensor_msgs::msg::Image to cv::Mat \n%s", e.what()); + return false; + } + + return true; +} + +bool TrafficLightSSDFineDetectorNodelet::fitInFrame( + cv::Point & lt, cv::Point & rb, const cv::Size & size) +{ + const int width = static_cast(size.width); + const int height = static_cast(size.height); + { + const int x_min = 0, x_max = width - 2; + const int y_min = 0, y_max = height - 2; + lt.x = std::min(std::max(lt.x, x_min), x_max); + lt.y = std::min(std::max(lt.y, y_min), y_max); + } + { + const int x_min = lt.x + 1, x_max = width - 1; + const int y_min = lt.y + 1, y_max = height - 1; + rb.x = std::min(std::max(rb.x, x_min), x_max); + rb.y = std::min(std::max(rb.y, y_min), y_max); + } + + return true; +} + +void TrafficLightSSDFineDetectorNodelet::cvRect2TlRoiMsg( + const cv::Rect & rect, const int32_t id, + autoware_auto_perception_msgs::msg::TrafficLightRoi & tl_roi) +{ + tl_roi.id = id; + tl_roi.roi.x_offset = rect.x; + tl_roi.roi.y_offset = rect.y; + tl_roi.roi.width = rect.width; + tl_roi.roi.height = rect.height; +} + +bool TrafficLightSSDFineDetectorNodelet::readLabelFile( + std::string filepath, std::vector & labels) +{ + std::ifstream labelsFile(filepath); + if (!labelsFile.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Could not open label file. [%s]", filepath.c_str()); + return false; + } + std::string label; + while (getline(labelsFile, label)) { + labels.push_back(label); + } + return true; +} + +bool TrafficLightSSDFineDetectorNodelet::getTlrIdFromLabel( + const std::vector & labels, int & tlr_id) +{ + for (size_t i = 0; i < labels.size(); ++i) { + if (labels.at(i) == "traffic_light") { + tlr_id = i; + return true; + } + } + return false; +} + +} // namespace traffic_light + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::TrafficLightSSDFineDetectorNodelet) diff --git a/perception/traffic_light_ssd_fine_detector/traffic_light_ssd_fine_detector.xml b/perception/traffic_light_ssd_fine_detector/traffic_light_ssd_fine_detector.xml new file mode 100644 index 000000000000..f92034cfde57 --- /dev/null +++ b/perception/traffic_light_ssd_fine_detector/traffic_light_ssd_fine_detector.xml @@ -0,0 +1,10 @@ + + + + + + + +