import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient

from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, PositionConstraint
from geometry_msgs.msg import PoseStamped
from shape_msgs.msg import SolidPrimitive

class MoveItActionClient(Node):
    def __init__(self):
        super().__init__('moveit_action_client')
        self.client = ActionClient(self, MoveGroup, '/move_action')

    def send_pose_goal(self, pose):
        goal = MoveGroup.Goal()
        req = goal.request

        req.group_name = "right_arm"      # ⚠️ 改成你的 planning group
        req.num_planning_attempts = 5
        req.allowed_planning_time = 5.0
        req.max_velocity_scaling_factor = 0.2
        req.max_acceleration_scaling_factor = 0.2

        pc = PositionConstraint()
        pc.header = pose.header
        pc.link_name = "openarm_right_link7"      # ⚠️ 改成你的 TCP

        box = SolidPrimitive()
        box.type = SolidPrimitive.BOX
        box.dimensions = [0.01, 0.01, 0.01]

        pc.constraint_region.primitives.append(box)
        pc.constraint_region.primitive_poses.append(pose.pose)
        pc.weight = 1.0

        constraints = Constraints()
        constraints.position_constraints.append(pc)
        req.goal_constraints.append(constraints)

        self.client.wait_for_server()
        send_future = self.client.send_goal_async(
            goal,
            feedback_callback=self.feedback_cb
        )
        send_future.add_done_callback(self.goal_response_cb)

    def goal_response_cb(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().error("❌ Goal 被 MoveIt 拒绝")
            return

        self.get_logger().info("✅ Goal 已接受，等待执行结果")
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.result_cb)

    def result_cb(self, future):
        result = future.result().result
        self.get_logger().info(f"🎯 执行完成，error_code = {result.error_code.val}")
        rclpy.shutdown()

    def feedback_cb(self, feedback):
        pass

def main():
    rclpy.init()
    node = MoveItActionClient()

    pose = PoseStamped()
    pose.header.frame_id = "openarm_body_link0"
    pose.pose.position.x = 0.01
    pose.pose.position.y = -0.153
    pose.pose.position.z = 0.462
    pose.pose.orientation.w = 1.0

    node.send_pose_goal(pose)
    rclpy.spin(node)

if __name__ == "__main__":
    main()
