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

Jump to bottom

feat(pointcloud_preprocessor): separate concatenate node to sync and concatenate filter #84

Draft
wants to merge 6 commits into
base: main
Choose a base branch
from
Draft

feat(pointcloud_preprocessor): separate concatenate node to sync and concatenate filter #84

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
f1b7294
apply concatenate node update
YoshiRi Jul 17, 2023
9a95283
Merge branch 'main' into HEAD
YoshiRi Dec 15, 2023
4624162
refactor: change node parameter name
YoshiRi Dec 20, 2023
d29075a
Merge branch 'main' into feat/separate_concatenate_node
YoshiRi Mar 8, 2024
1a249b9
fix: fix pointcloud preprocessor to run tutorial
YoshiRi Mar 8, 2024
3f2fc97
style(pre-commit): autofix
pre-commit-ci[bot] Mar 8, 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
118 changes: 96 additions & 22 deletions sample_sensor_kit_launch/launch/pointcloud_preprocessor.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,38 +20,110 @@
from launch.conditions import IfCondition
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import ComposableNodeContainer
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode


def launch_setup(context, *args, **kwargs):
# set concat filter as a component
concat_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="concatenate_data",
remappings=[
("~/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,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],

# separate components for backward compatibility
separate_pointcloud_sync_and_concatenate_nodes_str: str = LaunchConfiguration(
"concatenate_data__separate_pointcloud_sync_and_concatenate_nodes"
).perform(context)
separate_pointcloud_sync_and_concatenate_nodes: bool = (
separate_pointcloud_sync_and_concatenate_nodes_str.lower() == "true"
)

if not separate_pointcloud_sync_and_concatenate_nodes:
# legacy mode for backward compatibility. Not used in default.
sync_and_concat_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
name="concatenate_data",
remappings=[
("~/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",
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
concat_components = [sync_and_concat_component]
else:
time_sync_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudDataSynchronizerComponent",
name="synchronizer_filter",
remappings=[
("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
],
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"),
"approximate_sync": True,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)

concat_component = ComposableNode(
package="pointcloud_preprocessor",
plugin="pointcloud_preprocessor::PointCloudConcatenationComponent",
name="concatenate_filter",
remappings=[("output", "concatenated/pointcloud")],
parameters=[
{
# actual input topics becomes + "_synchronized"
"input_topics": [
"/sensing/lidar/top/pointcloud",
"/sensing/lidar/left/pointcloud",
"/sensing/lidar/right/pointcloud",
],
"output_frame": LaunchConfiguration("base_frame"),
"approximate_sync": True,
}
],
extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
)
concat_components = [time_sync_component, concat_component]

# set container to run all required components in the same process
# container = ComposableNodeContainer(
# name=LaunchConfiguration("pointcloud_container_name"),
# namespace="",
# package="rclcpp_components",
# executable=LaunchConfiguration("container_executable"),
# composable_node_descriptions=[],
# condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")),
# output="screen",
# )

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

# load concat or passthrough filter
concat_loader = LoadComposableNodes(
composable_node_descriptions=[concat_component],
composable_node_descriptions=concat_components,
# target_container=target_container,
target_container=LaunchConfiguration("pointcloud_container_name"),
condition=IfCondition(LaunchConfiguration("use_concat_filter")),
)
Expand All @@ -68,6 +140,8 @@ def add_launch_arg(name: str, default_value=None):
add_launch_arg("base_frame", "base_link")
add_launch_arg("use_multithread", "False")
add_launch_arg("use_intra_process", "False")
add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("concatenate_data__separate_pointcloud_sync_and_concatenate_nodes", "True")
add_launch_arg("pointcloud_container_name", "pointcloud_container")

set_container_executable = SetLaunchConfiguration(
Expand Down