Commit 69cccc46 authored by Hugo LEVY-FALK's avatar Hugo LEVY-FALK

Debug des nœuds de rate et triangle

parent 7355556a
......@@ -95,12 +95,13 @@ generate_messages(
generate_dynamic_reconfigure_options(
cfg/DetectTargets.cfg
cfg/triangle_control.cfg
cfg/PIDNode.cfg
cfg/RateNode.cfg
cfg/ProportionalNode.cfg
cfg/IntegralNode.cfg
cfg/DerivativeNode.cfg
cfg/SaturateNode.cfg
cfg/InputNode.cfg
cfg/Triangle.cfg
)
###################################
......
File mode changed from 100644 to 100755
#!/usr/bin/env python
PACKAGE = "detect_targets"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("camera_angle", double_t, 0, "The angle corresponding to the image width", 80, 50, 180)
gen.add("target_width", double_t, 0, "the real target width (m)", 1, 0.01, 1.5)
gen.add("target_depth", double_t, 0, "the real target depth (m)", .2, 0.01, 0.5)
exit(gen.generate(PACKAGE, "detect_targets", "Triangle"))
<launch>
<remap from="/reset" to="/bebop/reset"/>
<node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
<node name="Ratex_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratex $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="Ratey_param" pkg="dynamic_reconfigure" type="dynparam" args="load /ratey $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="Dx_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dx $(find detect_targets)/params/settings_test_D.yaml"/>
<node name="Dy_param" pkg="dynamic_reconfigure" type="dynparam" args="load /Dy $(find detect_targets)/params/settings_test_D.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_blue.yaml"/>
<include file="$(find bebop_driver)/launch/bebop_node.launch" />
<node name="targets" pkg="detect_targets" type="target_publisher.py">
......@@ -13,25 +19,22 @@
<node name="untwist" pkg="detect_targets" type="untwist.py" output="screen">
<remap from="input" to="/bebop/cmd_vel" />
</node>
<node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<node name="ratex" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_x" />
<remap from="output" to="derivative_x_input" />
</node>
<node name="Dx" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<remap from="input" to="derivative_x_input" />
<remap from="output" to="measure_x" />
</node>
<node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<node name="ratey" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="linear_y" />
<remap from="output" to="derivative_y_input" />
</node>
<node name="Dy" pkg="detect_targets" type="control_compute.py" args="derivative" output="screen">
<remap from="input" to="derivative_y_input" />
<remap from="output" to="measure_y" />
</node>
<include file="$(find detect_targets)/launch/control.launch" ns="controller_angular_z">
<arg name="reset" value="/reset" />
<arg name="measure" value="/angular_z" />
</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/internal_loop/output" />
<remap from="control_linear_y" to="controller_linear_y/internal_loop/output" />
</node>
<node name="safe" pkg="demo_teleop" type="safe_drone_teleop.py" output="screen" launch-prefix="xterm -e">
<remap from="takeoff" to="/bebop/takeoff"/>
......@@ -41,7 +44,11 @@
</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="view_targets" pkg="rqt_image_view" type="rqt_image_view" args="/img_targets"/>
<node name="plot" pkg="rqt_plot" type="rqt_plot" args="/measure_x /measure_linear_x">
</node>
<node name="graph" pkg="rqt_graph" type="rqt_graph" output="screen"></node>
</launch>
<launch>
<node name="reconf" pkg="rqt_reconfigure" type="rqt_reconfigure"/>
<node name="D_param" pkg="dynamic_reconfigure" type="dynparam" args="load /D $(find detect_targets)/params/settings_test_D.yaml"/>
<node name="Rate_param" pkg="dynamic_reconfigure" type="dynparam" args="load /rate $(find detect_targets)/params/settings_test_D_rate.yaml"/>
<node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/walk.csv" output="screen">
<node name="publisher" pkg="detect_targets" type="publish_csv.py" args="$(find detect_targets)/utils/data/walk.csv" output="screen">
<remap from="output" to="rate_input" />
</node>
<node name="rate" pkg="detect_targets" type="control_compute.py" args="rate" output="screen">
<remap from="input" to="rate_input" />
<remap from="output" to="derivative_input" />
</node>
......
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: true
blue_max: 255
blue_min: 24
green_max: 203
green_min: 91
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: true
blue_max: 255
blue_min: 24
green_max: 203
green_min: 91
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 75
red_min: 36
state: true
targets: true
type: ''
state: []
red_max: 75
red_min: 36
targets: true
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.05
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
state: true
type: ''
state: []
linear: 0.05
state: []
!!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 255
blue_min: 136
green_max: 182
green_min: 65
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 255
blue_min: 136
green_max: 182
green_min: 65
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 127
red_min: 44
state: true
targets: false
type: ''
state: []
red_max: 127
red_min: 44
targets: false
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: 1.0
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: 1.0
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: 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:
binary: false
blue_max: 92
blue_min: 30
green_max: 201
green_min: 109
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
dictitems:
binary: false
blue_max: 92
blue_min: 30
green_max: 201
green_min: 109
groups: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
id: 0
name: Default
parameters: !!python/object/new:dynamic_reconfigure.encoding.Config
state: []
parent: 0
red_max: 186
red_min: 130
state: true
targets: false
type: ''
state: []
red_max: 186
red_min: 130
targets: false
state: []
......@@ -11,12 +11,12 @@ from std_msgs.msg import Float64, Empty
import dynamic_reconfigure.server
from detect_targets.cfg import ProportionalNodeConfig, IntegralNodeConfig
from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig,
from detect_targets.cfg import SaturateNodeConfig, RateNodeConfig
from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig
from detect_targets.cfg import SaturateNodeConfig, RateNodeConfig
class ControlNode(object):
def __init__(self, refresh_time=1.0, config_class=None):
def __init__(self, refresh_time=0.05, config_class=None):
self.last_time = rospy.get_rostime()
self.refresh_time = rospy.Duration.from_sec(refresh_time)
self.output = 0.0
......@@ -59,31 +59,30 @@ class InputControlNode(ControlNode):
self.input_topics["input"] = rospy.Subscriber("input", Float64, self.on_compute)
class RateNode(InputControlNode):
def __init__(self, *args, **kwargs):
self.rate = rospy.Rate(20)
self.last_input = 0
def __init__(self, refresh_time=0.05):
self.lock = threading.Lock()
super(RateNode, self).__init__(*args, **kwargs)
super(RateNode, self).__init__(refresh_time, RateNodeConfig)
self.rate = rospy.Rate(1.0/self.refresh_time.to_sec())
def on_reconf(self, config, level):
c = super(SaturateNode, self).on_reconf(config, level)
self.rate = rospy.Rate(self.refresh_time)
c = super(RateNode, self).on_reconf(config, level)
self.rate = rospy.Rate(self.refresh_time.to_sec())
return c
def on_compute(self, value):
self.lock.acquire()
self.last_input = value.data
self.output = value.data
self.lock.release()
def on_mainloop(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.output.publish(self.last_input)
self.output_topic.publish(self.output)
self.lock.release()
self.rate.sleep()
class ProportionalNode(InputControlNode):
def __init__(self, k=1.0, refresh_time=1.0):
def __init__(self, k=1.0, refresh_time=0.05):
super(ProportionalNode, self).__init__(refresh_time, ProportionalNodeConfig)
self.k = k
......@@ -98,7 +97,7 @@ class ProportionalNode(InputControlNode):
super(ProportionalNode, self).on_compute(value)
class SaturateNode(InputControlNode):
def __init__(self, min=None, max=None, refresh_time=1.0, config=SaturateNodeConfig):
def __init__(self, min=None, max=None, refresh_time=0.05, config=SaturateNodeConfig):
super(SaturateNode, self).__init__(refresh_time, config)
self.min = min
self.max = max
......@@ -116,7 +115,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=1.0):
def __init__(self, k=1.0, min=None, max=None, refresh_time=0.05):
super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig)
self.k = k
......@@ -131,7 +130,7 @@ class IntegralNode(SaturateNode):
return super(IntegralNode, self).on_compute(value)
class DerivativeNode(InputControlNode):
def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=1.0):
def __init__(self, k=0.05, size=9, polyorder=3, deriv=1, refresh_time=0.05):
super(DerivativeNode, self).__init__(refresh_time, DerivativeNodeConfig)
self.k = k
self.filter = savgol_coeffs(size, polyorder, deriv, delta=refresh_time, use='dot')
......@@ -183,7 +182,7 @@ class DifferenciateNode(InputControlNode):
class InputNode(InputControlNode):
def __init__(self):
super(InputNode, self).__init__(refresh_time=1.0, config_class=InputNodeConfig)
super(InputNode, self).__init__(refresh_time=0.05, config_class=InputNodeConfig)
def on_reconf(self, config, level):
self.output = config["value"]
......@@ -196,7 +195,7 @@ class InputNode(InputControlNode):
class SumNode(ControlNode):
def __init__(self, nb_topics):
super(SumNode, self).__init__(refresh_time=1.0)
super(SumNode, self).__init__(refresh_time=0.05)
self.nb_topics = int(nb_topics)
self.inputs = dict()
for i in range(self.nb_topics):
......@@ -230,4 +229,4 @@ if __name__ == '__main__':
else:
node = node_class()
node.on_mailoop()
node.on_mainloop()
......@@ -11,6 +11,7 @@ if __name__ == '__main__':
rospy.init_node('publish_csv', anonymous=True)
pub = rospy.Publisher('output', Float64, queue_size=1)
file = sys.argv[1]
rospy.sleep(10.)
with open(file) as csvfile:
reader = csv.reader(csvfile)
r = rospy.Rate(20)
......
......@@ -94,7 +94,7 @@ class Triangle:
"component_centers", component_centers, self.on_comp, queue_size=1)
self.config_srv = dynamic_reconfigure.server.Server(
TriangleParamConfig, self.on_reconf)
TriangleConfig, self.on_reconf)
self.br = tf.TransformBroadcaster()
......
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:50 2019
%%CreationDate: Sun Jun 2 19:58:19 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments
......
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:49 2019
%%CreationDate: Sun Jun 2 19:58:18 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments
......
%!PS-Adobe-3.0 EPSF-3.0
%%Creator: matplotlib version 2.2.4, http://matplotlib.org/
%%CreationDate: Sun Jun 2 18:28:49 2019
%%CreationDate: Sun Jun 2 19:58:18 2019
%%Orientation: portrait
%%BoundingBox: -54 180 666 612
%%EndComments
......
This diff is collapsed.
using Plots
using DSP
using CSV
pyplot()
# Parameters
filename = joinpath(@__DIR__, "data", "walk.csv") # input file
h = 1/22 # sample time
orders = [
(title="quadratique", order=2, sizes=5:2:11),
(title="cubique", order=3, sizes=5:2:11),
(title="big_quadra", order=2, sizes=11:10:51)
]
function savgol(size::Int64, poly_order::Int64, deriv::Int64=0, delta::Float64=1.0, conv::Bool=false)
half_size, rem = divrem(size, 2)
if rem == 0
throw(ArgumentError("size must be odd."))
end
M = [-half_size:half_size;] .^ [0:poly_order;]';
y = zeros(poly_order+1)';
y[deriv+1] = factorial(deriv) / delta^deriv;
scal = y*inv(M'*M)*M'
if conv
scal = scal[end:-1:1]
end
scal
end
filter = savgol(19, 2, 1, h, true)
H = tf(filter, [1], h)
mag, phase, w = bode(H)
plot(w,log.(mag[:,1]), label="mag", title="plop")
savefig(joinpath(@__DIR__, "results", string("savgol_bode_", "plop", ".eps")))
show()
# for order in orders
# for size in order.sizes
# filter = savgol(size, order.order, 1, h, true)
# H = tf(filter, [1], 0.05)
# bode(H)
# end
# savefig(joinpath(@__DIR__, "results", string("savgol_bode_", order.title, ".eps")))
# end
# show()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment