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

feat: pointcloud_preprocessor load from param #98

Closed
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
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
/**:
ros__parameters:
maximum_queue_size: 5
timeout_sec: 0.2
is_motion_compensated: true
publish_synchronized_pointcloud: true
keep_input_frame_in_synchronized_pointcloud: true
publish_previous_but_late_pointcloud: false
synchronized_pointcloud_postfix: pointcloud
input_twist_topic_type: twist
input_topics: [
"/sensing/lidar/left/pointcloud_before_sync",
"/sensing/lidar/right/pointcloud_before_sync",
"/sensing/lidar/top/pointcloud_before_sync"
]
output_frame: base_link
lidar_timestamp_offsets: [0.0, 0.04, 0.08]
lidar_timestamp_noise_window: [0.01, 0.01, 0.01]
43 changes: 28 additions & 15 deletions sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# Copyright 2020 Tier IV, Inc. All rights reserved.
# Copyright 2024 Tier IV, Inc. All rights reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
Expand All @@ -12,7 +12,9 @@
# See the License for the specific language governing permissions and
# limitations under the License.

import os

from ament_index_python.packages import get_package_share_directory
import launch
from launch.actions import DeclareLaunchArgument
from launch.actions import OpaqueFunction
Expand All @@ -22,9 +24,17 @@
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode
from launch_ros.parameter_descriptions import ParameterFile


def launch_setup(context, *args, **kwargs):
# Pointcloud preprocessor parameters
concatenate_and_time_sync_node_param = ParameterFile(
param_file=LaunchConfiguration("concatenate_and_time_sync_node_param_path").perform(
context
Copy link
Contributor

Choose a reason for hiding this comment

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

),
allow_substs=True,
)
# set concat filter as a component
concat_component = ComposableNode(
package="autoware_pointcloud_preprocessor",
Expand All @@ -34,18 +44,7 @@ def launch_setup(context, *args, **kwargs):
("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
("output", "concatenated/pointcloud"),
],
parameters=[
{
"input_topics": [
"/sensing/lidar/top/pointcloud_before_sync",
"/sensing/lidar/left/pointcloud_before_sync",
"/sensing/lidar/right/pointcloud_before_sync",
],
"output_frame": LaunchConfiguration("base_frame"),
"input_twist_topic_type": "twist",
"publish_synchronized_pointcloud": True,
}
],
parameters=[concatenate_and_time_sync_node_param],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)

Expand All @@ -62,13 +61,27 @@ def launch_setup(context, *args, **kwargs):
def generate_launch_description():
launch_arguments = []

def add_launch_arg(name: str, default_value=None):
launch_arguments.append(DeclareLaunchArgument(name, default_value=default_value))
def add_launch_arg(name: str, default_value=None, description=None):
# a default_value of None is equivalent to not passing that kwarg at all
launch_arguments.append(
DeclareLaunchArgument(name, default_value=default_value, description=description)
)

sample_sensor_kit_share_dir = get_package_share_directory("sample_sensor_kit_launch")

add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg(
"concatenate_and_time_sync_node_param_path",
os.path.join(
sample_sensor_kit_share_dir,
"config",
"concatenate_and_time_sync_node.param.yaml",
),
description="path to parameter file of concatenate and time synchronization node",
)

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down
Loading