Commit 704168f2 authored by Hugo LEVY-FALK's avatar Hugo LEVY-FALK

réglages démo

parent 69cccc46
......@@ -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)" />
......
<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>
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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: []
!!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:
......
#! /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())