DJI M600 teleoperation ROS style

gistfile1.txt

#!/usr/bin/env python3
import sys, select, termios, tty

import rospy
from sensor_msgs.msg import Joy
import std_msgs.msg

update_rate = 0.1 # 100 hz loop cycle

msg = """
Control Your M600!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly
CTRL-C to quit
"""

moveBindings = {
        'i':(1,0,0,0),
        'o':(0,0,0,-1),
        'j':(0,1,0,0),
        'l':(0,-1,0,0),
        'u':(0,0,0,1),
        ',':(-1,0,0,0),
        '.':(0,0,0,1),
        'm':(0,0,0,-1),
        'O':(1,-1,0,0),
        'I':(1,0,0,0),
        'J':(0,1,0,0),
        'L':(0,-1,0,0),
        'U':(1,1,0,0),
        '<':(-1,0,0,0),
        '>':(-1,-1,0,0),
        'M':(-1,1,0,0),
        't':(0,0,1,0),
        'b':(0,0,-1,0),
    }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
    }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = 5
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('m600_teleop_joy')
    control_pub = rospy.Publisher("/dji_sdk/flight_control_setpoint_ENUvelocity_yawrate", Joy, queue_size=10)

    stamped = False
    x = 0
    y = 0
    z = 0
    th = 0
    status = 0
    count = 0
    acc = 0.5
    target_speed = 0
    target_speed_y = 0
    target_turn = 0
    control_speed = 0
    control_speed_y = 0
    control_turn = 0
    try:
        print(msg)
        print(vels(speed,turn))
        while(1):
            key = getKey()
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                y = moveBindings[key][1]
                z = moveBindings[key][2]
                th = moveBindings[key][3]
                count = 0
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]
                turn = turn * speedBindings[key][1]
                count = 0

                print(vels(speed,turn))
                if (status == 14):
                    print(msg)
                status = (status + 1) % 15
            elif key == ' ' or key == 'k' :
                x = 0
                y = 0
                th = 0
                control_speed = 0
                control_speed_y = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    y = 0
                    z = 0
                    th = 0
                if (key == '\x03'):
                    break

            target_speed = speed * x
            target_speed_y = speed * y
            target_turn = turn * th

            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + acc )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - acc )
            else:
                control_speed = target_speed

            if target_speed_y > control_speed_y:
                control_speed_y = min( target_speed_y, control_speed_y + acc )
            elif target_speed_y < control_speed_y:
                control_speed_y = max( target_speed_y, control_speed_y - acc )
            else:
                control_speed_y = target_speed_y

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + acc )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - acc )
            else:
                control_turn = target_turn

            # control_speed   = x
            # control_speed_y = y
            # z               = z
            # control_turn    = yaw
            message = [(control_speed), (control_speed_y), (z), (control_turn)]
            print(message)
            #print(x, y, z, th, control_speed, control_turn)
            joy_msg = Joy()
            joy_msg.axes = [0] * 4
            if stamped:
                print("Do logic for stamped")
            else:
                joy_msg.axes[0] = control_speed
                joy_msg.axes[1] = control_speed_y
                joy_msg.axes[2] = z
                joy_msg.axes[3] = control_turn
                control_pub.publish(joy_msg)


    except Exception as e:
        print(e)

    finally:
        message = [0,0,0]
        mess = ','.join([str(elem) for elem in message])
        #sock.sendto(bytes(mess, "utf-8"), (UDP_IP, UDP_PORT))
        #sock.sendto(bytes(mess, "utf-8"), (UDP_IP_2, UDP_PORT))
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)