klafyvel/drone-rigide
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

triangle_control -> triangle

Browse source
This commit is contained in:
Hugo LEVY-FALK 2019-05-25 21:33:36 +02:00
parent 6f4747f2b3
commit 875e6fae24
4 changed files with 353 additions and 353 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
workspace/src/detect_targets/launch/bebop-triangle-control.launch
View file

@ -5,7 +5,7 @@
<node name="targets" pkg="detect_targets" type="target_publisher.py">
</node>
<node name="triangle" pkg="detect_targets" type="triangle_control.py" output="screen">
<node name="triangle" pkg="detect_targets" type="triangle.py" output="screen">
<remap from="component_centers" to="targets"/>
</node>

107
workspace/src/detect_targets/scripts/triangle.py Executable file
View file

@ -0,0 +1,107 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import math
import time
import numpy as np
import roslib
import rospy
from std_msgs.msg import Float64
import tf
import dynamic_reconfigure.server
from detect_targets.cfg import TriangleParamConfig
from detect_targets.msg import control
from detect_targets.msg import component_centers
class Triangle:
def on_reconf(self, config, level):
self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2
self.tan_cam = math.tan(self.camera_angle)
self.target_width = config['target_width']
self.target_depth = config['target_depth']
return config
def on_comp(self, msg):
if len(msg.data) > 2:
msg.data.sort(key=lambda component: -component.nb_vertex)
pts = msg.data[0:3]
pts.sort(key=lambda component: -component.y)
H = pts[0]
L = pts[2]
R = pts[1]
if pts[1].x < pts[2].x:
L = pts[1]
R = pts[2]
self.triangle(L, H, R)
def triangle(self, L, H, R):
now = rospy.Time.now()
t = (now - self.first_time).to_sec()
self.Gx = (L.x + H.x + R.x)*.333333
Gy = (L.y + H.y + R.y)*.333333
w = R.x - L.x
h = H.x - .5 * (R.x + L.x)
self.alpha = math.atan(h*self.target_width/(1e-5+w*self.target_depth))
ca = math.cos(self.alpha)
sa = math.sin(self.alpha)
# why *.5.... I don't know.
self.d = self.target_width*ca/(w*self.tan_cam) * .5
self.z = -Gy*self.d*self.tan_cam
self.br.sendTransform((self.d * ca, self.d * sa, self.z),
tf.transformations.quaternion_from_euler(
0, 0, self.alpha + math.pi),
now,
'drone', 'target')
self.angular_z_pub.publish(data=-self.Gx * self.camera_angle)
self.linear_z_pub.publish(data=self.z)
self.linear_y_pub.publish(data=-self.alpha)
self.linear_x_pub.publish(data=self.d)
def __init__(self):
self.Gx = 0
self.alpha = 0
self.d = 0
self.z = 0
self.camera_angle = 80*math.pi/180./2.0
self.tan_cam = math.tan(self.camera_angle)
self.target_width = 1
self.target_depth = .2
self.angular_z_pub = rospy.Publisher(
'angular_z', Float64, queue_size=1)
self.linear_z_pub = rospy.Publisher(
'linear_z', Float64, queue_size=1)
self.linear_y_pub = rospy.Publisher(
'linear_y', Float64, queue_size=1)
self.linear_x_pub = rospy.Publisher(
'linear_x', Float64, queue_size=1)
self.comp_sub = rospy.Subscriber(
"component_centers", component_centers, self.on_comp, queue_size=1)
self.config_srv = dynamic_reconfigure.server.Server(
TriangleParamConfig, self.on_reconf)
self.br = tf.TransformBroadcaster()
if __name__ == '__main__':
rospy.init_node('triangle_control', anonymous=True)
triangle = Triangle()
rospy.spin()

256
workspace/src/detect_targets/scripts/triangle_control.py Executable file → Normal file
View file

