drone-rigide/scripts/safe_drone_teleop.py

222 lines
7.7 KiB
Python
Raw Normal View History

2019-06-06 19:05:10 +00:00
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import curses
import math
import rospy
import sys
import dynamic_reconfigure.server
from std_msgs.msg import String
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
from demo_teleop.cfg import SafeDroneTeleopConfig
class Status:
def __init__(self):
self.status = "landed" # "landing" "taking off" "automatic flight" "manual flight"
self.last_time = rospy.Time()
self.last_input = rospy.Time()
self.twist = Twist()
self.in_twist = Twist()
self.state_wait = 3.0 # transition between states (taking_off, landing, etc..)
self.watchdog_time = .25 # input crash security delay (no input twist => stop)
self.linear = 0.1 # initial value
self.angular = 0.5 # initial value
self.delay = 2.0 # initial value
self.status_pub = rospy.Publisher ('status', String, queue_size=1)
self.r_pub = rospy.Publisher ('reset', Empty, queue_size=1)
self.r_pid_pub = rospy.Publisher ('reset_pid', Empty, queue_size=1)
self.t_pub = rospy.Publisher ('takeoff', Empty, queue_size=1)
self.l_pub = rospy.Publisher ('land', Empty, queue_size=1)
self.twist_pub = rospy.Publisher ('cmd_vel_out', Twist, queue_size=1)
self.twist_sub = rospy.Subscriber('cmd_vel_in', Twist, self.on_twist, queue_size = 1)
self.config_srv = dynamic_reconfigure.server.Server(SafeDroneTeleopConfig, self.on_reconf)
def on_reconf(self, config, level):
self.angular = config['angular']
self.linear = config['linear']
self.delay = config['delay']
return config
def on_twist(self, ros_data):
self.in_twist = ros_data
self.last_input = rospy.Time.now()
if self.status == "automatic flight":
self.twist_pub.publish(self.in_twist)
def send_twist(self):
if self.status == "manual flight":
self.twist_pub.publish(self.twist)
def take_off(self):
if self.status == "landed" :
self.twist = Twist()
self.status = "taking off"
self.status_pub.publish(self.status)
self.r_pub.publish(Empty())
rospy.sleep(1.)
self.t_pub.publish(Empty())
self.last_time = rospy.Time.now()
def land(self):
self.last_time = rospy.Time.now()
self.twist = Twist()
self.status = "landing"
self.l_pub.publish(Empty())
def nop(self):
if self.status == "manual flight" :
time = rospy.Time.now()
if (time - self.last_time).to_sec() > self.delay :
self.status = "automatic flight"
self.r_pid_pub.publish(Empty())
elif self.status == "taking off":
time = rospy.Time.now()
if (time - self.last_time).to_sec() > self.state_wait :
self.status = "manual flight"
self.last_time = time
self.twist = Twist()
elif self.status == "landing":
time = rospy.Time.now()
if (time - self.last_time).to_sec() > self.state_wait :
self.status = "landed"
self.status_pub.publish(self.status)
self.send_twist()
def key_pressed(self):
self.last_time = rospy.Time.now()
if self.status != "manual flight":
self.twist = Twist()
if self.status == "automatic flight" :
self.status = "manual flight"
self.send_twist()
def main(stdscr):
xlevel = 0
ylevel = 0
zlevel = 0
alevel = 0
rospy.init_node('safe_drone_teleop', anonymous=True)
log_pub = rospy.Publisher ('log', String, queue_size=1)
rate = rospy.Rate(10)
keycode = -1
status = Status()
stdscr.addstr("Safe drone controller\n")
stdscr.addstr("---------------------\n")
stdscr.addstr("\n")
stdscr.addstr("Command\n")
stdscr.addstr(" - UP/DOWN : control linear x\n")
stdscr.addstr(" - LEFT/RIGHT : control linear y\n")
stdscr.addstr(" - e/r : control angular z\n")
stdscr.addstr(" - t/l : take off / land\n")
stdscr.addstr(" - PAGE UP/DOWN : elevation control\n")
stdscr.addstr(" - any key : reset of the twist\n")
# We set the "wait for a key press" period to 100 ms.
stdscr.timeout(100)
while (not rospy.is_shutdown()):
keycode = stdscr.getch() # Wait for a key press for at most 100ms
if keycode == -1 :
status.nop() # No key has been pressed, we keep current twist.
elif keycode == curses.KEY_UP :
status.key_pressed()
if xlevel == -1 :
status.twist.linear.x = 0
xlevel = 0
elif xlevel == 0:
status.twist.linear.x = status.linear
xlevel = 1
elif keycode == curses.KEY_DOWN :
status.key_pressed()
if xlevel == 0 :
status.twist.linear.x = -status.linear
xlevel = -1
elif xlevel == 1:
status.twist.linear.x = 0
xlevel = 0
elif keycode == curses.KEY_LEFT :
status.key_pressed()
if ylevel == -1 :
status.twist.linear.y = 0
ylevel = 0
elif ylevel == 0:
status.twist.linear.y = status.linear
ylevel = 1
elif keycode == curses.KEY_RIGHT :
status.key_pressed()
if ylevel == 0 :
status.twist.linear.y = -status.linear
ylevel = -1
elif ylevel == 1:
status.twist.linear.y = 0
ylevel = 0
elif keycode == curses.KEY_PPAGE :
status.key_pressed()
if zlevel == -1 :
status.twist.linear.z = 0
zlevel = 0
elif zlevel == 0:
status.twist.linear.z = status.linear
zlevel = 1
elif keycode == curses.KEY_NPAGE :
status.key_pressed()
if zlevel == 1 :
status.twist.linear.z = 0
zlevel = 0
elif zlevel == 0:
status.twist.linear.z = -status.linear
zlevel = -1
elif keycode == 101 : # e
status.key_pressed()
if alevel == -1 :
status.twist.angular.z = 0
alevel = 0
elif alevel == 0:
status.twist.angular.z = status.angular
alevel = 1
elif keycode == 114 : # r
status.key_pressed()
if alevel == 0 :
status.twist.angular.z = -status.angular
alevel = -1
elif alevel == 1:
status.twist.angular.z = 0
alevel = 0
elif keycode == 116 : # t
status.take_off()
xlevel = 0
ylevel = 0
zlevel = 0
alevel = 0
elif keycode == 108 : # l
status.land()
xlevel = 0
ylevel = 0
zlevel = 0
alevel = 0
else :
status.key_pressed()
status.twist = Twist()
xlevel = 0
ylevel = 0
zlevel = 0
alevel = 0
if status.status == "automatic flight" :
xlevel = 0
ylevel = 0
zlevel = 0
alevel = 0
status.twist = Twist()
# Starts curses (terminal handling) and run our main function.
if __name__ == '__main__':
try:
curses.wrapper(lambda w: main(w))
except rospy.ROSInterruptException:
pass