Update dispatch service with meeting contributions

ros/catkin_ws/src/grafchart/scripts/dispatchservice.py

@@ -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)
 
 # ----------------------------------------------------------