Skip to content

Commit

Permalink
Merge branch 'main' into fix/delete_front_vehicle_velocity_estimator
Browse files Browse the repository at this point in the history
  • Loading branch information
scepter914 authored Jan 31, 2024
2 parents fa257dd + 17a3070 commit 9ff3861
Show file tree
Hide file tree
Showing 39 changed files with 323 additions and 124 deletions.
3 changes: 2 additions & 1 deletion common/autoware_auto_perception_rviz_plugin/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,8 @@
<maintainer email="[email protected]">Satoshi Tanaka</maintainer>
<maintainer email="[email protected]">Taiki Tanaka</maintainer>
<maintainer email="[email protected]">Takeshi Miura</maintainer>

<maintainer email="[email protected]">Shunsuke Miura</maintainer>
<maintainer email="[email protected]">Yoshi Ri</maintainer>
<license>Apache 2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>
Expand Down
1 change: 1 addition & 0 deletions common/object_recognition_utils/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<maintainer email="[email protected]">Satoshi Tanaka</maintainer>
<maintainer email="[email protected]">Yusuke Muramatsu</maintainer>
<maintainer email="[email protected]">Shunsuke Miura</maintainer>
<maintainer email="[email protected]">Yoshi Ri</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
Expand Down
1 change: 1 addition & 0 deletions common/perception_utils/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
<maintainer email="[email protected]">Satoshi Tanaka</maintainer>
<maintainer email="[email protected]">Yusuke Muramatsu</maintainer>
<maintainer email="[email protected]">Shunsuke Miura</maintainer>
<maintainer email="[email protected]">Yoshi Ri</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>
Expand Down
3 changes: 3 additions & 0 deletions control/control_validator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,9 @@
<maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
<maintainer email="[email protected]">Takamasa Horibe</maintainer>
<maintainer email="[email protected]">Makoto Kurihara</maintainer>
<maintainer email="[email protected]">Mamoru Sobue</maintainer>
<maintainer email="[email protected]">Takayuki Murooka</maintainer>

<license>Apache License 2.0</license>

<author email="[email protected]">Kyoichi Sugahara</author>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@
<arg name="output/objects" value="far_objects"/>
<arg name="filter/angle_threshold" value="1.0472"/>
<arg name="filter/velocity_threshold" value="3.0"/>
<arg name="split/velocity_threshold" value="4.5"/>
<arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_fusion_param_path)"/>
<arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_fusion_param_path)"/>
<arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
<arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
Expand Down Expand Up @@ -177,6 +177,7 @@
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="output/objects" value="far_objects"/>
<arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_fusion_param_path)"/>
<arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_fusion_param_path)"/>
<arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
<arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
Expand Down Expand Up @@ -237,6 +238,7 @@
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
<arg name="input/radar" value="$(var input/radar)"/>
<arg name="output/objects" value="objects"/>
<arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_param_path)"/>
<arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_param_path)"/>
<arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
<arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
<arg name="output/objects" default="far_objects"/>
<arg name="filter/angle_threshold" default="1.221"/>
<arg name="filter/velocity_threshold" default="1.5"/>
<arg name="split/velocity_threshold" default="5.5"/>
<arg name="object_velocity_splitter_param_path" default="$(var object_recognition_detection_object_velocity_splitter_radar_param_path)"/>
<arg name="object_range_splitter_param_path" default="$(var object_recognition_detection_object_range_splitter_radar_param_path)"/>
<arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
<arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
Expand All @@ -22,7 +22,7 @@
<arg name="input/objects" value="noise_filtered_objects"/>
<arg name="output/low_speed_objects" value="low_speed_objects"/>
<arg name="output/high_speed_objects" value="high_speed_objects"/>
<arg name="velocity_threshold" value="$(var split/velocity_threshold)"/>
<arg name="param_path" value="$(var object_velocity_splitter_param_path)"/>
</include>

<include file="$(find-pkg-share object_range_splitter)/launch/object_range_splitter.launch.xml">
Expand Down
2 changes: 2 additions & 0 deletions launch/tier4_perception_launch/launch/perception.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
<arg name="object_recognition_detection_lidar_model_param_path"/>
<arg name="object_recognition_detection_radar_lanelet_filtering_range_param"/>
<arg name="object_recognition_detection_radar_object_clustering_param_path"/>
<arg name="object_recognition_detection_object_velocity_splitter_radar_param_path"/>
<arg name="object_recognition_detection_object_velocity_splitter_radar_fusion_param_path"/>
<arg name="object_recognition_detection_object_range_splitter_radar_param_path"/>
<arg name="object_recognition_detection_object_range_splitter_radar_fusion_param_path"/>
<arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
Expand Down
2 changes: 2 additions & 0 deletions mkdocs_macros.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ def format_param_range(param):
def extract_parameter_info(parameters, namespace=""):
params = []
for k, v in parameters.items():
if "$ref" in v.keys():
continue
if v["type"] != "object":
param = {}
param["Name"] = namespace + k
Expand Down
1 change: 1 addition & 0 deletions perception/ground_segmentation/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ if(BUILD_TESTING)

