updated to bouncy

src/init/ros2_client.py

@@ -28,7 +28,7 @@ import html
 rclpy.init()
 node = rclpy.create_node('some_client_node')
 
-cli = node.create_client(StrReqStrRes, 'n000e64101698_master/cmd')
+cli = node.create_client(StrReqStrRes, 'master_1467cd/cmd')
 
 wcnt = 0
 while not cli.wait_for_service(timeout_sec=1.0):
@@ -45,10 +45,9 @@ req.request = '''<document>
 </document>
 '''
 
-cli.call(req)
-cli.wait_for_future()
-
-res = cli.response.response
+future = cli.call_async(req)
+rclpy.spin_until_future_complete(node, future)
+res = future.result().response
 
 node.get_logger().info("cmd response:")
 node.get_logger().info("\n"+res)
@@ -63,14 +62,13 @@ req.request = '''<document>
 </document>
 '''
 
-cli.call(req)
-cli.wait_for_future()
-
-res = cli.response.response
+future = cli.call_async(req)
+rclpy.spin_until_future_complete(node, future)
+res = future.result().response
 
 node.get_logger().info("cmd response:")
 node.get_logger().info("\n"+res)
 ###############################################################################
 
 node.destroy_node()
-rclpy.shutdown()
+rclpy.shutdown()
\ No newline at end of file

src/init/ros2_config.py

@@ -4,15 +4,13 @@ import os
 import re
 import xml.etree.ElementTree as ET
 
-NODEPREFIX = 'n'
-
 # get function
-def get(f="ros2_config.xml"):
+def get(f="ros2_config.xml",pre='n'):
 
   cfg = {}
 
-  mac = get_mac(last_three_octets=False,capitalize=False)
-  cfg['self'] = NODEPREFIX+mac
+  mac = get_mac(last_three_octets=True,capitalize=False)
+  cfg['self'] = pre+mac
   cfg['role'] = None
 
   if os.path.isfile(f):
@@ -75,4 +73,4 @@ if __name__ == "__main__":
 
   # Test 2
   mac = get_mac(last_three_octets=False,keep_colons=True,capitalize=False)
-  print(mac)
+  print(mac)
\ No newline at end of file

src/init/ros2_master.py

 #!/usr/bin/env python3
 
-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 def pass_cmd(target,cmd):
-
-  cli = node.create_client(StrReqStrRes,target+'/cmd')
-
-  # 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 = '<request><cmd>'+cmd+'</cmd></request>'
-
-  cli.call(req)
-  cli.wait_for_future()
-
-  cmdres = cli.response.response
-
-  return cmdres
+import rclpy
+from rclpy.callback_groups import ReentrantCallbackGroup
 
+from elphel_interfaces.srv import StrReqStrRes
+import ros2_config
 
+TIMEOUT = 5
 
 def parse_command(string):
 
@@ -56,57 +37,128 @@ def parse_command(string):
 # callback for broadcast commands
 def cmd_callback(req,res):
 
+  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
+
   # default function
   cmd_func = pass_cmd
 
-  print("The raw command is:")
-  print(req.request)
+  #print("Raw command:")
+  #print(req.request)
 
   q = parse_command(req.request)
 
-  print("parsed command:")
+  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)
-
+  
+  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):
+      cmdres = "<response><node>"+tgt+"</node><state>dead</state></response>"
+    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)
+      cmdres = cmd_result.response
+    # store response
+    reslist.append(cmdres)
+
+  # convert to string
+  res.response = '\n'.join(reslist)
   return res
 
 
 # global
+g_node = None
 clients = []
-cfg = ros2_config.get()
+cfg = ros2_config.get(pre='master_')
 
-nodename = cfg['self']+'_master'
+nodename = cfg['self']
 
 for k,v in cfg['slaves'].items():
   clients.append(k)
 
 rclpy.init()
-node = rclpy.create_node(nodename)
-
-event_loop = asyncio.get_event_loop()
+g_node = rclpy.create_node(nodename)
 
 print('\nStarting master services:')
 print('  '+nodename+'/cmd')
-s1 = node.create_service(StrReqStrRes, nodename+'/cmd', cmd_callback)
+s1 = g_node.create_service(StrReqStrRes, nodename+'/cmd', cmd_callback)
 
-rclpy.spin(node)
+while rclpy.ok():
+  rclpy.spin_once(g_node)
 
-# Destroy the node explicitly
-# (optional - Done automatically when node is garbage collected)
-node.destroy_node()
+# 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)
 rclpy.shutdown()
 
-event_loop.close()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/init/ros2_slave.py

@@ -42,10 +42,12 @@ def cmd_callback(req,res):
 
     res.response = '<response><node>'+nodename+'</node><state>'+output.decode()+'</state></response>'
 
+  print('Slave sending: '+res.response)
+
   return res
 
 # global
-cfg = ros2_config.get()
+cfg = ros2_config.get(pre='_slave_')
 
 nodename = cfg['self']
 
@@ -84,4 +86,4 @@ rclpy.shutdown()
   #xml_str = ET.tostring(xml).decode()
   #xml_str = minidom.parseString(xml_str).toprettyxml(indent="  ")
 
-  #return xml_str
+  #return xml_str
\ No newline at end of file