Skip to content

Commit

Permalink
fix: update XT32 related perception pipeline
Browse files Browse the repository at this point in the history
Signed-off-by: Manato HIRABAYASHI <[email protected]>
  • Loading branch information
manato committed Sep 10, 2024
1 parent 5572599 commit a5d7824
Show file tree
Hide file tree
Showing 6 changed files with 41 additions and 21 deletions.
16 changes: 16 additions & 0 deletions edge_auto_launch/config/roi_cluster_fusion.param.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
/**:
ros__parameters:
fusion_distance: 100.0
trust_object_distance: 100.0
trust_object_iou_mode: "iou"
non_trust_object_iou_mode: "iou"
use_cluster_semantic_type: false
# only_allow_inside_cluster: true
only_allow_inside_cluster: false
roi_scale_factor: 1.1
iou_threshold: 0.3
unknown_iou_threshold: 0.1
remove_unknown: true
debug_mode: true
image_buffer_size: 1000

14 changes: 13 additions & 1 deletion edge_auto_launch/config/scan_ground_filter.param.yaml
Original file line number Diff line number Diff line change
@@ -1,6 +1,18 @@
/**:
ros__parameters:
global_slope_max_angle_deg: 10.0
local_slope_max_angle_deg: 30.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
use_lowest_point: true
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<node_container pkg="rclcpp_components" exec="component_container" name="nebula_xt32_node" namespace="/">
<composable_node pkg="nebula_ros" plugin="HesaiDriverRosWrapper" name="hesai_driver_roswrapper_node" namespace="$(var namespace)">
<param name="sensor_model" value="$(var sensor_model)"/>
<param name="sensor_ip" value="$(var sensor_ip)"/>
<param name="return_mode" value="$(var return_mode)"/>
<param name="frame_id" value="$(var frame_id)"/>
<param name="scan_phase" value="$(var scan_phase)"/>
Expand Down Expand Up @@ -50,6 +51,9 @@
<param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
<param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
<param name="setup_sensor" value="$(var setup_sensor)"/>
<param name="ptp_profile" value="1588v2"/>
<param name="ptp_transport_type" value="udp"/>
<param name="ptp_switch_type" value="non_tsn"/>
<extra_arg name="use_intra_process_comms" value="true"/>
</composable_node>
<composable_node pkg="nebula_ros" plugin="HesaiHwMonitorRosWrapper" name="hesai_hw_monitor_ros_wrapper_node" namespace="$(var namespace)">
Expand Down
18 changes: 3 additions & 15 deletions edge_auto_launch/launch/component/roi_cluster_fusion.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,7 @@
<arg name="input/clusters" default="input/clusters"/>
<arg name="output/clusters" default="output/clusters"/>
<arg name="sync_param_path" default="$(find-pkg-share edge_auto_launch)/config/roi_sync.param.yaml"/>
<arg name="remove_unknown" default="true"/>
<arg name="trust_distance" default="100.0"/>
<arg name="param_path" default="$(find-pkg-share edge_auto_launch)/config/roi_cluster_fusion.param.yaml"/>

<!-- for eval variable-->
<arg name="input_rois_number" default="$(var input/rois_number)"/>
Expand All @@ -20,23 +19,12 @@
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/rois1" to="$(var input/rois1)"/>
<set_remap if="$(eval &quot;'$(var input_rois_number)' >= '2' &quot;)" from="input/camera_info1" to="$(var input/camera_info1)"/>

<node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
<node pkg="autoware_image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
<param name="rois_number" value="$(var input/rois_number)"/>
<param from="$(var sync_param_path)"/>
<param from="$(var param_path)"/>
<remap from="input" to="$(var input/clusters)"/>
<remap from="output" to="$(var output/clusters)"/>

<param name="remove_unknown" value="$(var remove_unknown)"/>
<param name="trust_distance" value="$(var trust_distance)"/>

<param name="use_iou" value="true"/>
<param name="use_iou_x" value="false"/>
<param name="use_iou_y" value="false"/>
<param name="only_allow_inside_cluster" value="true"/>
<param name="roi_scale_factor" value="1.1"/>

<param name="iou_threshold" value="0.3"/>
<param name="debug_mode" value="false"/>
</node>
</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

<group>
<node_container pkg="rclcpp_components" exec="component_container" name="container" namespace="">
<composable_node pkg="euclidean_cluster" plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode" name="euclidean_cluster" namespace="">
<composable_node pkg="autoware_euclidean_cluster" plugin="autoware::euclidean_cluster::VoxelGridBasedEuclideanClusterNode" name="euclidean_cluster" namespace="">
<remap from="input" to="$(var input/pointcloud)"/>
<remap from="output" to="$(var output/objects)"/>
<param from="$(var param_path)"/>
Expand Down
8 changes: 4 additions & 4 deletions edge_auto_launch/launch/perception_xt32_sample.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<arg name="sensor_height" default="1.0"/>

<!-- vehicle_info -->
<include file="$(find-pkg-share vehicle_info_util)/launch/vehicle_info.launch.py">
<include file="$(find-pkg-share autoware_vehicle_info_utils)/launch/vehicle_info.launch.py">
<arg name="vehicle_info_param_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/vehicle_info.param.yaml"/>
</include>

Expand Down Expand Up @@ -60,7 +60,7 @@
<arg name="output/objects" value="objects_with_feature"/>
<arg name="node_name" value="shape_estimation"/>
</include>
<include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
<include file="$(find-pkg-share autoware_detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
<arg name="input" value="objects_with_feature"/>
<arg name="output" value="/perception/object_recognition/detection/euclidean_cluster/objects"/>
<arg name="node_name" value="detected_object_feature_remover"/>
Expand All @@ -78,12 +78,12 @@
<arg name="input/rois1" value="/perception/object_recognition/detection/rois1"/>
<arg name="input/camera_info1" value="/sensing/camera/camera1/camera_info"/>
</include>
<include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
<include file="$(find-pkg-share autoware_shape_estimation)/launch/shape_estimation.launch.xml">
<arg name="input/objects" value="clusters"/>
<arg name="output/objects" value="objects_with_feature"/>
<arg name="node_name" value="shape_estimation"/>
</include>
<include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
<include file="$(find-pkg-share autoware_detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
<arg name="input" value="objects_with_feature"/>
<arg name="output" value="/perception/object_recognition/detection/objects"/>
<arg name="node_name" value="detected_object_feature_remover"/>
Expand Down

0 comments on commit a5d7824

Please sign in to comment.