klafyvel
/
drone-rigide
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

réglages démo

master
Hugo LEVY-FALK 7 years ago
parent
69cccc4615
commit
704168f2ca
23 changed files with 741 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      launch/control.launch
  2. 94
      launch/simple_loop.launch
  3. 19
      params/rate.yaml
  4. 21
      params/safe.yaml
  5. 6
      params/settings_safe.yaml
  6. 25
      params/simple_loop/angular_z/D.yaml
  7. 27
      params/simple_loop/angular_z/I.yaml
  8. 19
      params/simple_loop/angular_z/P.yaml
  9. 17
      params/simple_loop/angular_z/input.yaml
  10. 25
      params/simple_loop/linear_x/D.yaml
  11. 27
      params/simple_loop/linear_x/I.yaml
  12. 19
      params/simple_loop/linear_x/P.yaml
  13. 17
      params/simple_loop/linear_x/input.yaml
  14. 25
      params/simple_loop/linear_y/D.yaml
  15. 27
      params/simple_loop/linear_y/I.yaml
  16. 19
      params/simple_loop/linear_y/P.yaml
  17. 17
      params/simple_loop/linear_y/input.yaml
  18. 25
      params/simple_loop/linear_z/D.yaml
  19. 27
      params/simple_loop/linear_z/I.yaml
  20. 19
      params/simple_loop/linear_z/P.yaml
  21. 17
      params/simple_loop/linear_z/input.yaml
  22. 32
      scripts/control_compute.py
  23. 221
      scripts/safe_drone_teleop.py

16
launch/control.launch
View File

@ -3,23 +3,31 @@
<arg name="output" default="output"/> <arg name="output" default="output"/>
<arg name="measure" default="measure"/> <arg name="measure" default="measure"/>
<arg name="reset" default="reset"/> <arg name="reset" default="reset"/>
<node name="input" pkg="detect_targets" type="control_compute.py" args="input"> <arg name="param_P" default="simple_loop/linear_x/P.yaml" />
<arg name="param_I" default="simple_loop/linear_x/I.yaml" />
<arg name="param_D" default="simple_loop/linear_x/D.yaml" />
<arg name="param_input" default="params/simple_loop/linear_x/input.yaml" />
<node name="P_param" pkg="dynamic_reconfigure" type="dynparam" args="load P $(find detect_targets)/params/$(arg param_P)"/>
<node name="I_param" pkg="dynamic_reconfigure" type="dynparam" args="load I $(find detect_targets)/params/$(arg param_I)"/>
<node name="D_param" pkg="dynamic_reconfigure" type="dynparam" args="load D $(find detect_targets)/params/$(arg param_D)"/>
<node name="input_param" pkg="dynamic_reconfigure" type="dynparam" args="load input $(find detect_targets)/params/$(arg param_input)"/>
<node name="input" pkg="detect_targets" type="control_compute.py" args="input" output="screen">
<remap from="input" to="$(arg input)" /> <remap from="input" to="$(arg input)" />
<remap from="output" to="set_point"/> <remap from="output" to="set_point"/>
<remap from="reset" to="$(arg reset)" /> <remap from="reset" to="$(arg reset)" />
</node> </node>
<node name="diff" pkg="detect_targets" type="control_compute.py" args="differenciate"> <node name="diff" pkg="detect_targets" type="control_compute.py" args="differenciate" output="screen">
<remap from="input" to="set_point"/> <remap from="input" to="set_point"/>
<remap from="output" to="epsilon"/> <remap from="output" to="epsilon"/>
<remap from="measure" to="$(arg measure)" /> <remap from="measure" to="$(arg measure)" />
<remap from="reset" to="$(arg reset)" /> <remap from="reset" to="$(arg reset)" />
</node> </node>
<node name="P" pkg="detect_targets" type="control_compute.py" args="proportional"> <node name="P" pkg="detect_targets" type="control_compute.py" args="proportional" output="screen">
<remap from="input" to="epsilon"/> <remap from="input" to="epsilon"/>
<remap from="output" to="p_out"/> <remap from="output" to="p_out"/>
<remap from="reset" to="$(arg reset)" /> <remap from="reset" to="$(arg reset)" />
</node> </node>
<node name="I" pkg="detect_targets" type="control_compute.py" args="integral"> <node name="I" pkg="detect_targets" type="control_compute.py" args="integral" output="screen">
<remap from="input" to="epsilon"/> <remap from="input" to="epsilon"/>
<remap from="output" to="i_out"/> <remap from="output" to="i_out"/>
<remap from="reset" to="$(arg reset)" /> <remap from="reset" to="$(arg reset)" />

94
launch/simple_loop.launch
View File

