triangle_control -> triangle

workspace/src/detect_targets/launch/bebop-triangle-control.launch

@@ -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>
 

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()

workspace/src/detect_targets/scripts/triangle_control.py

@@ -8,12 +8,12 @@ import numpy as np
 
 import roslib
 import rospy
-
+from geometry_msgs.msg import Twist
-from std_msgs.msg import Float64
 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.twist.angular.z = self.pid_angular_z(-self.Gx * self.camera_angle)
-        self.linear_z_pub.publish(data=self.z)
+        if self.angular_z_pub.get_num_connections() > 0:
-        self.linear_y_pub.publish(data=-self.alpha)
+            self.angular_z_info.target = 0
-        self.linear_x_pub.publish(data=self.d)
+            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()

workspace/src/detect_targets/scripts/triangle_control_save.py

@@ -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()