@ -8,12 +8,12 @@ import numpy as np
import roslib
import rospy
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
import tf
import dynamic_reconfigure.server
from simple_pid import PID
import dynamic_reconfigure.server
from detect_targets.cfg import TriangleParamConfig
from detect_targets.msg import control
@ -29,9 +29,109 @@ class TriangleControl:
self.target_width = config['target_width']
self.target_depth = config['target_depth']
self.target_distance = config['distance_to_target']
self.max_speed = config['max_speed']
self.sample_time = config['sample_time']
self.double_loop = config['double_loop']
#gains are reversed because of the chosen angle direction
self.pid_angular_z.Kp = - config['angular_z_Kp']
self.pid_angular_z.Ki = - config['angular_z_Ki']
self.pid_angular_z.Kd = - config['angular_z_Kd']
self.pid_angular_z.set_auto_mode(config['control_angular_z'], last_output=0.0)
self.pid_angular_z.sample_time = self.sample_time
self.pid_angular_z._integral = 0
if not config['control_angular_z']:
self.pid_angular_z._last_output = 0.0
self.pid_linear_z.Kp = config['linear_z_Kp']
self.pid_linear_z.Ki = config['linear_z_Ki']
self.pid_linear_z.Kd = config['linear_z_Kd']
self.pid_linear_z.set_auto_mode(config['control_linear_z'], last_output=0.0)
self.pid_linear_z.sample_time = self.sample_time
self.pid_linear_z._integral = 0
if not config['control_linear_z']:
self.pid_linear_z._last_output = 0.0
self.pid_linear_z.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.pid_linear_y.Kp = config['linear_y_Kp']
self.pid_linear_y.Ki = config['linear_y_Ki']
self.pid_linear_y.Kd = config['linear_y_Kd']
self.pid_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
self.pid_linear_y.sample_time = self.sample_time
self.pid_linear_y._integral = 0
self.pid_speed_linear_y.Kp = config['speed_linear_y_Kp']
self.pid_speed_linear_y.Ki = config['speed_linear_y_Ki']
self.pid_speed_linear_y.Kd = config['speed_linear_y_Kd']
self.pid_speed_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
self.pid_speed_linear_y.sample_time = self.sample_time
self.pid_speed_linear_y._integral = 0
if not config['control_linear_y']:
self.pid_linear_y._last_output = 0.0
self.pid_speed_linear_y._last_output = 0.0
self.pid_linear_y.output_limits = (
-config['max_acceleration'],
config['max_acceleration']
)
self.pid_speed_linear_y.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.speed_corrector_y = config['speed_corrector_y']
# X gains are reversed because of the chosen axis
self.pid_linear_x.Kp = - config['linear_x_Kp']
self.pid_linear_x.Ki = - config['linear_x_Ki']
self.pid_linear_x.Kd = - config['linear_x_Kd']
self.pid_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
self.pid_linear_x.sample_time = self.sample_time
self.pid_linear_x._integral = 0
self.pid_speed_linear_x.Kp = config['speed_linear_x_Kp']
self.pid_speed_linear_x.Ki = config['speed_linear_x_Ki']
self.pid_speed_linear_x.Kd = config['speed_linear_x_Kd']
self.pid_speed_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
self.pid_speed_linear_x.sample_time = self.sample_time
self.pid_speed_linear_x._integral = 0
if not config['control_linear_x']:
self.pid_linear_x._last_output = 0.0
self.pid_speed_linear_x._last_output = 0.0
self.pid_linear_x.output_limits = (
-config['max_acceleration'],
config['max_acceleration']
)
self.pid_speed_linear_x.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.speed_corrector_x = config['speed_corrector_x']
self.pid_linear_x.setpoint = self.target_distance
return config
def clear_controls(self):
self.error_angular_z.clear()
self.error_linear_z.clear()
self.error_linear_y.clear()
self.error_linear_x.clear()
def saturate_twist():
if self.twist.linear.x > self.max_speed:
self.twist.linear.x = self.max_speed
elif self.twist.linear.x < - self.max_speed:
self.twist.linear.x = - self.max_speed
if self.twist.linear.y > self.max_speed:
self.twist.linear.y = self.max_speed
elif self.twist.linear.y < - self.max_speed:
self.twist.linear.y = - self.max_speed
if self.twist.linear.z > self.max_speed:
self.twist.linear.z = self.max_speed
elif self.twist.linear.z < - self.max_speed:
self.twist.linear.z = - self.max_speed
def on_comp(self, msg):
self.twist = Twist()
if len(msg.data) > 2:
msg.data.sort(key=lambda component: -component.nb_vertex)
pts = msg.data[0:3]
@ -43,6 +143,7 @@ class TriangleControl:
L = pts[1]
R = pts[2]
self.triangle(L, H, R)
self.twist_pub.publish(self.twist)
def triangle(self, L, H, R):
now = rospy.Time.now()
@ -65,10 +166,58 @@ class TriangleControl:
now,
'drone', 'target')
self.angular_z_pub.publish(data=-self.Gx * self.camera_angle)
self.linear_z_pub.publish(data=self.z)
self.linear_y_pub.publish(data=-self.alpha)
self.linear_x_pub.publish(data=self.d)
self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
if self.angular_z_pub.get_num_connections() > 0:
self.angular_z_info.target = 0
self.angular_z_info.error = 0
self.angular_z_info.derror = 0
self.angular_z_info.cmd_vel = self.twist.angular.z
self.angular_z_pub.publish(self.angular_z_info)
self.twist.linear.z = self.pid_linear_z(self.z)
if self.linear_z_pub.get_num_connections() > 0:
self.linear_z_info.target = 0
self.linear_z_info.error = self.z
self.linear_z_info.derror = 0
self.linear_z_info.cmd_vel = self.twist.linear.z
self.linear_z_pub.publish(self.linear_z_info)
dt = time.time() - self.pid_linear_y._last_time
self.last_values_y = np.concatenate((self.last_values_y[1:7], [self.alpha]))
target_acceleration_y = self.pid_linear_y(-self.alpha)
self.pid_speed_linear_y.setpoint = target_acceleration_y
speed_y = self.last_values_y.dot(self.savgol_filter) / self.sample_time
if self.double_loop:
self.twist.linear.y = self.pid_speed_linear_y(speed_y)
else:
self.pid_speed_linear_y(speed_y)
self.twist.linear.y = target_acceleration_y
if self.linear_y_pub.get_num_connections() > 0:
self.linear_y_info.target = 0
self.linear_y_info.error = -self.alpha
self.linear_y_info.derror = 0
self.linear_y_info.cmd_vel = self.twist.linear.y
self.linear_y_pub.publish(self.linear_y_info)
dt = time.time() - self.pid_linear_x._last_time
self.last_values_x = np.concatenate((self.last_values_x[1:7], [self.d]))
target_acceleration_x = self.pid_linear_x(self.d)
speed_x = 0
self.pid_speed_linear_x.setpoint = target_acceleration_x
speed_x = self.last_values_x.dot(self.savgol_filter) / self.sample_time
if self.double_loop:
self.twist.linear.x = self.pid_speed_linear_x(speed_x)
else:
self.pid_speed_linear_x(speed_x)
self.twist.linear.x = target_acceleration_x
if self.linear_x_pub.get_num_connections() > 0:
self.linear_x_info.target = self.pid_linear_x.setpoint
self.linear_x_info.error = self.target_distance - self.d
self.linear_x_info.derror = speed_x
self.linear_x_info.cmd_vel = self.twist.linear.x
self.linear_x_pub.publish(self.linear_x_info)
def __init__(self):
@ -82,15 +231,97 @@ class TriangleControl:
self.tan_cam = math.tan(self.camera_angle)
self.target_width = 1
self.target_depth = .2
self.target_distance = 2
self.max_speed = .3
self.last_time_angular_z = 0
self.last_time_linear_z = 0
self.last_time_linear_y = 0
self.last_time_linear_x = 0
self.first_time = rospy.Time.now()
self.sample_time = 0.20
self.double_loop = True
self.pid_angular_z = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_z = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_y = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_speed_linear_y = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_x = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time,
setpoint=self.target_distance,
)
self.pid_speed_linear_x = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.savgol_filter = 1.0/28 * np.array([
[-3],
[-2],
[-1],
[0],
[1],
[2],
[3]
], dtype=np.float64)
self.last_values_x = np.zeros(7, dtype=np.float64)
self.last_values_y = np.zeros(7, dtype=np.float64)
self.speed_corrector_x = 30
self.speed_corrector_y = 30
# Control info
self.angular_z_info = control()
self.linear_x_info = control()
self.linear_y_info = control()
self.linear_z_info = control()
# ROS stuff
self.twist = Twist()
self.twist_pub = rospy.Publisher(
'cmd_vel', Twist, queue_size=1)
self.angular_z_pub = rospy.Publisher(
'angular_z', Float64, queue_size=1)
'angular_z_control', control, queue_size=1)
self.linear_z_pub = rospy.Publisher(
'linear_z', Float64, queue_size=1)
'linear_z_control', control, queue_size=1)
self.linear_y_pub = rospy.Publisher(
'linear_y', Float64, queue_size=1)
'linear_y_control', control, queue_size=1)
self.linear_x_pub = rospy.Publisher(
'linear_x', Float64, queue_size=1)
'linear_x_control', control, queue_size=1)
self.comp_sub = rospy.Subscriber(
"component_centers", component_centers, self.on_comp, queue_size=1)
@ -101,7 +332,10 @@ class TriangleControl:
if __name__ == '__main__':
print "running"
rospy.init_node('triangle_control', anonymous=True)
print "node created"
triangle = TriangleControl()
rospy.spin()

