ROS2_Moveit2_Ur5e_Grasp项目详解（八）：start_grasp详解

def launch_setup(context, *args, **kwargs):
    ## 启动设置函数，用于配置和启动抓取演示相关的节点

    ## 包含抓取演示的launch文件
    ## 该launch文件来自ur5e_gripper_control包，用于演示机器人抓取功能
    grasp_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [FindPackageShare("ur5e_gripper_control"), "/launch", "/demo.launch.py"]
        ),
    )

    ## 组装需要启动的节点列表
    nodes_to_launch = [
        grasp_launch,
    ]

    ## 返回待启动的节点列表
    return nodes_to_launch

def generate_launch_description():
    ## 构造机器人运动学配置文件路径
    ## 该文件包含机器人逆运动学求解器的配置参数
    robot_description_kinematics = PathJoinSubstitution(
        [FindPackageShare("ur5e_gripper_moveit_config"), "config", "kinematics.yaml"]
    )

    ## 获取目标位姿列表配置文件路径
    ## 该文件定义了机器人需要到达的一系列目标位姿
    target_pose_list = os.path.join(
        get_package_share_directory('ur5e_gripper_control'),
        'config', 
        'target_pose_list.yaml'
    )

    ## 返回LaunchDescription对象，包含需要启动的节点列表
    return LaunchDescription([
        ## 启动演示节点，用于控制机器人执行预定义的抓取任务
        Node(
            package='ur5e_gripper_control',     ## 节点所属的功能包名称
            executable='demo',                  ## 要执行的可执行文件名称
            name='demo_node',                   ## 节点名称
            parameters=[{                       ## 节点参数列表
                    "use_sim_time":True,        ## 使用仿真时间
                },
                robot_description_kinematics,   ## 机器人运动学参数文件
                target_pose_list                ## 目标位姿列表配置文件
            ], 
            output='screen'                     ## 输出方式：打印到屏幕
        ),
    ])

/**
 * @brief UR5e机器人抓取演示主函数
 * 
 * 该函数实现了完整的机器人抓取和放置流程，包括：
 * 1. 初始化ROS 2节点和机器人控制接口
 * 2. 获取预设的放置位置列表
 * 3. 通过TF获取立方体的位置信息
 * 4. 控制机器人依次抓取立方体并放置到指定位置
 * 5. 完成任务后回到准备位置
 * 
 * @param argc 命令行参数数量
 * @param argv 命令行参数数组
 * @return int 程序退出状态码
 */
