Commit ace36ad6 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

updated to bouncy

parent 7613fecc
......@@ -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()
\ No newline at end of file
rclpy.shutdown()
......@@ -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)
\ No newline at end of file
print(mac)
#!/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()
......@@ -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
\ No newline at end of file
#return xml_str
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment