Commit 7d45bcf2 authored by Mathias Haage's avatar Mathias Haage
Browse files

Add bridge to dispatchservice, update dispatchservice, update dot2jgrafchart

parent 24050044
......@@ -2,12 +2,29 @@
from grafchart.srv import *
import rospy
import libxml2
import time
import random
# ----------------------------------------------------------
def identifyobjects(context):
print("Calling identifyobjects")
print("Context %s"%(str(context)))
#sample use of libxml2 xpath to read xml parameter
doc = context['xml_doc']
xmlcontext = doc.xpathNewContext()
xmlcontext.xpathRegisterNs('sarafun', 'http://www.SARAFunXML.com')
object_interaction = str(xmlcontext.xpathEval('//sarafun:ObjectInteraction/@id')[0].children)
print("ObjectInteraction %s"%object_interaction)
time.sleep(2) # Simulate work
# AssemblyType/id=(folding)/general, description=initial
# call object detector - specified return values (needed for sfc transitions)
......@@ -15,9 +32,16 @@ def identifyobjects(context):
# return result to state machine that triggers transition
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def grasp():
def grasp(context):
print("Calling grasp")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# id=(folding)/general, description=grasping
# call grasp planner with arguments for pcb grasp - returns grasp -> store grasp suggestions
......@@ -29,17 +53,33 @@ def grasp():
# feedback to HRI?
# call node to execute grasp (OMG)
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[random.randint(0,1)] # Pick result yes or no randomly
print("Result %s"%reply)
return reply
def pickup(context):
print("Calling pick")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType/id=(folding)/general, description=picking
# create target - grasping keyframe has position of pcb, search for next keyframe in sequence that is pick keyframe. Need position of PCB in picking key frame. Will estimate transformation based on pose of PCB and grasping state in the actual scene and in the keyframe. Will apply this transformation to PCB transformation in the picking keyframe. call ros service - input PCB transform in ... -> transformation -> store in kif for now
# call OMG to do pick of PCB with context.KeyFrame as parameter -> specified return values
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[random.randint(0,1)] # Pick result yes or no randomly
print("Result %s"%reply)
return reply
def move():
def move(context):
print("Calling move")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType/id=(folding)/general, description=moving
# transform: Need the cover transformation - in robot system
......@@ -47,17 +87,33 @@ def move():
# only relative positions
# call OMG to move - parameter: target position of pcb in robot coordinate system
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def folding_align():
def folding_align(context):
print("Calling folding_align")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType/id=folding, description=align
# need to apply transformation of cover to get pcb target in robot coordinate system
# call move operation for align target using OMG service
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def folding_contact():
def folding_contact(context):
print("Calling folding_contact")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType/id=folding, description=contact
# need to apply transformation of cover to get pcb target in robot coordinate system
......@@ -72,44 +128,106 @@ def folding_contact():
# Strategy what to do with object detector information in case of no contact - go back to align with corrected parameters
# If there is contact - first call folding controller and then ask HRI to verify if contact is ok
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def folding_callfoldingcontroller():
def folding_callfoldingcontroller(context):
print("Calling folding_callfoldingcontroller")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# No keyframe - incorporate automatically in sequence of a folding/contact state
# call foldingcontroller with appropriate parameters as specified by KTH
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def folding_calldeformationcontroller():
def folding_calldeformationcontroller(context):
print("Calling folding_calldeformationcontroller")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# For later (after november)
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def folding_verifyfold():
def folding_verifyfold(context):
print("Calling folding_verifyfold")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# No keyframe
# Call HRI to prompt user to examine the assembly status - specified outcome yes, no
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def completed_assembly():
def completed_assembly(context):
print("Calling completed_assembly")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# No keyframe
# Let go of pcb - open gripper
# If not ok - transition back to align
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def retract():
def retract(context):
print("Calling retract")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType=general, description=retract
# retract from current pose in specified direction
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def gotoinitialposition():
def gotoinitialposition(context):
print("Calling gotoinitialposition")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# Go to initial position
pass
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
def finish(context):
pass
print("Calling finish")
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
print("Result %s"%reply)
return reply
# Context
......@@ -119,8 +237,8 @@ def finish(context):
def addtwoints(context):
reply = {}
rospy.wait_for_service('add_two_ints')
try:
rospy.wait_for_service('add_two_ints', timeout=5)
add_two_ints = rospy.ServiceProxy('add_two_ints', TestService2)
resp1 = add_two_ints(int(context['x']), int(context['y']))
reply['sum'] = resp1.sum
......@@ -131,8 +249,8 @@ def addtwoints(context):
def appendstring(context):
reply = {}
rospy.wait_for_service('string2string')
try:
rospy.wait_for_service('string2string', timeout=5)
string2string = rospy.ServiceProxy('string2string', TestService1)
resp1 = string2string(context['a'])
reply['b'] = resp1.b
......@@ -146,7 +264,14 @@ def appendstring(context):
fmap = []
fmap += [("addtwoints", addtwoints)]
fmap += [("appendstring", appendstring)]
fmap += [("identifyobjects", identifyobjects)]
fmap += [("folding_initial", identifyobjects)]
fmap += [("folding_grasp", grasp)]
fmap += [("folding_pickup", pickup)]
fmap += [("folding_align", folding_align)]
fmap += [("folding_contact", folding_contact)]
fmap += [("folding_assembled", completed_assembly)]
fmap += [("folding_retract", retract)]
registry = dict(fmap)
# ----------------------------------------------------------
......@@ -155,7 +280,7 @@ registry = dict(fmap)
# Comma-separated string with key=value pairs.
# I.e. a=12,b=3,c=hello
def string2dict(s):
values = dict(kv.split("=") for kv in s.split(","))
values = dict(kv.split("=") for kv in s.strip().split(","))
return values
def dict2string(d):
......@@ -173,9 +298,12 @@ def dict2string(d):
def handle_dispatch(req):
print "Dispatching [%s]"%(req.request,)
try:
context = string2dict(req.request)
context = string2dict(req.request.strip().split('|')[1])
if 'xml_file' in context:
context['xml_doc'] = libxml2.parseFile(context['xml_file'])
#print("context %s"%str(context))
reply = dict2string(registry[context["call"]](context))
print "Reply [%s]"%(reply,)
#print "Reply [%s]"%(reply,)
except Exception as e:
reply = "error=%s"%(str(e),)
print "Error [%s]"%(reply,)
......
#!/usr/bin/env python
import select
import socket
from grafchart.srv import *
import rospy
import time
def dispatch(line):
reply = {}
try:
rospy.wait_for_service('dispatch', timeout=5)
mydispatch = rospy.ServiceProxy('dispatch', DispatchService)
resp = mydispatch(line)
reply = resp.response
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return "error=%s"%e
return reply
if __name__ == '__main__':
print("Starting jgrafchart bridge")
serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
serversocket.bind(('localhost', 4242))
serversocket.listen(1)
while True:
try:
print("Waiting for connection...")
connection, address = serversocket.accept()
print("Connected")
f = connection.makefile()
line = f.readline()
while True:
print "R "+line.strip()
cmdline=line.strip().split('|')[1]
kv_in = dict([kv.strip().split("=") for kv in cmdline.strip().split(',')])
reply = dispatch(line)
kv_out = dict([kv.strip().split("=") for kv in reply.strip().split(',')])
if 'error' in kv_out:
print("ERROR %s"%kv_out['error'])
break
for k in kv_out:
jgrafchart_reply = '%s|%s\n'%(k, kv_out[k].strip())
print("S %s"%jgrafchart_reply.strip())
f.write(jgrafchart_reply)
f.flush()
reset = False
if 'result_reset' in kv_in:
result_name = kv_in['result_name']
if result_name in kv_out:
if kv_out[kv_in['result_name']] == kv_in['result_reset']:
time.sleep(2) # Possible race condition
jgrafchart_reply = '%s|%s\n'%(kv_in['result_name'].strip(), "")
print("S %s"%jgrafchart_reply.strip())
f.write(jgrafchart_reply)
f.flush()
reset = True
line = f.readline()
counter = 0
if reset:
try:
while True:
#print("READLINE %d %s"%(counter,line))
counter = counter+1
connection.settimeout(1.0)
line = f.readline()
except Exception as e:
print("EXCEPTION %s"%str(e))
connection.settimeout(None)
except Exception as e:
print("ERROR "+str(e))
try:
connection.close()
except Exception as e:
print(str(e))
......@@ -38,7 +38,7 @@ if __name__ == "__main__":
print('<?xml version="1.0" encoding="UTF-8" standalone="yes"?>')
print('<GCDocument color="-1" dimTicks="25" dpwsInterface="" dpwsPort="-1" height="1609" horizontalScrollBar="1" modifiable="1" name="appname" saveVersion="8" scale="1.0" simulationMode="1" socketHost="localhost" socketIsServer="0" socketPort="4242" socketSendMode="Changed" threadSpeed="40" tokenLuminance="0" verticalScrollBar="1" viewPositionX="0" viewPositionY="0" width="1223" x="0" y="0">')
print('<GCDocument color="-1" dimTicks="25" dpwsInterface="" dpwsPort="-1" height="1609" horizontalScrollBar="1" modifiable="1" name="appname" saveVersion="8" scale="1.0" simulationMode="1" socketHost="localhost" socketIsServer="0" socketPort="4242" socketSendMode="Assigned" threadSpeed="40" tokenLuminance="0" verticalScrollBar="1" viewPositionX="0" viewPositionY="0" width="1223" x="0" y="0">')
uuid2width = {}
......
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