int main(int argc, char **argv) {
  // 初始化ROS 2客户端库
  rclcpp::init(argc, argv);
  
  // 创建节点选项并设置自动声明参数
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  
  // 创建UR5e机器人控制节点实例
  auto node = std::make_shared<UR5eGripper>(node_options);
  
  // 初始化机器人控制接口
  node->init();

  // 创建单线程执行器并添加节点
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  
  // 启动独立线程运行执行器
  std::thread([&executor]() { executor.spin(); }).detach();

  // 获取预设的目标放置位置列表
  std::vector<std::vector<double>> target_pose_list;
  node->get_target_pose_list(target_pose_list);
  
  // 设置坐标变换参考帧
  std::string from_frame = "base_link";
  
  // 定义需要抓取的立方体坐标系名称列表
  std::vector<std::string> to_frame_list = {"cube1", "cube2", "cube3", "cube4", "cube5", "cube6"};

  // 存储立方体抓取位置的列表
  std::vector<std::vector<double>> cube_pose_list;
  
  // 遍历所有立方体，获取它们的位置信息并调整为抓取位置
  for (size_t i = 0; i < to_frame_list.size(); i++) {
    std::vector<double> cube_pose;
    
    // 通过TF获取立方体相对于base_link的位姿
    node->get_cube_pose(from_frame, to_frame_list[i], cube_pose);
    
    // 检查是否成功获取位姿信息
    if (cube_pose.empty()) {
      RCLCPP_WARN(rclcpp::get_logger("demo4"), "Failed to get pose for %s, skipping", to_frame_list[i].c_str());
      continue;
    }

    // 调整抓取位置，确保机器人从立方体上方抓取
    cube_pose[0] -= 0.012 ;  // 微调X坐标
    cube_pose[1] += 0.01;    // 微调Y坐标
    //cube_pose[2] += 0.14;  // 注释掉的Z轴调整
    cube_pose[2] += 0.14;    // Z轴增加0.14米，确保从上方抓取
    cube_pose[3] = 0.0;      // 设置roll角度为0
    cube_pose[4] = M_PI;     // 设置pitch角度为π(180度)，使夹爪向下
    cube_pose[5] = 0.0;      // 设置yaw角度为0
    
    // 打印调整后的立方体抓取位置
    RCLCPP_INFO(rclcpp::get_logger("demo4"), "Adjusted cube pose for %s: x=%f, y=%f, z=%f",
                to_frame_list[i].c_str(), cube_pose[0], cube_pose[1], cube_pose[2]);
    
    // 将调整后的抓取位置添加到列表中
    cube_pose_list.push_back(cube_pose);
  }

  // 循环执行抓取和放置操作，最多处理6个立方体
  for (size_t i = 0; i < std::min<size_t>(6, cube_pose_list.size()); i++) {
    // 控制机器人移动到立方体抓取位置
    bool grasp_success = node->plan_and_execute(cube_pose_list[i]);
    
    // 如果移动失败，则跳过当前立方体
    if (!grasp_success) {
      continue;
    }
    
    // 闭合夹爪抓取立方体，0.36为夹爪闭合位置
    node->grasp(0.36);
    
    // 等待1秒确保夹爪完全闭合
    rclcpp::sleep_for(std::chrono::seconds(1));

    // 如果还有预设的放置位置，则执行放置操作
    if (i < target_pose_list.size()) {
      // 控制机器人移动到预设的放置位置
      bool place_success = node->plan_and_execute(target_pose_list[i]);
      
      // 如果移动成功，则执行放置操作
      if (place_success) {
        // 打开夹爪释放立方体，0为夹爪完全打开位置
        node->grasp(0);
        
        // 等待1秒确保立方体稳定放置
        rclcpp::sleep_for(std::chrono::seconds(1));
      }
    }
	
  }
  
  // 完成所有抓取放置任务后，回到准备位置
  node->go_to_ready_position();
  
  // 关闭ROS 2客户端库
  rclcpp::shutdown();
  
  // 程序正常退出
  return 0;
}

for (size_t i = 0; i < to_frame_list.size(); i++) {
    std::vector<double> cube_pose;
    
    // 通过TF获取立方体相对于base_link的位姿
    node->get_cube_pose(from_frame, to_frame_list[i], cube_pose);
    
    if (cube_pose.empty()) {
      RCLCPP_WARN(rclcpp::get_logger("demo4"), "Failed to get pose for %s, skipping", to_frame_list[i].c_str());
      continue;
    }

    // 调整抓取位置
    cube_pose[0] -= 0.012 ;  // 微调X坐标
    cube_pose[1] += 0.01;    // 微调Y坐标
    cube_pose[2] += 0.14;    // Z轴抬高0.14米，确保从上方垂直抓取
    cube_pose[3] = 0.0;      // roll = 0°
    cube_pose[4] = M_PI;     // pitch = 180°（夹爪向下）
    cube_pose[5] = 0.0;      // yaw = 0°

    RCLCPP_INFO(...);  // 打印日志
    
    cube_pose_list.push_back(cube_pose);
}

[程序启动]
     ↓
rclcpp::init() → 初始化 ROS 2
     ↓
创建 UR5eGripper 节点
     ↓
node->init() → 初始化 MoveIt 和夹爪
     ↓
启动 executor.spin()（后台线程）
     ↓
获取放置点列表 target_pose_list
     ↓
循环遍历 cube1~cube6：
   ├─ 通过 TF 查询 cube_i 相对于 base_link 的位置
   ├─ 调整为抓取位姿（抬高 Z，设置 pitch=π）
   └─ 存入 cube_pose_list
     ↓
再次循环抓取每个立方体：
   ├─ plan_and_execute(抓取位姿)
   ├─ grasp(0.36)  // 抓
   ├─ plan_and_execute(放置位姿)
   └─ grasp(0)     // 放
     ↓
go_to_ready_position()  // 回家
     ↓
rclcpp::shutdown() → 关闭系统