diff --git a/workspace/src/detect_targets/launch/bebop-triangle-control.launch b/workspace/src/detect_targets/launch/bebop-triangle-control.launch
index 6f7817a..4597596 100644
--- a/workspace/src/detect_targets/launch/bebop-triangle-control.launch
+++ b/workspace/src/detect_targets/launch/bebop-triangle-control.launch
@@ -5,7 +5,7 @@
-
+
diff --git a/workspace/src/detect_targets/scripts/triangle.py b/workspace/src/detect_targets/scripts/triangle.py
new file mode 100755
index 0000000..7132391
--- /dev/null
+++ b/workspace/src/detect_targets/scripts/triangle.py
@@ -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()
diff --git a/workspace/src/detect_targets/scripts/triangle_control.py b/workspace/src/detect_targets/scripts/triangle_control.py
old mode 100755
new mode 100644
index f034241..8fcda00
--- a/workspace/src/detect_targets/scripts/triangle_control.py
+++ b/workspace/src/detect_targets/scripts/triangle_control.py
@@ -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()
diff --git a/workspace/src/detect_targets/scripts/triangle_control_save.py b/workspace/src/detect_targets/scripts/triangle_control_save.py
deleted file mode 100644
index 8fcda00..0000000
--- a/workspace/src/detect_targets/scripts/triangle_control_save.py
+++ /dev/null
@@ -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()