From 4d8fd783d30c7f1efa40c73bb0806b5ed5768eaa Mon Sep 17 00:00:00 2001 From: karishma Date: Wed, 6 Dec 2023 17:43:32 +0530 Subject: [PATCH] perception-traffic-light-classifier --- .../schema/traffic_light_classifier.json | 154 ++++++++++++++++++ .../src/cnn_classifier.cpp | 6 +- .../src/color_classifier.cpp | 36 ++-- 3 files changed, 175 insertions(+), 21 deletions(-) create mode 100644 perception/traffic_light_classifier/schema/traffic_light_classifier.json diff --git a/perception/traffic_light_classifier/schema/traffic_light_classifier.json b/perception/traffic_light_classifier/schema/traffic_light_classifier.json new file mode 100644 index 0000000000000..68ed65f6a9ae7 --- /dev/null +++ b/perception/traffic_light_classifier/schema/traffic_light_classifier.json @@ -0,0 +1,154 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for traffic_light_classifier", + "type": "object", + "definitions": { + "traffic_light_classifier": { + "type": "object", + "properties": { + "classifier_label_path": { + "type": "string", + "default": "labels.txt", + "description": "path to the model file." + }, + "classifier_model_path": { + "type": "string", + "default": "model.onnx", + "description": "path to the label file." + }, + "classifier_precision": { + "type": "string", + "default": "fp16", + "description": "TensorRT precision, `fp16` or `int8." + }, + "classifier_mean": { + "type": "number", + "default": 123.675, + "description": "3-channel input image mean." + }, + "classifier_std": { + "type": "number", + "default": 58.395, + "description": "3-channel input image std." + }, + "apply_softmax": { + "type": "boolean", + "description": "whether or not apply softmax." + }, + "green_min_h": { + "type": "number", + "default": 10, + "description": "the minimum hue of green color." + }, + "green_min_s": { + "type": "number", + "default": 100, + "description": "the minimum saturation of green color." + }, + "green_min_v": { + "type": "number", + "default": 150, + "description": "the minimum value (brightness) of green color." + }, + "green_max_h": { + "type": "number", + "default": 120, + "description": "the maximum hue of green color." + }, + "green_max_s": { + "type": "number", + "default": 200, + "description": "the maximum saturation of green color." + }, + "green_max_v": { + "type": "number", + "default": 255, + "description": "the maximum value (brightness) of green color." + }, + "yellow_min_h": { + "type": "number", + "default": 0, + "description": "the minimum hue of yellow color." + }, + "yellow_min_s": { + "type": "number", + "default": 80, + "description": "the minimum saturation of yellow color." + }, + "yellow_min_v": { + "type": "number", + "default": 150, + "description": "the minimum value (brightness) of yellow color." + }, + "yellow_max_h": { + "type": "number", + "default": 50, + "description": "the maximum hue of yellow color." + }, + "yellow_max_s": { + "type": "number", + "default": 200, + "description": "the maximum saturation of yellow color." + }, + "yellow_max_v": { + "type": "number", + "default": 255, + "description": "the maximum value (brightness) of yellow color." + }, + "red_min_h": { + "type": "number", + "default": 160, + "description": "the minimum hue of red color." + }, + "red_min_s": { + "type": "number", + "default": 100, + "description": "the minimum saturation of red color." + }, + "red_min_v": { + "type": "number", + "default": 150, + "description": "the minimum value (brightness) of red color." + }, + "red_max_h": { + "type": "number", + "default": 180, + "description": "the maximum hue of red color." + }, + "red_max_s": { + "type": "number", + "default": 255, + "description": "the maximum saturation of red color." + }, + "red_max_v": { + "type": "number", + "default": 255, + "description": "the maximum value (brightness) of red color." + } + }, + "required": [ + "warn_rate", + "window_size", + "timeout", + "error_rate", + "update_rate", + "topic", + "transient_local", + "best_effort", + "diag_name" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/traffic_light_classifier" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/traffic_light_classifier/src/cnn_classifier.cpp b/perception/traffic_light_classifier/src/cnn_classifier.cpp index 680314d98d794..9b156cbf90a03 100644 --- a/perception/traffic_light_classifier/src/cnn_classifier.cpp +++ b/perception/traffic_light_classifier/src/cnn_classifier.cpp @@ -33,9 +33,9 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) std::string precision; std::string label_file_path; std::string model_file_path; - precision = node_ptr_->declare_parameter("classifier_precision", "fp16"); - label_file_path = node_ptr_->declare_parameter("classifier_label_path", "labels.txt"); - model_file_path = node_ptr_->declare_parameter("classifier_model_path", "model.onnx"); + precision = node_ptr_->declare_parameter("classifier_precision"); + label_file_path = node_ptr_->declare_parameter("classifier_label_path"); + model_file_path = node_ptr_->declare_parameter("classifier_model_path"); // ros param does not support loading std::vector // we have to load std::vector and transfer to std::vector auto mean_d = diff --git a/perception/traffic_light_classifier/src/color_classifier.cpp b/perception/traffic_light_classifier/src/color_classifier.cpp index 51be12fce86e8..0edaea76073af 100644 --- a/perception/traffic_light_classifier/src/color_classifier.cpp +++ b/perception/traffic_light_classifier/src/color_classifier.cpp @@ -27,24 +27,24 @@ ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) 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); + hsv_config_.green_min_h = node_ptr_->declare_parameter("green_min_h"); + hsv_config_.green_min_s = node_ptr_->declare_parameter("green_min_s"); + hsv_config_.green_min_v = node_ptr_->declare_parameter("green_min_v"); + hsv_config_.green_max_h = node_ptr_->declare_parameter("green_max_h"); + hsv_config_.green_max_s = node_ptr_->declare_parameter("green_max_s"); + hsv_config_.green_max_v = node_ptr_->declare_parameter("green_max_v"); + hsv_config_.yellow_min_h = node_ptr_->declare_parameter("yellow_min_h"); + hsv_config_.yellow_min_s = node_ptr_->declare_parameter("yellow_min_s"); + hsv_config_.yellow_min_v = node_ptr_->declare_parameter("yellow_min_v"); + hsv_config_.yellow_max_h = node_ptr_->declare_parameter("yellow_max_h"); + hsv_config_.yellow_max_s = node_ptr_->declare_parameter("yellow_max_s"); + hsv_config_.yellow_max_v = node_ptr_->declare_parameter("yellow_max_v"); + hsv_config_.red_min_h = node_ptr_->declare_parameter("red_min_h"); + hsv_config_.red_min_s = node_ptr_->declare_parameter("red_min_s"); + hsv_config_.red_min_v = node_ptr_->declare_parameter("red_min_v"); + hsv_config_.red_max_h = node_ptr_->declare_parameter("red_max_h"); + hsv_config_.red_max_s = node_ptr_->declare_parameter("red_max_s"); + hsv_config_.red_max_v = node_ptr_->declare_parameter("red_max_v"); // set parameter callback set_param_res_ = node_ptr_->add_on_set_parameters_callback(