import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node

from pymoveit2 import MoveIt2, MoveIt2State
from pymoveit2.robots import openarm as robot
from sensor_msgs.msg import JointState

class MoveIt2ControlNode(Node):
    def __init__(self):
        super().__init__('moveit2_control_node')
        self.get_logger().info('正在初始化MoveIt2控制节点...')
        self.subscriber_ = self.create_subscription(
            JointState, 
            "/joint_sensor_states", 
            self.listener_callback, 
            10
        )
        # Create callback group that allows execution of callbacks in parallel without restrictions
        callback_group = ReentrantCallbackGroup()

        # Create MoveIt 2 interface
        self.moveit2 = MoveIt2(
            node=self,
            joint_names=robot.joint_names(),
            base_link_name=robot.base_link_name(),
            end_effector_name=robot.end_effector_name(),
            group_name=robot.MOVE_GROUP_ARM,
            callback_group=callback_group,
        )
        self.moveit2.planner_id = (
            "RRTConnectkConfigDefault"
        )
        # Scale down velocity and acceleration of joints (percentage of maximum)
        self.moveit2.max_velocity = 0.5
        self.moveit2.max_acceleration = 0.5

    def listener_callback(self, msg):
        # self.send_goal([count, count1, 0.0, 0.0, 0.0, 0.0, 0.0])
        self.get_logger().info(f'Received joint states: positions: {list(msg.position)}')
        self.moveit2.move_to_configuration(msg.position)
        # self.get_logger().info("Waiting until execution is complete...")
        self.moveit2.wait_until_executed()

def main():
    rclpy.init()
    node = MoveIt2ControlNode()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()