#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from cyberdog_marker.srv import PlaceMarker

class MarkerClient(Node):
    def __init__(self):
        super().__init__('marker_client')
        self.client = self.create_client(PlaceMarker, 'place_marker')
        while not self.client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('服务不可用，等待...')
        self.req = PlaceMarker.Request()

    def send_request(self, x, y, z, color):
        self.req.x = x
        self.req.y = y
        self.req.z = z
        self.req.color = color
        self.future = self.client.call_async(self.req)
        rclpy.spin_until_future_complete(self, self.future)
        return self.future.result()
    

class MarkerRunner:
    def __init__(self):
        self.marker_client = None

    def run(self):
        rclpy.init()
        self.marker_client = MarkerClient()
    
    def destroy(self):
        self.marker_client.destroy_node()
        rclpy.shutdown()
    
    def send_request(self, x, y, z, color, observe=False):
        response = self.marker_client.send_request(x, y, z, color)
        if observe:
            self.marker_client.get_logger().info(
                f'结果: {response.success}, 消息: {response.message}')
        return response
    

def main(args=None):
    rclpy.init(args=args)
    client = MarkerClient()
    response = client.send_request(1.0, 1.0, 0.0, 'blue')
    client.get_logger().info(
        f'结果: {response.success}, 消息: {response.message}')
    client.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()