RoboRally/remote_control/position_controller.py

# startup:
# roscore -> start ros
# rosparam set cv_camera/device_id 0 -> set appropriate camera device
# rosrun cv_camera cv_camera_node -> start the camera
# roslaunch aruco_detect aruco_detect.launch camera:=cv_camera image:=image_raw dictionary:=16 transport:= fiducial_len:=0.1 # aruco marker detection
# python fiducial_to_2d_pos_angle.py -> compute position and angle of markers in 2d plane

import sys
import rospy
import pygame
import numpy as np
import socket
import scipy.integrate

import threading
from copy import deepcopy

import matplotlib.pyplot as plt
import matplotlib.animation as anim

import time

from marker_pos_angle.msg import id_pos_angle

class Robot:
    def __init__(self, id, ip=None):
        self.pos = None
        self.orient = None

        self.id = id

        self.pos = None
        self.euler = None

        self.ip = ip

def f_ode(t, x, u):
    # dynamical model of the two-wheeled robot
    # TODO: find exact values for these parameters
    r = 0.03
    R = 0.05
    d = 0.02

    theta = x[2]

    omega_r = u[0]
    omega_l = u[1]

    dx = np.zeros(3)

    dx[0] = (r/2.0 * np.cos(theta) - r*d/(2.0*R) * np.sin(theta)) * omega_r \
          + (r/2.0 * np.cos(theta) + r*d/(2.0 * R) * np.sin(theta)) * omega_l
    dx[1] = (r/2.0 * np.sin(theta) + r*d/(2.0*R) * np.cos(theta)) * omega_r \
          + (r/2 * np.sin(theta) - r*d/(2.0*R) * np.cos(theta)) * omega_l
    dx[2] = -r/(2.0*R) * (omega_r - omega_l)

    return dx