@ -0,0 +1,94 @@
<launch>
<node name="Ratex_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratex $(find detect_targets)/params/rate.yaml"/>
<node name="Ratey_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratey $(find detect_targets)/params/rate.yaml"/>
<node name="Ratez_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratez $(find detect_targets)/params/rate.yaml"/>
<node name="Rateangz_param" pkg="dynamic_reconfigure" type="dynparam" args="load /rate_ang_z $(find detect_targets)/params/rate.yaml"/>
<node name="safe_param" pkg="dynamic_reconfigure" type="dynparam" args="load /safe $(find detect_targets)/params/settings_safe.yaml"/>
<node name="targets_param" pkg="dynamic_reconfigure" type="dynparam" args="load /targets $(find detect_targets)/params/settings_targets.yaml"/>
<!-- <remap from="/reset" to="/bebop/reset"/> -->
<include file="$(find bebop_driver)/launch/bebop_node.launch" />
<node name="targets" pkg="detect_targets" type="target_publisher.py">
</node>
<node name="triangle" pkg="detect_targets" type="triangle.py" output="screen">
<remap from="component_centers" to="targets"/>
</node>
<node name="ratex" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_x" />
<remap from="output" to="rated_x" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_linear_x">
<arg name="reset" value="/reset_pid" />
<arg name="measure" value="/rated_x" />
<arg name="param_P" value="simple_loop/linear_x/P.yaml" />
<arg name="param_I" value="simple_loop/linear_x/I.yaml" />
<arg name="param_D" value="simple_loop/linear_x/D.yaml" />
<arg name="param_input" value="simple_loop/linear_x/input.yaml" />
</include>
<node name="ratey" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_y" />
<remap from="output" to="rated_y" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_linear_y">
<arg name="reset" value="/reset_pid" />
<arg name="measure" value="/rated_y" />
<arg name="param_P" value="simple_loop/linear_y/P.yaml" />
<arg name="param_I" value="simple_loop/linear_y/I.yaml" />
<arg name="param_D" value="simple_loop/linear_y/D.yaml" />
<arg name="param_input" value="simple_loop/linear_y/input.yaml" />
</include>
<node name="ratez" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_z" />
<remap from="output" to="rated_z" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_linear_z">
<arg name="reset" value="/reset_pid" />
<arg name="measure" value="/rated_z" />
<arg name="param_P" value="simple_loop/linear_z/P.yaml" />
<arg name="param_I" value="simple_loop/linear_z/I.yaml" />
<arg name="param_D" value="simple_loop/linear_z/D.yaml" />
<arg name="param_input" value="simple_loop/linear_z/input.yaml" />
</include>
<node name="rate_ang_z" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="angular_z" />
<remap from="output" to="rated_ang_z" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_angular_z">
<arg name="reset" value="/reset_pid" />
<arg name="measure" value="/rated_ang_z" />
<arg name="param_P" value="simple_loop/angular_z/P.yaml" />
<arg name="param_I" value="simple_loop/angular_z/I.yaml" />
<arg name="param_D" value="simple_loop/angular_z/D.yaml" />
<arg name="param_input" value="simple_loop/angular_z/input.yaml" />
</include>
<node name="twister" pkg="detect_targets" type="twist_controls.py">
<remap from="control_linear_z" to="controller_linear_z/output" />
<remap from="control_angular_z" to="controller_angular_z/output" />
<remap from="control_linear_x" to="controller_linear_x/output" />
<remap from="control_linear_y" to="controller_linear_y/output" />
</node>
<node name="safe" pkg="detect_targets" type="safe_drone_teleop.py" output="screen" launch-prefix="xterm -e">
<remap from="takeoff" to="/bebop/takeoff"/>
<remap from="land" to="/bebop/land"/>
<remap from="reset" to="/bebop/reset"/>
<remap from="cmd_vel_out" to="/bebop/cmd_vel"/>
<remap from="cmd_vel_in" to="/cmd_vel"/>
</node>
<node name="view" pkg="rqt_image_view" type="rqt_image_view" args="/bebop/image_raw"/>
<node name="view_targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
<node name="graph" pkg="rqt_graph" type="rqt_graph" output="screen"></node>
<node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
</launch>

19
params/rate.yaml
View File

@ -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: []

21
params/safe.yaml
View File

@ -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: []

6
params/settings_safe.yaml
View File

@ -1,6 +1,6 @@
!!python/object/new:dynamic_reconfigure.encoding.Config !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems: dictitems:
angular: 0.5 angular: 2.0
delay: 2.0 delay: 2.0
groups: !!python/object/new:dynamic_reconfigure.encoding.Config groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems: dictitems:
@ -9,7 +9,7 @@ dictitems:
groups: !!python/object/new:dynamic_reconfigure.encoding.Config groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: [] state: []
id: 0 id: 0
linear: 0.05 linear: 0.5
name: Default name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: [] state: []
@ -17,5 +17,5 @@ dictitems:
state: true state: true
type: '' type: ''
state: [] state: []
linear: 0.05 linear: 0.5
state: [] state: []

