Skip to content

Commit

Permalink
add param and schema file, edit readme
Browse files Browse the repository at this point in the history
Signed-off-by: oguzkaganozt <[email protected]>
  • Loading branch information
oguzkaganozt committed Apr 4, 2024
1 parent 41aab51 commit af4df4b
Show file tree
Hide file tree
Showing 5 changed files with 113 additions and 22 deletions.
5 changes: 4 additions & 1 deletion perception/occupancy_grid_map_outlier_filter/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,7 @@ rclcpp_components_register_node(occupancy_grid_map_outlier_filter
PLUGIN "occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent"
EXECUTABLE occupancy_grid_map_outlier_filter_node)

ament_auto_package(INSTALL_TO_SHARE)
ament_auto_package(
INSTALL_TO_SHARE
config
)
12 changes: 1 addition & 11 deletions perception/occupancy_grid_map_outlier_filter/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -40,17 +40,7 @@ The following video is a sample. Yellow points are high occupancy probability, g

## Parameters

| Name | Type | Description |
| ------------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `map_frame` | string | map frame id |
| `base_link_frame` | string | base link frame id |
| `cost_threshold` | int | Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle. |
| `enable_debugger` | bool | Whether to output the point cloud for debugging. |
| `use_radius_search_2d_filter` | bool | Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map. |
| `radius_search_2d_filter/search_radius` | float | Radius when calculating the density |
| `radius_search_2d_filter/min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. |
| `radius_search_2d_filter/min_points` | int | Minimum number of point clouds per radius |
| `radius_search_2d_filter/max_points` | int | Maximum number of point clouds per radius |
{{ json_to_markdown("perception/occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }}

## Assumptions / Known limits

Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
/**:
ros__parameters:
radius_search_2d_filter.search_radius: 1.0f
radius_search_2d_filter.min_points_and_distance_ratio: 400.0f
radius_search_2d_filter.min_points: 4
radius_search_2d_filter.max_points: 70
radius_search_2d_filter.max_filter_points_nb: 15000
map_frame: "map"
base_link_frame: "base_link"
cost_threshold: 45
use_radius_search_2d_filter: true
enable_debugger: false
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for occupancy_grid_map_outlier",
"type": "object",
"definitions": {
"occupancy_grid_map_outlier": {
"type": "object",
"properties": {
"radius_search_2d_filter.search_radius": {
"type": "number",
"default": 1.0,
"description": "Radius when calculating the density"
},
"radius_search_2d_filter.min_points_and_distance_ratio": {
"type": "number",
"default": 400.0,
"description": "Threshold value of the number of point clouds per radius when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink"
},
"radius_search_2d_filter.min_points": {
"type": "number",
"default": 4,
"description": "Minimum number of point clouds per radius"
},
"radius_search_2d_filter.max_points": {
"type": "number",
"default": 70,
"description": "Maximum number of point clouds per radius"
},
"radius_search_2d_filter.max_filter_points_nb": {
"type": "number",
"default": 15000,
"description": "Maximum number of point clouds to be filtered"
},
"map_frame": {
"type": "string",
"default": "map",
"description": "map frame id"
},
"base_link_frame": {
"type": "string",
"default": "base_link",
"description": "base link frame id"
},
"cost_threshold": {
"type": "number",
"default": 45,
"description": "Cost threshold of occupancy grid map (0~100). 100 means 100% probability that there is an obstacle, close to 50 means that it is indistinguishable whether it is an obstacle or free space, 0 means that there is no obstacle"
},
"use_radius_search_2d_filter": {
"type": "boolean",
"default": true,
"description": "Whether or not to apply density-based outlier filters to objects that are judged to have low probability of occupancy on the occupancy grid map"
},
"enable_debugger": {
"type": "boolean",
"default": false,
"description": "Whether to output the point cloud for debugging"
}
},
"required": [
"radius_search_2d_filter.search_radius",
"radius_search_2d_filter.min_points_and_distance_ratio",
"radius_search_2d_filter.min_points",
"radius_search_2d_filter.max_points",
"radius_search_2d_filter.max_filter_points_nb",
"map_frame",
"base_link_frame",
"cost_threshold",
"use_radius_search_2d_filter",
"enable_debugger"
]
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/occupancy_grid_map_outlier"
}
},
"required": ["ros__parameters"]
}
},
"required": ["/**"]
}
Original file line number Diff line number Diff line change
Expand Up @@ -105,13 +105,13 @@ namespace occupancy_grid_map_outlier_filter
{
RadiusSearch2dFilter::RadiusSearch2dFilter(rclcpp::Node & node)
{
search_radius_ = node.declare_parameter("radius_search_2d_filter.search_radius", 1.0f);
search_radius_ = node.declare_parameter<float>("radius_search_2d_filter.search_radius");
min_points_and_distance_ratio_ =
node.declare_parameter("radius_search_2d_filter.min_points_and_distance_ratio", 400.0f);
min_points_ = node.declare_parameter("radius_search_2d_filter.min_points", 4);
max_points_ = node.declare_parameter("radius_search_2d_filter.max_points", 70);
node.declare_parameter<float>("radius_search_2d_filter.min_points_and_distance_ratio");
min_points_ = node.declare_parameter<int>("radius_search_2d_filter.min_points");
max_points_ = node.declare_parameter<int>("radius_search_2d_filter.max_points", 70);
max_filter_points_nb_ =
node.declare_parameter("radius_search_2d_filter.max_filter_points_nb", 15000);
node.declare_parameter<int>("radius_search_2d_filter.max_filter_points_nb", 15000);
kd_tree_ = pcl::make_shared<pcl::search::KdTree<pcl::PointXY>>(false);
}

Expand Down Expand Up @@ -206,11 +206,11 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
}

/* params */
map_frame_ = declare_parameter("map_frame", "map");
base_link_frame_ = declare_parameter("base_link_frame", "base_link");
cost_threshold_ = declare_parameter("cost_threshold", 45);
auto use_radius_search_2d_filter = declare_parameter("use_radius_search_2d_filter", true);
auto enable_debugger = declare_parameter("enable_debugger", false);
map_frame_ = declare_parameter<std::string>("map_frame");
base_link_frame_ = declare_parameter<std::string>("base_link_frame");
cost_threshold_ = declare_parameter<int>("cost_threshold");
auto use_radius_search_2d_filter = declare_parameter<bool>("use_radius_search_2d_filter");
auto enable_debugger = declare_parameter<bool>("enable_debugger");

/* tf */
tf2_ = std::make_shared<tf2_ros::Buffer>(get_clock());
Expand Down

0 comments on commit af4df4b

Please sign in to comment.