Commit 7613fecc authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

testing

parent c198abeb

src/init/ros2_client.py

+47 −2
Original line number Diff line number Diff line
@@ -6,6 +6,25 @@ import rclpy 
from elphel_interfaces.srv import StrReqStrRes

import ros2_config



import sys

import html



# etree

#a = '<document><cmd>&</cmd></document>'



#xml = ET.Element("document")

#ET.SubElement(xml, "cmd").text = "begin & end"



#a = ET.tostring(xml)

#print(a)



#e = ET.fromstring(a)

#print(e)



#print(html.escape(e.find("cmd").text))



#sys.exit()



rclpy.init()

node = rclpy.create_node('some_client_node')
@@ -16,8 +35,33 @@ while not cli.wait_for_service(timeout_sec=1.0): 
  print('Service is not available, waiting '+str(wcnt)+'...')

  wcnt += 1



###############################################################################



print("TEST1")



req = StrReqStrRes.Request()

req.request = '''<document>

  <cmd>state</cmd>

</document>

'''



cli.call(req)

cli.wait_for_future()



res = cli.response.response



node.get_logger().info("cmd response:")

node.get_logger().info("\n"+res)



###############################################################################



print("TEST2")



req = StrReqStrRes.Request()

req.request = "wget -qO- -o /dev/null 'http://localhost/parsedit.php?immediate&TRIG'"

req.request = '''<document>

  <cmd>wget -qO- -o /dev/null 'http://localhost/parsedit.php?immediate&amp;TRIG'</cmd>

</document>

'''



cli.call(req)

cli.wait_for_future()
@@ -25,7 +69,8 @@ cli.wait_for_future() 
res = cli.response.response



node.get_logger().info("cmd response:")

node.get_logger().info(res)

node.get_logger().info("\n"+res)

###############################################################################



node.destroy_node()

rclpy.shutdown()
 
 No newline at end of file

src/init/ros2_init.py

+1 −3
Original line number Diff line number Diff line
@@ -5,8 +5,6 @@ import ros2_config 


cfg = ros2_config.get()



print(cfg)



if cfg['role']=='master':

  subprocess.Popen(['python3','ros2_master.py'])

src/init/ros2_master.py

+52 −16
Original line number Diff line number Diff line
@@ -2,44 +2,81 @@ 


import asyncio

import xml.etree.ElementTree as ET

import xml.dom.minidom as minidom

import rclpy

from elphel_interfaces.srv import StrReqStrRes

import ros2_config

import urllib.parse

import html



TIMEOUT = 5



# async coroutine for cmdall_callback

async def pass_cmd(target,cmd):



  #node = rclpy.create_node(target+'_client')

  cli = node.create_client(StrReqStrRes,target+'/cmd')



  slave = cfg['slaves'][target]

  cli = node.create_client(StrReqStrRes,slave+'/cmd')



  wcnt = 0

  while not cli.wait_for_service(timeout_sec=1.0):

    print('Service is not available, waiting '+str(wcnt)+'...')

    wcnt += 1

    if wcnt==TIMEOUT:

      break

  # if n/a

  if not cli.wait_for_service(timeout_sec=1.0):

    return "<response><node>"+target+"</node><state>dead</state></response>"



  # if alive

  req = StrReqStrRes.Request()

  req.request = cmd

  req.request = '<request><cmd>'+cmd+'</cmd></request>'



  cli.call(req)

  cli.wait_for_future()



  cmdres = cli.response.response



  return cmdres







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





# callback for broadcast commands

def cmd_callback(req,res):



  cmd = req.request

  tasks = [pass_cmd(c,cmd) for c in clients]

  # default function

  cmd_func = pass_cmd



  print("The raw command is:")

  print(req.request)



  q = parse_command(req.request)



  print("parsed command:")

  print(q)



  if q['cmd']=='state':

    cmd_func = pass_cmd



  #if q['cmd']=='status':

  #  cmd_func = cmd_get_status



  tasks = [cmd_func(cfg['slaves'][c],q['cmd']) for c in q['targets']]

  results = event_loop.run_until_complete(asyncio.gather(*tasks))



  print("Printing results:")

  print(results)



  res.response = '\n'.join(results)
@@ -54,7 +91,6 @@ cfg = ros2_config.get() 
nodename = cfg['self']+'_master'



for k,v in cfg['slaves'].items():

  print(k+' '+v)

  clients.append(k)



rclpy.init()
@@ -62,7 +98,7 @@ node = rclpy.create_node(nodename) 


event_loop = asyncio.get_event_loop()



print('Starting master services:')

print('\nStarting master services:')

print('  '+nodename+'/cmd')

s1 = node.create_service(StrReqStrRes, nodename+'/cmd', cmd_callback)

src/init/ros2_slave.py

+54 −9
Original line number Diff line number Diff line
@@ -9,16 +9,38 @@ import ros2_config 


TIMEOUT = 5



def parse_command(string):



  msg = {}



  e = ET.fromstring(string)



  tmp = e.find('cmd');

  if tmp!=None:

    msg['cmd'] = tmp.text

  else:

    msg['cmd'] = 'state'



  return msg





# slave callbacks

def cmd_callback(req,res):

  cmd = req.request



  q = parse_command(req.request)

  print("parsed command by slave:")

  print(q)



  if q['cmd']=='state':

    res.response = '<response><node>'+nodename+'</node><state>idle</state></response>'

  else:

    # call to localhost here, get response and send it back

    try:

    output = subprocess.check_output(cmd,shell=True)

      output = subprocess.check_output(q['cmd'],shell=True)

    except subprocess.CalledProcessError:

    output = "<error>-1</error>"

      output = "<error>CalledProcessError</error>".encode()



  res.response = '<cmd><node>'+nodename+'</node><status>'+str(output)+'</status></cmd>'

    res.response = '<response><node>'+nodename+'</node><state>'+output.decode()+'</state></response>'



  return res
@@ -30,7 +52,7 @@ nodename = cfg['self'] 
rclpy.init()

node = rclpy.create_node(nodename)



print('Starting slave services:')

print('\nStarting slave services:')

print('  '+nodename+'/cmd')

s = node.create_service(StrReqStrRes, nodename+'/cmd', cmd_callback)
@@ -40,3 +62,26 @@ rclpy.spin(node) 
# (optional - Done automatically when node is garbage collected)

node.destroy_node()

rclpy.shutdown()





  #xml = ET.Element("response")



  ##ET.SubElement(doc, "field1", name="blah").text = "some value1"

  ##ET.SubElement(doc, "field2", name="asdfasd").text = "some vlaue2"



  #for k,v in cfg['slaves'].items():

    #xml_node = ET.SubElement(xml, "node")

    #ET.SubElement(xml_node, "name").text = k

    #ET.SubElement(xml_node, "id").text = v



    #if v in nodelist:

      #s = "online"

    #else:

      #s = "offline"



    #ET.SubElement(xml_node, "status").text = s



  #xml_str = ET.tostring(xml).decode()

  #xml_str = minidom.parseString(xml_str).toprettyxml(indent="  ")



  #return xml_str
 
 No newline at end of file