203 lines
6.9 KiB
Python
203 lines
6.9 KiB
Python
|
#! /usr/bin/python
|
||
|
|
|
||
|
|
from scipy.signal import savgol_coeffs
|
||
|
|
import numpy as np
|
||
|
|
import sys
|
||
|
|
|
||
|
|
import rospy
|
||
|
|
|
||
|
|
from std_msgs.msg import Float64, Bool
|
||
|
|
|
||
|
|
import dynamic_reconfigure.server
|
||
|
|
from detect_targets.cfg import ProportionalNodeConfig, IntegralNodeConfig
|
||
|
|
from detect_targets.cfg import DerivativeNodeConfig, InputNodeConfig, SaturateNodeConfig
|
||
|
|
|
||
|
|
|
||
|
|
class ControlNode(object):
|
||
|
|
def __init__(self, refresh_time=1.0, config_class=None):
|
||
|
|
self.last_time = rospy.get_rostime()
|
||
|
|
self.refresh_time = rospy.Duration.from_sec(refresh_time)
|
||
|
|
self.output = 0.0
|
||
|
|
|
||
|
|
self.output_topic = rospy.Publisher('output', Float64, queue_size=1)
|
||
|
|
|
||
|
|
self.input_topics = {
|
||
|
|
"reset": rospy.Subscriber("reset", Bool, self.on_reset),
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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_reconf(self, config, level):
|
||
|
|
self.refresh_time = rospy.Duration.from_sec(config["refresh_time"])
|
||
|
|
return config
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
self.output_topic.publish(data=self.output)
|
||
|
|
self.last_time = rospy.get_rostime()
|
||
|
|
|
||
|
|
def check_time(self, delta_time=0.0):
|
||
|
|
t = rospy.get_rostime()
|
||
|
|
if delta_time == 0.0:
|
||
|
|
delta_time = t - self.last_time
|
||
|
|
|
||
|
|
return delta_time >= self.refresh_time, delta_time.to_sec()
|
||
|
|
|
||
|
|
class InputControlNode(ControlNode):
|
||
|
|
def __init__(self, *args, **kwargs):
|
||
|
|
super(InputControlNode, self).__init__(*args, **kwargs)
|
||
|
|
self.input_topics["input"] = rospy.Subscriber("input", Float64, self.on_compute)
|
||
|
|
|
||
|
|
class ProportionalNode(InputControlNode):
|
||
|
|
def __init__(self, k=1.0, refresh_time=1.0):
|
||
|
|
super(ProportionalNode, self).__init__(refresh_time, ProportionalNodeConfig)
|
||
|
|
self.k = k
|
||
|
|
|
||
|
|
def on_reconf(self, config, level):
|
||
|
|
self.k = config["k"]
|
||
|
|
return super(ProportionalNode, self).on_reconf(config, level)
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
time_ok,_ = self.check_time()
|
||
|
|
if time_ok:
|
||
|
|
self.output = self.k * value.data
|
||
|
|
super(ProportionalNode, self).on_compute(value)
|
||
|
|
|
||
|
|
class SaturateNode(InputControlNode):
|
||
|
|
def __init__(self, min=None, max=None, refresh_time=1.0, config=SaturateNodeConfig):
|
||
|
|
super(SaturateNode, self).__init__(refresh_time, config)
|
||
|
|
self.min = min
|
||
|
|
self.max = max
|
||
|
|
|
||
|
|
def on_reconf(self, config, level):
|
||
|
|
self.min = config["min"] if config["use_min"] else None
|
||
|
|
self.max = config["max"] if config["use_max"] else None
|
||
|
|
return super(SaturateNode, self).on_reconf(config, level)
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
time_ok,_ = self.check_time()
|
||
|
|
if time_ok:
|
||
|
|
self.output = min(self.max or value.data, value.data)
|
||
|
|
self.output = max(self.min or self.output, self.output)
|
||
|
|
super(SaturateNode, self).on_compute(value)
|
||
|
|
|
||
|
|
class IntegralNode(SaturateNode):
|
||
|
|
def __init__(self, k=1.0, min=None, max=None, refresh_time=1.0):
|
||
|
|
super(IntegralNode, self).__init__(min, max, refresh_time, IntegralNodeConfig)
|
||
|
|
self.k = k
|
||
|
|
|
||
|
|
def on_reconf(self, config, level):
|
||
|
|
self.k = config["k"]
|
||
|
|
return super(IntegralNode, self).on_reconf(config, level)
|
||
|
|
|
||
|
|
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)
|
||
|
|
|
||
|
|
class DerivativeNode(InputControlNode):
|
||
|
|
def __init__(self, k=1.0, size=3, polyorder=1, deriv=1, refresh_time=1.0):
|
||
|
|
super(DerivativeNode, self).__init__(refresh_time, DerivativeNodeConfig)
|
||
|
|
self.k = k
|
||
|
|
self.filter = savgol_coeffs(size, polyorder, deriv, delta=refresh_time, use='dot')
|
||
|
|
|
||
|
|
def on_reconf(self, config, level):
|
||
|
|
self.k = config["k"]
|
||
|
|
try:
|
||
|
|
self.filter = savgol_coeffs(
|
||
|
|
config["size"],
|
||
|
|
config["poly_order"],
|
||
|
|
config["deriv"],
|
||
|
|
delta=config["refresh_time"],
|
||
|
|
use='dot'
|
||
|
|
)
|
||
|
|
except ValueError as e:
|
||
|
|
rospy.logerr(e)
|
||
|
|
self.last_points = np.zeros(config["size"])
|
||
|
|
return super(DerivativeNode, self).on_reconf(config, level)
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
time_ok, delta = self.check_time()
|
||
|
|
if time_ok:
|
||
|
|
np.concatenate([self.last_points[1:], [value.data]])
|
||
|
|
self.output = 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)
|
||
|
|
|
||
|
|
class DifferenciateNode(InputControlNode):
|
||
|
|
def __init__(self):
|
||
|
|
self.measure = 0.0
|
||
|
|
self.input = 0.0
|
||
|
|
super(DifferenciateNode, self).__init__()
|
||
|
|
self.input_topics["measure"] = rospy.Subscriber("measure", Float64, self.on_compute_measure)
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
self.input = value.data
|
||
|
|
self.output = self.input - self.measure
|
||
|
|
super(DifferenciateNode, self).on_compute(value)
|
||
|
|
|
||
|
|
def on_compute_measure(self, value):
|
||
|
|
self.measure = value.data
|
||
|
|
self.output = self.input - self.measure
|
||
|
|
super(DifferenciateNode, self).on_compute(value)
|
||
|
|
|
||
|
|
class InputNode(InputControlNode):
|
||
|
|
def __init__(self):
|
||
|
|
super(InputNode, self).__init__(refresh_time=1.0, config_class=InputNodeConfig)
|
||
|
|
|
||
|
|
def on_reconf(self, config, level):
|
||
|
|
self.output = config["value"]
|
||
|
|
super(InputNode, self).on_compute(config["value"])
|
||
|
|
return config
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
self.output = value.data
|
||
|
|
super(InputNode, self).on_compute(value)
|
||
|
|
|
||
|
|
class SumNode(ControlNode):
|
||
|
|
def __init__(self, nb_topics):
|
||
|
|
super(SumNode, self).__init__(refresh_time=1.0)
|
||
|
|
self.nb_topics = int(nb_topics)
|
||
|
|
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))
|
||
|
|
|
||
|
|
def on_compute(self, value):
|
||
|
|
self.output = sum(self.inputs.values())
|
||
|
|
super(SumNode, self).on_compute(value)
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
nodes = {
|
||
|
|
'sum' : (SumNode, True),
|
||
|
|
'differenciate' : (DifferenciateNode, False),
|
||
|
|
'input' : (InputNode, False),
|
||
|
|
'saturate' : (SaturateNode, False),
|
||
|
|
'derivative' : (DerivativeNode, False),
|
||
|
|
'integral' : (IntegralNode, False),
|
||
|
|
'proportional' : (ProportionalNode, False),
|
||
|
|
}
|
||
|
|
rospy.init_node('control_node', anonymous=True)
|
||
|
|
try:
|
||
|
|
node_class, need_param = nodes[sys.argv[1]]
|
||
|
|
except KeyError as e:
|
||
|
|
raise ValueError(sys.argv[1] + " is not a valid node name.")
|
||
|
|
|
||
|
|
if need_param:
|
||
|
|
param = sys.argv[2]
|
||
|
|
node = node_class(param)
|
||
|
|
else:
|
||
|
|
node = node_class()
|
||
|
|
|
||
|
|
rospy.spin()
|