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.

Sign up for GitHub

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

fix: add blockage for xx1 #191

Merged
merged 13 commits into from
Mar 19, 2024
Merged

fix: add blockage for xx1 #191

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
ca44474
fix: enable blocakge diag for xx1
badai-nguyen Dec 20, 2023
8e3ac8b
fix: xx1 launch
badai-nguyen Dec 26, 2023
4748c45
fix: x2 launch
badai-nguyen Dec 26, 2023
2bfc94c
ci(pre-commit): autofix
pre-commit-ci[bot] Dec 26, 2023
15cb8e3
fix: remove duplicated angle_range param
badai-nguyen Dec 28, 2023
fa93588
fix: remove x2 redundant param
badai-nguyen Dec 28, 2023
f052504
Merge branch 'tier4/universe' into fix/add_blockage_xx1
badai-nguyen Jan 22, 2024
9b4b8ef
chore: update config file
badai-nguyen Feb 19, 2024
fd4bc4a
chore: disable xx1 blockage as default
badai-nguyen Feb 27, 2024
f5b67cc
Merge branch 'tier4/universe' into fix/add_blockage_xx1
badai-nguyen Feb 27, 2024
57612b6
fix: velodyne launch
badai-nguyen Mar 15, 2024
38eb072
Revert "fix: remove x2 redundant param"
badai-nguyen Mar 18, 2024
d0e4c12
Revert "fix: x2 launch"
badai-nguyen Mar 18, 2024
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
25 changes: 25 additions & 0 deletions aip_x2_launch/launch/lidar.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<arg name="vehicle_mirror_param_file" />
<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"/>
miursh marked this conversation as resolved.
Show resolved Hide resolved

<group>
<push-ros-namespace namespace="lidar" />
Expand All @@ -25,6 +26,9 @@
<arg name="return_mode" value="Strongest" />
<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" 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)" />
Expand All @@ -45,6 +49,9 @@
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="40" />
<arg name="return_mode" value="First" />
<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" 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)" />
Expand All @@ -67,6 +74,9 @@
<arg name="horizontal_ring_id" value="12" />
<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="return_mode" value="Dual" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand All @@ -88,6 +98,9 @@
<arg name="distance_range" value="[0.1, 7.5]"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="54" />
<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="return_mode" value="First" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand All @@ -111,6 +124,9 @@
<arg name="horizontal_ring_id" value="12" />
<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="return_mode" value="Dual" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand All @@ -132,6 +148,9 @@
<arg name="distance_range" value="[0.1, 7.5]"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="54" />
<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="return_mode" value="First" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand All @@ -155,6 +174,9 @@
<arg name="horizontal_ring_id" value="12" />
<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="return_mode" value="Strongest" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand All @@ -176,6 +198,9 @@
<arg name="distance_range" value="[0.1, 7.5]"/>
<arg name="vertical_bins" value ="64" />
<arg name="horizontal_ring_id" value="40" />
<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="return_mode" value="First" />
<arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
<arg name="launch_driver" value="$(var launch_driver)" />
Expand Down
10 changes: 9 additions & 1 deletion aip_x2_launch/launch/pandar_node_container.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,10 @@ def get_pandar_monitor_info():
return p


def str2vector(string):
return [float(x) for x in string.strip("[]").split(",")]


def get_vehicle_info(context):
# TODO(TIER IV): Use Parameter Substitution after we drop Galactic support
# https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py
Expand Down Expand Up @@ -232,6 +236,7 @@ def create_parameter_dict(*args):
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)