target_link_libraries(test_ray_ground_filter
ground_segmentation
${YAML_CPP_LIBRARIES}
)

ament_add_ros_isolated_gtest(test_scan_ground_filter
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,3 +29,7 @@
gnd_grid_buffer_size: 4
detection_range_z_max: 2.5
elevation_grid_mode: true
low_priority_region_x: -20.0
center_pcl_shift: 0.0
radial_divider_angle_deg: 1.0
use_recheck_ground_cluster: true
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
/**:
ros__parameters:
base_frame: "base_link"
unit_axis: "z"
max_iterations: 1000
min_trial: 5000
min_points: 1000
outlier_threshold: 0.01
plane_slope_threshold: 10.0
voxel_size_x: 0.04
voxel_size_y: 0.04
voxel_size_z: 0.04
height_threshold: 0.01
debug: false
14 changes: 14 additions & 0 deletions perception/ground_segmentation/config/ray_ground_filter.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
/**:
ros__parameters:
min_x: -0.01
max_x: 0.01
min_y: -0.01
max_y: 0.01
use_vehicle_footprint: false
general_max_slope: 8.0
local_max_slope: 6.0
initial_max_slope: 3.0
radial_divider_angle: 1.0
min_height_threshold: 0.15
concentric_divider_distance: 0.0
reclass_distance_threshold: 0.1
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
/**:
ros__parameters:
global_slope_max_angle_deg: 10.0
local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
split_points_distance_tolerance: 0.2
use_virtual_ground_point: True
split_height_distance: 0.2
non_ground_height_threshold: 0.20
grid_size_m: 0.1
grid_mode_switch_radius: 20.0
gnd_grid_buffer_size: 4
detection_range_z_max: 2.5
elevation_grid_mode: true
low_priority_region_x: -20.0
center_pcl_shift: 0.0
radial_divider_angle_deg: 1.0
use_recheck_ground_cluster: true
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ransac_ground_filter.param.yaml"/>
<arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
<arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<node pkg="ground_segmentation" exec="ransac_ground_filter_node" name="ransac_ground_filter_node" output="screen">
<remap from="input" to="$(var input/pointcloud)"/>
<remap from="output" to="$(var output/pointcloud)"/>
<param from="$(var ground_segmentation_param_file)"/>
</node>
</launch>
11 changes: 11 additions & 0 deletions perception/ground_segmentation/launch/ray_ground_filter.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ray_ground_filter.param.yaml"/>
<arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
<arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<node pkg="ground_segmentation" exec="ray_ground_filter_node" name="ray_ground_filter_node" output="screen">
<remap from="input" to="$(var input/pointcloud)"/>
<remap from="output" to="$(var output/pointcloud)"/>
<param from="$(var ground_segmentation_param_file)"/>
</node>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
<?xml version="1.0"?>
<launch>
<arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/scan_ground_filter.param.yaml"/>

<!-- vehicle info -->
<arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
<arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
<arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
<node pkg="ground_segmentation" exec="scan_ground_filter_node" name="scan_ground_filter_node" output="screen">
<remap from="input" to="$(var input/pointcloud)"/>
<remap from="output" to="$(var output/pointcloud)"/>
<param from="$(var ground_segmentation_param_file)"/>
<param from="$(var vehicle_info_param_file)"/>
</node>
</launch>
24 changes: 12 additions & 12 deletions perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param;
RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
: Filter("RANSACGroundFilter", options)
{
base_frame_ = declare_parameter("base_frame", "base_link");
unit_axis_ = declare_parameter("unit_axis", "z");
max_iterations_ = declare_parameter("max_iterations", 1000);
min_inliers_ = declare_parameter("min_trial", 5000);
min_points_ = declare_parameter("min_points", 1000);
outlier_threshold_ = declare_parameter("outlier_threshold", 0.01);
plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0);
voxel_size_x_ = declare_parameter("voxel_size_x", 0.04);
voxel_size_y_ = declare_parameter("voxel_size_y", 0.04);
voxel_size_z_ = declare_parameter("voxel_size_z", 0.04);
height_threshold_ = declare_parameter("height_threshold", 0.01);
debug_ = declare_parameter("debug", false);
base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
unit_axis_ = declare_parameter<std::string>("unit_axis");
max_iterations_ = declare_parameter<int>("max_iterations");
min_inliers_ = declare_parameter<int>("min_trial");
min_points_ = declare_parameter<int>("min_points");
outlier_threshold_ = declare_parameter<double>("outlier_threshold");
plane_slope_threshold_ = declare_parameter<double>("plane_slope_threshold");
voxel_size_x_ = declare_parameter<double>("voxel_size_x");
voxel_size_y_ = declare_parameter<double>("voxel_size_y");
voxel_size_z_ = declare_parameter<double>("voxel_size_z");
height_threshold_ = declare_parameter<double>("height_threshold");
debug_ = declare_parameter<bool>("debug");

if (unit_axis_ == "x") {
unit_vec_ = Eigen::Vector3d::UnitX();
Expand Down
24 changes: 12 additions & 12 deletions perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o
grid_precision_ = 0.2;
ray_ground_filter::generateColors(colors_, color_num_);

min_x_ = declare_parameter("min_x", -0.01);
max_x_ = declare_parameter("max_x", 0.01);
min_y_ = declare_parameter("min_y", -0.01);
max_y_ = declare_parameter("max_y", 0.01);
min_x_ = declare_parameter<double>("min_x");
max_x_ = declare_parameter<double>("max_x");
min_y_ = declare_parameter<double>("min_y");
max_y_ = declare_parameter<double>("max_y");

setVehicleFootprint(min_x_, max_x_, min_y_, max_y_);

use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false);
use_vehicle_footprint_ = declare_parameter<bool>("use_vehicle_footprint", false);

general_max_slope_ = declare_parameter("general_max_slope", 8.0);
local_max_slope_ = declare_parameter("local_max_slope", 6.0);
initial_max_slope_ = declare_parameter("initial_max_slope", 3.0);
radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0);
min_height_threshold_ = declare_parameter("min_height_threshold", 0.15);
concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0);
reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1);
general_max_slope_ = declare_parameter<double>("general_max_slope");
local_max_slope_ = declare_parameter<double>("local_max_slope");
initial_max_slope_ = declare_parameter<double>("initial_max_slope");
radial_divider_angle_ = declare_parameter<double>("radial_divider_angle");
min_height_threshold_ = declare_parameter<double>("min_height_threshold");
concentric_divider_distance_ = declare_parameter<double>("concentric_divider_distance");
reclass_distance_threshold_ = declare_parameter<double>("reclass_distance_threshold");
}

using std::placeholders::_1;
Expand Down
34 changes: 16 additions & 18 deletions perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
{
// set initial parameters
{
low_priority_region_x_ = static_cast<float>(declare_parameter("low_priority_region_x", -20.0f));
detection_range_z_max_ = static_cast<float>(declare_parameter("detection_range_z_max", 2.5f));
center_pcl_shift_ = static_cast<float>(declare_parameter("center_pcl_shift", 0.0));
non_ground_height_threshold_ =
static_cast<float>(declare_parameter("non_ground_height_threshold", 0.20));
grid_mode_switch_radius_ =
static_cast<float>(declare_parameter("grid_mode_switch_radius", 20.0));

grid_size_m_ = static_cast<float>(declare_parameter("grid_size_m", 0.5));
gnd_grid_buffer_size_ = static_cast<int>(declare_parameter("gnd_grid_buffer_size", 4));
elevation_grid_mode_ = static_cast<bool>(declare_parameter("elevation_grid_mode", true));
global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0));
local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0));
radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0));
split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2);
split_height_distance_ = declare_parameter("split_height_distance", 0.2);
use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true);
use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true);
low_priority_region_x_ = declare_parameter<float>("low_priority_region_x");
detection_range_z_max_ = declare_parameter<float>("detection_range_z_max");
center_pcl_shift_ = declare_parameter<float>("center_pcl_shift");
non_ground_height_threshold_ = declare_parameter<float>("non_ground_height_threshold");
grid_mode_switch_radius_ = declare_parameter<float>("grid_mode_switch_radius");

grid_size_m_ = declare_parameter<float>("grid_size_m");
gnd_grid_buffer_size_ = declare_parameter<int>("gnd_grid_buffer_size");
elevation_grid_mode_ = declare_parameter<bool>("elevation_grid_mode");
global_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("global_slope_max_angle_deg"));
local_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("local_slope_max_angle_deg"));
radial_divider_angle_rad_ = deg2rad(declare_parameter<float>("radial_divider_angle_deg"));
split_points_distance_tolerance_ = declare_parameter<float>("split_points_distance_tolerance");
split_height_distance_ = declare_parameter<float>("split_height_distance");
use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();

Expand Down
Loading

0 comments on commit 9ff3861

Please sign in to comment.