Skip to content

Commit 8f6a83d

Browse files
committed
perception-traffic-light-classifier
Signed-off-by: karishma <karishma@interpl.ai>
1 parent 60b4030 commit 8f6a83d

File tree

3 files changed

+175
-21
lines changed

3 files changed

+175
-21
lines changed
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for traffic_light_classifier",
4+
"type": "object",
5+
"definitions": {
6+
"traffic_light_classifier": {
7+
"type": "object",
8+
"properties": {
9+
"classifier_label_path": {
10+
"type": "string",
11+
"default": "labels.txt",
12+
"description": "path to the model file."
13+
},
14+
"classifier_model_path": {
15+
"type": "string",
16+
"default": "model.onnx",
17+
"description": "path to the label file."
18+
},
19+
"classifier_precision": {
20+
"type": "string",
21+
"default": "fp16",
22+
"description": "TensorRT precision, `fp16` or `int8."
23+
},
24+
"classifier_mean": {
25+
"type": "number",
26+
"default": 123.675,
27+
"description": "3-channel input image mean."
28+
},
29+
"classifier_std": {
30+
"type": "number",
31+
"default": 58.395,
32+
"description": "3-channel input image std."
33+
},
34+
"apply_softmax": {
35+
"type": "boolean",
36+
"description": "whether or not apply softmax."
37+
},
38+
"green_min_h": {
39+
"type": "number",
40+
"default": 10,
41+
"description": "the minimum hue of green color."
42+
},
43+
"green_min_s": {
44+
"type": "number",
45+
"default": 100,
46+
"description": "the minimum saturation of green color."
47+
},
48+
"green_min_v": {
49+
"type": "number",
50+
"default": 150,
51+
"description": "the minimum value (brightness) of green color."
52+
},
53+
"green_max_h": {
54+
"type": "number",
55+
"default": 120,
56+
"description": "the maximum hue of green color."
57+
},
58+
"green_max_s": {
59+
"type": "number",
60+
"default": 200,
61+
"description": "the maximum saturation of green color."
62+
},
63+
"green_max_v": {
64+
"type": "number",
65+
"default": 255,
66+
"description": "the maximum value (brightness) of green color."
67+
},
68+
"yellow_min_h": {
69+
"type": "number",
70+
"default": 0,
71+
"description": "the minimum hue of yellow color."
72+
},
73+
"yellow_min_s": {
74+
"type": "number",
75+
"default": 80,
76+
"description": "the minimum saturation of yellow color."
77+
},
78+
"yellow_min_v": {
79+
"type": "number",
80+
"default": 150,
81+
"description": "the minimum value (brightness) of yellow color."
82+
},
83+
"yellow_max_h": {
84+
"type": "number",
85+
"default": 50,
86+
"description": "the maximum hue of yellow color."
87+
},
88+
"yellow_max_s": {
89+
"type": "number",
90+
"default": 200,
91+
"description": "the maximum saturation of yellow color."
92+
},
93+
"yellow_max_v": {
94+
"type": "number",
95+
"default": 255,
96+
"description": "the maximum value (brightness) of yellow color."
97+
},
98+
"red_min_h": {
99+
"type": "number",
100+
"default": 160,
101+
"description": "the minimum hue of red color."
102+
},
103+
"red_min_s": {
104+
"type": "number",
105+
"default": 100,
106+
"description": "the minimum saturation of red color."
107+
},
108+
"red_min_v": {
109+
"type": "number",
110+
"default": 150,
111+
"description": "the minimum value (brightness) of red color."
112+
},
113+
"red_max_h": {
114+
"type": "number",
115+
"default": 180,
116+
"description": "the maximum hue of red color."
117+
},
118+
"red_max_s": {
119+
"type": "number",
120+
"default": 255,
121+
"description": "the maximum saturation of red color."
122+
},
123+
"red_max_v": {
124+
"type": "number",
125+
"default": 255,
126+
"description": "the maximum value (brightness) of red color."
127+
}
128+
},
129+
"required": [
130+
"warn_rate",
131+
"window_size",
132+
"timeout",
133+
"error_rate",
134+
"update_rate",
135+
"topic",
136+
"transient_local",
137+
"best_effort",
138+
"diag_name"
139+
]
140+
}
141+
},
142+
"properties": {
143+
"/**": {
144+
"type": "object",
145+
"properties": {
146+
"ros__parameters": {
147+
"$ref": "#/definitions/traffic_light_classifier"
148+
}
149+
},
150+
"required": ["ros__parameters"]
151+
}
152+
},
153+
"required": ["/**"]
154+
}