341
workspace/src/detect_targets/scripts/triangle_control_save.py
View file

@ -1,341 +0,0 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
import math
import time
import numpy as np
import roslib
import rospy
from geometry_msgs.msg import Twist
import tf
from simple_pid import PID
import dynamic_reconfigure.server
from detect_targets.cfg import TriangleParamConfig
from detect_targets.msg import control
from detect_targets.msg import component_centers
class TriangleControl:
def on_reconf(self, config, level):
self.camera_angle = config['camera_angle']*math.pi/360.0 # theta/2
self.tan_cam = math.tan(self.camera_angle)
self.target_width = config['target_width']
self.target_depth = config['target_depth']
self.target_distance = config['distance_to_target']
self.max_speed = config['max_speed']
self.sample_time = config['sample_time']
self.double_loop = config['double_loop']
#gains are reversed because of the chosen angle direction
self.pid_angular_z.Kp = - config['angular_z_Kp']
self.pid_angular_z.Ki = - config['angular_z_Ki']
self.pid_angular_z.Kd = - config['angular_z_Kd']
self.pid_angular_z.set_auto_mode(config['control_angular_z'], last_output=0.0)
self.pid_angular_z.sample_time = self.sample_time
self.pid_angular_z._integral = 0
if not config['control_angular_z']:
self.pid_angular_z._last_output = 0.0
self.pid_linear_z.Kp = config['linear_z_Kp']
self.pid_linear_z.Ki = config['linear_z_Ki']
self.pid_linear_z.Kd = config['linear_z_Kd']
self.pid_linear_z.set_auto_mode(config['control_linear_z'], last_output=0.0)
self.pid_linear_z.sample_time = self.sample_time
self.pid_linear_z._integral = 0
if not config['control_linear_z']:
self.pid_linear_z._last_output = 0.0
self.pid_linear_z.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.pid_linear_y.Kp = config['linear_y_Kp']
self.pid_linear_y.Ki = config['linear_y_Ki']
self.pid_linear_y.Kd = config['linear_y_Kd']
self.pid_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
self.pid_linear_y.sample_time = self.sample_time
self.pid_linear_y._integral = 0
self.pid_speed_linear_y.Kp = config['speed_linear_y_Kp']
self.pid_speed_linear_y.Ki = config['speed_linear_y_Ki']
self.pid_speed_linear_y.Kd = config['speed_linear_y_Kd']
self.pid_speed_linear_y.set_auto_mode(config['control_linear_y'], last_output=0.0)
self.pid_speed_linear_y.sample_time = self.sample_time
self.pid_speed_linear_y._integral = 0
if not config['control_linear_y']:
self.pid_linear_y._last_output = 0.0
self.pid_speed_linear_y._last_output = 0.0
self.pid_linear_y.output_limits = (
-config['max_acceleration'],
config['max_acceleration']
)
self.pid_speed_linear_y.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.speed_corrector_y = config['speed_corrector_y']
# X gains are reversed because of the chosen axis
self.pid_linear_x.Kp = - config['linear_x_Kp']
self.pid_linear_x.Ki = - config['linear_x_Ki']
self.pid_linear_x.Kd = - config['linear_x_Kd']
self.pid_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
self.pid_linear_x.sample_time = self.sample_time
self.pid_linear_x._integral = 0
self.pid_speed_linear_x.Kp = config['speed_linear_x_Kp']
self.pid_speed_linear_x.Ki = config['speed_linear_x_Ki']
self.pid_speed_linear_x.Kd = config['speed_linear_x_Kd']
self.pid_speed_linear_x.set_auto_mode(config['control_linear_x'], last_output=0.0)
self.pid_speed_linear_x.sample_time = self.sample_time
self.pid_speed_linear_x._integral = 0
if not config['control_linear_x']:
self.pid_linear_x._last_output = 0.0
self.pid_speed_linear_x._last_output = 0.0
self.pid_linear_x.output_limits = (
-config['max_acceleration'],
config['max_acceleration']
)
self.pid_speed_linear_x.output_limits = (
-config['max_speed'],
config['max_speed']
)
self.speed_corrector_x = config['speed_corrector_x']
self.pid_linear_x.setpoint = self.target_distance
return config
def clear_controls(self):
self.error_angular_z.clear()
self.error_linear_z.clear()
self.error_linear_y.clear()
self.error_linear_x.clear()
def saturate_twist():
if self.twist.linear.x > self.max_speed:
self.twist.linear.x = self.max_speed
elif self.twist.linear.x < - self.max_speed:
self.twist.linear.x = - self.max_speed
if self.twist.linear.y > self.max_speed:
self.twist.linear.y = self.max_speed
elif self.twist.linear.y < - self.max_speed:
self.twist.linear.y = - self.max_speed
if self.twist.linear.z > self.max_speed:
self.twist.linear.z = self.max_speed
elif self.twist.linear.z < - self.max_speed:
self.twist.linear.z = - self.max_speed
def on_comp(self, msg):
self.twist = Twist()
if len(msg.data) > 2:
msg.data.sort(key=lambda component: -component.nb_vertex)
pts = msg.data[0:3]
pts.sort(key=lambda component: -component.y)
H = pts[0]
L = pts[2]
R = pts[1]
if pts[1].x < pts[2].x:
L = pts[1]
R = pts[2]
self.triangle(L, H, R)
self.twist_pub.publish(self.twist)
def triangle(self, L, H, R):
now = rospy.Time.now()
t = (now - self.first_time).to_sec()
self.Gx = (L.x + H.x + R.x)*.333333
Gy = (L.y + H.y + R.y)*.333333
w = R.x - L.x
h = H.x - .5 * (R.x + L.x)
self.alpha = math.atan(h*self.target_width/(1e-5+w*self.target_depth))
ca = math.cos(self.alpha)
sa = math.sin(self.alpha)
# why *.5.... I don't know.
self.d = self.target_width*ca/(w*self.tan_cam) * .5
self.z = -Gy*self.d*self.tan_cam
self.br.sendTransform((self.d * ca, self.d * sa, self.z),
tf.transformations.quaternion_from_euler(
0, 0, self.alpha + math.pi),
now,
'drone', 'target')
self.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
if self.angular_z_pub.get_num_connections() > 0:
self.angular_z_info.target = 0
self.angular_z_info.error = 0
self.angular_z_info.derror = 0
self.angular_z_info.cmd_vel = self.twist.angular.z
self.angular_z_pub.publish(self.angular_z_info)
self.twist.linear.z = self.pid_linear_z(self.z)
if self.linear_z_pub.get_num_connections() > 0:
self.linear_z_info.target = 0
self.linear_z_info.error = self.z
self.linear_z_info.derror = 0
self.linear_z_info.cmd_vel = self.twist.linear.z
self.linear_z_pub.publish(self.linear_z_info)
dt = time.time() - self.pid_linear_y._last_time
self.last_values_y = np.concatenate((self.last_values_y[1:7], [self.alpha]))
target_acceleration_y = self.pid_linear_y(-self.alpha)
self.pid_speed_linear_y.setpoint = target_acceleration_y
speed_y = self.last_values_y.dot(self.savgol_filter) / self.sample_time
if self.double_loop:
self.twist.linear.y = self.pid_speed_linear_y(speed_y)
else:
self.pid_speed_linear_y(speed_y)
self.twist.linear.y = target_acceleration_y
if self.linear_y_pub.get_num_connections() > 0:
self.linear_y_info.target = 0
self.linear_y_info.error = -self.alpha
self.linear_y_info.derror = 0
self.linear_y_info.cmd_vel = self.twist.linear.y
self.linear_y_pub.publish(self.linear_y_info)
dt = time.time() - self.pid_linear_x._last_time
self.last_values_x = np.concatenate((self.last_values_x[1:7], [self.d]))
target_acceleration_x = self.pid_linear_x(self.d)
speed_x = 0
self.pid_speed_linear_x.setpoint = target_acceleration_x
speed_x = self.last_values_x.dot(self.savgol_filter) / self.sample_time
if self.double_loop:
self.twist.linear.x = self.pid_speed_linear_x(speed_x)
else:
self.pid_speed_linear_x(speed_x)
self.twist.linear.x = target_acceleration_x
if self.linear_x_pub.get_num_connections() > 0:
self.linear_x_info.target = self.pid_linear_x.setpoint
self.linear_x_info.error = self.target_distance - self.d
self.linear_x_info.derror = speed_x
self.linear_x_info.cmd_vel = self.twist.linear.x
self.linear_x_pub.publish(self.linear_x_info)
def __init__(self):
self.Gx = 0
self.alpha = 0
self.d = 0
self.z = 0
self.camera_angle = 80*math.pi/180./2.0
self.tan_cam = math.tan(self.camera_angle)
self.target_width = 1
self.target_depth = .2
self.target_distance = 2
self.max_speed = .3
self.last_time_angular_z = 0
self.last_time_linear_z = 0
self.last_time_linear_y = 0
self.last_time_linear_x = 0
self.first_time = rospy.Time.now()
self.sample_time = 0.20
self.double_loop = True
self.pid_angular_z = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_z = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_y = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_speed_linear_y = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.pid_linear_x = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time,
setpoint=self.target_distance,
)
self.pid_speed_linear_x = PID(
1,
0,
0,
auto_mode=True,
sample_time=self.sample_time
)
self.savgol_filter = 1.0/28 * np.array([
[-3],
[-2],
[-1],
[0],
[1],
[2],
[3]
], dtype=np.float64)
self.last_values_x = np.zeros(7, dtype=np.float64)
self.last_values_y = np.zeros(7, dtype=np.float64)
self.speed_corrector_x = 30
self.speed_corrector_y = 30
# Control info
self.angular_z_info = control()
self.linear_x_info = control()
self.linear_y_info = control()
self.linear_z_info = control()
# ROS stuff
self.twist = Twist()
self.twist_pub = rospy.Publisher(
'cmd_vel', Twist, queue_size=1)
self.angular_z_pub = rospy.Publisher(
'angular_z_control', control, queue_size=1)
self.linear_z_pub = rospy.Publisher(
'linear_z_control', control, queue_size=1)
self.linear_y_pub = rospy.Publisher(
'linear_y_control', control, queue_size=1)
self.linear_x_pub = rospy.Publisher(
'linear_x_control', control, queue_size=1)
self.comp_sub = rospy.Subscriber(
"component_centers", component_centers, self.on_comp, queue_size=1)
self.config_srv = dynamic_reconfigure.server.Server(
TriangleParamConfig, self.on_reconf)
self.br = tf.TransformBroadcaster()
if __name__ == '__main__':
print "running"
rospy.init_node('triangle_control', anonymous=True)
print "node created"
triangle = TriangleControl()
rospy.spin()