Skip to content

Commit

Permalink
Merge branch 'beta/v0.29.0' into feat/build_xacro
Browse files Browse the repository at this point in the history
  • Loading branch information
Owen-Liuyuxuan authored Aug 28, 2024
2 parents 2600a5f + 7aef84c commit 5e772bc
Show file tree
Hide file tree
Showing 35 changed files with 587 additions and 1,989 deletions.
6 changes: 3 additions & 3 deletions aip_x2_launch/config/dual_return_filter.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,9 @@
ros__parameters:
roi_mode: "Fixed_azimuth_ROI" # description="options: `No_ROI`, `Fixed_xyz_ROI` or `Fixed_azimuth_ROI`"/>
weak_first_local_noise_threshold: 2 # description="for No_ROI roi_mode, recommended value is 10" />
visibility_error_threshold: 0.5
visibility_warn_threshold: 0.9
max_distance: 5.0
visibility_error_threshold: 0.95
visibility_warn_threshold: 0.97
max_distance: 10.0
x_max: 18.0
x_min: -12.0
y_max: 2.0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,9 @@
# gnss
gnss: default

# imu
yaw_rate_status: default

# lidar
pandar_connection: default
pandar_temperature: default
Expand Down
4 changes: 4 additions & 0 deletions aip_x2_launch/launch/imu.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@
<arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/>
<arg name="gyro_bias_estimator_param_file" value="$(var gyro_bias_estimator_param_file)"/>
</include>

<include file="$(find-pkg-share imu_monitor)/launch/imu_monitor.launch.xml">
<arg name="config_file" value="$(find-pkg-share imu_monitor)/config/imu_monitor.param.yaml"/>
</include>
</group>

</launch>
211 changes: 85 additions & 126 deletions aip_x2_launch/launch/lidar.launch.xml

Large diffs are not rendered by default.

289 changes: 289 additions & 0 deletions aip_x2_launch/launch/lidar.nebula.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,289 @@
<launch>

<arg name="launch_driver" default="true" />
<arg name="use_concat_filter" default="true" />
<arg name="vehicle_id" default="$(env VEHICLE_ID default)" />
<arg name="vehicle_mirror_param_file" />
<arg name="use_pointcloud_container" default="false" description="launch pointcloud container"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="dual_return_filter_param_file" default="$(find-pkg-share aip_x2_launch)/config/dual_return_filter.param.yaml"/>
<arg name="enable_blockage_diag" default="true"/>

