位姿估计输出 (PoseStamped)
    ↓
MoveIt! PlanningScene (碰撞检测)
    ↓
MoveGroupInterface (路径规划)
    ↓
JointTrajectoryController (执行)

# Ubuntu 22.04 + ROS 2 Humble
sudo apt update
sudo apt install ros-humble-moveit
sudo apt install ros-humble-moveit-setup-assistant
sudo apt install ros-humble-moveit-servo
sudo apt install ros-humble-moveit-plugins

# 启动MoveIt配置助手
ros2 run moveit_setup_assistant moveit_setup_assistant

# 步骤：
# 1. Select → Browse → 加载你的 ur5_with_camera.urdf.xacro
# 2. Self-Collisions → Generate Collision Matrix (采样10000次)
# 3. Virtual Joints → 已存在"virtual_joint"连接world和base_link，跳过
# 4. Planning Groups → Add Group:
#    - Group Name: manipulator
#    - Kinematic Solver: kdl_kinematics_plugin/KDLKinematicsPlugin
#    - Add Chain: base_link → tool0
# 5. Robot Poses → Add Pose "home": [0, -π/2, 0, -π/2, 0, 0]
# 6. End Effectors → 跳过（UR5无夹爪）
# 7. Passive Joints → 跳过
# 8. Configuration Files → Save → 选择 ~/vision_ws/src/ur5_moveit_config

# 克隆官方示例配置（需根据你的URDF修改）
cd ~/vision_ws/src
git clone -b humble https://github.com/ros-planning/moveit2_tutorials
cp -r moveit2_tutorials/doc/examples/ur5_moveit_config .
# 手动修改config/ur5.srdf 中的模型路径

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Pose
from moveit.planning import MoveGroupInterface
from moveit_msgs.msg import CollisionObject, PlanningScene, Constraints, OrientationConstraint
from shape_msgs.msg import SolidPrimitive
import time

class GraspController(Node):
    def __init__(self):
        super().__init__('grasp_controller')
        
        # 参数
        self.declare_parameter('planning_group', 'manipulator')
        self.declare_parameter('pre_grasp_height', 0.15)  # 预抓取高度（米）
        self.declare_parameter('grasp_height', 0.05)      # 抓取高度
        self.declare_parameter('approach_min_dist', 0.05) # 接近最小距离
        self.declare_parameter('approach_desired_dist', 0.1) # 接近期望距离
        
        # MoveIt接口
        group_name = self.get_parameter('planning_group').value
        self.move_group = MoveGroupInterface(
            group_name,
            'base_link',
            panda_joint_states_topic='/joint_states',
            panda_trajectory_execution_event_topic='/execute_trajectory/cancel',
            panda_planning_scene_service='/get_planning_scene',
            panda_trajectory_execution_action_name='/execute_trajectory',
            panda_get_planning_scene_service='/get_planning_scene',
            panda_planning_pipelines=['ompl'],
        )
        
        # PlanningScene接口
        self.planning_scene_interface = self.move_group.get_planning_scene_interface()
        
        # 发布规划场景
        self.scene_pub = self.create_publisher(PlanningScene, '/planning_scene', 10)
        
        self.get_logger().info(f'GraspController已启动 - 规划组: {group_name}')
        
        # 等待MoveIt服务就绪
        self.move_group.wait_for_planning_scene()
        self.add_collision_objects()
    
    def add_collision_objects(self):
        """添加碰撞体：桌面和地面"""
        self.get_logger().info('添加碰撞场景...')
        
        # 桌面
        table = CollisionObject()
        table.id = "table"
        table.header.frame_id = "base_link"
        table.operation = CollisionObject.ADD
        
        table_pose = Pose()
        table_pose.position.x = 0.5
        table_pose.position.y = 0.0
        table_pose.position.z = -0.05  # 桌面在基座下方5cm
        
        table.primitive_poses = [table_pose]
        table.primitives = [SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[1.2, 1.2, 0.1])]
        
        self.planning_scene_interface.apply_collision_object(table)
        
        # 地面
        ground = CollisionObject()
        ground.id = "ground"
        ground.header.frame_id = "base_link"
        ground.operation = CollisionObject.ADD
        
        ground_pose = Pose()
        ground_pose.position.z = -0.1
        
        ground.primitive_poses = [ground_pose]
        ground.primitives = [SolidPrimitive(type=SolidPrimitive.BOX, dimensions=[3.0, 3.0, 0.01])]
        
        self.planning_scene_interface.apply_collision_object(ground)
        
        time.sleep(1.0)  # 等待场景更新
    
    def plan_to_pose(self, target_pose, constraints=None):
        """通用规划函数"""
        self.move_group.set_pose_target(target_pose)
        
        if constraints:
            self.move_group.set_path_constraints(constraints)
        
        # 规划参数
        self.move_group.set_planning_time(5.0)
        self.move_group.set_num_planning_attempts(3)
        self.move_group.set_goal_tolerance(0.01)  # 1cm误差容忍
        
        # 执行规划和执行
        self.get_logger().info('规划中...')
        plan_result = self.move_group.plan()
        
        if plan_result.planning_result.error_code.val == 1:  # SUCCESS
            self.get_logger().info('规划成功，执行中...')
            self.move_group.execute(plan_result.trajectory, wait=True)
            return True
        else:
            self.get_logger().error(f'规划失败: {plan_result.planning_result.error_code.val}')
            return False
    
    def grasp_object(self, object_pose: PoseStamped):
        """主抓取流程"""
        self.get_logger().info('=== 开始抓取流程 ===')
        
        # 1. 移动到预抓取位置（物体上方）
        pre_grasp_pose = PoseStamped()
        pre_grasp_pose.header.frame_id = "base_link"
        pre_grasp_pose.pose.position = object_pose.pose.position
        pre_grasp_pose.pose.position.z += self.get_parameter('pre_grasp_height').value
        
        # 保持垂直向下的朝向（简化）
        pre_grasp_pose.pose.orientation.x = 0.0
        pre_grasp_pose.pose.orientation.y = 0.707  # 90度旋转
        pre_grasp_pose.pose.orientation.z = 0.0
        pre_grasp_pose.pose.orientation.w = 0.707
        
        self.get_logger().info('步骤1: 移动到预抓取点')
        if not self.plan_to_pose(pre_grasp_pose):
            return False
        
        # 2. 设置方向约束（保持末端垂直）
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header.frame_id = "base_link"
        orientation_constraint.link_name = "tool0"
        orientation_constraint.orientation = pre_grasp_pose.pose.orientation
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14  # 允许绕Z轴旋转
        orientation_constraint.weight = 1.0
        
        constraints = Constraints()
        constraints.orientation_constraints = [orientation_constraint]
        
        # 3. 接近物体
        grasp_pose = PoseStamped()
        grasp_pose.header.frame_id = "base_link"
        grasp_pose.pose = object_pose.pose
        
        self.get_logger().info('步骤2: 接近抓取点')
        if not self.plan_to_pose(grasp_pose, constraints):
            return False
        
        # 4. 模拟闭合夹爪（UR5无夹爪，用等待代替）
        self.get_logger().info('步骤3: 闭合夹爪（模拟）')
        time.sleep(2.0)
        
        # 5. 提升物体
        lift_pose = PoseStamped()
        lift_pose.header.frame_id = "base_link"
        lift_pose.pose = object_pose.pose
        lift_pose.pose.position.z += 0.2  # 提升20cm
        
        self.get_logger().info('步骤4: 提升物体')
        if not self.plan_to_pose(lift_pose, constraints):
            return False
        
        self.get_logger().info('=== 抓取流程完成 ===')
        return True
    
    def return_home(self):
        """返回初始位置"""
        home_pose = PoseStamped()
        home_pose.header.frame_id = "base_link"
        
        # 使用已命名的"home"位姿
        self.move_group.set_named_target("home")
        plan_result = self.move_group.plan()
        
        if plan_result.planning_result.error_code.val == 1:
            self.move_group.execute(plan_result.trajectory, wait=True)


