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