From 704168f2ca762c4eff27c52994598d0255e37a53 Mon Sep 17 00:00:00 2001 From: Hugo LEVY-FALK Date: Thu, 6 Jun 2019 21:05:10 +0200 Subject: [PATCH] =?UTF-8?q?r=C3=A9glages=20d=C3=A9mo?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/control.launch | 16 +- launch/simple_loop.launch | 94 ++++++++++ params/rate.yaml | 19 ++ params/safe.yaml | 21 +++ params/settings_safe.yaml | 6 +- params/simple_loop/angular_z/D.yaml | 25 +++ params/simple_loop/angular_z/I.yaml | 27 +++ params/simple_loop/angular_z/P.yaml | 19 ++ params/simple_loop/angular_z/input.yaml | 17 ++ params/simple_loop/linear_x/D.yaml | 25 +++ params/simple_loop/linear_x/I.yaml | 27 +++ params/simple_loop/linear_x/P.yaml | 19 ++ params/simple_loop/linear_x/input.yaml | 17 ++ params/simple_loop/linear_y/D.yaml | 25 +++ params/simple_loop/linear_y/I.yaml | 27 +++ params/simple_loop/linear_y/P.yaml | 19 ++ params/simple_loop/linear_y/input.yaml | 17 ++ params/simple_loop/linear_z/D.yaml | 25 +++ params/simple_loop/linear_z/I.yaml | 27 +++ params/simple_loop/linear_z/P.yaml | 19 ++ params/simple_loop/linear_z/input.yaml | 17 ++ scripts/control_compute.py | 32 ++-- scripts/safe_drone_teleop.py | 221 ++++++++++++++++++++++++ 23 files changed, 741 insertions(+), 20 deletions(-) create mode 100644 launch/simple_loop.launch create mode 100644 params/rate.yaml create mode 100644 params/safe.yaml create mode 100644 params/simple_loop/angular_z/D.yaml create mode 100644 params/simple_loop/angular_z/I.yaml create mode 100644 params/simple_loop/angular_z/P.yaml create mode 100644 params/simple_loop/angular_z/input.yaml create mode 100644 params/simple_loop/linear_x/D.yaml create mode 100644 params/simple_loop/linear_x/I.yaml create mode 100644 params/simple_loop/linear_x/P.yaml create mode 100644 params/simple_loop/linear_x/input.yaml create mode 100644 params/simple_loop/linear_y/D.yaml create mode 100644 params/simple_loop/linear_y/I.yaml create mode 100644 params/simple_loop/linear_y/P.yaml create mode 100644 params/simple_loop/linear_y/input.yaml create mode 100644 params/simple_loop/linear_z/D.yaml create mode 100644 params/simple_loop/linear_z/I.yaml create mode 100644 params/simple_loop/linear_z/P.yaml create mode 100644 params/simple_loop/linear_z/input.yaml create mode 100755 scripts/safe_drone_teleop.py diff --git a/launch/control.launch b/launch/control.launch index 75a2f77..93e8f36 100644 --- a/launch/control.launch +++ b/launch/control.launch @@ -3,23 +3,31 @@ - + + + + + + + + + - + - + - + diff --git a/launch/simple_loop.launch b/launch/simple_loop.launch new file mode 100644 index 0000000..5eb1e6a --- /dev/null +++ b/launch/simple_loop.launch @@ -0,0 +1,94 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/params/rate.yaml b/params/rate.yaml new file mode 100644 index 0000000..f7c767a --- /dev/null +++ b/params/rate.yaml @@ -0,0 +1,19 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 1.0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + state: [] + k: 1.0 + refresh_time: 0.05 +state: [] diff --git a/params/safe.yaml b/params/safe.yaml new file mode 100644 index 0000000..ddd31d8 --- /dev/null +++ b/params/safe.yaml @@ -0,0 +1,21 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + angular: 0.5 + delay: 2.0 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + angular: 0.5 + delay: 2.0 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + linear: 0.5 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + state: [] + linear: 0.5 +state: [] diff --git a/params/settings_safe.yaml b/params/settings_safe.yaml index afcfbcd..a95356c 100644 --- a/params/settings_safe.yaml +++ b/params/settings_safe.yaml @@ -1,6 +1,6 @@ !!python/object/new:dynamic_reconfigure.encoding.Config dictitems: - angular: 0.5 + angular: 2.0 delay: 2.0 groups: !!python/object/new:dynamic_reconfigure.encoding.Config dictitems: @@ -9,7 +9,7 @@ dictitems: groups: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] id: 0 - linear: 0.05 + linear: 0.5 name: Default parameters: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] @@ -17,5 +17,5 @@ dictitems: state: true type: '' state: [] - linear: 0.05 + linear: 0.5 state: [] diff --git a/params/simple_loop/angular_z/D.yaml b/params/simple_loop/angular_z/D.yaml new file mode 100644 index 0000000..cbbd1dd --- /dev/null +++ b/params/simple_loop/angular_z/D.yaml @@ -0,0 +1,25 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: -0.67 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + poly_order: 2 + refresh_time: 0.05 + size: 19 + state: true + type: '' + state: [] + k: -0.67 + poly_order: 2 + refresh_time: 0.05 + size: 19 +state: [] diff --git a/params/simple_loop/angular_z/I.yaml b/params/simple_loop/angular_z/I.yaml new file mode 100644 index 0000000..a475a04 --- /dev/null +++ b/params/simple_loop/angular_z/I.yaml @@ -0,0 +1,27 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: -0.8 + max: 0.0 + min: 0.0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + use_max: false + use_min: false + state: [] + k: -0.8 + max: 0.0 + min: 0.0 + refresh_time: 0.05 + use_max: false + use_min: false +state: [] diff --git a/params/simple_loop/angular_z/P.yaml b/params/simple_loop/angular_z/P.yaml new file mode 100644 index 0000000..e0a6796 --- /dev/null +++ b/params/simple_loop/angular_z/P.yaml @@ -0,0 +1,19 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: -1.1 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + state: [] + k: -1.1 + refresh_time: 0.05 +state: [] diff --git a/params/simple_loop/angular_z/input.yaml b/params/simple_loop/angular_z/input.yaml new file mode 100644 index 0000000..ce878ce --- /dev/null +++ b/params/simple_loop/angular_z/input.yaml @@ -0,0 +1,17 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + value: 0.0 + state: [] + value: 0.0 +state: [] diff --git a/params/simple_loop/linear_x/D.yaml b/params/simple_loop/linear_x/D.yaml new file mode 100644 index 0000000..31be080 --- /dev/null +++ b/params/simple_loop/linear_x/D.yaml @@ -0,0 +1,25 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: -0.1 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + poly_order: 2 + refresh_time: 0.05 + size: 11 + state: true + type: '' + state: [] + k: -0.1 + poly_order: 2 + refresh_time: 0.05 + size: 11 +state: [] diff --git a/params/simple_loop/linear_x/I.yaml b/params/simple_loop/linear_x/I.yaml new file mode 100644 index 0000000..9acb9cf --- /dev/null +++ b/params/simple_loop/linear_x/I.yaml @@ -0,0 +1,27 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 0.0 + max: 0.0 + min: 0.0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + use_max: false + use_min: false + state: [] + k: 0.0 + max: 0.0 + min: 0.0 + refresh_time: 0.05 + use_max: false + use_min: false +state: [] diff --git a/params/simple_loop/linear_x/P.yaml b/params/simple_loop/linear_x/P.yaml new file mode 100644 index 0000000..50f5fe6 --- /dev/null +++ b/params/simple_loop/linear_x/P.yaml @@ -0,0 +1,19 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: -0.07 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + state: [] + k: -0.07 + refresh_time: 0.05 +state: [] diff --git a/params/simple_loop/linear_x/input.yaml b/params/simple_loop/linear_x/input.yaml new file mode 100644 index 0000000..0958225 --- /dev/null +++ b/params/simple_loop/linear_x/input.yaml @@ -0,0 +1,17 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + value: 2.5 + state: [] + value: 2.5 +state: [] diff --git a/params/simple_loop/linear_y/D.yaml b/params/simple_loop/linear_y/D.yaml new file mode 100644 index 0000000..8578e1f --- /dev/null +++ b/params/simple_loop/linear_y/D.yaml @@ -0,0 +1,25 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 0.14 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + poly_order: 2 + refresh_time: 0.05 + size: 11 + state: true + type: '' + state: [] + k: 0.14 + poly_order: 2 + refresh_time: 0.05 + size: 11 +state: [] diff --git a/params/simple_loop/linear_y/I.yaml b/params/simple_loop/linear_y/I.yaml new file mode 100644 index 0000000..9acb9cf --- /dev/null +++ b/params/simple_loop/linear_y/I.yaml @@ -0,0 +1,27 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 0.0 + max: 0.0 + min: 0.0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + use_max: false + use_min: false + state: [] + k: 0.0 + max: 0.0 + min: 0.0 + refresh_time: 0.05 + use_max: false + use_min: false +state: [] diff --git a/params/simple_loop/linear_y/P.yaml b/params/simple_loop/linear_y/P.yaml new file mode 100644 index 0000000..310436b --- /dev/null +++ b/params/simple_loop/linear_y/P.yaml @@ -0,0 +1,19 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 0.27 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + state: [] + k: 0.27 + refresh_time: 0.05 +state: [] diff --git a/params/simple_loop/linear_y/input.yaml b/params/simple_loop/linear_y/input.yaml new file mode 100644 index 0000000..ce878ce --- /dev/null +++ b/params/simple_loop/linear_y/input.yaml @@ -0,0 +1,17 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + value: 0.0 + state: [] + value: 0.0 +state: [] diff --git a/params/simple_loop/linear_z/D.yaml b/params/simple_loop/linear_z/D.yaml new file mode 100644 index 0000000..d07ab2f --- /dev/null +++ b/params/simple_loop/linear_z/D.yaml @@ -0,0 +1,25 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + deriv: 1 + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 0.63 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + poly_order: 2 + refresh_time: 0.05 + size: 11 + state: true + type: '' + state: [] + k: 0.63 + poly_order: 2 + refresh_time: 0.05 + size: 11 +state: [] diff --git a/params/simple_loop/linear_z/I.yaml b/params/simple_loop/linear_z/I.yaml new file mode 100644 index 0000000..81b7e42 --- /dev/null +++ b/params/simple_loop/linear_z/I.yaml @@ -0,0 +1,27 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 1.52 + max: 0.0 + min: 0.0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + use_max: false + use_min: false + state: [] + k: 1.52 + max: 0.0 + min: 0.0 + refresh_time: 0.05 + use_max: false + use_min: false +state: [] diff --git a/params/simple_loop/linear_z/P.yaml b/params/simple_loop/linear_z/P.yaml new file mode 100644 index 0000000..ab40fda --- /dev/null +++ b/params/simple_loop/linear_z/P.yaml @@ -0,0 +1,19 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + k: 1.2 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + refresh_time: 0.05 + state: true + type: '' + state: [] + k: 1.2 + refresh_time: 0.05 +state: [] diff --git a/params/simple_loop/linear_z/input.yaml b/params/simple_loop/linear_z/input.yaml new file mode 100644 index 0000000..ce878ce --- /dev/null +++ b/params/simple_loop/linear_z/input.yaml @@ -0,0 +1,17 @@ +!!python/object/new:dynamic_reconfigure.encoding.Config +dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + dictitems: + groups: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + id: 0 + name: Default + parameters: !!python/object/new:dynamic_reconfigure.encoding.Config + state: [] + parent: 0 + state: true + type: '' + value: 0.0 + state: [] + value: 0.0 +state: [] diff --git a/scripts/control_compute.py b/scripts/control_compute.py index e5b4645..b6c847c 100755 --- a/scripts/control_compute.py +++ b/scripts/control_compute.py @@ -4,6 +4,7 @@ from scipy.signal import savgol_coeffs import numpy as np import sys import threading +import functools import rospy @@ -31,12 +32,12 @@ class ControlNode(object): if config_class is not None: self.server = dynamic_reconfigure.server.Server(config_class, self.on_reconf) - def on_reset(self, value): - if value: - self.output = 0.0 + def on_reset(self, value=None): + self.output = 0.0 def on_reconf(self, config, level): self.refresh_time = rospy.Duration.from_sec(config["refresh_time"]) + self.on_reset() return config def on_compute(self, value): @@ -115,7 +116,7 @@ class SaturateNode(InputControlNode): super(SaturateNode, self).on_compute(value) class IntegralNode(SaturateNode): - def __init__(self, k=1.0, min=None, max=None, refresh_time=0.05): + def __init__(self, k=0.0, min=None, max=None, refresh_time=0.05): super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig) self.k = k @@ -126,8 +127,9 @@ class IntegralNode(SaturateNode): def on_compute(self, value): time_ok, delta = self.check_time() if time_ok: - self.output += value.data * delta - return super(IntegralNode, self).on_compute(value) + old_output = self.output + self.output += self.k * value.data * delta + return super(IntegralNode, self).on_compute(Float64(self.output)) class DerivativeNode(InputControlNode): def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=0.05): @@ -155,13 +157,12 @@ class DerivativeNode(InputControlNode): time_ok, delta = self.check_time() if time_ok: self.last_points = np.concatenate([self.last_points[1:], [value.data]]) - self.output = self.last_points.dot(self.filter) + self.output = self.k * self.last_points.dot(self.filter) return super(DerivativeNode, self).on_compute(value) - def on_reset(self, value): - if value: - self.last_points = np.zeros(len(self.filter)) - return super(DerivativeNode, self).on_reset(value) + def on_reset(self, value=None): + self.last_points = np.zeros(len(self.filter)) + return super(DerivativeNode, self).on_reset(value) class DifferenciateNode(InputControlNode): def __init__(self): @@ -186,7 +187,7 @@ class InputNode(InputControlNode): def on_reconf(self, config, level): self.output = config["value"] - super(InputNode, self).on_compute(config["value"]) + super(InputNode, self).on_compute(Float64(config["value"])) return config def on_compute(self, value): @@ -200,7 +201,11 @@ class SumNode(ControlNode): self.inputs = dict() for i in range(self.nb_topics): topic_name = 'input_'+str(i) - self.input_topics[topic_name] = rospy.Subscriber(topic_name, Float64, lambda v: self.inputs.update(dict(topic_name=v.data)) or self.on_compute(v)) + self.input_topics[topic_name] = rospy.Subscriber(topic_name, Float64, functools.partial(self.update_value, topic_name)) + + def update_value(self, key, value): + self.inputs.update({key:value.data}) + self.on_compute(value) def on_compute(self, value): self.output = sum(self.inputs.values()) @@ -218,6 +223,7 @@ if __name__ == '__main__': 'rate': (RateNode, False), } rospy.init_node('control_node', anonymous=True) + rospy.loginfo("Running node " + sys.argv[1]) try: node_class, need_param = nodes[sys.argv[1]] except KeyError as e: diff --git a/scripts/safe_drone_teleop.py b/scripts/safe_drone_teleop.py new file mode 100755 index 0000000..61ab067 --- /dev/null +++ b/scripts/safe_drone_teleop.py @@ -0,0 +1,221 @@ +#! /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