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.

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: fix ring_outlier_filter_param #18

Merged
merged 1 commit into from
Oct 2, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
1 change: 1 addition & 0 deletions common_awsim_sensor_launch/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -10,5 +10,6 @@ if(BUILD_TESTING)
endif()

ament_auto_package(INSTALL_TO_SHARE
config
launch
)
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
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,10 @@
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterFile
import yaml
from ament_index_python.packages import get_package_share_directory
from pathlib import Path


def get_vehicle_info(context):
Expand Down Expand Up @@ -106,6 +109,20 @@
)
)

ring_outlier_filter_node_param = ParameterFile(
param_file=LaunchConfiguration("ring_outlier_filter_node_param_path").perform(
context
),
allow_substs=True,
)

# 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_output_frame = {"output_frame": LaunchConfiguration("frame_id")}
else:
# keep the output frame as the input frame
ring_outlier_output_frame = {"output_frame": ""}

nodes.append(
ComposableNode(
package="autoware_pointcloud_preprocessor",
Expand All @@ -115,6 +132,7 @@
("input", "rectified/pointcloud_ex"),
("output", "pointcloud"),
],
parameters=[ring_outlier_filter_node_param, ring_outlier_output_frame],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
)
Expand Down Expand Up @@ -179,6 +197,10 @@
DeclareLaunchArgument(name, default_value=default_value, description=description)
)

common_sensor_share_dir = Path(
get_package_share_directory("common_awsim_sensor_launch")
)

add_launch_arg("base_frame", "base_link", "base frame id")
add_launch_arg("container_name", "velodyne_composable_node_container", "container name")
add_launch_arg("input_frame", LaunchConfiguration("base_frame"), "use for cropbox")
Expand All @@ -187,7 +209,14 @@
"vehicle_mirror_param_file", description="path to the file of vehicle mirror position yaml"
)
add_launch_arg("use_multithread", "False", "use multithread")
add_launch_arg("use_intra_process", "False", "use ROS2 component container communication")

Check warning on line 212 in common_awsim_sensor_launch/launch/velodyne_node_container.launch.py

View workflow job for this annotation

GitHub Actions / spell-check-differential

Forbidden word (ROS2)
add_launch_arg("output_as_sensor_frame", "False", "output final pointcloud in sensor frame")
add_launch_arg("frame_id", "base_link", "frame id")
add_launch_arg(
"ring_outlier_filter_node_param_path",
str(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
Loading