更新启动指南和脚本，优化启动流程，添加错误处理和动态路径检测

dummy2_debug/README.md

@@ -11,16 +11,16 @@
 
 ## 🚀 快速启动
 
-### 一键启动 (推荐)
+### 标准启动流程 (推荐)
 ```bash
-# 检查系统状态
-./start_dummy2_unilab.sh check
+# 1. 启动CAN2ETH通信服务 (终端1)
+./start_dummy2_unilab.sh can2eth
 
-# 启动硬件接口 (终端1)
-./start_dummy2_unilab.sh hw
+# 2. 启动MoveIt服务 (终端2)
+./start_dummy2_unilab.sh moveit no-gui
 
-# 运行控制测试 (终端2)  
-./start_dummy2_unilab.sh test
+# 3. 运行控制测试 (终端3)
+./start_dummy2_unilab.sh test direct
 ```
 
 ## 🎮 控制脚本

dummy2_debug/UNILAB_STARTUP_GUIDE.md

@@ -2,21 +2,30 @@
 
 ## 🚀 快速启动 (推荐)
 
-### 使用统一脚本
+### 标准启动流程 (三步启动)
 ```bash
 cd /home/hh/Uni-Lab-OS/dummy2_debug
 
+# 1. 启动CAN2ETH通信服务 (终端1)
+./start_dummy2_unilab.sh can2eth
+
+# 2. 启动MoveIt服务 (终端2)
+./start_dummy2_unilab.sh moveit no-gui
+
+# 3. 运行控制测试 (终端3)
+./start_dummy2_unilab.sh test direct
+```
+
+### 快速启动 (如果CAN2ETH已运行)
+```bash
 # 1. 检查状态
 ./start_dummy2_unilab.sh check
 
 # 2. 启动硬件接口 (终端1)
 ./start_dummy2_unilab.sh hw
 
-# 3. 运行控制测试 (终端2) 
-./start_dummy2_unilab.sh test
-
-# 4. 启动MoveIt服务 (可选，终端3)
-./start_dummy2_unilab.sh moveit
+# 3. 运行控制测试 (终端2)
+./start_dummy2_unilab.sh test direct
 ```
 
 ## 📋 详细步骤
