From 9eca06ebd87795d6cf5fb23923446a69558e3ea6 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

---
 README.md                                    |  2 +-
 mini_pupper_bringup/launch/bringup.launch.py | 27 +++++++++---
 mini_pupper_bringup/launch/lidar.launch.py   | 44 ++++++++++++++++++++
 3 files changed, 66 insertions(+), 7 deletions(-)
 create mode 100644 mini_pupper_bringup/launch/lidar.launch.py

diff --git a/README.md b/README.md
index be1f0cdc..521018e0 100755
--- a/README.md
+++ b/README.md
@@ -133,7 +133,7 @@ Open 2 terminals and ssh login to Mini Pupper on both.
 ```sh
 # Terminal 1 (ssh)
 . ~/ros2_ws/install/setup.bash # setup.zsh if you use zsh instead of bash
-ros2 launch mini_pupper_bringup bringup.launch.py
+ros2 launch mini_pupper_bringup bringup.launch.py lidar:=false
 
 # Terminal 2 (ssh)
 ros2 run teleop_twist_keyboard teleop_twist_keyboard
diff --git a/mini_pupper_bringup/launch/bringup.launch.py b/mini_pupper_bringup/launch/bringup.launch.py
index 2aab11dd..62b48688 100644
--- a/mini_pupper_bringup/launch/bringup.launch.py
+++ b/mini_pupper_bringup/launch/bringup.launch.py
@@ -57,10 +57,14 @@ 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")
     joint_hardware_connected = LaunchConfiguration("joint_hardware_connected")
+    lidar_connected = LaunchConfiguration("lidar_connected")
     rviz = LaunchConfiguration("rviz")
 
     declare_robot_name = DeclareLaunchArgument(
@@ -87,6 +91,12 @@ def generate_launch_description():
             description='Set to true if connected to a physical robot'
         )
 
+    declare_lidar_connected = DeclareLaunchArgument(
+            name='lidar_connected',
+            default_value='true',
+            description='Set to true if connected to a lidar'
+        )
+
     rviz2_node = Node(
             package="rviz2",
             namespace="",
@@ -114,11 +124,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 and lidar_connected),
     )
 
     return LaunchDescription([
@@ -126,7 +139,9 @@ def generate_launch_description():
         declare_sim,
         declare_rviz,
         declare_hardware_connected,
+        declare_lidar_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 00000000..cd422e4a
--- /dev/null
+++ b/mini_pupper_bringup/launch/lidar.launch.py
@@ -0,0 +1,44 @@
+#!/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():
+
+    lidar_node = 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}],
+    ),
+
+    return LaunchDescription([
+        lidar_node
+    ])