Commit 0f4d8a53 authored by Mathias Haage's avatar Mathias Haage
Browse files

Update dispatch service with meeting contributions

parent 9baf8062
Pipeline #2208 skipped
......@@ -6,6 +6,117 @@ import rospy
# ----------------------------------------------------------
def identifyobjects(context):
# 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
# return result to state machine that triggers transition
reply = {}
return reply
def grasp():
# id=(folding)/general, description=grasping
# 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)
pass
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
pass
def move():
# AssemblyType/id=(folding)/general, description=moving
# transform: Need the cover transformation - in robot system
# apply above transform to the PCB position in the move keyframe
# only relative positions
# call OMG to move - parameter: target position of pcb in robot coordinate system
pass
def folding_align():
# 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
def folding_contact():
# AssemblyType/id=folding, description=contact
# need to apply transformation of cover to get pcb target in robot coordinate system
# call move action from OMG for contacting cover
# 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
# If there is contact - first call folding controller and then ask HRI to verify if contact is ok
pass
def folding_callfoldingcontroller():
# No keyframe - incorporate automatically in sequence of a folding/contact state
# call foldingcontroller with appropriate parameters as specified by KTH
pass
def folding_calldeformationcontroller():
# For later (after november)
pass
def folding_verifyfold():
# No keyframe
# Call HRI to prompt user to examine the assembly status - specified outcome yes, no
pass
def completed_assembly():
# No keyframe
# Let go of pcb - open gripper
# If not ok - transition back to align
pass
def retract():
# AssemblyType=general, description=retract
# retract from current pose in specified direction
pass
def gotoinitialposition():
# Go to initial position
pass
def finish(context):
pass
# Context
# Python hashtable called "dict" - contains an attribute dom - which is dom object of the keyframe xml, as well as attributes specified in a comma-separated string sent with the state machine
def addtwoints(context):
reply = {}
rospy.wait_for_service('add_two_ints')
......@@ -35,7 +146,7 @@ def appendstring(context):
fmap = []
fmap += [("addtwoints", addtwoints)]
fmap += [("appendstring", appendstring)]
fmap += [("identifyobjects", identifyobjects)]
registry = dict(fmap)
# ----------------------------------------------------------
......
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