@@ -28,13 +37,19 @@ cd /home/hh/Uni-Lab-OS/dummy2_debug
 
 ### 手动启动流程
 ```bash
-# 硬件接口 (终端1)
+# CAN2ETH通信服务 (终端1)
 cd /home/hh/dummy2/ros2/dummy2_ws
 mamba activate unilab
 source install/setup.bash
-ros2 launch dummy2_hw dummy2_hw.launch.py
+ros2 launch dummy2_can2eth dummy2_can2eth_server.launch.py
 
-# 控制脚本 (终端2)
+# MoveIt服务 (终端2)
+cd /home/hh/dummy2/ros2/dummy2_ws
+mamba activate unilab
+source install/setup.bash
+ros2 launch dummy2_moveit_config demo.launch.py use_rviz:=false
+
+# 控制脚本 (终端3)
 cd /home/hh/Uni-Lab-OS/dummy2_debug  
 mamba activate unilab
 source /home/hh/dummy2/ros2/dummy2_ws/install/setup.bash
@@ -45,14 +60,17 @@ python dummy2_direct_move.py
 
 | 命令 | 功能 |
 |------|------|
-| `./start_dummy2_unilab.sh check` | 检查系统状态 |
-| `./start_dummy2_unilab.sh build` | 构建工作空间 |
+| `./start_dummy2_unilab.sh can2eth` | 启动CAN2ETH通信服务 |
 | `./start_dummy2_unilab.sh hw` | 启动硬件接口 |
-| `./start_dummy2_unilab.sh test` | 运行控制测试 |
-| `./start_dummy2_unilab.sh moveit` | 启动MoveIt服务 |
+| `./start_dummy2_unilab.sh moveit` | 启动MoveIt服务 (带图形界面) |
+| `./start_dummy2_unilab.sh moveit no-gui` | 启动MoveIt服务 (无图形界面) |
+| `./start_dummy2_unilab.sh test [类型]` | 运行控制测试 |
+| `./start_dummy2_unilab.sh check` | 检查系统状态 |
+| `./start_dummy2_unilab.sh info` | 显示配置信息 |
 
 ## ⚠️ 重要提示
 - ✅ 使用 `mamba activate unilab` (ROS2已包含在内)
 - ❌ 不需要 `source /opt/ros/humble/setup.bash`
-- ✅ 确保硬件接口先启动
+- ✅ **必须先启动CAN2ETH通信服务**
+- ✅ 确保按顺序启动各个服务
 - ✅ 机械臂需在安全位置

dummy2_debug/final_demo.py

@@ -116,7 +116,8 @@ class FinalDemo(Node):
         planning_targets = [
             {"name": "规划到目标位置1", "joint": "Joint1", "value": 0.4},
             {"name": "规划到目标位置2", "joint": "Joint2", "value": 0.3},
-            {"name": "规划回到Home", "joint": "Joint1", "value": 0.0}
+            {"name": "规划回到Home", "joints": ["Joint1", "Joint2", "Joint3", "Joint4", "Joint5", "Joint6"], 
+             "values": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}
         ]
         
         for i, target in enumerate(planning_targets):
@@ -127,15 +128,29 @@ class FinalDemo(Node):
             goal_msg.request.group_name = "dummy2_arm"
             
             # 设置关节约束
-            joint_constraint = JointConstraint()
-            joint_constraint.joint_name = target["joint"]
-            joint_constraint.position = target["value"]
-            joint_constraint.tolerance_above = 0.01
-            joint_constraint.tolerance_below = 0.01
-            joint_constraint.weight = 1.0
-            
             constraints = Constraints()
-            constraints.joint_constraints = [joint_constraint]
+            
+            # 处理单关节或多关节约束
+            if "joint" in target:  # 单关节约束
+                joint_constraint = JointConstraint()
+                joint_constraint.joint_name = target["joint"]
+                joint_constraint.position = target["value"]
+                joint_constraint.tolerance_above = 0.01
+                joint_constraint.tolerance_below = 0.01
+                joint_constraint.weight = 1.0
+                constraints.joint_constraints = [joint_constraint]
+            elif "joints" in target:  # 多关节约束 (Home位置)
+                joint_constraints = []
+                for joint_name, joint_value in zip(target["joints"], target["values"]):
+                    joint_constraint = JointConstraint()
+                    joint_constraint.joint_name = joint_name
+                    joint_constraint.position = joint_value
+                    joint_constraint.tolerance_above = 0.01
+                    joint_constraint.tolerance_below = 0.01
+                    joint_constraint.weight = 1.0
+                    joint_constraints.append(joint_constraint)
+                constraints.joint_constraints = joint_constraints
+            
             goal_msg.request.goal_constraints = [constraints]
             
             # 设置规划选项
@@ -166,11 +181,39 @@ class FinalDemo(Node):
                     if result.error_code.val == 1:  # SUCCESS
                         trajectory_points = len(result.planned_trajectory.joint_trajectory.points)
                         print(f"     🎉 规划成功! 生成 {trajectory_points} 个轨迹点")
+                        
+                        # 显示目标关节位置
+                        if "joint" in target:
+                            print(f"     🎯 目标: {target['joint']} = {target['value']}")
+                        elif "joints" in target:
+                            target_info = ", ".join([f"{j}={v}" for j, v in zip(target["joints"], target["values"])])
+                            print(f"     🎯 目标: {target_info}")
+                        
                         print(f"     🤖 执行运动...")
+                        
+                        # 等待执行完成
+                        time.sleep(3)
+                        print(f"     ✅ 运动执行完成")
                     else:
                         print(f"     ⚠️ 规划失败，错误码: {result.error_code.val}")
+                        
+                        # 提供错误码解释
+                        error_meanings = {
+                            -1: "失败",
+                            -2: "无效的组名",
+                            -3: "无效的目标约束",
+                            -4: "无效的机器人状态",
+                            -5: "无效的链接名",
+                            -10: "规划超时",
+                            -11: "规划失败",
+                            -12: "无效起始状态",
+                            -13: "无效目标状态"
+                        }
+                        if result.error_code.val in error_meanings:
+                            print(f"     📝 错误说明: {error_meanings[result.error_code.val]}")
                 else:
                     print(f"     ❌ 规划请求被拒绝")
+                    print(f"     💡 建议: 检查MoveIt服务状态和机器人配置")
                     
             except Exception as e:
                 print(f"     ❌ 规划异常: {e}")

dummy2_debug/start_dummy2_unilab.sh

@@ -4,8 +4,36 @@
 
 set -e
 
-# 配置
-DUMMY2_WS="/home/hh/dummy2/ros2/dummy2_ws"
+# 动态配置路径
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_ROOT="$(dirname "$SCRIPT_DIR")"
+
+# 自动检测 dummy2_ws 路径
+DUMMY2_WS_CANDIDATES=(
+    "$HOME/dummy2/ros2/dummy2_ws"
+    "$HOME/ros2/dummy2_ws"
+    "$PROJECT_ROOT/../dummy2_ws"
+    "$(find "$HOME" -maxdepth 3 -name "dummy2_ws" -type d 2>/dev/null | head -1)"
+)
+
+DUMMY2_WS=""
+for ws in "${DUMMY2_WS_CANDIDATES[@]}"; do
+    if [ -d "$ws" ] && [ -f "$ws/src/dummy2_hw/package.xml" ]; then
+        DUMMY2_WS="$ws"
+        break
+    fi
+done
+
+if [ -z "$DUMMY2_WS" ]; then
+    echo "错误: 无法找到 dummy2_ws 工作空间"
+    echo "请确保以下位置之一存在 dummy2_ws:"
+    for ws in "${DUMMY2_WS_CANDIDATES[@]}" ; do
+        echo "  $ws"
+    done
+    exit 1
+fi
+
+echo "使用 dummy2_ws: $DUMMY2_WS"
 
 # 初始化mamba
 eval "$(mamba shell hook --shell bash)"
@@ -13,14 +41,30 @@ eval "$(mamba shell hook --shell bash)"
 # 函数：激活环境并检查
 setup_environment() {
     echo "激活unilab环境..."
-    mamba activate unilab
     
-    if ! command -v ros2 &> /dev/null; then
-        echo "错误: ROS2在unilab环境中不可用"
+    # 检查 mamba 是否可用
+    if ! command -v mamba &> /dev/null; then
+        echo "错误: mamba 未安装或不在PATH中"
+        echo "请先安装 mamba 或 conda"
         exit 1
     fi
     
-    echo "✓ ROS2环境准备就绪"
+    # 激活 unilab 环境
+    if ! mamba activate unilab 2>/dev/null; then
+        echo "错误: 无法激活 unilab 环境"
+        echo "请确保 unilab 环境已创建"
+        exit 1
+    fi
+    
+    # 检查 ROS2 是否可用
+    if ! command -v ros2 &> /dev/null; then
+        echo "错误: ROS2在unilab环境中不可用"
+        echo "请在 unilab 环境中安装 ROS2"
+        exit 1
+    fi
+    
+    echo "✓ ROS2环境准备就绪 ($(ros2 --version 2>/dev/null || echo '未知版本'))"
+    echo "✓ 工作空间: $DUMMY2_WS"
 }
 
 # 函数：构建工作空间
@@ -41,6 +85,22 @@ build_workspace() {
     echo "✓ 构建完成"
 }
 
+# 函数：启动CAN2ETH通信服务
+start_can2eth() {
+    echo "==================================="
+    echo "启动CAN2ETH通信服务"
+    echo "==================================="
+    
+    setup_environment
+    cd "$DUMMY2_WS"
+    source install/setup.bash
+    
+    echo "启动CAN2ETH通信服务..."
+    echo "作用: 建立与机械臂硬件的CAN总线通信连接"
+    echo "按Ctrl+C停止"
+    ros2 launch dummy2_can2eth dummy2_can2eth_server.launch.py
+}
+
 # 函数：启动硬件接口
 start_hardware() {
     echo "==================================="
@@ -52,6 +112,7 @@ start_hardware() {
     source install/setup.bash
     
     echo "启动硬件接口节点..."
+    echo "注意: 请确保CAN2ETH服务已在另一个终端启动"
     echo "按Ctrl+C停止"
     ros2 launch dummy2_hw dummy2_hw.launch.py
 }
@@ -66,9 +127,18 @@ start_moveit() {
     cd "$DUMMY2_WS"
     source install/setup.bash
     
-    echo "启动MoveIt2规划服务..."
-    echo "按Ctrl+C停止"
-    ros2 launch dummy2_moveit_config demo.launch.py
+    # 检查是否指定无图形界面模式
+    if [ "$2" = "no-gui" ] || [ "$2" = "headless" ]; then
+        echo "启动MoveIt2规划服务 (无图形界面)..."
+        echo "作用: 启动MoveIt后端服务进行路径规划和运动控制"
+        echo "按Ctrl+C停止"
+        ros2 launch dummy2_moveit_config demo.launch.py use_rviz:=false
+    else
+        echo "启动MoveIt2规划服务 (带RViz图形界面)..."
+        echo "作用: 启动MoveIt服务和RViz可视化界面"
+        echo "按Ctrl+C停止"
+        ros2 launch dummy2_moveit_config demo.launch.py
+    fi
 }
 
 # 函数：检查状态
@@ -99,10 +169,103 @@ run_test() {
     
     setup_environment
     source "$DUMMY2_WS/install/setup.bash"
-    cd /home/hh/Uni-Lab-OS
+    cd "$SCRIPT_DIR"
     
-    echo "运行直接控制测试..."
-    python dummy2_debug/dummy2_direct_move.py
+    # 检查可用的测试脚本
+    AVAILABLE_TESTS=()
+    if [ -f "dummy2_direct_move.py" ]; then
+        AVAILABLE_TESTS+=("direct:dummy2_direct_move.py")
+    fi
+    if [ -f "force_home.py" ]; then
+        AVAILABLE_TESTS+=("home:force_home.py")
+    fi
+    if [ -f "test_complete_integration.py" ]; then
+        AVAILABLE_TESTS+=("integration:test_complete_integration.py")
+    fi
+    if [ -f "dummy2_move_demo.py" ]; then
+        AVAILABLE_TESTS+=("demo:dummy2_move_demo.py")
+    fi
+    if [ -f "final_demo.py" ]; then
+        AVAILABLE_TESTS+=("final:final_demo.py")
+    fi
+    
+    if [ ${#AVAILABLE_TESTS[@]} -eq 0 ]; then
+        echo "错误: 未找到测试脚本"
+        exit 1
+    fi
+    
+    # 如果指定了测试类型
+    if [ -n "$2" ]; then
+        case "$2" in
+            "direct"|"d")
+                echo "运行直接控制测试..."
+                python dummy2_direct_move.py
+                ;;
+            "home"|"h")
+                echo "运行归位测试..."
+                python force_home.py
+                ;;
+            "integration"|"i")
+                echo "运行集成测试..."
+                python test_complete_integration.py
+                ;;
+            "demo")
+                echo "运行移动演示..."
+                python dummy2_move_demo.py
+                ;;
+            "final"|"f")
+                echo "运行最终演示..."
+                python final_demo.py
+                ;;
+            *)
+                echo "未知测试类型: $2"
+                echo "可用类型: direct, home, integration, demo, final"
+                exit 1
+                ;;
+        esac
+    else
+        # 默认运行直接控制测试
+        echo "运行直接控制测试..."
+        python dummy2_direct_move.py
+    fi
+}
+
+# 函数：显示配置信息
+show_info() {
+    echo "==================================="
+    echo "Dummy2 配置信息"
+    echo "==================================="
+    
+    echo "脚本位置: $SCRIPT_DIR"
+    echo "项目根目录: $PROJECT_ROOT"
+    echo "工作空间: $DUMMY2_WS"
+    echo ""
+    
+    echo "环境检查:"
+    if command -v mamba &> /dev/null; then
+        echo "✓ mamba 可用: $(which mamba)"
+    else
+        echo "✗ mamba 不可用"
+    fi
+    
+    echo ""
+    echo "可用测试脚本:"
+    cd "$SCRIPT_DIR"
+    for script in dummy2_direct_move.py force_home.py test_complete_integration.py dummy2_move_demo.py final_demo.py; do
+        if [ -f "$script" ]; then
+            echo "✓ $script"
+        else
+            echo "✗ $script (未找到)"
+        fi
+    done
+    
+    echo ""
+    echo "工作空间状态:"
+    if [ -f "$DUMMY2_WS/install/setup.bash" ]; then
+        echo "✓ 工作空间已构建"
+    else
+        echo "✗ 工作空间未构建 (运行: $0 build)"
+    fi
 }
 
 # 主程序
@@ -110,35 +273,73 @@ case "${1:-help}" in
     "build")
         build_workspace
         ;;
+    "can2eth"|"can")
+        start_can2eth
+        ;;
     "hw"|"hardware")
         start_hardware
         ;;
     "moveit")
-        start_moveit
+        start_moveit "$@"
         ;;
     "check"|"status")
         check_status
         ;;
+    "info")
+        show_info
+        ;;
     "test")
-        run_test
+        run_test "$@"
         ;;
     "help"|*)
         echo "Dummy2 Unilab环境启动脚本"
         echo ""
-        echo "用法: $0 [命令]"
+        echo "用法: $0 [命令] [选项]"
         echo ""
         echo "命令:"
-        echo "  build     - 构建工作空间"
-        echo "  hw        - 启动硬件接口"
-        echo "  moveit    - 启动MoveIt服务"
-        echo "  check     - 检查服务状态"
-        echo "  test      - 运行控制测试"
-        echo "  help      - 显示此帮助信息"
+        echo "  build         - 构建工作空间"
+        echo "  can2eth (can) - 启动CAN2ETH通信服务"
+        echo "  hw            - 启动硬件接口"
+        echo "  moveit [模式] - 启动MoveIt服务"
+        echo "  check         - 检查服务状态"
+        echo "  info          - 显示配置信息"
+        echo "  test [类型]   - 运行控制测试"
+        echo "  help          - 显示此帮助信息"
         echo ""
-        echo "启动顺序:"
-        echo "1. $0 build     (首次或更新后)"
-        echo "2. $0 hw        (终端1)"
-        echo "3. $0 moveit    (终端2)"
-        echo "4. $0 test      (终端3)"
+        echo "MoveIt模式:"
+        echo "  (默认)       - 启动带RViz图形界面"
+        echo "  no-gui       - 启动无图形界面模式"
+        echo "  headless     - 同no-gui"
+        echo ""
+        echo "测试类型:"
+        echo "  direct (d)     - 直接关节控制测试 (默认)"
+        echo "  home (h)       - 归位控制测试"
+        echo "  integration (i) - Unilab集成测试"
+        echo "  demo           - 移动演示"
+        echo "  final (f)      - 最终演示"
+        echo ""
+        echo "启动顺序 (标准流程):"
+        echo "1. $0 build              (首次或更新后)"
+        echo "2. $0 can2eth            (终端1 - CAN2ETH通信)"
+        echo "3. $0 moveit no-gui      (终端2 - MoveIt服务)"
+        echo "4. $0 test [类型]        (终端3 - 控制测试)"
+        echo ""
+        echo "快速启动 (如果已有CAN2ETH):"
+        echo "1. $0 hw                 (终端1 - 硬件接口)"
+        echo "2. $0 test [类型]        (终端2 - 控制测试)"
+        echo ""
+        echo "示例:"
+        echo "  $0 can2eth              # 启动CAN2ETH通信"
+        echo "  $0 moveit no-gui        # 启动MoveIt(无界面)"
+        echo "  $0 test direct          # 运行直接控制测试"
+        echo "  $0 test home            # 运行归位测试"
+        echo ""
+        echo "自动检测路径:"
+        echo "  脚本目录: $SCRIPT_DIR"
+        if [ -n "$DUMMY2_WS" ]; then
+            echo "  工作空间: $DUMMY2_WS"
+        else
+            echo "  工作空间: 未找到"
+        fi
         ;;
 esac