réglages démo
This commit is contained in:
Hugo LEVY-FALK
parent
69cccc4615
commit
704168f2ca
23 changed files with 741 additions and 20 deletions
|
|
@ -3,23 +3,31 @@
|
|||
<arg name="output" default="output"/>
|
||||
<arg name="measure" default="measure"/>
|
||||
<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="output" to="set_point"/>
|
||||
<remap from="reset" to="$(arg reset)" />
|
||||
</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="output" to="epsilon"/>
|
||||
<remap from="measure" to="$(arg measure)" />
|
||||
<remap from="reset" to="$(arg reset)" />
|
||||
</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="output" to="p_out"/>
|
||||
<remap from="reset" to="$(arg reset)" />
|
||||
</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="output" to="i_out"/>
|
||||
<remap from="reset" to="$(arg reset)" />
|
||||
|
|
|
|||
94
launch/simple_loop.launch
Normal file
94
launch/simple_loop.launch
Normal 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
Normal file
19
params/rate.yaml
Normal 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
Normal file
21
params/safe.yaml
Normal 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: []
|
||||
|
|
@ -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: []
|
||||
|
|
|
|||
25
params/simple_loop/angular_z/D.yaml
Normal file
25
params/simple_loop/angular_z/D.yaml
Normal 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
Normal file
27
params/simple_loop/angular_z/I.yaml
Normal 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
Normal file
19
params/simple_loop/angular_z/P.yaml
Normal 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
Normal file
17
params/simple_loop/angular_z/input.yaml
Normal 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
Normal file
25
params/simple_loop/linear_x/D.yaml
Normal 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
Normal file
27
params/simple_loop/linear_x/I.yaml
Normal 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
Normal file
19
params/simple_loop/linear_x/P.yaml
Normal 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
Normal file
17
params/simple_loop/linear_x/input.yaml
Normal 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
Normal file
25
params/simple_loop/linear_y/D.yaml
Normal 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
Normal file
27
params/simple_loop/linear_y/I.yaml
Normal 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
Normal file
19
params/simple_loop/linear_y/P.yaml
Normal 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
Normal file
17
params/simple_loop/linear_y/input.yaml
Normal 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
Normal file
25
params/simple_loop/linear_z/D.yaml
Normal 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
Normal file
27
params/simple_loop/linear_z/I.yaml
Normal 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
Normal file
19
params/simple_loop/linear_z/P.yaml
Normal 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
Normal file
17
params/simple_loop/linear_z/input.yaml
Normal 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: []
|
||||
|
|
@ -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:
|
||||
|
|
|
|||
221
scripts/safe_drone_teleop.py
Executable file
221
scripts/safe_drone_teleop.py
Executable 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
|
||||
Loading…
Reference in a new issue