From 83a6242c32f2d242806f3ee59290fd8d8b2564b5 Mon Sep 17 00:00:00 2001
From: FallenSigh <fallensigh@gmail.com>
Date: Fri, 19 Jun 2026 09:58:28 +0800
Subject: [PATCH] feat(udp_teleop): add vision grasp bringup launch file

Bundles arm_control + vision_grasp + optional box_detection_grasp into single ros2 launch.
Launch arguments: detection (enable YOLO), auto_grasp, udp_ip, show_debug.

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
---
 .../udp_teleop/launch/vision_grasp.launch.py  | 75 +++++++++++++++++++
 ros2/src/udp_teleop/setup.py                  |  2 +
 2 files changed, 77 insertions(+)
 create mode 100644 ros2/src/udp_teleop/launch/vision_grasp.launch.py

diff --git a/ros2/src/udp_teleop/launch/vision_grasp.launch.py b/ros2/src/udp_teleop/launch/vision_grasp.launch.py
new file mode 100644
index 0000000..fa3fd73
--- /dev/null
+++ b/ros2/src/udp_teleop/launch/vision_grasp.launch.py
@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+"""视觉抓取 bringup：机械臂控制 + 视觉抓取 + (可选) YOLO 方框检测。
+
+组件依赖：arm_control → vision_grasp → box_detection_grasp
+三个节点通过 ROS 服务通信，启动顺序由 lifecycle 自发处理。
+
+用法：
+  ros2 launch udp_teleop vision_grasp.launch.py
+  ros2 launch udp_teleop vision_grasp.launch.py udp_ip:=192.168.4.1 detection:=true
+  ros2 launch udp_teleop vision_grasp.launch.py detection:=true auto_grasp:=true
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory('udp_teleop')
+
+    arm_params = os.path.join(pkg_share, 'config', 'arm_control.yaml')
+    vision_params = os.path.join(pkg_share, 'config', 'vision_grasp.yaml')
+    detection_params = os.path.join(pkg_share, 'config', 'box_detection_grasp.yaml')
+
+    detection = LaunchConfiguration('detection')
+    auto_grasp = LaunchConfiguration('auto_grasp')
+    udp_ip = LaunchConfiguration('udp_ip')
+    show_debug = LaunchConfiguration('show_debug')
+
+    return LaunchDescription([
+        DeclareLaunchArgument('detection', default_value='false',
+                              description='是否启动 YOLO 方框检测节点'),
+        DeclareLaunchArgument('auto_grasp', default_value='false',
+                              description='检测到方框后自动抓取'),
+        DeclareLaunchArgument('udp_ip', default_value='192.168.245.67',
+                              description='ESP32 UDP IP 地址'),
+        DeclareLaunchArgument('show_debug', default_value='false',
+                              description='显示 YOLO 检测调试窗口'),
+
+        # ===== arm_control =====
+        Node(
+            package='udp_teleop',
+            executable='arm_control',
+            name='arm_control',
+            output='screen',
+            parameters=[arm_params, {'udp_ip': udp_ip}],
+        ),
+
+        # ===== vision_grasp =====
+        Node(
+            package='udp_teleop',
+            executable='vision_grasp',
+            name='vision_grasp',
+            output='screen',
+            parameters=[vision_params],
+        ),
+
+        # ===== box_detection_grasp (可选) =====
+        Node(
+            package='udp_teleop',
+            executable='box_detection_grasp',
+            name='box_detection_grasp',
+            output='screen',
+            parameters=[detection_params, {
+                'auto_grasp': auto_grasp,
+                'show_debug_window': show_debug,
+            }],
+            condition=IfCondition(detection),
+        ),
+    ])
diff --git a/ros2/src/udp_teleop/setup.py b/ros2/src/udp_teleop/setup.py
index 9bbf03d..c0c3016 100644
--- a/ros2/src/udp_teleop/setup.py
+++ b/ros2/src/udp_teleop/setup.py
@@ -15,6 +15,8 @@ setup(
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share', package_name, 'config'),
             glob('config/*.yaml')),
+        (os.path.join('share', package_name, 'launch'),
+            glob('launch/*.launch.py')),
         (os.path.join('share', package_name, 'models'),
             glob('models/*.pt')),
     ],