def main(args=None):
    rclpy.init(args=args)
    controller = GraspController()
    
    # 测试用：等待2秒后自动执行
    time.sleep(2.0)
    controller.grasp_object(PoseStamped())  # 需传入真实位姿
    
    rclpy.shutdown()


if __name__ == '__main__':
    main()

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
import time

class GraspDemo(Node):
    def __init__(self):
        super().__init__('grasp_demo')
        
        # 订阅3D位姿
        self.object_pose = None
        self.pose_sub = self.create_subscription(
            PoseStamped, '/detection/object_pose_3d', self.pose_callback, 10
        )
        
        # 创建GraspController客户端（简化：直接实例化）
        from grasp_controller import GraspController
        self.controller = GraspController()
        
        self.get_logger().info('GraspDemo主循环已启动 - 等待物体位姿...')
    
    def pose_callback(self, msg):
        self.object_pose = msg
        self.get_logger().info(f'接收到物体位姿: {msg.pose.position}')
    
    def run(self):
        """主循环：识别 → 规划 → 抓取"""
        while rclpy.ok() and self.object_pose is None:
            rclpy.spin_once(self, timeout_sec=0.1)
            self.get_logger().info('等待物体检测...')
            time.sleep(1.0)
        
        if self.object_pose:
            success = self.controller.grasp_object(self.object_pose)
            if success:
                self.get_logger().info('🎉 抓取成功！')
                # 移动到放置位置（示例）
                place_pose = PoseStamped()
                place_pose.header.frame_id = "base_link"
                place_pose.pose.position.x = 0.3
                place_pose.pose.position.y = -0.3
                place_pose.pose.position.z = 0.05
                self.controller.grasp_object(place_pose)
            else:
                self.get_logger().error('❌ 抓取失败')
        
        self.controller.return_home()


def main(args=None):
    rclpy.init(args=args)
    demo = GraspDemo()
    
    try:
        demo.run()
    except KeyboardInterrupt:
        demo.get_logger().info('演示已停止')
    finally:
        demo.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

grasp_controller:
  ros__parameters:
    planning_group: "manipulator"
    pre_grasp_height: 0.15  # 预抓取高度15cm
    grasp_height: 0.05      # 最终抓取高度5cm
    approach_min_dist: 0.05
    approach_desired_dist: 0.1

# 在 launch_setup 中
grasp_controller_node = Node(
    package='vision_grasp_demo',
    executable='grasp_controller.py',
    name='grasp_controller',
    output='screen',
    parameters=[PathJoinSubstitution([
        FindPackageShare('vision_grasp_demo'), 'config', 'grasp_params.yaml'
    ])],
    remappings=[('/joint_states', '/joint_states')]  # 确保话题正确
)

# 在 nodes_to_start 中追加
grasp_controller_node,

cd ~/vision_ws
colcon build --packages-select vision_grasp_demo --symlink-install
source install/setup.bash

# 一键启动完整系统
ros2 launch vision_grasp_demo ur5_camera_bringup.launch.py

# 系统应自动：
# 1. 启动Gazebo + UR5 + 摄像头 + 红色方块
# 2. 识别物体并计算3D位姿
# 3. 规划抓取路径
# 4. 执行抓取并提升物体

# 查看MoveIt规划日志
ros2 topic echo /move_action/result

# 可视化规划场景
ros2 run moveit_ros_planning_interface moveit_visual_tools