testing

src/init/ros2_client.py

@@ -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

@@ -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

@@ -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

@@ -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