drone-rigide/scripts/triangle_control.py

#! /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 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()