From d4858758b45dc3d733bb6635f971246130ba2eb5 Mon Sep 17 00:00:00 2001
From: Daisuke Sato <tiryoh@gmail.com>
Date: Sat, 4 Mar 2023 16:14:01 +0900
Subject: [PATCH] Add lidar launch

Co-authored-by: 0nhc <thefoxfoxfox@outlook.com>
---
 mini_pupper_bringup/launch/bringup.launch.py | 21 ++++++----
 mini_pupper_bringup/launch/lidar.launch.py   | 41 ++++++++++++++++++++
 2 files changed, 55 insertions(+), 7 deletions(-)
 create mode 100644 mini_pupper_bringup/launch/lidar.launch.py

diff --git a/mini_pupper_bringup/launch/bringup.launch.py b/mini_pupper_bringup/launch/bringup.launch.py
index 2aab11d..9a23e48 100644
--- a/mini_pupper_bringup/launch/bringup.launch.py
+++ b/mini_pupper_bringup/launch/bringup.launch.py
@@ -26,7 +26,7 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
@@ -57,6 +57,9 @@ def generate_launch_description():
     servo_interface_launch_path = PathJoinSubstitution(
         [FindPackageShare('mini_pupper_control'), 'launch', 'servo_interface.launch.py']
     )
+    lidar_launch_path = PathJoinSubstitution(
+        [FindPackageShare('mini_pupper_bringup'), 'launch', 'lidar.launch.py']
+    )
 
     robot_name = LaunchConfiguration("robot_name")
     sim = LaunchConfiguration("sim")
@@ -114,11 +117,14 @@ def generate_launch_description():
         }.items(),
     )
 
-    servo_interface_launch = (
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(servo_interface_launch_path),
-            condition=IfCondition(joint_hardware_connected),
-        ),
+    servo_interface_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(servo_interface_launch_path),
+        condition=IfCondition(joint_hardware_connected),
+    )
+
+    lidar_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(lidar_launch_path),
+        condition=IfCondition(joint_hardware_connected),
     )
 
     return LaunchDescription([
@@ -128,5 +134,6 @@ def generate_launch_description():
         declare_hardware_connected,
         rviz2_node,
         bringup_launch,
-        servo_interface_launch
+        servo_interface_launch,
+        lidar_launch,
     ])
diff --git a/mini_pupper_bringup/launch/lidar.launch.py b/mini_pupper_bringup/launch/lidar.launch.py
new file mode 100644
index 0000000..7b762a7
--- /dev/null
+++ b/mini_pupper_bringup/launch/lidar.launch.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+
+# SPDX-License-Identifier: Apache-2.0
+#
+# Copyright (c) 2022-2023 MangDang
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='ldlidar_stl_ros2',
+            executable='ldlidar_stl_ros2_node',
+            name='LD06',
+            output='screen',
+            parameters=[
+                {'product_name': 'LDLiDAR_LD06'},
+                {'topic_name': 'scan'},
+                {'frame_id': 'base_lidar'},
+                {'port_name': '/dev/ttyUSB0'},
+                {'port_baudrate': 230400},
+                {'laser_scan_dir': True},
+                {'enable_angle_crop_func': False},
+                {'angle_crop_min': 135.0},
+                {'angle_crop_max': 225.0}],
+        ),
+    ])