ros2_master.py 3.23 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 23 24 25 26 27 28 29 30 31 32 33 34 35 36

def parse_command(string):

  msg = {}

  e = ET.fromstring(string)

  tmp = e.find('cmd')
  if tmp!=None:
    # have to escape: &
    msg['cmd'] = html.escape(tmp.text)
  else:
    msg['cmd'] = 'state'

  tmp = e.findall('target')

  if not tmp:
    msg['targets'] = clients

  return msg


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

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
  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
61 62 63
  # default function
  cmd_func = pass_cmd

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

  q = parse_command(req.request)

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
69
  print("Parsed command:")
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
70 71
  print(q)

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
72 73 74 75
  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
76 77
  if q['cmd']=='state':
    cmd_func = pass_cmd
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
  
  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
95
      cmdres = "<response><node>"+t+"</node><state>dead</state></response>"
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
96 97 98 99 100 101 102 103 104 105 106
    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
107
      cmdres  = "<response><node>"+t+"</node>"+cmd_result.response+"</response>"
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
108 109 110 111
    # store response
    reslist.append(cmdres)

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


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

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
121
nodename = cfg['self']
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
122

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

rclpy.init()
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
127
g_node = rclpy.create_node(nodename)
Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
128

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

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

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
136 137 138 139
# 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
140 141
rclpy.shutdown()

Oleg Dzhimiev's avatar
Oleg Dzhimiev committed
142 143 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