distance_range = str2vector(context.perform_substitution(LaunchConfiguration("distance_range")))
blockage_diag_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::BlockageDiagComponent",
Expand All @@ -245,7 +250,9 @@ def create_parameter_dict(*args):
"angle_range": LaunchConfiguration("angle_range"),
"horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
"vertical_bins": LaunchConfiguration("vertical_bins"),
"model": LaunchConfiguration("model"),
"is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
"max_distance_range": distance_range[1],
"horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
}
]
+ [load_composable_node_param("blockage_diagnostics_param_file")],
Expand Down Expand Up @@ -323,6 +330,7 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("input_frame", LaunchConfiguration("base_frame"))
add_launch_arg("output_frame", LaunchConfiguration("base_frame"))
add_launch_arg("dual_return_filter_param_file")
add_launch_arg("horizontal_resolution", "0.4")
add_launch_arg(
"blockage_diagnostics_param_file",
[FindPackageShare("aip_x2_launch"), "/config/blockage_diagnostics_param_file.yaml"],
Expand Down
9 changes: 9 additions & 0 deletions aip_xx1_launch/config/diagnostic_aggregator/sensor_kit.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,15 @@
type: diagnostic_aggregator/AnalyzerGroup
path: lidar
analyzers:
performance_monitoring:
type: diagnostic_aggregator/AnalyzerGroup
path: performance_monitoring
analyzers:
blockage:
type: diagnostic_aggregator/GenericAnalyzer
path: blockage
contains: [": blockage_validation"]
timeout: 1.0
velodyne:
type: diagnostic_aggregator/AnalyzerGroup
path: velodyne
Expand Down
21 changes: 21 additions & 0 deletions aip_xx1_launch/launch/lidar.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<arg name ="vehicle_id" default="$(env VEHICLE_ID default)"/>
<arg name="vehicle_mirror_param_file"/>
<arg name="pointcloud_container_name" default="pointcloud_container"/>
<arg name="enable_blockage_diag" default="false"/>

<group>
<push-ros-namespace namespace="lidar"/>
Expand All @@ -22,6 +23,11 @@
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="pointcloud_container"/>
<arg name="vertical_bins" value="128"/>
<arg name="horizontal_ring_id" value="64"/>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[Q]
Is ring_id: 64 correct?
Has this algorithm taken into account the arrangement pattern of the lasers?
image

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@miursh
As confirmation in Rviz, the velodyne ring IDs are arranged in the order as the assumption of the current algorithm that ring_id will be whether Top to Bottom or Bottom to Top.
In VLS128 the order is from the bottom.
image

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@badai-nguyen
Ah, I see. Understood.
However, is 64 is really horizontal?
Typically, there are more downward-facing lasers than upward-facing ones, so I doubt that the center would be horizontal.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@miursh Thank you for your comment.
Currently, I checked on the kind of empty environment and I think 64 is acceptable.
image
image

I think it also could be adjusted if any feedback after the function is enabled for operation.

<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
</include>
</group>

Expand All @@ -39,6 +45,11 @@
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="pointcloud_container"/>
<arg name="vertical_bins" value="16"/>
<arg name="horizontal_ring_id" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
</include>
</group>

Expand All @@ -56,6 +67,11 @@
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="pointcloud_container"/>
<arg name="vertical_bins" value="16"/>
<arg name="horizontal_ring_id" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
</include>
</group>

Expand All @@ -73,6 +89,11 @@
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="pointcloud_container"/>
<arg name="vertical_bins" value="16"/>
<arg name="horizontal_ring_id" value="0"/>
<arg name="is_channel_order_top2down" value="false"/>
<arg name="horizontal_resolution" value="0.4"/>
<arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
</include>
</group>

Expand Down
1 change: 1 addition & 0 deletions common_sensor_launch/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,5 @@ endif()
ament_auto_package(INSTALL_TO_SHARE
config
launch
config
)
12 changes: 12 additions & 0 deletions common_sensor_launch/config/blockage_diagnostics_param_file.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
/**:
ros__parameters:
blockage_ratio_threshold: 0.1
blockage_count_threshold: 50
blockage_buffering_frames: 2
blockage_buffering_interval: 1
dust_ratio_threshold: 0.2
dust_count_threshold: 10
dust_kernel_size: 2
dust_buffering_frames: 10
dust_buffering_interval: 1
blockage_kernel: 10
62 changes: 53 additions & 9 deletions common_sensor_launch/launch/nebula_node_container.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.substitutions import FindPackageShare
import yaml


Expand Down Expand Up @@ -54,14 +55,11 @@ def get_vehicle_info(context):
return p


def get_vehicle_mirror_info(context):
path = LaunchConfiguration("vehicle_mirror_param_file").perform(context)
with open(path, "r") as f:
p = yaml.safe_load(f)["/**"]["ros__parameters"]
return p


def launch_setup(context, *args, **kwargs):
def load_composable_node_param(param_path):
with open(LaunchConfiguration(param_path).perform(context), "r") as f:
return yaml.safe_load(f)["/**"]["ros__parameters"]

def create_parameter_dict(*args):
result = {}
for x in args:
Expand Down Expand Up @@ -151,7 +149,7 @@ def create_parameter_dict(*args):
)
)

mirror_info = get_vehicle_mirror_info(context)
mirror_info = load_composable_node_param("vehicle_mirror_param_file")
cropbox_parameters["min_x"] = mirror_info["min_longitudinal_offset"]
cropbox_parameters["max_x"] = mirror_info["max_longitudinal_offset"]
cropbox_parameters["min_y"] = mirror_info["min_lateral_offset"]
Expand Down Expand Up @@ -239,13 +237,49 @@ def create_parameter_dict(*args):
],
)

blockage_diag_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::BlockageDiagComponent",
name="blockage_diag",
remappings=[
("input", "pointcloud_raw_ex"),
("output", "blockage_diag/pointcloud"),
],
parameters=[
{
"angle_range": [
float(context.perform_substitution(LaunchConfiguration("cloud_min_angle"))),
float(context.perform_substitution(LaunchConfiguration("cloud_max_angle"))),
],
"horizontal_ring_id": LaunchConfiguration("horizontal_ring_id"),
"vertical_bins": LaunchConfiguration("vertical_bins"),
"is_channel_order_top2down": LaunchConfiguration("is_channel_order_top2down"),
"max_distance_range": LaunchConfiguration("max_range"),
"horizontal_resolution": LaunchConfiguration("horizontal_resolution"),
}
]
+ [load_composable_node_param("blockage_diagnostics_param_file")],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)

target_container = (
container
if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context)
else LaunchConfiguration("container_name")
)

driver_component_loader = LoadComposableNodes(
composable_node_descriptions=[driver_component],
target_container=container,
condition=IfCondition(LaunchConfiguration("launch_driver")),
)
blockage_diag_loader = LoadComposableNodes(
composable_node_descriptions=[blockage_diag_component],
target_container=target_container,
condition=IfCondition(LaunchConfiguration("enable_blockage_diag")),
)

return [container, driver_component_loader]
return [container, driver_component_loader, blockage_diag_loader]


def generate_launch_description():
Expand Down Expand Up @@ -284,6 +318,16 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
add_launch_arg("container_name", "nebula_node_container")

add_launch_arg("enable_blockage_diag", "true")
add_launch_arg("horizontal_ring_id", "64")
add_launch_arg("vertical_bins", "128")
add_launch_arg("is_channel_order_top2down", "true")
add_launch_arg("horizontal_resolution", "0.4")
add_launch_arg(
"blockage_diagnostics_param_file",
[FindPackageShare("common_sensor_launch"), "/config/blockage_diagnostics_param_file.yaml"],
)

set_container_executable = SetLaunchConfiguration(
"container_executable",
"component_container",
Expand Down
Loading