import rclpy
from rclpy.node import Node
# from control_msgs.msg import JointTrajectoryControllerState
from control_msgs.action import FollowJointTrajectory
from rclpy.action import ActionClient
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from geometry_msgs.msg import Twist


# class ControlPub(Node):
#     def __init__(self, node_name):
#         super().__init__(node_name)
#         self.status_publisher_ = self.create_publisher(
#             JointTrajectoryControllerState, '/right_joint_trajectory_controller/controller_state', 100)
#         self.timer = self.create_timer(0.02, self.timer_callback)

#     def timer_callback(self):
#         arm_pos = [0.15, 0.15, 0.15, 0.15, 0.15, 0.15, 0.15]
#         arm_zero = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
#         msg = JointTrajectoryControllerState()
#         msg.reference.positions = arm_zero
#         self.get_logger().info(f'发布:{msg}')
#         self.status_publisher_.publish(msg)

# def main():
#     rclpy.init()
#     node = ControlPub('control_pub')
#     rclpy.spin(node)
#     rclpy.shutdown()
count = 0
count1 = 0
class ProgressActionClient(Node):
    
    
    def __init__(self):
        super().__init__('progress_action_client')

        self.subscriber_ = self.create_subscription(
            Twist, 
            "/cmd_vel", 
            self.listener_callback, 
            100
        )
        # 3-1.创建动作客户端；
        self._action_client = ActionClient(self, FollowJointTrajectory, '/right_joint_trajectory_controller/follow_joint_trajectory')
 
        # 3. 创建订阅者
        # 参数1：消息类型（如String，与发布者一致）
        # 参数2：话题名称（如"chatter"，与发布者一致）
        # 参数3：回调函数（消息到达时触发，用于处理消息）
        # 参数4：队列长度（10，缓存消息的最大数量）

        self.subscriber_  # 防止未使用变量警告
 
    # 4. 消息处理回调函数（核心）
    # 当订阅到消息时，自动调用此函数处理消息
    def listener_callback(self, msg):
        global count
        global count1
        count += msg.linear.x
        count1 += msg.linear.z
        # 打印接收的消息（可根据需求修改处理逻辑，如存储、解析、控制硬件等）
        self.get_logger().info(f"接收到消息: {msg.linear.x} {msg.linear.z} angle:{count}")
        self.send_goal([count, count1, 0.0, 0.0, 0.0, 0.0, 0.0])

    def send_goal(self, arm_pos):
        arm_joint = ['openarm_right_joint1', 'openarm_right_joint2', 'openarm_right_joint3', 'openarm_right_joint4', 'openarm_right_joint5', 'openarm_right_joint6', 'openarm_right_joint7']
        arm_effort = [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0]
        arm_zero = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        # 3-2.发送请求；
        goal_msg = FollowJointTrajectory.Goal()
        goal_msg.trajectory.joint_names = arm_joint
        goal_msg.trajectory.points.append(JointTrajectoryPoint())
        goal_msg.trajectory.points[0].positions = arm_pos
        # goal_msg.trajectory.points[0].effort = arm_effort
        print(goal_msg)

        self._action_client.wait_for_server()
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
        self._send_goal_future.add_done_callback(self.goal_response_callback)
 
    def goal_response_callback(self, future):
        # 3-3.处理目标发送后的反馈；
        goal_handle = future.result()
        print(goal_handle)
        if not goal_handle.accepted:
            self.get_logger().info('请求被拒绝')
            return
 
        self.get_logger().info('请求被接收，开始执行任务！')
 
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self.get_result_callback)
 
    # 3-5.处理最终响应。
    def get_result_callback(self, future):
        return
        # result = future.result().result
        # self.get_logger().info('最终计算结果：sum = %d' % result.sum)
        # 5.释放资源。
        # rclpy.shutdown()
 
    # 3-4.处理连续反馈；
    def feedback_callback(self, feedback_msg):
        feedback = (int)(feedback_msg.feedback.progress * 100)
        self.get_logger().info('当前进度: %d%%' % feedback)

def main():
    rclpy.init()
    node = ProgressActionClient()
    # node.send_goal([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
    rclpy.spin(node)
    rclpy.shutdown()
