delcommand.py

#!/usr/bin/python3

from __future__ import print_function

import rospy
import sys
import time
import re
import os
import roslib.packages

from delegation.mission import *
from delegation.missionobject import *
from pyutil.geoconv import *

from optparse import OptionParser

parser = OptionParser()
parser.add_option ("", "--heading", action="store", type="float", dest="heading", default=45.0, help="heading for test25 and test1")
parser.add_option ("-x", "--xpos", action="store", type="float", dest="x", default=0, help="X positin")
parser.add_option ("-y", "--ypos", action="store", type="float", dest="y", default=0, help="Y position")
parser.add_option ("-z", "--zpos", action="store", type="float", dest="z", default=2.0, help="Z position")
parser.add_option ("", "--xpos1", action="store", type="float", dest="x1", default=20, help="X1 positin")
parser.add_option ("", "--ypos1", action="store", type="float", dest="y1", default=20, help="Y1 position")
parser.add_option ("", "--zpos1", action="store", type="float", dest="z1", default=2.0, help="Z1 position")
parser.add_option ("", "--pan", action="store", type="float", dest="pan", default=0, help="Pan")
parser.add_option ("", "--tilt", action="store", type="float", dest="tilt", default=-45.0, help="Tilt")
parser.add_option ("", "--speed", action="store", type="float", dest="speed", default=0.5, help="Speed (default 0.5)")
parser.add_option ("", "--latlon", action="store_true", dest="latlon", help="Coordinates to look-at are lat lon")
parser.add_option ("", "--tellop", action="store", type="string", dest="tellop", help="Tell Operator")
parser.add_option ("", "--coords", action="store", type="string", dest="coords", help="Coordinates speraded by ;")
parser.add_option ("", "--unit", action="store", type="string", dest="unit", help="unit to do things with", default="/lq1")
parser.add_option ("", "--noyaw", action="store_true", dest="noyaw", help="Do not yaw")
parser.add_option ("", "--motalascan", action="store_true", dest="motalascan", help="Scan in Motala")
parser.add_option ("", "--drive", action="store_true", dest="drive", help="DriveTo")
parser.add_option ("", "--grasp", action="store_true", dest="grasp", help="Grasp")
parser.add_option ("", "--release", action="store_true", dest="release", help="Release")
parser.add_option ("", "--nav", action="store_true", dest="nav", help="Navto")
parser.add_option ("", "--path", action="store_true", dest="path", help="FlyPath")
parser.add_option ("", "--fly", action="store_true", dest="fly", help="Flyto")
parser.add_option ("", "--flyback", action="store_true", dest="flyback", help="Flyto point and then back to origin")
parser.add_option ("", "--yaw", action="store_true", dest="yaw", help="Yaw")
parser.add_option ("", "--climb", action="store_true", dest="climb", help="Climb")
parser.add_option ("", "--takeoff", action="store_true", dest="takeoff", help="Take off")
parser.add_option ("", "--throwtakeoff", action="store_true", dest="throwtakeoff", help="Throw Take off")
parser.add_option ("", "--land", action="store_true", dest="land", help="Land")

parser.add_option ("", "--pickup", action="store_true", dest="pickup", help="Pick Up Payload")
parser.add_option ("", "--drop", action="store_true", dest="drop", help="Drop Payload")
parser.add_option ("", "--load", action="store_true", dest="load", help="Load Payload")

parser.add_option ("", "--lookat", action="store_true", dest="lookat", help="LookAt")
parser.add_option ("", "--stop-lookat", action="store_true", dest="stop_lookat", help="Stop LookAt")
parser.add_option ("", "--testlookat", action="store_true", dest="testlookat", help="Test LookAt")
parser.add_option ("", "--op", action="store", type="string", dest="op", default="/op0", help="Op")
parser.add_option ("", "--pantilt", action="store_true", dest="pantilt", help="PanTilt")
parser.add_option ("", "--mplan", action="store_true", dest="mplan", help="MotionPlan from (X,Y,Z) to (Z1,Y1,Z1)")

