From bc6e1e79d18eef2f15cc28b928c018db75660e22 Mon Sep 17 00:00:00 2001
From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com>
Date: Wed, 12 Oct 2022 15:11:37 +0900
Subject: [PATCH] feat: add pandar monitor (#83)

* feat: add pandar monitor

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../launch/pandar_node_container.launch.py    | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)

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,
     ]