#!/usr/bin/python3.6

import rclpy
from rclpy.node import Node
import sys
from random import choice
from string import ascii_letters
from subprocess import check_output, CalledProcessError
from time import sleep
from importlib import import_module

rclpy.init(args=None)
node = Node('call_service_' + ''.join(choice(ascii_letters) for _ in range(8)))

server = node.declare_parameter('simple_launch.server', '').value
verbosity = node.declare_parameter('simple_launch.verbosity', '').value

req_v = 'req' in verbosity
res_v = 'res' in verbosity

if not server:
    sys.exit(0)

if server[0] != '/' and len(node.get_namespace()) > 1:
    server = f'{node.get_namespace()}/{server}'
if server[0] != '/':
    server = f'/{server}'

# wait and identify which service type is here
srv = ''
if req_v:
    node.get_logger().info(f'Checking service {server}')

while srv == '':
    sleep(1)
    try:
        srv = check_output(f'ros2 service type {server}'.split()).decode().strip()
    except CalledProcessError:
        pass

if req_v:
    node.get_logger().info(f'Calling service {server}')

pkg,_,srv = srv.split('/')
pkg = import_module(pkg + '.srv')
srv = getattr(pkg, srv)
req = srv.Request()

# declare request fields as node params
for field in req.get_fields_and_field_types():
    default_val = getattr(req,field)
    try:
        param = node.declare_parameter(field, default_val)
    except TypeError:   # some numpy array
        param = node.declare_parameter(field, default_val.tolist())

    # handle namespace
    if param.value == '__ns':
        setattr(req, field, node.get_namespace())
    else:
        setattr(req, field, param.value)

# call and exit
client = node.create_client(srv, server)
client.wait_for_service()
res = client.call_async(req)

while not res.done():
    rclpy.spin_once(node)

if res_v:
    res = dict((name, getattr(res.result(), name)) for name in res.result().get_fields_and_field_types())
    node.get_logger().info(f'{server} response is {res}')
    print(f'{server} response is {res}')
elif req_v:
    node.get_logger().info(f'Call to {server} done')
    print(f'Call to {server} done')

node.destroy_node()
rclpy.shutdown()