parser.add_option ("", "--json", action="store_true", dest="json", help="Print json of the tree used to create the TST")
parser.add_option ("", "--execjson", action="store_true", dest="execjson", help="Print json of the execution TST")
parser.add_option ("", "--deljson", action="store_true", dest="deljson", help="Print json of the delegation TST")
parser.add_option ("", "--json_validate", action="store_true", dest="json_validate", help="Validate (NYI)")
parser.add_option ("-v", "--verbose", action="store_true", dest="verbose", help="Verbose output")
parser.add_option ("", "--disp", action="store_true", dest="display", help="Display TST")
parser.add_option ("", "--exec", action="store_true", dest="exectree", help="Start execution of the tree")
parser.add_option ("", "--testgc", action="store_true", dest="testgc", help="Test gc")
parser.add_option ("", "--abort", action="store_true", dest="abort", help="Abort all abortable terminals in the tree starting in node given by id option (NYI)")
parser.add_option ("", "--pause", action="store_true", dest="pause", help="Pause all nodes (NYI)")
parser.add_option ("", "--continue", action="store_true", dest="continue_flag", help="Continue all nodes (NYI)")
parser.add_option ("", "--enough", action="store_true", dest="enough", help="Enough all nodes (NYI)")
parser.add_option ("", "--abortunit", action="store_true", dest="abortunit", help="Abort unit")
parser.add_option ("", "--pauseunit", action="store_true", dest="pauseunit", help="Pause unit")
parser.add_option ("", "--continueunit", action="store_true", dest="continueunit", help="Continue unit")
parser.add_option ("", "--enoughunit", action="store_true", dest="enoughunit", help="Enough unit")
parser.add_option ("", "--executingtrees", action="store_true", dest="executingtrees", help="Get all executing trees")
parser.add_option ("", "--dispexecutingtrees", action="store_true", dest="dispexecutingtrees", help="Disdplay all executing trees")
parser.add_option ("", "--goal", action="store_true", dest="goal", help="Test case for TFPOP goal")
parser.add_option ("", "--igoal", action="store_true", dest="igoal", help="Test case for TFPOP interleave goal, using visit-squares")
parser.add_option ("", "--igoaldeliver", action="store_true", dest="igoaldeliver", help="Test case for TFPOP interleave goal, using delivery")
parser.add_option ("", "--surfacesearch", action="store_true", dest="surfacesearch", help="Test case for surface search")
parser.add_option ("", "--pathtest", action="store_true", dest="pathtest", help="Test buggy case granso")
parser.add_option ("", "--pathtest2", action="store_true", dest="pathtest2", help="Debug path flying")
parser.add_option ("", "--pathtest3", action="store_true", dest="pathtest3", help="Debug path flying")
parser.add_option ("", "--pathtest4", action="store_true", dest="pathtest4", help="Debug path flying")
parser.add_option ("", "--search-area", action="store_true", dest="search_area", help="Test search area")
parser.add_option ("", "--search-areas", action="store_true", dest="search_areas", help="Test search areas")
parser.add_option ("", "--n-areas", action="store", type="int", dest="n_areas", default=3, help="Specify number of areas to search")
parser.add_option ("", "--paper-scenario1", action="store_true", dest="paper_scenario1", help="Paper Scenario 1")
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):
    return gc.point_to_geo_point (get_point_stamped (x, y, z, "world"))

def takeoff(ns, unit):
    inair = Node("in-air-goal", {"execunit": unit})    
