import rclpy
from rclpy.node import Node

from interfaces.srv import VisionObjectRecognition


class Client(Node):
    def __init__(self, name):
        super().__init__(name)
        self.client_ = self.create_client(VisionObjectRecognition, "/vision_object_recognition",)
        self.timer_ = self.create_timer(2.0, self._timer_callback)
    
    def _timer_callback(self):
        self.send_request("left")

    def send_request(self, camera_position):
        while not self.client_.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('service not available, waiting again...')
        request = VisionObjectRecognition.Request()
        request.camera_position = camera_position
        self.client_.call_async(request).add_done_callback(self._result_callback)
    
    def _result_callback(self, result):
        response = result.result()
        if response.success:
            pose_list = []
            for obj in response.objects:
                pose_list.append(
                    {
                        "class_id": obj.class_id,
                        "class_name": obj.class_name,
                        "pose": obj.pose
                    }
                )
            self.get_logger().info(f"{pose_list}")
        else:
            self.get_logger().error(f"{response.info}")


def main(args=None):
    rclpy.init(args=args)
    node = Client('get_camera_service_client')
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

        




