diff --git a/aip_x2_launch/launch/pandar_node_container.launch.py b/aip_x2_launch/launch/pandar_node_container.launch.py index 5563f1ce..441db25b 100644 --- a/aip_x2_launch/launch/pandar_node_container.launch.py +++ b/aip_x2_launch/launch/pandar_node_container.launch.py @@ -12,6 +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 @@ -23,6 +26,7 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode import yaml @@ -34,6 +38,17 @@ def get_dual_return_filter_info(context): return p +def get_pandar_monitor_info(): + path = os.path.join( + get_package_share_directory("pandar_monitor"), + "config", + "pandar_monitor.param.yaml", + ) + with open(path, "r") as f: + p = yaml.safe_load(f)["/**"]["ros__parameters"] + return p + + def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py @@ -66,6 +81,20 @@ def create_parameter_dict(*args): result[x] = LaunchConfiguration(x) return result + monitor_node = Node( + executable="pandar_monitor_node", + package="pandar_monitor", + name="pandar_monitor", + parameters=[ + { + "ip_address": LaunchConfiguration("device_ip"), + } + ] + + [get_pandar_monitor_info()], + condition=IfCondition(LaunchConfiguration("launch_driver")), + output="screen", + ) + driver_component = ComposableNode( package="pandar_driver", plugin="pandar_driver::PandarDriver", @@ -277,6 +306,7 @@ def create_parameter_dict(*args): ring_outlier_filter_loader, dual_return_filter_loader, blockage_diag_loader, + monitor_node, ]