Adding loop-sel node executor.

src/delcommand.py

@@ -89,6 +89,7 @@ parser.add_option ("", "--paper-scenario1", action="store_true", dest="paper_sce
 parser.add_option ("", "--paper-scenario2", action="store_true", dest="paper_scenario2", help="Paper Scenario 2")
 parser.add_option ("", "--paper-scenario3", action="store_true", dest="paper_scenario3", help="Paper Scenario 3")
 parser.add_option ("", "--frame-rate", action="store", type="int", dest="frame_rate", default=15, help="frame_rate")
+parser.add_option ("", "--gesture-test", action="store_true", dest="gesture_test", help="Test Gesture")
 (options, args) = parser.parse_args()
 
 def wtogp (x, y, z):
@@ -424,6 +425,47 @@ def search_area(ns, unit):
     (res, reason, timing) = mo.delegation_process(tree, options.exectree)
     return (res, reason, timing)
 
+def gesture_test(ns, unit):
+    loop_sel = Node("loop-sel", {"execution-ns": unit, "key": "ALL", "default-command": "forget", "commands": ["take-off", "land", "follow", "forget"]})
+    inair = Node("in-air-goal", {"execunit": unit})
+    noop1 = Node("no-op", {"execunit": unit})    
+    noop2 = Node("no-op", {"execunit": unit})    
+    land = Node("land", {"execunit": unit})
+    try_node = Node("try", {"execunit": unit})    
+    try_node.add_child(land)
+    try_node.add_child(noop1)
+    try_node.add_child(noop2)
+    wait3 = Node("wait", {"execution-ns": unit, "duration": -1})    
+    wait4 = Node("wait", {"execution-ns": unit, "duration": -1})
+
+    joytopic = "/op0/joy"
+    print("leashing:", unit, joytopic)
+    ocuuid = str(uuid.uuid4())
+    conc = Node("conc", {"execution-ns": unit})
+    leashing = Node("leashing", {"execution-ns": unit,
+                                 "joy-topic": joytopic,
+                                 "node-uuid": ocuuid,
+                                 "command-topic": "leashing_command",
+                                 "status-topic": "leashing_status",
+                                 "leash-to-static-position": False,
+                                 "use-image-processing-input": False,
+                                 "position-geopoint": wtogp(5, 0, 0),
+                                 "position-topic": "/gps0/geopoint"})
+    joytr = Node("joy-trigger", {"execution-ns": unit, "enough-node-uuid": ocuuid, "joy-topic": "/op0/joy"})
+    conc.add_child(leashing)
+    conc.add_child(joytr)
+    
+    loop_sel.add_child(inair)
+    loop_sel.add_child(try_node)
+    loop_sel.add_child(conc)
+    loop_sel.add_child(wait4)
+    tree = Node("seq", {"execution-ns": unit})
+    # tree.add_child(abort)
+    tree.add_child(loop_sel)
+    mo = MissionObject("search_areas", ns, options.display, options.verbose)
+    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
+    return (res, reason, timing)
+
 
 def search_areas(ns, unit):
     abort = Node("abort", {"execunit": unit})
@@ -711,5 +753,8 @@ if __name__ == "__main__":
     if options.paper_scenario3:
         paper_scenario3(ns, unit)
         
+    if options.gesture_test:
+        res = gesture_test(ns, unit)
+        
     print("RES:", res)