perception/traffic_light_classifier/src/cnn_classifier.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -33,9 +33,9 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr)
3333
std::string precision;
3434
std::string label_file_path;
3535
std::string model_file_path;
36-
precision = node_ptr_->declare_parameter("classifier_precision", "fp16");
37-
label_file_path = node_ptr_->declare_parameter("classifier_label_path", "labels.txt");
38-
model_file_path = node_ptr_->declare_parameter("classifier_model_path", "model.onnx");
36+
precision = node_ptr_->declare_parameter<std::string>("classifier_precision");
37+
label_file_path = node_ptr_->declare_parameter<std::string>("classifier_label_path");
38+
model_file_path = node_ptr_->declare_parameter<std::string>("classifier_model_path");
3939
// ros param does not support loading std::vector<float>
4040
// we have to load std::vector<double> and transfer to std::vector<float>
4141
auto mean_d =

perception/traffic_light_classifier/src/color_classifier.cpp

+18-18
Original file line numberDiff line numberDiff line change
@@ -27,24 +27,24 @@ ColorClassifier::ColorClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr)
2727
image_pub_ = image_transport::create_publisher(
2828
node_ptr_, "~/debug/image", rclcpp::QoS{1}.get_rmw_qos_profile());
2929

30-
hsv_config_.green_min_h = node_ptr_->declare_parameter("green_min_h", 50);
31-
hsv_config_.green_min_s = node_ptr_->declare_parameter("green_min_s", 100);
32-
hsv_config_.green_min_v = node_ptr_->declare_parameter("green_min_v", 150);
33-
hsv_config_.green_max_h = node_ptr_->declare_parameter("green_max_h", 120);
34-
hsv_config_.green_max_s = node_ptr_->declare_parameter("green_max_s", 200);
35-
hsv_config_.green_max_v = node_ptr_->declare_parameter("green_max_v", 255);
36-
hsv_config_.yellow_min_h = node_ptr_->declare_parameter("yellow_min_h", 0);
37-
hsv_config_.yellow_min_s = node_ptr_->declare_parameter("yellow_min_s", 80);
38-
hsv_config_.yellow_min_v = node_ptr_->declare_parameter("yellow_min_v", 150);
39-
hsv_config_.yellow_max_h = node_ptr_->declare_parameter("yellow_max_h", 50);
40-
hsv_config_.yellow_max_s = node_ptr_->declare_parameter("yellow_max_s", 200);
41-
hsv_config_.yellow_max_v = node_ptr_->declare_parameter("yellow_max_v", 255);
42-
hsv_config_.red_min_h = node_ptr_->declare_parameter("red_min_h", 160);
43-
hsv_config_.red_min_s = node_ptr_->declare_parameter("red_min_s", 100);
44-
hsv_config_.red_min_v = node_ptr_->declare_parameter("red_min_v", 150);
45-
hsv_config_.red_max_h = node_ptr_->declare_parameter("red_max_h", 180);
46-
hsv_config_.red_max_s = node_ptr_->declare_parameter("red_max_s", 255);
47-
hsv_config_.red_max_v = node_ptr_->declare_parameter("red_max_v", 255);
30+
hsv_config_.green_min_h = node_ptr_->declare_parameter<int>("green_min_h");
31+
hsv_config_.green_min_s = node_ptr_->declare_parameter<int>("green_min_s");
32+
hsv_config_.green_min_v = node_ptr_->declare_parameter<int>("green_min_v");
33+
hsv_config_.green_max_h = node_ptr_->declare_parameter<int>("green_max_h");
34+
hsv_config_.green_max_s = node_ptr_->declare_parameter<int>("green_max_s");
35+
hsv_config_.green_max_v = node_ptr_->declare_parameter<int>("green_max_v");
36+
hsv_config_.yellow_min_h = node_ptr_->declare_parameter<int>("yellow_min_h");
37+
hsv_config_.yellow_min_s = node_ptr_->declare_parameter<int>("yellow_min_s");
38+
hsv_config_.yellow_min_v = node_ptr_->declare_parameter<int>("yellow_min_v");
39+
hsv_config_.yellow_max_h = node_ptr_->declare_parameter<int>("yellow_max_h");
40+
hsv_config_.yellow_max_s = node_ptr_->declare_parameter<int>("yellow_max_s");
41+
hsv_config_.yellow_max_v = node_ptr_->declare_parameter<int>("yellow_max_v");
42+
hsv_config_.red_min_h = node_ptr_->declare_parameter<int>("red_min_h");
43+
hsv_config_.red_min_s = node_ptr_->declare_parameter<int>("red_min_s");
44+
hsv_config_.red_min_v = node_ptr_->declare_parameter<int>("red_min_v");
45+
hsv_config_.red_max_h = node_ptr_->declare_parameter<int>("red_max_h");
46+
hsv_config_.red_max_s = node_ptr_->declare_parameter<int>("red_max_s");
47+
hsv_config_.red_max_v = node_ptr_->declare_parameter<int>("red_max_v");
4848

4949
// set parameter callback
5050
set_param_res_ = node_ptr_->add_on_set_parameters_callback(

0 commit comments

Comments
 (0)