25
params/simple_loop/angular_z/D.yaml
View File

@ -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: []

27
params/simple_loop/angular_z/I.yaml
View File

@ -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: []

19
params/simple_loop/angular_z/P.yaml
View File

@ -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: []

17
params/simple_loop/angular_z/input.yaml
View File

@ -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: []

25
params/simple_loop/linear_x/D.yaml
View File

@ -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: []

27
params/simple_loop/linear_x/I.yaml
View File

@ -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: []

19
params/simple_loop/linear_x/P.yaml
View File

@ -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: []

17
params/simple_loop/linear_x/input.yaml
View File

@ -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: []

25
params/simple_loop/linear_y/D.yaml
View File

@ -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: []

27
params/simple_loop/linear_y/I.yaml
View File

@ -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: []

19
params/simple_loop/linear_y/P.yaml
View File

@ -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: []

17
params/simple_loop/linear_y/input.yaml
View File

@ -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: []

25
params/simple_loop/linear_z/D.yaml
View File

@ -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: []

27
params/simple_loop/linear_z/I.yaml
View File

@ -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: []

19
params/simple_loop/linear_z/P.yaml
View File

@ -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: []

17
params/simple_loop/linear_z/input.yaml
View File

@ -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: []

32
scripts/control_compute.py
View File

@ -4,6 +4,7 @@ from scipy.signal import savgol_coeffs
import numpy as np import numpy as np
import sys import sys
import threading import threading
import functools
import rospy import rospy
@ -31,12 +32,12 @@ class ControlNode(object):
if config_class is not None: if config_class is not None:
self.server = dynamic_reconfigure.server.Server(config_class, self.on_reconf) self.server = dynamic_reconfigure.server.Server(config_class, self.on_reconf)
def on_reset(self, value): def on_reset(self, value=None):
if value: self.output = 0.0
self.output = 0.0
def on_reconf(self, config, level): def on_reconf(self, config, level):
self.refresh_time = rospy.Duration.from_sec(config["refresh_time"]) self.refresh_time = rospy.Duration.from_sec(config["refresh_time"])
self.on_reset()
return config return config
def on_compute(self, value): def on_compute(self, value):
@ -115,7 +116,7 @@ class SaturateNode(InputControlNode):
super(SaturateNode, self).on_compute(value) super(SaturateNode, self).on_compute(value)
class IntegralNode(SaturateNode): 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) super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig)
self.k = k self.k = k
@ -126,8 +127,9 @@ class IntegralNode(SaturateNode):
def on_compute(self, value): def on_compute(self, value):
time_ok, delta = self.check_time() time_ok, delta = self.check_time()
if time_ok: if time_ok:
self.output += value.data * delta old_output = self.output
return super(IntegralNode, self).on_compute(value) self.output += self.k * value.data * delta
return super(IntegralNode, self).on_compute(Float64(self.output))
class DerivativeNode(InputControlNode): class DerivativeNode(InputControlNode):
def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=0.05): 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() time_ok, delta = self.check_time()
if time_ok: if time_ok:
self.last_points = np.concatenate([self.last_points[1:], [value.data]]) 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) return super(DerivativeNode, self).on_compute(value)
def on_reset(self, value): def on_reset(self, value=None):
if value: self.last_points = np.zeros(len(self.filter))
self.last_points = np.zeros(len(self.filter)) return super(DerivativeNode, self).on_reset(value)
return super(DerivativeNode, self).on_reset(value)
class DifferenciateNode(InputControlNode): class DifferenciateNode(InputControlNode):
def __init__(self): def __init__(self):
@ -186,7 +187,7 @@ class InputNode(InputControlNode):
def on_reconf(self, config, level): def on_reconf(self, config, level):
self.output = config["value"] self.output = config["value"]
super(InputNode, self).on_compute(config["value"]) super(InputNode, self).on_compute(Float64(config["value"]))
return config return config
def on_compute(self, value): def on_compute(self, value):
@ -200,7 +201,11 @@ class SumNode(ControlNode):
self.inputs = dict() self.inputs = dict()
for i in range(self.nb_topics): for i in range(self.nb_topics):
topic_name = 'input_'+str(i) 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): def on_compute(self, value):
self.output = sum(self.inputs.values()) self.output = sum(self.inputs.values())
@ -218,6 +223,7 @@ if __name__ == '__main__':
'rate': (RateNode, False), 'rate': (RateNode, False),
} }
rospy.init_node('control_node', anonymous=True) rospy.init_node('control_node', anonymous=True)
rospy.loginfo("Running node " + sys.argv[1])
try: try:
node_class, need_param = nodes[sys.argv[1]] node_class, need_param = nodes[sys.argv[1]]
except KeyError as e: except KeyError as e:

221
scripts/safe_drone_teleop.py
View File

@ -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
Write Preview
Loading…
Cancel
Save