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="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
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
|
!!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
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 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,11 +157,10 @@ 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)
|
||||||
|
|
||||||
|
|
@ -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
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