fix:debug dummy2

dummy2_debug/start_dummy2_ros2.sh

@@ -0,0 +1,211 @@
+#!/bin/bash
+# Dummy2 ROS2服务启动脚本
+# 用于启动Dummy2机械臂的ROS2服务
+
+echo "==================================="
+echo "Dummy2 ROS2服务启动脚本"
+echo "==================================="
+
+# 设置变量
+DUMMY2_WS="/home/hh/dummy2/ros2/dummy2_ws"
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+
+# 检查workspace是否存在
+if [ ! -d "$DUMMY2_WS" ]; then
+    echo "错误: Dummy2工作空间不存在: $DUMMY2_WS"
+    exit 1
+fi
+
+echo "Dummy2工作空间: $DUMMY2_WS"
+
+# 函数：检查ROS2环境
+check_ros2_environment() {
+    echo "检查ROS2环境..."
+    
+    if [ -z "$ROS_DISTRO" ]; then
+        echo "警告: ROS_DISTRO环境变量未设置"
+        echo "尝试设置ROS2 Humble环境..."
+        source /opt/ros/humble/setup.bash
+    fi
+    
+    echo "ROS2版本: $ROS_DISTRO"
+    
+    # 检查ROS2命令是否可用
+    if command -v ros2 &> /dev/null; then
+        echo "✓ ROS2命令可用"
+    else
+        echo "✗ ROS2命令不可用"
+        echo "请确保ROS2已正确安装"
+        exit 1
+    fi
+}
+
+# 函数：构建workspace
+build_workspace() {
+    echo ""
+    echo "构建Dummy2工作空间..."
+    
+    cd "$DUMMY2_WS"
+    
+    # 设置ROS2环境
+    source /opt/ros/humble/setup.bash
+    
+    # 构建workspace
+    echo "运行colcon build..."
+    if colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release; then
+        echo "✓ 构建成功"
+    else
+        echo "✗ 构建失败"
+        return 1
+    fi
+    
+    # 设置环境
+    source install/setup.bash
+    echo "✓ 环境设置完成"
+}
+
+# 函数：启动硬件接口
+start_hardware_interface() {
+    echo ""
+    echo "启动Dummy2硬件接口..."
+    
+    cd "$DUMMY2_WS"
+    source /opt/ros/humble/setup.bash
+    source install/setup.bash
+    
+    echo "启动命令: ros2 launch dummy2_hw dummy2_hw.launch.py"
+    echo "注意: 这将在前台运行，按Ctrl+C停止"
+    echo "启动后请在新终端中运行MoveIt服务"
+    echo ""
+    
+    # 启动硬件接口
+    ros2 launch dummy2_hw dummy2_hw.launch.py
+}
+
+# 函数：启动MoveIt服务
+start_moveit_service() {
+    echo ""
+    echo "启动MoveIt2服务..."
+    
+    cd "$DUMMY2_WS"
+    source /opt/ros/humble/setup.bash
+    source install/setup.bash
+    
+    echo "启动命令: ros2 launch dummy2_moveit_config demo.launch.py"
+    echo "注意: 这将在前台运行，按Ctrl+C停止"
+    echo ""
+    
+    # 启动MoveIt服务
+    ros2 launch dummy2_moveit_config demo.launch.py
+}
+
+# 函数：检查服务状态
+check_services() {
+    echo ""
+    echo "检查ROS2服务状态..."
+    
+    source /opt/ros/humble/setup.bash
+    
+    echo "ROS2话题:"
+    ros2 topic list | head -10
+    
+    echo ""
+    echo "ROS2服务:"
+    ros2 service list | head -10
+    
+    echo ""
+    echo "ROS2节点:"
+    ros2 node list
+}
+
+# 主菜单
+show_menu() {
+    echo ""
+    echo "请选择操作:"
+    echo "1. 构建Dummy2工作空间"
+    echo "2. 启动硬件接口"
+    echo "3. 启动MoveIt服务"
+    echo "4. 检查服务状态"
+    echo "5. 显示启动说明"
+    echo "0. 退出"
+    echo ""
+    read -p "请输入选项 (0-5): " choice
+}
+
+# 显示启动说明
+show_instructions() {
+    echo ""
+    echo "==================================="
+    echo "Dummy2启动说明"
+    echo "==================================="
+    echo ""
+    echo "完整启动流程:"
+    echo ""
+    echo "1. 首先构建工作空间 (选项1)"
+    echo ""
+    echo "2. 在终端1启动硬件接口 (选项2):"
+    echo "   ./start_dummy2_ros2.sh"
+    echo "   然后选择选项2"
+    echo ""
+    echo "3. 在终端2启动MoveIt服务 (选项3):"
+    echo "   打开新终端，运行:"
+    echo "   cd $DUMMY2_WS"
+    echo "   source /opt/ros/humble/setup.bash"
+    echo "   source install/setup.bash"
+    echo "   ros2 launch dummy2_moveit_config demo.launch.py"
+    echo ""
+    echo "4. 在终端3测试Unilab控制:"
+    echo "   cd /home/hh/Uni-Lab-OS"
+    echo "   python test_dummy2_real_control.py --test-control"
+    echo ""
+    echo "注意事项:"
+    echo "- 确保Dummy2硬件已连接"
+    echo "- 检查CAN2ETH网络设置"
+    echo "- 确保机械臂在安全位置"
+}
+
+# 主程序
+main() {
+    check_ros2_environment
+    
+    if [ "$1" = "hw" ]; then
+        start_hardware_interface
+    elif [ "$1" = "moveit" ]; then
+        start_moveit_service
+    elif [ "$1" = "check" ]; then
+        check_services
+    elif [ "$1" = "build" ]; then
+        build_workspace
+    else
+        while true; do
+            show_menu
+            case $choice in
+                1)
+                    build_workspace
+                    ;;
+                2)
+                    start_hardware_interface
+                    ;;
+                3)
+                    start_moveit_service
+                    ;;
+                4)
+                    check_services
+                    ;;
+                5)
+                    show_instructions
+                    ;;
+                0)
+                    echo "退出"
+                    exit 0
+                    ;;
+                *)
+                    echo "无效选项，请重新选择"
+                    ;;
+            esac
+        done
+    fi
+}
+
+# 运行主程序
+main "$@"