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 ("", "--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")
(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 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("nav-to")
    nav.set_params({"execution-ns": unit, "p": p, "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 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)    


if __name__ == "__main__":

    rospy.init_node ("deltester")

    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.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, "standard")

    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)

    print("RES:", res)