ros2_master.py 3.32 KB
Newer Older
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
1 2 3
#!/usr/bin/env python3

import xml.etree.ElementTree as ET
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
4 5 6
import xml.dom.minidom as minidom
import urllib.parse
import html
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
7

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
8 9
import rclpy
from rclpy.callback_groups import ReentrantCallbackGroup
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
10

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
11 12
from elphel_interfaces.srv import StrReqStrRes
import ros2_config
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
13

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
14
TIMEOUT = 5
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
15 16 17 18 19 20 21 22

def parse_command(string):

  msg = {}

  e = ET.fromstring(string)

  tmp = e.find('cmd')
23
  if tmp!=None and tmp.text!=None:
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
24 25 26 27 28
    # have to escape: &
    msg['cmd'] = html.escape(tmp.text)
  else:
    msg['cmd'] = 'state'

29
  tmp = e.find('targets')
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
30

31 32 33
  if tmp!=None and tmp.text!=None:
    msg['targets'] = tmp.text.split(",")
  else:
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
34 35 36 37 38
    msg['targets'] = clients

  return msg


Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
39
# callback for broadcast commands
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
40
def cmd_callback(req,res):
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
41

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62
  global g_node

  # put async function here?
  async def pass_cmd():
    global g_node
    nonlocal req
    nonlocal cli, did_run, did_get_result, cmd_result
    did_run = True
    try:
      requ = StrReqStrRes.Request()
      requ.request = req.request
      future = cli.call_async(requ)
      result = await future
      if result is not None:
        g_node.get_logger().info('Got response: '+result.response)
      else:
        g_node.get_logger().info('Service call failed %r' % (future.exception(),))
    finally:
      cmd_result = result
      did_get_result = True

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
63 64 65
  # default function
  cmd_func = pass_cmd

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
66 67
  #print("Raw command:")
  #print(req.request)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
68 69 70

  q = parse_command(req.request)

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
71
  print("Parsed command:")
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
72 73
  print(q)

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
74 75 76 77
  if q['cmd']=='list':
    res.response = '<?xml version="1.0"?><document>'+'\n'.join(['<target>'+c+'</target>' for c in q['targets']])+'</document>'
    return res

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
78 79
  if q['cmd']=='state':
    cmd_func = pass_cmd
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96
  
  cb_group = ReentrantCallbackGroup()
  reslist = []

  # serial operation. TODO: make parallel
  for t in q['targets']:
    # get target by name
    tgt = cfg['slaves'][t]
    cli = g_node.create_client(StrReqStrRes, tgt+"/cmd",callback_group=cb_group)

    did_run = False
    did_get_result = False
    cmdres = None
    cmd_result = None

    # if n/a
    if not cli.wait_for_service(timeout_sec=1.0):
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
97
      cmdres = "<response><node>"+t+"</node><state>dead</state></response>"
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
98 99 100 101 102 103 104 105 106 107 108
    else:
      # timer?!
      timer = g_node.create_timer(0.0, cmd_func, callback_group=cb_group)
      while rclpy.ok() and not did_run:
        rclpy.spin_once(g_node)
      # call timer callback only once
      if did_run:
        timer.cancel()
      # spin until it gets "blue"
      while rclpy.ok() and not did_get_result:
        rclpy.spin_once(g_node)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
109
      cmdres  = "<response><node>"+t+"</node>"+cmd_result.response+"</response>"
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
110 111 112 113
    # store response
    reslist.append(cmdres)

  # convert to string
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
114
  res.response = '<?xml version="1.0"?><document>'+'\n'.join(reslist)+'</document>'
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
115 116 117 118
  return res


# global
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
119
g_node = None
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
120
clients = []
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
121
cfg = ros2_config.get(pre='master_')
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
122

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
123
nodename = cfg['self']
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
124

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
125 126
for k,v in cfg['slaves'].items():
  clients.append(k)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
127 128

rclpy.init()
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
129
g_node = rclpy.create_node(nodename)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
130

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
131
print('\nStarting master services:')
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
132
print('  '+nodename+'/cmd')
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
133
s1 = g_node.create_service(StrReqStrRes, nodename+'/cmd', cmd_callback)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
134

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
135 136
while rclpy.ok():
  rclpy.spin_once(g_node)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
137

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
138 139 140 141
# Destroy the service attached to the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
g_node.destroy_service(srv)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
142 143
rclpy.shutdown()

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170