<group>
<push-ros-namespace namespace="lidar" />
<group>
<push-ros-namespace namespace="front_lower" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="Pandar40P" />
<arg name="frame_id" value="pandar_40p_front" />
<arg name="sensor_ip" value="192.168.110.201" />
<arg name="data_port" value="2321" />
<arg name="gnss_port" value="10121" />
<arg name="scan_phase" value="280.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="min_range" value="0.5"/>
<arg name="max_range" value="200.0"/>
<arg name="blockage_range" value="[90.0, 270.0]"/>
<arg name="vertical_bins" value ="40" />
<arg name="horizontal_ring_id" value="12" />
<arg name="return_mode" value="Dual" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="min_azimuth_deg" value="135.0"/>
<arg name="max_azimuth_deg" value="225.0"/>
<arg name="is_channel_order_top2down" value="true"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="front_upper" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="PandarQT64" />
<arg name="frame_id" value="pandar_qt_front" />
<arg name="sensor_ip" value="192.168.120.211" />
<arg name="data_port" value="2331" />
<arg name="gnss_port" value="10131" />
<arg name="scan_phase" value="280.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="blockage_range" value="[90.0, 270.0]"/>
<!--for j6#1#2-->
<!--arg name="scan_phase" value="310.0" /-->
<!--arg name="cloud_min_angle" value="120"/-->
<!--arg name="cloud_max_angle" value="300"/-->
<!--arg name="blockage_range" value="[90.0, 270.0]"/-->
<arg name="min_range" value="0.1"/>
<arg name="max_range" value="20.0"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="40" />
<arg name="return_mode" value="First" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.6"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="left_upper" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="Pandar40P" />
<arg name="frame_id" value="pandar_40p_left" />
<arg name="sensor_ip" value="192.168.110.202" />
<arg name="data_port" value="2322" />
<arg name="gnss_port" value="10122" />
<arg name="scan_phase" value="315.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="305"/>
<arg name="min_range" value="0.5"/>
<arg name="max_range" value="200.0"/>
<arg name="blockage_range" value="[90.0, 305.0]"/>
<arg name="vertical_bins" value ="40" />
<arg name="horizontal_ring_id" value="12" />
<arg name="return_mode" value="Dual" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="min_azimuth_deg" value="225.0"/>
<arg name="max_azimuth_deg" value="315.0"/>
<arg name="is_channel_order_top2down" value="true"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="left_lower" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="PandarQT64" />
<arg name="frame_id" value="pandar_qt_left" />
<arg name="sensor_ip" value="192.168.120.212" />
<arg name="data_port" value="2332" />
<arg name="gnss_port" value="10132" />
<arg name="scan_phase" value="280.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="min_range" value="0.1"/>
<arg name="max_range" value="20.0"/>
<arg name="blockage_range" value="[120.0, 240.0]"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="24" />
<arg name="return_mode" value="First" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.6"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="right_upper" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="Pandar40P" />
<arg name="frame_id" value="pandar_40p_right" />
<arg name="sensor_ip" value="192.168.120.203" />
<arg name="data_port" value="2323" />
<arg name="gnss_port" value="10121" />
<arg name="scan_phase" value="270.0" />
<arg name="cloud_min_angle" value="55"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="min_range" value="0.5"/>
<arg name="max_range" value="200.0"/>
<arg name="blockage_range" value="[55.0, 270.0]"/>
<arg name="vertical_bins" value ="40" />
<arg name="horizontal_ring_id" value="12" />
<arg name="return_mode" value="Strongest" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="min_azimuth_deg" value="45.0"/>
<arg name="max_azimuth_deg" value="135.0"/>
<arg name="is_channel_order_top2down" value="true"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="right_lower" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="PandarQT64" />
<arg name="frame_id" value="pandar_qt_right" />
<arg name="sensor_ip" value="192.168.120.213" />
<arg name="data_port" value="2333" />
<arg name="gnss_port" value="10133" />
<arg name="scan_phase" value="270.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="min_range" value="0.1"/>
<arg name="max_range" value="20.0"/>
<arg name="blockage_range" value="[150.0, 240.0]"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="24" />
<arg name="return_mode" value="First" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.6"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="rear_lower" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="Pandar40P" />
<arg name="frame_id" value="pandar_40p_rear" />
<arg name="sensor_ip" value="192.168.110.204" />
<arg name="data_port" value="2324" />
<arg name="gnss_port" value="10124" />
<arg name="scan_phase" value="180.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="min_range" value="0.5"/>
<arg name="max_range" value="200.0"/>
<arg name="blockage_range" value="[90.0, 270.0]"/>
<arg name="vertical_bins" value ="40" />
<arg name="horizontal_ring_id" value="12" />
<arg name="return_mode" value="Strongest" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="min_azimuth_deg" value="135.0"/>
<arg name="max_azimuth_deg" value="225.0"/>
<arg name="is_channel_order_top2down" value="true"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<group>
<push-ros-namespace namespace="rear_upper" />
<include file="$(find-pkg-share aip_x2_launch)/launch/nebula_node_container.launch.py">
<arg name="sensor_model" value="PandarQT64" />
<arg name="frame_id" value="pandar_qt_rear" />
<arg name="sensor_ip" value="192.168.120.214" />
<arg name="data_port" value="2334" />
<arg name="gnss_port" value="10134" />
<arg name="scan_phase" value="180.0" />
<arg name="cloud_min_angle" value="90"/>
<arg name="cloud_max_angle" value="270"/>
<arg name="blockage_range" value="[90.0, 270.0]"/>
<!--for j6#1#2-->
<!--arg name="scan_phase" value="200.0" /-->
<!--arg name="cloud_min_angle" value="110"/-->
<!--arg name="cloud_max_angle" value="290"/-->
<!--arg name="blockage_range" value="[110.0, 290.0]"/-->
<arg name="min_range" value="0.1"/>
<arg name="max_range" value="20.0"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="40" />
<arg name="return_mode" value="First" />
<arg name="ptp_profile" value="1588v2"/>
<arg name="ptp_transport_type" value="UDP"/>
<arg name="ptp_switch_type" value="NON_TSN"/>
<arg name="ptp_domain" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.6"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
<arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
<arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
</include>
</group>

<include file="$(find-pkg-share aip_x2_launch)/launch/pointcloud_preprocessor.launch.py">
<arg name="base_frame" value="base_link" />
<arg name="use_intra_process" value="true" />
<arg name="use_multithread" value="true" />
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)" />
<arg name="container_name" value="$(var pointcloud_container_name)"/>
</include>

</group>
</launch>
1 change: 1 addition & 0 deletions aip_x2_launch/launch/nebula_node_container.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -440,6 +440,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("min_azimuth_deg", "135.0")
add_launch_arg("max_azimuth_deg", "225.0")
add_launch_arg("output_as_sensor_frame", "True")
add_launch_arg("dual_return_filter_param_file")

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down
4 changes: 4 additions & 0 deletions aip_x2_launch/launch/sensing.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,10 @@
<arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
<arg name="config_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/vehicle_velocity_converter.param.yaml" />
</include>

<!-- Topic state monitor for each sensor -->
<include file="$(find-pkg-share aip_x2_launch)/launch/topic_state_monitor.launch.py"/>

</group>

</launch>
Loading

0 comments on commit 5e772bc

Please sign in to comment.