Commit 84e06d0e authored by Mathias Haage's avatar Mathias Haage
Browse files

Untested update to handle sfc cycles

parent 7cf86790
......@@ -14,7 +14,7 @@ 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()
......@@ -26,7 +26,7 @@ def identifyobjects(context):
time.sleep(2) # Simulate work
# AssemblyType/id=(folding)/general, description=initial
# call object detector - specified return values (needed for sfc transitions)
# call ros node calculating transformation between cover pose to actual scene, need to transform/update all keyframes -> store in kif for now
......@@ -47,9 +47,9 @@ def grasp(context):
# call grasp planner with arguments for pcb grasp - returns grasp -> store grasp suggestions
# send actual position of pcb to online motion generator (OMG)
# call OMG action to set grasp suggestions -> returns actual grasp
# feedback to HRI?
# call node to execute grasp (OMG)
......@@ -67,7 +67,7 @@ def pickup(context):
# 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
reply = {}
result_name = context['result_name'].strip()
......@@ -123,7 +123,7 @@ def folding_contact(context):
# need feedback from force sensor:
# 1. get contact feedback before reaching target
# 2. reaching target without contact - should move further until contact (with limit) - HRI feedback?
# Mockup - if state terminates without contact call identify object again and locate pcb
# Strategy what to do with object detector information in case of no contact - go back to align with corrected parameters
......@@ -187,7 +187,7 @@ def completed_assembly(context):
reply = {}
result_name = context['result_name'].strip()
result_values = context['result_values'].strip().split(';')
reply[result_name] = result_values[0]
reply[result_name] = result_values[random.randint(0,1)]
print("Result %s"%reply)
return reply
......@@ -196,7 +196,7 @@ def retract(context):
print("Context %s"%(str(context)))
time.sleep(2) # Simulate work
# AssemblyType=general, description=retract
# retract from current pose in specified direction
reply = {}
result_name = context['result_name'].strip()
......@@ -221,7 +221,7 @@ def finish(context):
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(';')
......@@ -270,6 +270,7 @@ fmap += [("folding_pickup", pickup)]
fmap += [("folding_align", folding_align)]
fmap += [("folding_contact", folding_contact)]
fmap += [("folding_assembled", completed_assembly)]
fmap += [("folding_move", move)]
fmap += [("folding_retract", retract)]
registry = dict(fmap)
......
......@@ -55,6 +55,17 @@ if __name__ == '__main__':
print("ERROR %s"%kv_out['error'])
break
if 'result_pre_reset' in kv_in:
result_name = kv_in['result_name']
if result_name in kv_out:
result_pre_reset = kv_in['result_pre_reset']
if kv_out[result_name] == result_pre_reset:
result_reset_actions = [a for a in kv_in["result_pre_reset_%s"%(kv_out[result_name])].strip().split(';')]
for a in result_reset_actions:
print("pre %s"%a)
f.write(a)
f.flush()
for k in kv_out:
jgrafchart_reply = '%s|%s\n'%(k, kv_out[k].strip())
print("S %s"%jgrafchart_reply.strip())
......
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