diff --git a/dummy2_debug/README.md b/dummy2_debug/README.md index b62cab7..7c94830 100644 --- a/dummy2_debug/README.md +++ b/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 ``` ## 🎮 控制脚本 diff --git a/dummy2_debug/UNILAB_STARTUP_GUIDE.md b/dummy2_debug/UNILAB_STARTUP_GUIDE.md index d4d40e1..713d47e 100644 --- a/dummy2_debug/UNILAB_STARTUP_GUIDE.md +++ b/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通信服务** +- ✅ 确保按顺序启动各个服务 - ✅ 机械臂需在安全位置 diff --git a/dummy2_debug/final_demo.py b/dummy2_debug/final_demo.py index a97071a..c9ccfc1 100644 --- a/dummy2_debug/final_demo.py +++ b/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}") diff --git a/dummy2_debug/start_dummy2_unilab.sh b/dummy2_debug/start_dummy2_unilab.sh index ab81a82..b4a69a6 100755 --- a/dummy2_debug/start_dummy2_unilab.sh +++ b/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