class RemoteController:
    def __init__(self):

        self.robots = [Robot(3)]

        self.robot_ids = {}
        for r in self.robots:
            self.robot_ids[r.id] = r

        # connect to robot
        self.rc_socket = socket.socket()
        try:
            pass
            self.rc_socket.connect(('192.168.1.101', 1234))  # connect to robot
        except socket.error:
            print("could not connect to socket")

        self.t = time.time()

        # variables for simulated state
        self.x0 = None
        self.ts = np.array([])
        self.xs = []

        # variables for measurements
        self.tms_0 = None
        self.xm_0 = None
        self.tms = None
        self.xms = None

        self.mutex = threading.Lock()

        marker_sub = rospy.Subscriber("/marker_id_pos_angle", id_pos_angle, self.measurement_callback)


        # pid parameters
        self.k = 0
        self.ii = 0.1
        self.pp = 0.4

        self.inc = 0.0

        self.alphas = []

        self.speed = 1.0
        self.controlling = False

        self.u1 = 0.0
        self.u2 = 0.0

        # animation
        self.fig = plt.figure()
        self.ax = self.fig.add_subplot(1,1,1)
        self.xdata, self.ydata = [], []
        self.line, = self.ax.plot([],[])
        self.line_sim, = self.ax.plot([], [])
        self.dirm, = self.ax.plot([], [])
        self.dirs, = self.ax.plot([], [])
        plt.xlabel('x-position')
        plt.ylabel('y-position')

    def ani(self):
        self.ani = anim.FuncAnimation(self.fig, init_func=self.ani_init, func=self.ani_update, interval=10, blit=True)
        plt.ion()
        plt.show(block=True)

    def ani_init(self):
        self.ax.set_xlim(-2, 2)
        self.ax.set_ylim(-2, 2)
        self.ax.set_aspect('equal', adjustable='box')

        return self.line, self.line_sim, self.dirm, self.dirs,

    def ani_update(self, frame):
        #print("plotting")
        self.mutex.acquire()
        try:
            # copy data for plot from global arrays
            if self.tms is not None:
                tm_local = deepcopy(self.tms)
                xm_local = deepcopy(self.xms)

                if len(tm_local) > 0:
                    # plot path of the robot
                    self.line.set_data(xm_local[:,0], xm_local[:,1])

                    # compute and plot direction the robot is facing
                    a = xm_local[-1, 0]
                    b = xm_local[-1, 1]

                    a2 = a + np.cos(xm_local[-1, 2]) * 1.0
                    b2 = b + np.sin(xm_local[-1, 2]) * 1.0

                    self.dirm.set_data(np.array([a, a2]), np.array([b, b2]))

            ts_local = deepcopy(self.ts)
            xs_local = deepcopy(self.xs)

            if len(ts_local) > 0:
                # plot simulated path of the robot
                self.line_sim.set_data(xs_local[:,0], xs_local[:,1])

                # compute and plot direction the robot is facing
                a = xs_local[-1, 0]
                b = xs_local[-1, 1]

                a2 = a + np.cos(xs_local[-1, 2]) * 1.0
                b2 = b + np.sin(xs_local[-1, 2]) * 1.0

                self.dirs.set_data(np.array([a, a2]), np.array([b, b2]))
        finally:
            self.mutex.release()

        return self.line, self.line_sim, self.dirm, self.dirs,

    def measurement_callback(self, data):
        #print("data = {}".format(data))
        if data.id in self.robot_ids:
            r = self.robot_ids[data.id]

            r.pos = (data.x, data.y) # only x and y component are important for us
            r.euler = data.angle

            #print("r.pos = {}".format(r.pos))
            #print("r.angle = {}".format(r.euler))

            # save measured position and angle for plotting
            measurement = np.array([r.pos[0], r.pos[1], r.euler])
            if self.tms_0 is None:
                self.tms_0 = time.time()
                self.xm_0 = measurement

                self.mutex.acquire()
                try:
                    self.tms = np.array([0.0])
                    self.xms = measurement
                finally:
                    self.mutex.release()
            else:
                self.mutex.acquire()
                try:
                    self.tms = np.vstack((self.tms, time.time() - self.tms_0))
                    self.xms = np.vstack((self.xms, measurement))
                finally:
                    self.mutex.release()

    def controller(self):
        print("starting control")
        while True:

            keyboard_control = False
            keyboard_control_speed_test = False
            pid = True

            if keyboard_control: # keyboard controller
                events = pygame.event.get()
                speed = 0.5
                for event in events:
                    if event.type == pygame.KEYDOWN:
                        if event.key == pygame.K_LEFT:
                            self.u1 = -speed
                            self.u2 = speed
                            #print("turn left: ({},{})".format(u1, u2))
                        elif event.key == pygame.K_RIGHT:
                            self.u1 = speed
                            self.u2 = -speed
                            #print("turn right: ({},{})".format(u1, u2))
                        elif event.key == pygame.K_UP:
                            self.u1 = speed
                            self.u2 = speed
                            #print("forward: ({},{})".format(self.u1, self.u2))
                        elif event.key == pygame.K_DOWN:
                            self.u1 = -speed
                            self.u2 = -speed
                            #print("forward: ({},{})".format(u1, u2))
                        self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))
                    elif event.type == pygame.KEYUP:
                        self.u1 = 0
                        self.u2 = 0
                        #print("key released, resetting: ({},{})".format(u1, u2))
                        self.rc_socket.send('({},{})\n'.format(self.u1, self.u2))

                tnew = time.time()
                dt = tnew - self.t
                r = scipy.integrate.ode(f_ode)
                r.set_f_params(np.array([self.u1 * 13.32, self.u2 * 13.32]))

                #print(self.x0)
                if self.x0 is None:
                    if self.xm_0 is not None:
                        self.x0 = self.xm_0
                        self.xs = self.x0
                    else:
                        print("error: no measurement available to initialize simulation")
                x = self.x0
                r.set_initial_value(x, self.t)
                xnew = r.integrate(r.t + dt)

                self.t = tnew
                self.x0 = xnew

                self.mutex.acquire()
                try:
                    self.ts = np.append(self.ts, tnew)
                    self.xs = np.vstack((self.xs, xnew))
                finally:
                    self.mutex.release()


            elif keyboard_control_speed_test:
                events = pygame.event.get()
                for event in events:
                    if event.type == pygame.KEYDOWN:
                        if event.key == pygame.K_LEFT:
                            self.speed = self.speed / np.sqrt(np.sqrt(np.sqrt(10.0)))
                        elif event.key == pygame.K_RIGHT:
                            self.speed = self.speed * np.sqrt(np.sqrt(np.sqrt(10.0)))
                        elif event.key == pygame.K_UP:
                            u1 = self.speed
                            u2 = -self.speed
                        elif event.key == pygame.K_DOWN:
                            u1 = 0.0
                            u2 = 0.0
                        print("speed = {}".format(self.speed))
                        self.rc_socket.send('({},{})\n'.format(u1, u2))

            elif pid:
                # pid controller

                events = pygame.event.get()
                for event in events:
                    if event.type == pygame.KEYDOWN:
                        if event.key == pygame.K_LEFT:
                            self.ii = self.ii / np.sqrt(np.sqrt(np.sqrt(10.0)))
                            print("ii = {}".format(self.pp))
                        elif event.key == pygame.K_RIGHT:
                            self.ii = self.ii * np.sqrt(np.sqrt(np.sqrt(10.0)))
                            print("ii = {}".format(self.pp))
                        elif event.key == pygame.K_UP:
                            self.controlling = True
                        elif event.key == pygame.K_DOWN:
                            self.controlling = False
                            self.rc_socket.send('({},{})\n'.format(0, 0))

                dt = 0.05

                if self.controlling:
                    # test: turn robot such that angle is zero
                    for r in self.robots:
                        if r.euler is not None:
                            self.k = self.k + 1

                            alpha = r.euler
                            self.alphas.append(alpha)

                            # compute error
                            e = alpha - 0

                            # compute integral of error (approximately)
                            self.inc += e * dt

                            # PID
                            p = self.pp * e
                            i = self.ii * self.inc
                            d = 0.0

                            # compute controls for robot from PID
                            u1 = p + i + d
                            u2 = - p - i - d
                            print("alpha = {}, u = ({}, {})".format(alpha, u1, u2))
                            self.rc_socket.send('({},{})\n'.format(u1, u2))

                            time.sleep(dt)

def main(args):
    rospy.init_node('controller_node', anonymous=True)

    rc = RemoteController()

    pygame.init()

    screenheight = 480
    screenwidth = 640
    screen = pygame.display.set_mode([screenwidth, screenheight])

    threading.Thread(target=rc.controller).start()

    rc.ani()


if __name__ == '__main__':
    main(sys.argv)