#    inair = Node("in-air-goal", {"execunit": unit, "height-above-takeoff": 30.0})
#    tree = Node("seq", {"execunit": unit}, [inair])
    tree = Node("seq", {"execunit": ns}, [inair])
    mo = MissionObject("takeoff", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def throw_takeoff(ns, unit):
    takeoff = Node("throw-take-off", {"execunit": unit})    
#    inair = Node("in-air-goal", {"execunit": unit})    
#    inair = Node("in-air-goal", {"execunit": unit, "height-above-takeoff": 30.0})
#    tree = Node("seq", {"execunit": unit}, [inair])
    tree = Node("seq", {"execunit": ns}, [takeoff])
    mo = MissionObject("takeoff", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def land(ns, unit):
    land = Node("land", {"execunit": unit})
    tree = Node("seq", {"execunit": unit}, [land])
    mo = MissionObject("land", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def pickup(ns, unit):
    pickup = Node("pick-up", {"execunit": unit, "do-not-ask-for-help": False, "type": "gun"})
    tree = Node("seq", {"execunit": unit}, [pickup])
    mo = MissionObject("pickup", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def drop(ns, unit):
    drop = Node("drop", {"execunit": unit, "type": "gun"})
    tree = Node("seq", {"execunit": unit}, [drop])
    mo = MissionObject("drop", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def load(ns, unit):
    load = Node("load", {"execunit": unit, "type": "gun", "unit": "/dji0",
                         "attachment_point": 0,
                         "auto_approve_delegation_flag": True,
                         "auto_succeed_flag": True})
    tree = Node("seq", {"execunit": unit}, [load])
    mo = MissionObject("load", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def climb (ns, unit, z):
    p = wtogp (0, 0, z)
    print(p)
    climb = Node("climb")
    climb.set_params({"execution-ns": unit, "z": p.altitude })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(climb)
    mo = MissionObject("climb", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def yaw(ns, unit, heading, rate):
    yaw = Node("yaw")
    yaw.set_params({"execution-ns": unit, "heading": heading, "yaw-rate": rate })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(yaw)
    mo = MissionObject("yaw", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def lookat(ns, unit, x, y, z):
    p = wtogp (x, y, z)
    print(p)
    la = Node("look-at")
    la.set_params({"execution-ns": unit, "p": p })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(la)
    mo = MissionObject("look-at", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def stop_lookat(ns, unit):
    la = Node("stop-look-at")
    la.set_params({"execution-ns": unit})
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(la)
    mo = MissionObject("stop-look-at", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def flyto (ns, unit, x, y, z, speed):
    p = wtogp (x, y, z)
    print(p)
    print("speed:", speed)
    fly = Node("fly-to")
    fly.set_params({"execution-ns": unit, "p": p, "commanded-speed": speed, "do-not-yaw-flag": bool(options.noyaw) })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(fly)
    mo = MissionObject("flyto", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def flypath (ns, unit, points, speed):
    wps = [wtogp(x[0], x[1], x[2]) for x in points]
    print(wps)
    print("speed:", speed)
    fly = Node("fly-path")
    fly.set_params({"execution-ns": unit, "waypoints": wps, "commanded-speed": speed, "do-not-yaw-flag": bool(options.noyaw) })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(fly)
    mo = MissionObject("flyto", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def navto (ns, unit, x, y, z, speed):
    p = wtogp (x, y, z)
    print(p)
    print("speed:", speed)
    nav = Node("navigate")
    nav.set_params({"execution-ns": unit, "waypoints": [p], "commanded-speed": [speed]})
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(nav)
    mo = MissionObject("navto", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def flyto_back (ns, unit, x, y, z, speed):
    p0 = wtogp (0, 0, z)
    p = wtogp (x, y, z)
    print(p)
    print("speed:", speed)
    fly = Node("fly-to")
    fly.set_params({"execution-ns": unit, "p": p, "commanded-speed": speed, "do-not-yaw-flag": bool(options.noyaw) })
    fly0 = Node("fly-to")
    fly0.set_params({"execution-ns": unit, "p": p0, "commanded-speed": speed, "do-not-yaw-flag": bool(options.noyaw) })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(fly)
    tree.add_child(fly0)
    mo = MissionObject("flyto", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def goal(ns, unit):
    goal = Node("goal")
    goal.set_params({"execution-ns": unit, "units": ["/dji0", "/dji1", "/dji2", "/depot0"] })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(goal)
    mo = MissionObject("goal", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def igoal(ns, unit):
    pd = roslib.packages.get_pkg_dir('lrs_tfpop')    
    goal = Node("goal")
    goal.set_params({"interleave-flag": True,
                     "units": ["/dji0", "/dji1", "/dji2", "/depot0"],
                     "domain-file": pd + "/problems/visit-squares.tfpop",
                     "problem-file": pd + "/problems/visit-squares/visit-squares-2.tfpop"})
    tree = Node("seq")
    tree.add_child(goal)
    mo = MissionObject("tfpop visitsquares", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def igoal_delivery(ns, unit):
    pd = roslib.packages.get_pkg_dir('lrs_tfpop')
    goal = Node("goal")
    goal.set_params({"interleave-flag": True,
                     "units": ["/dji0", "/dji1", "/dji2", "/depot0"],                     
                     "domain-file": pd + "/problems/delivery.tfpop",
                     "problem-file": pd + "/problems/delivery/delivery-small.tfpop"})
    tree = Node("seq")
    tree.add_child(goal)
    mo = MissionObject("tfpop delivery", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def surface_search(ns, unit):
    p0 = wtogp(-100, -200, 0)
    p1 = wtogp(100, -200, 0)
    p2 = wtogp(100, 200, 0)
    p3 = wtogp(-100, 200, 0)
    ss = Node("surface-search")
    ss.set_params({"execution-ns": unit, "area": [p0, p1, p2, p3] })
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(ss)
    mo = MissionObject("surface search", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def pathtest2(ns, unit):
    path = [
#        wtogp(128.582542, 28.811256, 28.410561),
#        wtogp(135.825722, 9.353999, 22.200001),
#        wtogp(135.825722, 9.353999, 22.200001)
        wtogp(50, -50, 20),
        wtogp(50, -50, 30),
        wtogp(51, -50, 30),
        wtogp(51, -50, 31)
    ]
#CURRENT POS: 107.951488 43.589480 28.182882
#dji-sim_1         | [ERROR] [1621977327.478779416]: TARGET  POS: 128.582542 28.811256 28.410561
#dji-sim_1         | [ERROR] [1621977327.479897595]:       SPEED: 7.000000
#dji-sim_1         | [ERROR] [1621977327.484889228]: TIME FOR SEGMENT: 3.625556
#dji-sim_1         | [ERROR] [1621977327.495336854]: CURRENT POS: 128.582542 28.811256 28.410561
#dji-sim_1         | [ERROR] [1621977327.495422176]: TARGET  POS: 135.825722 9.353999 22.200001
#dji-sim_1         | [ERROR] [1621977327.495747449]:       SPEED: 7.000000
#dji-sim_1         | [ERROR] [1621977327.505345803]: TIME FOR SEGMENT: 3.095815
#dji-sim_1         | [ERROR] [1621977327.508608279]: CURRENT POS: 135.825722 9.353999 22.200001
#dji-sim_1         | [ERROR] [1621977327.508682148]: TARGET  POS: 135.825722 9.353999 22.200001
#dji-sim_1         | [ERROR] [1621977327.510412626]:       SPEED: 7.000000
#dji-sim_1         | [ INFO] [1621977399.468966466]: DjiSim::set_target_from_trajectory: 16 - 45
#dji-sim_1         | [ INFO] [1621977399.469069244]: LEN - SPEED - time: 25.586102 - 7.057153 - 3.625556
#dji-sim_1         | [ INFO] [1621977403.248373487]: TARGET ACHIEVED: 128.582542 28.811256 28.410561
#dji-sim_1         | [ INFO] [1621977403.248472699]: DjiSim::set_target_from_trajectory: 17 - 45
#exec-dji_1        | [ERROR] [1621977502.538116367]: wait_for_wp_mission_finished: NOT IN FMODE
#exec-dji_1        | [ERROR] [1621977502.540130982]: Executor::abort_fail: Leaving fmode - 70
#del-tst-unit_1    | [ERROR] [1621977502.789489849]: tstfactory set_aborted: 70
#del-tst-unit_1    | [ERROR] [1621977503.115784522]: Executor::fail: sequence: Child 2 did not succeed
#exec-dji_1        | [ERROR] [1621977504.076793837]: Executor::fail: do_seq_work() failed

    movepath = Node("move-path", {"execunit": unit, "speed": "standard", "waypoints": path})
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(movepath)
    mo = MissionObject("pathtest2", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def get_path_test_node(ns, unit):
    path = [
        GeoPoint(57.76070401747153, 16.6807756930952, 44.7),
        GeoPoint(57.76039245100845, 16.681594964143564, 44.7),
        GeoPoint(57.76042035259227, 16.682161481357863, 44.7),
        GeoPoint(57.76098768012404, 16.683730298259007, 44.7),
        GeoPoint(57.76061101283842, 16.685046361326098, 44.7),
        GeoPoint(57.7609272276143, 16.685708750684377, 44.7),
        GeoPoint(57.7613922443168, 16.685290399510716, 44.7),
        GeoPoint(57.761331792483865, 16.68454956930741, 44.7),
        GeoPoint(57.761787503808854, 16.6847413135953, 44.7),
        GeoPoint(57.7613922443168, 16.682553685583162, 44.7),
        GeoPoint(57.76079702186526, 16.681072025176512, 44.7),
        GeoPoint(57.760104133380885, 16.6818215710293, 44.7),
        GeoPoint(57.76089002601959, 16.683521122672197, 44.7)
    ]
    movepath = Node("move-path", {"execunit": unit, "speed": "standard", "waypoints": path})
    return movepath

def pathtest(ns, unit):
    abort = Node("abort", {"execunit": unit})
    look_at_pos = Node("look-at-pos", {"execunit": unit, "p": GeoPoint(57.76115240652829, 16.684114415321048, 44.7)})
    conc = Node("conc", {"execution-ns": unit})
    # conc.add_child(look_at_pos)
    movepath = get_path_test_node(ns, unit)
    conc.add_child(movepath)
    tree = Node("seq", {"execution-ns": unit})
    #    tree.add_child(abort)
    tree.add_child(conc)
    mo = MissionObject("pathtest", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def pathtest3(ns):
    abort = Node("abort", {"execunit": unit})    
    conc = Node("conc", {"execution-ns": unit})
    # conc.add_child(look_at_pos)
    movepath1 = get_path_test_node(ns, "/dji21")
    movepath2 = get_path_test_node(ns, "/dji22")
    conc.add_child(movepath1)
    conc.add_child(movepath2)
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(abort)    
    tree.add_child(conc)
    mo = MissionObject("pathtest", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def pathtest4(ns):
    abort = Node("abort", {"execunit": unit})    
    conc = Node("conc", {"execution-ns": unit})
    # conc.add_child(look_at_pos)
    movepath1 = get_path_test_node(ns, "/dji21")
    movepath2 = get_path_test_node(ns, "/dji22")
    movepath3 = get_path_test_node(ns, "/dji0")
    movepath4 = get_path_test_node(ns, "/dji2")
    conc.add_child(movepath1)
    conc.add_child(movepath2)
    conc.add_child(movepath3)
    conc.add_child(movepath4)
    tree = Node("seq", {"execution-ns": unit})
    # tree.add_child(abort)    
    tree.add_child(conc)
    mo = MissionObject("pathtest", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def get_area(x0, y0, r):
    res = [
        wtogp(x0-r, y0-r, 0),
        wtogp(x0+r, y0-r, 0),
        wtogp(x0+r, y0+r, 0),
        wtogp(x0-r, y0+r, 0),
    ]
    return res

def get_hang_path():
    alt = 44.7
    gps = [
        GeoPoint(57.75966144938757, 16.68103467533825, alt),
        GeoPoint(57.759489691043505, 16.68019771793411, alt),
        GeoPoint(57.758384692843755, 16.68003676458714, alt),
        GeoPoint(57.75612894701629, 16.680413100382026, alt),
        GeoPoint(57.756107577478524, 16.681514382122586, alt),
        GeoPoint(57.756257163977445, 16.682034988036325, alt),
        GeoPoint(57.757282883292056, 16.683356526124967, alt),
        GeoPoint(57.75844746680692, 16.683136269776885, alt),
        GeoPoint(57.75967611515345, 16.682155127862572, alt),
        GeoPoint(57.75982568688258, 16.681514382122586, alt)
    ]
    return gps

def search_area(ns, unit):
    alt = 29.8
    gps = get_hang_path()
    sa = Node("search-area", {"execunit": unit, "area": gps, "target-type": "human", "target-size": 4.0, "area-type": "field"})
    tree = Node("seq", {"execution-ns": unit})
    tree.add_child(sa)
    mo = MissionObject("search_area", ns, options.display, options.verbose)
    (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 me", "commands": ["take off", "land", "follow me", "forget me"]})
    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": "/human0/gps/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})
    a1 = get_area(-150, 0, 50)
    a2 = get_area(0, 0, 50)
    a3 = get_area(150, 0, 50)
    areas = [a1, a2, a3]
    while(len(areas) < options.n_areas):
        areas.append(a1)
        areas.append(a2)
        areas.append(a3)
    sa = Node("search-areas", {"execunit": unit, "areas": areas, "target-type": "human", "target-size": 4.0, "area-type": "field"})
    tree = Node("seq", {"execution-ns": unit})
    # tree.add_child(abort)
    tree.add_child(sa)
    mo = MissionObject("search_areas", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)



def motalascan(ns, unit):

    execns = unit

#    area = get_scan_area (0, -20, 23, 24)
    area = get_scan_area (ns, 20, 0, 60, 30)
#    area = get_scan_area (-40, -40, 60, 30) # barn

    ooi = { "type": "POINTCLOUD-Map",
            "min-density": 10*10 }

    region = {"points": area,
              "dimensions": "2d"
    }

    sensors = {}
    sensors["altitude_adjustable_scan_laser"] = {"altitude": 15.0,
                                                 "fov_in_deg": 45.0,
                                                 "overlap": 0.2}

    sensors["hor_moving_scan_laser"] = {"speed": 2.5, "fly_speed": "slow"}
#    sensors["hor_moving_scan_laser"] = {"speed": 1.0, "fly_speed": "standard"}

    sensors["scan_point_cloud"] = {"resolution": 0.5}

    result_uuid = str(uuid.uuid4())

#    redis_set("latest-task", result_uuid)
#    redis_set("task-name:" + result_uuid, "test62")
    
    m = {"type": "scan",
         "region": region,
         "ooi": ooi,
         "sensor_params": sensors,
         "single_scan_flag": 1,
         "uuid": result_uuid
    }

    mission = NodeEncoder(sort_keys=True, indent=4, separators=(',', ': ')).encode(m)
    print("MISSION:", mission)
    
    request = Node("request", 
                   {"execution-ns": execns,
                    "mission-spec-json": mission,
                    "expand-on-same-unit": True})
    tree = Node("seq", {"execution-ns": ns})
    tree.add_child(request)
    mo = MissionObject("test62", ns, options.display, options.verbose)
    return mo.delegation_process(tree, options.exectree)    

def paper_scenario1(ns, unit):
    conc = Node("conc", {"execution-ns": ns})
    det =  Node("image-object-detection", {"execution-ns": "/gpu0"})

    det_uuid = str(uuid.uuid4())
    add_task_uuid = str(uuid.uuid4())    
    body_det_uuid = str(uuid.uuid4())    
    
    objtypes = ["person", "car", "all"]    
    det.set_params({"execution-ns": "/gpu0",
                    "node-uuid": det_uuid,
                    "image-input-url": "rtsp://178.174.249.11:5000/dji0",
                    "image-input-ros-topic": "",
                    "score": 0.4,
                    "annotation": "standard",
                    "detections-sensor-topic": "sensor/detections",
                    "wanted-detection-rate": options.frame_rate,
                    "object-types": objtypes})
    conc.add_child(det)

    body_detected = Node("body-detection", {"execution-ns": unit, "node-uuid": body_det_uuid})
    conc.add_child(body_detected)

    seq = Node("seq", {"execution-ns": unit})
    speed = 7.0
    searcharea = Node("search-area")
    searcharea.set_params({"execution-ns": unit, "area": get_area(0, 0, 200), "speed": "standard" })
    seq.add_child(searcharea)

    send_signal_1 = Node("send-signal", {"execution-ns": unit, "uuid": det_uuid, "signal": "$enough"})
    send_signal_2 = Node("send-signal", {"execution-ns": unit, "uuid": body_det_uuid, "signal": "$enough"})
    send_signal_3 = Node("send-signal", {"execution-ns": unit, "uuid": add_task_uuid, "signal": "$enough"})
    seq.add_child(send_signal_1)
    seq.add_child(send_signal_2)
    seq.add_child(send_signal_3)

    conc.add_child(seq)
    
    add_task = Node("add-task-to-team-queue", {"execution-ns": unit, "node-uuid": add_task_uuid})
    conc.add_child(add_task)
    
    tree = Node("seq", {"execution-ns": options.op}, [conc])
    mo = MissionObject("search_area", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)


def paper_scenario2(ns, unit):
    gp = wtogp (20, 50, 0)
    observerpos = Node("observe-position", {"execution-ns": unit, "geopoint": gp})
    
    tree = Node("seq", {"execution-ns": options.op}, [observerpos])
    mo = MissionObject("observe_position", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)

def paper_scenario3(ns, unit):
    conc = Node("conc", {"execution-ns": ns})

    det_uuid = str(uuid.uuid4())
    loop_uuid = str(uuid.uuid4())
    body_uuid = str(uuid.uuid4())
    
    body_detected = Node("body-detection", {"execution-ns": unit, "node-uuid": body_uuid})    

    det =  Node("image-object-detection", {"execution-ns": "/gpu0"})
    objtypes = ["person", "car", "all"]    
    det.set_params({"execution-ns": "/gpu0",
                    "node-uuid": det_uuid,
                    "image-input-url": "rtsp://178.174.249.11:5000/dji0",
                    "image-input-ros-topic": "",
                    "score": 0.4,
                    "annotation": "standard",
                    "detections-sensor-topic": "sensor/detections",
                    "wanted-detection-rate": options.frame_rate,
                    "object-types": objtypes})
    conc.add_child(det)
#    conc.add_child(body_detected)

    seq = Node("seq", {"execution-ns": unit})
    speed = 7.0
    searcharea = Node("search-area", {"execution-ns": unit})
    searcharea.set_params({"execution-ns": ns, "area": get_area(0, 0, 200), "speed": "standard" })
    seq.add_child(searcharea)

    send_signal_1 = Node("send-signal", {"execution-ns": unit, "uuid": det_uuid, "signal": "$enough"})
    send_signal_2 = Node("send-signal", {"execution-ns": unit, "uuid": body_uuid, "signal": "$enough"})
#    send_signal_3 = Node("send-signal", {"execution-ns": unit, "uuid": loop_uuid, "signal": "$enough"})
    seq.add_child(send_signal_1)
    seq.add_child(send_signal_2)
#    seq.add_child(send_signal_3)

    conc.add_child(seq)
   
    add_task = Node("add-task-to-team-queue", {"execution-ns": unit, "node-uuid": loop_uuid})

    conc.add_child(add_task)
    
    tree = Node("seq", {"execution-ns": options.op}, [conc])
    mo = MissionObject("search_area", ns, options.display, options.verbose)
    (res, reason, timing) = mo.delegation_process(tree, options.exectree)
    return (res, reason, timing)



if __name__ == "__main__":

    rospy.init_node ("delcommand")

    import signal
    signal.signal(signal.SIGINT, signal.SIG_DFL)

    plat = rospy.get_param("/world_origin_lat", None)
    plon = rospy.get_param("/world_origin_lon", None)

    gc = GeoConv(lat=plat, lon=plon)

    
    disppub = rospy.Publisher("/tst_display_request", DisplayRequest, queue_size=10)
    resource_config_pub = rospy.Publisher("/resource_config", ResourceConfig, queue_size=10)

    ns = options.op

    unit = rospy.get_namespace ().rstrip("/")

    res = []

    if options.takeoff:
        res = takeoff(ns, unit)

    if options.throwtakeoff:
        res = throw_takeoff(ns, unit)

    if options.land:
        res = land(ns, unit)

    if options.pickup:
        res = pickup(ns, unit)

    if options.drop:
        res = drop(ns, unit)

    if options.load:
        res = load(ns, unit)

    if options.climb:
        res = climb (ns, unit, options.z)
        
    if options.fly:
        res = flyto (ns, unit, options.x, options.y, options.z, options.speed)

    if options.path:
        strcoords = options.coords.split(";")
        print("STRCOORDS:", strcoords)
        coords = [[float(x.split(" ")[0]), float(x.split(" ")[1]), float(x.split(" ")[2])] for x in strcoords]
        print("COORDS:", coords)
        print(coords[0][0] + coords[0][1])
        res = flypath (ns, unit, coords, options.speed)

    if options.nav:
        res = navto (ns, unit, options.x, options.y, options.z, options.speed)

    if options.flyback:
        res = flyto_back (ns, unit, options.x, options.y, options.z, options.speed)

    if options.lookat:
        res = lookat (ns, unit, options.x, options.y, options.z)

    if options.stop_lookat:
        res = stop_lookat (ns, unit)

    if options.yaw:
        res = yaw (ns, unit, options.heading, "fast")

    if options.motalascan:
        res = motalascan(ns, unit)

    if options.goal:
        res = goal(ns, unit)

    if options.igoal:
        res = igoal(ns, unit)

    if options.igoaldeliver:
        res = igoal_delivery(ns, unit)

    if options.surfacesearch:
        res = surface_search(ns, unit)

    if options.pathtest:
        res = pathtest(ns, unit)

    if options.pathtest2:
        res = pathtest2(ns, unit)

    if options.pathtest3:
        res = pathtest3(ns)

    if options.pathtest4:
        res = pathtest4(ns)

    if options.search_area:
        res = search_area(ns, unit)

    if options.search_areas:
        res = search_areas(ns, unit)

    if options.paper_scenario1:
        paper_scenario1(ns, unit)
        
    if options.paper_scenario2:
        paper_scenario2(ns, unit)
        
    if options.paper_scenario3:
        paper_scenario3(ns, unit)
        
    if options.gesture_test:
        res = gesture_test(ns, unit)
        
    print("RES:", res)