From c5d5fa39ff7186e6bbd0ffce480536ed66464501 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 5 Jul 2024 09:41:29 +0900 Subject: [PATCH 1/4] fix: update param declare Signed-off-by: badai-nguyen --- .../segmentation_pointcloud_fusion.param.yaml | 44 +++---- .../docs/segmentation-pointcloud-fusion.md | 4 +- .../segmentation_pointcloud_fusion/node.hpp | 9 +- ...segmentation_pointcloud_fusion.schema.json | 119 ++++++++++++++++++ .../segmentation_pointcloud_fusion/node.cpp | 13 +- 5 files changed, 156 insertions(+), 33 deletions(-) create mode 100644 perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 2120a909cd672..759e8237b2e7c 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -1,32 +1,26 @@ /**: ros__parameters: # if the semantic label is applied for pointcloud filtering + filter_semantic_label_target: - [ - true, # road - true, # sidewalk - true, # building - true, # wall - true, # fence - true, # pole - true, # traffic_light - true, # traffic_sign - true, # vegetation - true, # terrain - true, # sky - false, # person - false, # ride - false, # car - false, # truck - false, # bus - false, # train - false, # motorcycle - false, # bicycle - false, # others - ] - # the maximum distance of pointcloud to be applied filter, - # this is selected based on semantic segmentation model accuracy, - # calibration accuracy and unknown reaction distance + UNKNOWN: false + BUILDING: true + WALL: true + OBSTACLE: false + TRAFFIC_LIGHT: false + TRAFFIC_SIGN: false + PERSON: false + VEHICLE: false + BIKE: false + ROAD: true + SIDEWALK: false + ROADPAINT: false + CURBSTONE: false + CROSSWALK: false + VEGETATION: true + SKY: false + + # the maximum distance of pointcloud to be applied filter filter_distance_threshold: 60.0 # debug diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index d59e804f1228c..3c469ac15c6e7 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -32,9 +32,7 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Core Parameters -| Name | Type | Description | -| ------------- | ---- | ------------------------ | -| `rois_number` | int | the number of input rois | +{{ json_to_markdown("perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json") }} ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 26bcfd5ed802a..9a2b001be9733 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #if __has_include() @@ -36,7 +37,13 @@ class SegmentPointCloudFusionNode : public FusionNode::SharedPtr pub_pointcloud_ptr_; std::vector filter_semantic_label_target_; float filter_distance_threshold_; - /* data */ + // declare list of semantic label target, depend on trained data of yolox segmentation model + std::vector> filter_semantic_label_target_list_ = { + {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, + {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROADPAINT", false}, + {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; + public: explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json new file mode 100644 index 0000000000000..50ace081e727f --- /dev/null +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -0,0 +1,119 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Segmentation Pointcloud Fusion Node", + "type": "object", + "definitions": { + "segmentation_pointcloud_fusion": { + "type": "object", + "properties": { + "filter_semantic_label_target":{ + "type": "object", + "properties": { + "UNKNOWN": { + "type": "boolean", + "description": "If true, UNKNOWN class of semantic will be filtered.", + "default": false + }, + "BUILDING": { + "type": "boolean", + "description": "If true, BUILDING class of semantic will be filtered.", + "default": true + }, + "WALL": { + "type": "boolean", + "description": "If true, WALL class of semantic will be filtered.", + "default": true + }, + "OBSTACLE": { + "type": "boolean", + "description": "If true, OBSTACLE class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_LIGHT": { + "type": "boolean", + "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_SIGN": { + "type": "boolean", + "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", + "default": false + }, + "PERSON": { + "type": "boolean", + "description": "If true, PERSON class of semantic will be filtered.", + "default": false + }, + "VEHICLE": { + "type": "boolean", + "description": "If true, VEHICLE class of semantic will be filtered.", + "default": false + }, + "BIKE": { + "type": "boolean", + "description": "If true, BIKE class of semantic will be filtered.", + "default": false + }, + "ROAD": { + "type": "boolean", + "description": "If true, ROAD class of semantic will be filtered.", + "default": true + }, + "SIDEWALK": { + "type": "boolean", + "description": "If true, SIDEWALK class of semantic will be filtered.", + "default": false + }, + "ROADPAINT": { + "type": "boolean", + "description": "If true, ROADPAINT class of semantic will be filtered.", + "default": false + }, + "CURBSTONE": { + "type": "boolean", + "description": "If true, CURBSTONE class of semantic will be filtered.", + "default": false + }, + "CROSSWALK": { + "type": "boolean", + "description": "If true, CROSSWALK class of semantic will be filtered.", + "default": false + }, + "VEGETATION": { + "type": "boolean", + "description": "If true, VEGETATION class of semantic will be filtered.", + "default": true + }, + "SKY": { + "type": "boolean", + "description": "If true, SKY class of semantic will be filtered.", + "default": false + } + }, + "required": [ + "UNKNOWN", "BUILDING", "WALL", "OBSTACLE", "TRAFFIC_LIGHT", "TRAFFIC_SIGN", "PERSON", "VEHICLE", "BIKE", "ROAD", "SIDEWALK", "ROADPAINT", "CURBSTONE", "CROSSWALK", "VEGETATION", "SKY"] + }, + "filter_distance_threshold":{ + "type": "number", + "description": "A maximum distance of pointcloud to apply filter [m].", + "default": 60.0, + "minimum": 0.0 + } + }, + "required": ["filter_semantic_label_target","filter_distance_threshold"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/segmentation_pointcloud_fusion" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} + \ No newline at end of file diff --git a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index a3117125a46b1..9a983252af436 100644 --- a/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -31,8 +31,13 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio : FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); - filter_semantic_label_target_ = - declare_parameter>("filter_semantic_label_target"); + for (auto & item : filter_semantic_label_target_list_) { + item.second = declare_parameter("filter_semantic_label_target." + item.first); + } + for (const auto & item : filter_semantic_label_target_list_) { + RCLCPP_INFO( + this->get_logger(), "filter_semantic_label_target: %s %d", item.first.c_str(), item.second); + } } void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) @@ -129,12 +134,12 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( // skip filtering pointcloud where semantic id out of the defined list uint8_t semantic_id = mask.at( static_cast(projected_point.y()), static_cast(projected_point.x())); - if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { + if (static_cast(semantic_id) >= filter_semantic_label_target_list_.size()) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } - if (!filter_semantic_label_target_.at(semantic_id)) { + if (!filter_semantic_label_target_list_.at(semantic_id).second) { copyPointCloud( input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } From cb865a02230d807d340e5c375eea24b33265d7d3 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 5 Jul 2024 09:53:29 +0900 Subject: [PATCH 2/4] chore: remove unnecessary params Signed-off-by: badai-nguyen --- .../config/segmentation_pointcloud_fusion.param.yaml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index 759e8237b2e7c..d8b053dd9475a 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -22,12 +22,3 @@ # the maximum distance of pointcloud to be applied filter filter_distance_threshold: 60.0 - - # debug - debug_mode: false - filter_scope_min_x: -100.0 - filter_scope_max_x: 100.0 - filter_scope_min_y: -100.0 - filter_scope_max_y: 100.0 - filter_scope_min_z: -100.0 - filter_scope_max_z: 100.0 From 9a3607bd3ed2eaa5d725ad50bd29e1dc6bc5835f Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 5 Jul 2024 09:56:36 +0900 Subject: [PATCH 3/4] fix: pre-commit Signed-off-by: badai-nguyen --- ...segmentation_pointcloud_fusion.schema.json | 185 ++++++++++-------- 1 file changed, 100 insertions(+), 85 deletions(-) diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json index 50ace081e727f..2f5f3f0a7ca3d 100644 --- a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -6,101 +6,117 @@ "segmentation_pointcloud_fusion": { "type": "object", "properties": { - "filter_semantic_label_target":{ + "filter_semantic_label_target": { "type": "object", "properties": { - "UNKNOWN": { - "type": "boolean", - "description": "If true, UNKNOWN class of semantic will be filtered.", - "default": false - }, - "BUILDING": { - "type": "boolean", - "description": "If true, BUILDING class of semantic will be filtered.", - "default": true - }, - "WALL": { - "type": "boolean", - "description": "If true, WALL class of semantic will be filtered.", - "default": true - }, - "OBSTACLE": { - "type": "boolean", - "description": "If true, OBSTACLE class of semantic will be filtered.", - "default": false - }, - "TRAFFIC_LIGHT": { - "type": "boolean", - "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", - "default": false - }, - "TRAFFIC_SIGN": { - "type": "boolean", - "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", - "default": false - }, - "PERSON": { - "type": "boolean", - "description": "If true, PERSON class of semantic will be filtered.", - "default": false - }, - "VEHICLE": { - "type": "boolean", - "description": "If true, VEHICLE class of semantic will be filtered.", - "default": false - }, - "BIKE": { - "type": "boolean", - "description": "If true, BIKE class of semantic will be filtered.", - "default": false - }, - "ROAD": { - "type": "boolean", - "description": "If true, ROAD class of semantic will be filtered.", - "default": true - }, - "SIDEWALK": { - "type": "boolean", - "description": "If true, SIDEWALK class of semantic will be filtered.", - "default": false - }, - "ROADPAINT": { - "type": "boolean", - "description": "If true, ROADPAINT class of semantic will be filtered.", - "default": false - }, - "CURBSTONE": { - "type": "boolean", - "description": "If true, CURBSTONE class of semantic will be filtered.", - "default": false - }, - "CROSSWALK": { - "type": "boolean", - "description": "If true, CROSSWALK class of semantic will be filtered.", - "default": false - }, - "VEGETATION": { - "type": "boolean", - "description": "If true, VEGETATION class of semantic will be filtered.", - "default": true - }, - "SKY": { - "type": "boolean", - "description": "If true, SKY class of semantic will be filtered.", - "default": false - } + "UNKNOWN": { + "type": "boolean", + "description": "If true, UNKNOWN class of semantic will be filtered.", + "default": false + }, + "BUILDING": { + "type": "boolean", + "description": "If true, BUILDING class of semantic will be filtered.", + "default": true + }, + "WALL": { + "type": "boolean", + "description": "If true, WALL class of semantic will be filtered.", + "default": true + }, + "OBSTACLE": { + "type": "boolean", + "description": "If true, OBSTACLE class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_LIGHT": { + "type": "boolean", + "description": "If true, TRAFFIC_LIGHT class of semantic will be filtered.", + "default": false + }, + "TRAFFIC_SIGN": { + "type": "boolean", + "description": "If true, TRAFFIC_SIGN class of semantic will be filtered.", + "default": false + }, + "PERSON": { + "type": "boolean", + "description": "If true, PERSON class of semantic will be filtered.", + "default": false + }, + "VEHICLE": { + "type": "boolean", + "description": "If true, VEHICLE class of semantic will be filtered.", + "default": false + }, + "BIKE": { + "type": "boolean", + "description": "If true, BIKE class of semantic will be filtered.", + "default": false + }, + "ROAD": { + "type": "boolean", + "description": "If true, ROAD class of semantic will be filtered.", + "default": true + }, + "SIDEWALK": { + "type": "boolean", + "description": "If true, SIDEWALK class of semantic will be filtered.", + "default": false + }, + "ROADPAINT": { + "type": "boolean", + "description": "If true, ROADPAINT class of semantic will be filtered.", + "default": false + }, + "CURBSTONE": { + "type": "boolean", + "description": "If true, CURBSTONE class of semantic will be filtered.", + "default": false + }, + "CROSSWALK": { + "type": "boolean", + "description": "If true, CROSSWALK class of semantic will be filtered.", + "default": false + }, + "VEGETATION": { + "type": "boolean", + "description": "If true, VEGETATION class of semantic will be filtered.", + "default": true + }, + "SKY": { + "type": "boolean", + "description": "If true, SKY class of semantic will be filtered.", + "default": false + } }, "required": [ - "UNKNOWN", "BUILDING", "WALL", "OBSTACLE", "TRAFFIC_LIGHT", "TRAFFIC_SIGN", "PERSON", "VEHICLE", "BIKE", "ROAD", "SIDEWALK", "ROADPAINT", "CURBSTONE", "CROSSWALK", "VEGETATION", "SKY"] + "UNKNOWN", + "BUILDING", + "WALL", + "OBSTACLE", + "TRAFFIC_LIGHT", + "TRAFFIC_SIGN", + "PERSON", + "VEHICLE", + "BIKE", + "ROAD", + "SIDEWALK", + "ROADPAINT", + "CURBSTONE", + "CROSSWALK", + "VEGETATION", + "SKY" + ] }, - "filter_distance_threshold":{ + "filter_distance_threshold": { "type": "number", "description": "A maximum distance of pointcloud to apply filter [m].", "default": 60.0, "minimum": 0.0 } }, - "required": ["filter_semantic_label_target","filter_distance_threshold"] + "required": ["filter_semantic_label_target", "filter_distance_threshold"] } }, "properties": { @@ -116,4 +132,3 @@ }, "required": ["/**"] } - \ No newline at end of file From 7e13702f6c1a972feec98e6f4d71e9becb2e9321 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Fri, 5 Jul 2024 10:44:16 +0900 Subject: [PATCH 4/4] fix: cspell error Signed-off-by: badai-nguyen --- .../config/segmentation_pointcloud_fusion.param.yaml | 2 +- .../segmentation_pointcloud_fusion/node.hpp | 2 +- .../schema/segmentation_pointcloud_fusion.schema.json | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml index d8b053dd9475a..fdabb0c7055d8 100644 --- a/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml +++ b/perception/image_projection_based_fusion/config/segmentation_pointcloud_fusion.param.yaml @@ -14,7 +14,7 @@ BIKE: false ROAD: true SIDEWALK: false - ROADPAINT: false + ROAD_PAINT: false CURBSTONE: false CROSSWALK: false VEGETATION: true diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 9a2b001be9733..89b33775f7898 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -41,7 +41,7 @@ class SegmentPointCloudFusionNode : public FusionNode> filter_semantic_label_target_list_ = { {"UNKNOWN", false}, {"BUILDING", false}, {"WALL", false}, {"OBSTACLE", false}, {"TRAFFIC_LIGHT", false}, {"TRAFFIC_SIGN", false}, {"PERSON", false}, {"VEHICLE", false}, - {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROADPAINT", false}, + {"BIKE", false}, {"ROAD", false}, {"SIDEWALK", false}, {"ROAD_PAINT", false}, {"CURBSTONE", false}, {"CROSSWALK", false}, {"VEGETATION", false}, {"SKY", false}}; public: diff --git a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json index 2f5f3f0a7ca3d..f5ab1be5eac34 100644 --- a/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json +++ b/perception/image_projection_based_fusion/schema/segmentation_pointcloud_fusion.schema.json @@ -64,9 +64,9 @@ "description": "If true, SIDEWALK class of semantic will be filtered.", "default": false }, - "ROADPAINT": { + "ROAD_PAINT": { "type": "boolean", - "description": "If true, ROADPAINT class of semantic will be filtered.", + "description": "If true, ROAD_PAINT class of semantic will be filtered.", "default": false }, "CURBSTONE": { @@ -102,7 +102,7 @@ "BIKE", "ROAD", "SIDEWALK", - "ROADPAINT", + "ROAD_PAINT", "CURBSTONE", "CROSSWALK", "VEGETATION",