Skip to content

Commit

Permalink
fix: now works
Browse files Browse the repository at this point in the history
Signed-off-by: kminoda <[email protected]>
  • Loading branch information
kminoda committed Jan 16, 2024
1 parent 70fa963 commit eb50925
Show file tree
Hide file tree
Showing 5 changed files with 11 additions and 35 deletions.
22 changes: 4 additions & 18 deletions common_sensor_launch/launch/nebula_node_container.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -195,21 +195,14 @@ def create_parameter_dict(*args):

# set container to run all required components in the same process
container = ComposableNodeContainer(
name=LaunchConfiguration("container_name"),
name=LaunchConfiguration("lidar_container_name"),
namespace="pointcloud_preprocessor",
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",
Expand Down Expand Up @@ -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():
Expand Down Expand Up @@ -287,8 +274,7 @@ 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")
add_launch_arg("lidar_container_name", "nebula_node_container")

set_container_executable = SetLaunchConfiguration(
"container_executable",
Expand Down
6 changes: 2 additions & 4 deletions common_sensor_launch/launch/velodyne_VLP16.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,7 @@
<arg name="cloud_min_angle" default="0"/>
<arg name="cloud_max_angle" default="360"/>
<arg name="vehicle_mirror_param_file"/>
<arg name="use_pointcloud_container" default="false"/>
<arg name="container_name" default="velodyne_node_container"/>
<arg name="lidar_container_name" default="pointcloud_container"/>

<include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
<arg name="launch_driver" value="$(var launch_driver)"/>
Expand All @@ -32,8 +31,7 @@
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="false"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="lidar_container_name" value="$(var lidar_container_name)"/>
</include>

<!-- Velodyne Monitor -->
Expand Down
6 changes: 2 additions & 4 deletions common_sensor_launch/launch/velodyne_VLS128.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,7 @@
<arg name="cloud_min_angle" default="0"/>
<arg name="cloud_max_angle" default="360"/>
<arg name="vehicle_mirror_param_file"/>
<arg name="use_pointcloud_container" default="false"/>
<arg name="container_name" default="velodyne_node_container"/>
<arg name="lidar_container_name" default="pointcloud_container"/>

<include file="$(find-pkg-share common_sensor_launch)/launch/nebula_node_container.launch.py">
<arg name="launch_driver" value="$(var launch_driver)"/>
Expand All @@ -32,8 +31,7 @@
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="container_name" value="$(var container_name)"/>
<arg name="lidar_container_name" value="$(var lidar_container_name)"/>
</include>

<!-- Velodyne Monitor -->
Expand Down
6 changes: 1 addition & 5 deletions sample_sensor_kit_launch/launch/lidar.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
<arg name="scan_phase" value="300.0"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>

Expand All @@ -38,7 +37,6 @@
<arg name="cloud_max_angle" value="60"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>

Expand All @@ -55,7 +53,6 @@
<arg name="cloud_max_angle" value="60"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>

Expand All @@ -72,15 +69,14 @@
<arg name="cloud_max_angle" value="60"/>
<arg name="launch_driver" value="$(var launch_driver)"/>
<arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/>
<arg name="container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>

<include file="$(find-pkg-share sample_sensor_kit_launch)/launch/pointcloud_preprocessor.launch.py">
<arg name="base_frame" value="base_link"/>
<arg name="use_intra_process" value="true"/>
<arg name="use_multithread" value="true"/>
<arg name="include_concat_node_in_pointcloud_container" value="$(var include_concat_node_in_pointcloud_container)"/>
<arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
</include>
</group>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -64,9 +64,7 @@ def launch_setup(context, *args, **kwargs):

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

Expand All @@ -89,7 +87,7 @@ 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("include_concat_node_in_pointcloud_container", "False")
add_launch_arg("use_pointcloud_container", "False")
add_launch_arg("pointcloud_container_name", "pointcloud_container")
add_launch_arg("individual_container_name", "concatenate_container")

Expand Down

0 comments on commit eb50925

Please sign in to comment.