Skip to content

Commit

Permalink
feature(single_lidar_common_launch): load from parameter (#3)
Browse files Browse the repository at this point in the history
Signed-off-by: vividf <[email protected]>
  • Loading branch information
vividf authored Nov 13, 2024
1 parent ce550a8 commit f649db1
Show file tree
Hide file tree
Showing 3 changed files with 41 additions and 14 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -3,3 +3,4 @@
base_frame: base_link
use_imu: true
use_3d_distortion_correction: false
update_azimuth_and_distance: false
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
/**:
ros__parameters:
distance_ratio: 1.03
object_length_threshold: 0.1
num_points_threshold: 4
max_rings_num: 128
max_points_num_per_ring: 4000
publish_outlier_pointcloud: false
min_azimuth_deg: 0.0
max_azimuth_deg: 360.0
max_distance: 12.0
vertical_bins: 128
horizontal_bins: 36
noise_threshold: 2
40 changes: 26 additions & 14 deletions single_lidar_common_launch/launch/nebula_node_container.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,10 @@ def create_parameter_dict(*args):
param_file=LaunchConfiguration("distortion_correction_node_param_path").perform(context),
allow_substs=True,
)
ring_outlier_filter_node_param = ParameterFile(
param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(context),
allow_substs=True,
)

nodes = []

Expand Down Expand Up @@ -152,8 +156,8 @@ def create_parameter_dict(*args):

nodes.append(
ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::CropBoxFilterComponent",
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
name="crop_box_filter_self",
remappings=[
("input", "pointcloud_raw_ex"),
Expand All @@ -174,8 +178,8 @@ def create_parameter_dict(*args):

nodes.append(
ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::CropBoxFilterComponent",
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::CropBoxFilterComponent",
name="crop_box_filter_mirror",
remappings=[
("input", "self_cropped/pointcloud_ex"),
Expand All @@ -188,8 +192,8 @@ def create_parameter_dict(*args):

nodes.append(
ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::DistortionCorrectorComponent",
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::DistortionCorrectorComponent",
name="distortion_corrector_node",
remappings=[
("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
Expand All @@ -204,29 +208,27 @@ def create_parameter_dict(*args):

# Ring Outlier Filter is the last component in the pipeline, so control the output frame here
if LaunchConfiguration("output_as_sensor_frame").perform(context).lower() == "true":
ring_outlier_filter_parameters = {"output_frame": LaunchConfiguration("frame_id")}
ring_outlier_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
else:
ring_outlier_filter_parameters = {
"output_frame": ""
} # keep the output frame as the input frame
ring_outlier_output_frame = {"output_frame": ""} # keep the output frame as the input frame
nodes.append(
ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::RingOutlierFilterComponent",
package="autoware_pointcloud_preprocessor",
plugin="autoware::pointcloud_preprocessor::RingOutlierFilterComponent",
name="ring_outlier_filter",
remappings=[
("input", "rectified/pointcloud_ex"),
("output", "pointcloud_before_sync"),
],
parameters=[ring_outlier_filter_parameters],
parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)

# set container to run all required components in the same process
container = ComposableNodeContainer(
name=LaunchConfiguration("container_name"),
namespace="pointcloud_preprocessor",
namespace="autoware_pointcloud_preprocessor",
package="rclcpp_components",
executable=LaunchConfiguration("container_executable"),
composable_node_descriptions=nodes,
Expand Down Expand Up @@ -274,6 +276,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
add_launch_arg(
"vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
)

add_launch_arg(
"distortion_correction_node_param_path",
os.path.join(
Expand All @@ -283,6 +286,15 @@ def add_launch_arg(name: str, default_value=None, description=None):
),
description="path to parameter file of distortion correction node",
)
add_launch_arg(
"ring_outlier_filter_node_param_path",
os.path.join(
common_sensor_share_dir,
"config",
"ring_outlier_filter_node.param.yaml",
),
description="path to parameter file of ring outlier filter node",
)

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down

0 comments on commit f649db1

Please sign in to comment.