Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(autoware_pointcloud_preprocessor): rework vector map inside area filter parameters #8493

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
src/distortion_corrector/distortion_corrector_node.cpp
src/blockage_diag/blockage_diag_node.cpp
src/polygon_remover/polygon_remover.cpp
src/vector_map_filter/vector_map_inside_area_filter.cpp
src/vector_map_filter/vector_map_inside_area_filter_node.cpp
src/utility/geometry.cpp
)

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
/**:
ros__parameters:
polygon_type: "no_obstacle_segmentation_area"
use_z_filter: false
z_threshold: 0.0
Original file line number Diff line number Diff line change
Expand Up @@ -33,10 +33,6 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class,

### Core Parameters

| Name | Type | Description |
| -------------- | ------ | --------------------------- |
| `polygon_type` | string | polygon type to be filtered |
| `use_z` | bool | use z value for filtering |
| `z_threshold` | float | z threshold for filtering |
{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/vector_map_inside_area_filter_node.schema.json") }}

## Assumptions / Known limits
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_
#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_NODE_HPP_ // NOLINT
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_NODE_HPP_ // NOLINT

#include "autoware/pointcloud_preprocessor/filter.hpp"
#include "autoware/pointcloud_preprocessor/utility/geometry.hpp"
Expand Down Expand Up @@ -58,4 +58,6 @@ class VectorMapInsideAreaFilterComponent : public autoware::pointcloud_preproces

} // namespace autoware::pointcloud_preprocessor

#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_HPP_
// clang-format off
#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__VECTOR_MAP_FILTER__VECTOR_MAP_INSIDE_AREA_FILTER_NODE_HPP_ // NOLINT
// clang-format on
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<launch>
<arg name="input_vector_map" default="/map/vector_map"/>
<arg name="input_pointcloud" default="compare_map_filtered/pointcloud"/>
<arg name="ouput_pointcloud" default="vector_map_inside_area_filtered/pointcloud"/>

Check warning on line 4 in sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (ouput)
<arg name="vector_map_inside_area_filter_param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/vector_map_inside_area_filter_node.param.yaml"/>
<arg name="filter_param_file" default="$(find-pkg-share autoware_pointcloud_preprocessor)/config/filter.param.yaml"/>
<node pkg="autoware_pointcloud_preprocessor" exec="vector_map_inside_area_filter_node" name="vector_map_inside_area_filter_node">
<param from="$(var vector_map_inside_area_filter_param_file)"/>
<param from="$(var filter_param_file)"/>
<remap from="input/vector_map" to="$(var input_vector_map)"/>
<remap from="input" to="$(var input_pointcloud)"/>
<remap from="output" to="$(var ouput_pointcloud)"/>

Check warning on line 12 in sensing/autoware_pointcloud_preprocessor/launch/vector_map_inside_area_filter_node.launch.xml

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (ouput)
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for Vector Map Inside Area Filter Node",
"type": "object",
"definitions": {
"vector_map_inside_area_filter": {
"type": "object",
"properties": {
"polygon_type": {
"type": "string",
"description": "polygon type to be filtered",
"default": "no_obstacle_segmentation_area"
},
"use_z_filter": {
"type": "boolean",
"description": "use z value for filtering",
"default": "false"
},
"z_threshold": {
amadeuszsz marked this conversation as resolved.
Show resolved Hide resolved
"type": "number",
"description": "z threshold for filtering",
"default": "0.0"
}
},
"required": ["polygon_type", "use_z_filter", "z_threshold"],
"additionalProperties": false
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/vector_map_inside_area_filter"
}
},
"required": ["ros__parameters"],
"additionalProperties": false
}
},
"required": ["/**"],
"additionalProperties": false
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 Tier IV, Inc.
// Copyright 2024 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.
Expand All @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp"
#include "autoware/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter_node.hpp"

namespace
{
Expand Down Expand Up @@ -67,8 +67,7 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
const rclcpp::NodeOptions & node_options)
: Filter("VectorMapInsideAreaFilter", node_options)
{
polygon_type_ =
static_cast<std::string>(declare_parameter("polygon_type", "no_obstacle_segmentation_area"));
polygon_type_ = declare_parameter<std::string>("polygon_type");

using std::placeholders::_1;
// Set subscriber
Expand All @@ -77,8 +76,8 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1));

// Set parameters
use_z_filter_ = declare_parameter<bool>("use_z_filter", false);
z_threshold_ = declare_parameter<float>("z_threshold", 0.0f); // defined in the base_link frame
use_z_filter_ = declare_parameter<bool>("use_z_filter");
z_threshold_ = declare_parameter<float>("z_threshold"); // defined in the base_link frame

// Set tf
{
Expand Down
Loading