From d87a1d47c1a008c7e1f55c1777f27973f22b88fc Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 19 Jan 2024 10:00:15 +0900 Subject: [PATCH 1/2] feat: always separate lidar preprocessing from pointcloud_container (#197) * feat: update aip_xx1 Signed-off-by: kminoda * ci(pre-commit): autofix * fix: fix for xx1 * feat: update aip_x1 Signed-off-by: kminoda * feat: update aip_x2 Signed-off-by: kminoda * ci(pre-commit): autofix * revert: revert rename of use_pointcloud_container Signed-off-by: kminoda * ci(pre-commit): autofix * change default container name Signed-off-by: kminoda * revert: fix comment Signed-off-by: kminoda * revert: fix comment Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- aip_x1_launch/launch/lidar.launch.xml | 3 +-- .../launch/pointcloud_preprocessor.launch.py | 7 ++++--- aip_x2_launch/launch/lidar.launch.xml | 4 ++-- .../launch/pointcloud_preprocessor.launch.py | 7 ++++--- aip_xx1_launch/launch/lidar.launch.xml | 16 ++++++---------- .../launch/pointcloud_preprocessor.launch.py | 16 +++++----------- .../launch/hesai_XT32.launch.xml | 2 -- .../launch/nebula_node_container.launch.py | 18 ++---------------- .../launch/velodyne_VLP16.launch.xml | 2 -- .../launch/velodyne_VLP32C.launch.xml | 2 -- .../launch/velodyne_VLS128.launch.xml | 2 -- 11 files changed, 24 insertions(+), 55 deletions(-) diff --git a/aip_x1_launch/launch/lidar.launch.xml b/aip_x1_launch/launch/lidar.launch.xml index e59b1fa3..a6ceb956 100644 --- a/aip_x1_launch/launch/lidar.launch.xml +++ b/aip_x1_launch/launch/lidar.launch.xml @@ -22,8 +22,7 @@ - - + diff --git a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py index 4a3ad9e1..8d9892b0 100644 --- a/aip_x1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_launch/launch/pointcloud_preprocessor.launch.py @@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs): # set container to run all required components in the same process container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -64,7 +64,7 @@ def launch_setup(context, *args, **kwargs): target_container = ( container if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("container_name") + else LaunchConfiguration("pointcloud_container_name") ) # load concat or passthrough filter @@ -86,7 +86,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml index 6f478053..c7e21aa1 100644 --- a/aip_x2_launch/launch/lidar.launch.xml +++ b/aip_x2_launch/launch/lidar.launch.xml @@ -189,8 +189,8 @@ - - + + diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index 4ef586ff..6aa031b0 100644 --- a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py @@ -58,7 +58,7 @@ def launch_setup(context, *args, **kwargs): # set container to run all required components in the same process container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), @@ -70,7 +70,7 @@ def launch_setup(context, *args, **kwargs): target_container = ( container if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("container_name") + else LaunchConfiguration("pointcloud_container_name") ) # load concat or passthrough filter @@ -93,7 +93,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "True") add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/aip_xx1_launch/launch/lidar.launch.xml b/aip_xx1_launch/launch/lidar.launch.xml index 77ec443f..0adfe218 100644 --- a/aip_xx1_launch/launch/lidar.launch.xml +++ b/aip_xx1_launch/launch/lidar.launch.xml @@ -22,8 +22,7 @@ - - + @@ -40,8 +39,7 @@ - - + @@ -58,8 +56,7 @@ - - + @@ -76,8 +73,7 @@ - - + @@ -105,8 +101,8 @@ - - + + diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index a2a0fd45..e53dfb70 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py @@ -57,19 +57,12 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - # set container to run all required components in the same process container = ComposableNodeContainer( - name=LaunchConfiguration("container_name"), + name=LaunchConfiguration("individual_container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[glog_component], condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) @@ -77,12 +70,12 @@ def launch_setup(context, *args, **kwargs): target_container = ( container if UnlessCondition(LaunchConfiguration("use_pointcloud_container")).evaluate(context) - else LaunchConfiguration("container_name") + else LaunchConfiguration("pointcloud_container_name") ) # load concat or passthrough filter concat_loader = LoadComposableNodes( - composable_node_descriptions=[concat_component, glog_component], + composable_node_descriptions=[concat_component], target_container=target_container, condition=IfCondition(LaunchConfiguration("use_concat_filter")), ) @@ -100,7 +93,8 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_multithread", "False") add_launch_arg("use_intra_process", "False") add_launch_arg("use_pointcloud_container", "False") - add_launch_arg("container_name", "pointcloud_preprocessor_container") + add_launch_arg("pointcloud_container_name", "pointcloud_container") + add_launch_arg("individual_container_name", "concatenate_container") set_container_executable = SetLaunchConfiguration( "container_executable", diff --git a/common_sensor_launch/launch/hesai_XT32.launch.xml b/common_sensor_launch/launch/hesai_XT32.launch.xml index ff41b367..56587f72 100644 --- a/common_sensor_launch/launch/hesai_XT32.launch.xml +++ b/common_sensor_launch/launch/hesai_XT32.launch.xml @@ -14,7 +14,6 @@ - @@ -32,7 +31,6 @@ - diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index a79681b8..d1385bce 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -200,16 +200,9 @@ def create_parameter_dict(*args): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=nodes, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) - component_loader = LoadComposableNodes( - composable_node_descriptions=nodes, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - driver_component = ComposableNode( package="nebula_ros", plugin=sensor_make + "HwInterfaceRosWrapper", @@ -238,19 +231,13 @@ def create_parameter_dict(*args): ], ) - 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=target_container, + target_container=container, condition=IfCondition(LaunchConfiguration("launch_driver")), ) - return [container, component_loader, driver_component_loader] + return [container, driver_component_loader] def generate_launch_description(): @@ -287,7 +274,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("use_multithread", "False", "use multithread") add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication") - add_launch_arg("use_pointcloud_container", "false") add_launch_arg("container_name", "nebula_node_container") set_container_executable = SetLaunchConfiguration( diff --git a/common_sensor_launch/launch/velodyne_VLP16.launch.xml b/common_sensor_launch/launch/velodyne_VLP16.launch.xml index f374e1db..faa32305 100644 --- a/common_sensor_launch/launch/velodyne_VLP16.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP16.launch.xml @@ -15,7 +15,6 @@ - @@ -34,7 +33,6 @@ - diff --git a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml index 531efd4d..c3db6428 100644 --- a/common_sensor_launch/launch/velodyne_VLP32C.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLP32C.launch.xml @@ -15,7 +15,6 @@ - @@ -34,7 +33,6 @@ - diff --git a/common_sensor_launch/launch/velodyne_VLS128.launch.xml b/common_sensor_launch/launch/velodyne_VLS128.launch.xml index 89305c79..f1d32469 100644 --- a/common_sensor_launch/launch/velodyne_VLS128.launch.xml +++ b/common_sensor_launch/launch/velodyne_VLS128.launch.xml @@ -15,7 +15,6 @@ - @@ -34,7 +33,6 @@ - From dcf25c377104384ea66733605c82bda2924c9dfd Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 29 Jan 2024 13:32:23 +0900 Subject: [PATCH 2/2] chore(nebula_node_container): enhance logging (#199) Signed-off-by: kminoda --- .../launch/nebula_node_container.launch.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/common_sensor_launch/launch/nebula_node_container.launch.py b/common_sensor_launch/launch/nebula_node_container.launch.py index d1385bce..c14c1390 100644 --- a/common_sensor_launch/launch/nebula_node_container.launch.py +++ b/common_sensor_launch/launch/nebula_node_container.launch.py @@ -86,6 +86,14 @@ def create_parameter_dict(*args): nodes = [] + nodes.append( + ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + ) + nodes.append( ComposableNode( package="nebula_ros", @@ -200,7 +208,7 @@ def create_parameter_dict(*args): package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=nodes, - output="screen", + output="both", ) driver_component = ComposableNode(