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

from rbs_skill_interfaces.action import MoveitSendPose


class TestMoveToPose(Node):

    def __init__(self, robot_name: str):
        super().__init__('test_move_to_pose')
        self._action_client = ActionClient(self, MoveitSendPose, "/"+ robot_name + "/move_topose")

    def send_goal(self, msg):
        goal_msg = MoveitSendPose.Goal()
        # goal_msg.target_pose
        goal_msg.target_pose.position.x = msg["target_pose"]["position"]["x"]
        goal_msg.target_pose.position.y = msg["target_pose"]["position"]["y"]
        goal_msg.target_pose.position.z = msg["target_pose"]["position"]["z"]

        goal_msg.target_pose.orientation.x = msg["target_pose"]["orientation"]["x"]
        goal_msg.target_pose.orientation.y = msg["target_pose"]["orientation"]["y"]
        goal_msg.target_pose.orientation.z = msg["target_pose"]["orientation"]["z"]
        goal_msg.target_pose.orientation.w = msg["target_pose"]["orientation"]["w"]

        goal_msg.robot_name = msg["robot_name"]
        goal_msg.end_effector_velocity = msg["end_effector_velocity"]
        goal_msg.end_effector_acceleration = msg["end_effector_acceleration"]

        self._action_client.wait_for_server()

        return self._action_client.send_goal_async(goal_msg)


def main(args=None):
    rclpy.init(args=args)

    robot_name = "arm0"

    action_client = TestMoveToPose(robot_name)

    msg = {
        "constraint_mode": 0,
        "target_pose": {
            "position": {
                "x": 0.2,
                "y": 0.2,
                "z": 0.35,
            },
            "orientation": {
                "x": 0.0,
                "y": 0.0,
                "z": 0.0,
                "w": 1.0,
            },
        },
        "robot_name": robot_name,
        "end_effector_velocity": 1.0,
        "end_effector_acceleration": 1.0,
        "timeout_seconds": 0.0
    }

    future = action_client.send_goal(msg)

    rclpy.spin_until_future_complete(action_client, future)


if __name__ == '__main__':
    main()