test

gistfile1.txt

import numpy as np
import matplotlib.patches as patches


class BSpline():
    def __init__(self):
        pass
        self.warp = 4.
        self.count = 26*2


    def process(self, Px, Py, Pw, ax, MODE=1, draw=True):
        self.draw = draw
        self.mode = MODE # Can be 0 or 1 for terminal tangents
        self.ax = ax
        self.Px = Px
        self.Py = Py
        self.Pw = Pw

        count = self.count
        if self.mode == 0:
            self.n = len(Px) - 2
        else:
            self.n = len(Px) - 2
        self.n1 = self.n+1;
        self.B0 = [None]*count
        self.B1 = [None]*count
        self.B2 = [None]*count
        self.B3 = [None]*count
        self.dx = [None]*self.n
        self.dy = [None]*self.n

        t = 0.;
        self.ts = []
        for i in range(0, count):
            t1 = 1-t
            t12 = t1*t1
            t2 = t*t
            self.B0[i] = t1*t12
            self.B1[i] = 3*t*t12
            self.B2[i] = 3*t2*t1
            self.B3[i] = t*t2
            self.ts.append(t)
            t += 1.00/count

        return self.drawSpline()

    def findCPoints(self):
        if self.mode == 0:
            self.dx[0] = self.Px[self.n] - self.Px[0];  
            self.dy[0] = self.Py[self.n] - self.Py[0];
            self.dx[self.n-1] = -(self.Px[self.n1] - self.Px[self.n-1])
            self.dy[self.n-1] = -(self.Py[self.n1] - self.Py[self.n - 1])
        else:
            DIV = 3.
            self.dx[0] = (self.Px[1] - self.Px[0])/DIV;  
            self.dy[0] = (self.Py[1] - self.Py[0])/DIV;
            self.dx[self.n-1] = (self.Px[self.n-1] - self.Px[self.n-2])/DIV;  
            self.dy[self.n-1] = (self.Py[self.n-1] - self.Py[self.n-2])/DIV;

        #self.warp += 0.05

        warps = self.Pw

        Ax = [None]*self.n
        Ay = [None]*self.n
        Bi = [None]*self.n
        Bi[1] = -1/warps[1]
        Ax[1] = -(self.Px[2] - self.Px[0] - self.dx[0])*Bi[1]
        Ay[1] = -(self.Py[2] - self.Py[0] - self.dy[0])*Bi[1];
        for i in range(2, self.n-1):
            warpval = warps[i]
            Bi[i] = -1/(warpval+ Bi[i-1]);
            Ax[i] = -(self.Px[i+1] - self.Px[i-1] - Ax[i-1])*Bi[i];
            Ay[i] = -(self.Py[i+1] - self.Py[i-1] - Ay[i-1])*Bi[i];
        for i in range(self.n-2, 0, -1):
            self.dx[i] = Ax[i] + self.dx[i+1]*Bi[i];  
            self.dy[i] = Ay[i] + self.dy[i+1]*Bi[i];

        #self.dx[1] = 0
        #self.dy[0] = 0
        #self.dy[1] = 0
        # for i in range(0, len(self.dx)):
        #     self.dx[i] = 0
        #     self.dy[i] = 0
        # print self.dx
        # print self.dy


    def drawSpline(self):
        self.findCPoints()

        w = 1.
        h = 1.
        d = 0.
        h1 = h
        d2 = d
        step = 1./w
        t = step;
        scPx = [None]*self.n
        scPy = [None]*self.n
        scDx = [None]*self.n
        scDy = [None]*self.n
        X = None
        Y = None

        squaresize = 0.3
        for i in range(0, self.n):
           X = scPx[i] = self.Px[i]*w
           Y = scPy[i] = self.Py[i]*h; 
           scDx[i] = self.dx[i]*w;   
           scDy[i] = self.dy[i]*h;
           if self.draw:
               rect = patches.Rectangle((X - squaresize/2.,Y - squaresize/2.),squaresize,squaresize,linewidth=1,edgecolor='r',facecolor='none')
               self.ax.add_patch(rect)

        #if self.mode == 0:
        if self.draw:
            self.ax.add_patch(patches.Rectangle((scPx[0]+scDx[0] - squaresize/2., (scPy[0]+scDy[0])- squaresize/2.),squaresize,squaresize,
                linewidth=1,edgecolor='b',facecolor='none'))
            self.ax.add_patch(patches.Rectangle((scPx[self.n-1]-scDx[self.n-1] - squaresize/2., (scPy[self.n-1]-scDy[self.n-1])- squaresize/2.),squaresize,squaresize,
                linewidth=1,edgecolor='b',facecolor='none'))

        # if self.n > 1:
        #     paths = [[scPx[0], scPy[0]]]
        #     for i in range(1, self.n):
        #         paths.append([scPx[i-1]+scDx[i-1], scPy[i-1]-scDy[i-1]])
        #         paths.append([scPx[i]-scDx[i], scPy[i]+scDy[i]])
        #         paths.append([scPx[i], scPy[i]])
        #     paths_arr = np.array(paths)
        #     plt.plot(paths_arr[:, 0], paths_arr[:, 1])
        #     #for path in paths:

        # paths = [[scPx[0], scPy[0]]]
        # for i in range(0, self.n-1):
        #     for k in range(0, self.count):
        #         X = (scPx[i]*self.B0[k] + (scPx[i] + scDx[i])*self.B1[k] +
        #             (scPx[i+1] - scDx[i+1])*self.B2[k] + scPx[i+1]*self.B3[k])
        #         Y = (scPy[i]*self.B0[k] + (scPy[i] + scDy[i])*self.B1[k] +
        #             (scPy[i+1] - scDy[i+1])*self.B2[k] + scPy[i+1]*self.B3[k]);
        #         paths.append([X,Y])
        # paths_arr = np.array(paths)
        # plt.scatter(paths_arr[:, 0], paths_arr[:, 1], s=0.2)



        paths = [[scPx[0], scPy[0]]]
        for i in range(0, self.n-1):

            dist = np.sqrt((self.Px[i]-self.Px[i+1])**2 + (self.Py[i]-self.Py[i+1])**2)
            count = int(dist/0.01)
            # i want a point every 1cm

            for t in np.linspace(0., 1.-(1./count), count):
                t1 = 1-t
                t12 = t1*t1
                t2 = t*t
                B0 = t1*t12
                B1 = 3*t*t12
                B2 = 3*t2*t1
                B3 = t*t2
                X = (scPx[i]*B0 + (scPx[i] + scDx[i])*B1 +
                    (scPx[i+1] - scDx[i+1])*B2 + scPx[i+1]*B3)
                Y = (scPy[i]*B0 + (scPy[i] + scDy[i])*B1 +
                    (scPy[i+1] - scDy[i+1])*B2 + scPy[i+1]*B3);
                paths.append([X,Y])


        paths_arr = np.array(paths)
        if self.draw:
            plt.scatter(paths_arr[:, 0], paths_arr[:, 1], s=0.2)


        return paths_arr

# cv = np.array(
# [[ 2.70967742,  1.30411255,  4.        ], # D
#  [ 2.45564516,  6.28246753,  4.        ], # C
#  [-0.90322581,  6.39069264,  4.        ],
#  [-1.04435484,  1.27705628,  4.        ],
#  [-5.75806452,  1.41233766,  4.        ], # B
#  [-5.95564516,  6.14718615,  4.        ], # A
#  [ 2.51209677,  3.79329004,  4.        ], # T: Should be center of CD
#  [-6.06854839,  4.11796537,  4.        ]] # T: Should be center of AB
#     )


# Case that doesnt make sense. A point is completly not imaged cos the 
cv = np.array(
[[ 3.21774194,  7.04004329,  4.,        ],
 [ 1.75,        7.17532468,  4.,        ],
 [ 1.07258065,  3.27922078,  4.,        ],
 [-0.56451613,  3.38528139,  4.,        ],
 [-1.60887097,  6.85064935,  4.,        ],
 [-2.87903226,  6.7965368,   4.,        ],
 [-3.86693548,  3.2521645,   4.,        ],
 [ 6.29435484,  5.76623377,  4.,        ],
 [-6.06854839,  6.11796537,  4.,        ]]
)
cv[:,2] = 5

# cv = np.array([
#     [2.33076, 5.08771, 100000000],
#     [1.18207, 5.43119, 100000000],
#     [0.325818, 4.84136, 100000000],
#     [0.269373, 6.14113, 100000000],
#     [-2.61584, 5.66069, 100000000],
#     [0,0,0],
#     [0,0,0]
# ])

# # Alternating points
# cv = np.array(
# [[  3.38709677,   4.57792208, 100.        ],
#  [  1.18207,      5.43119,    100.        ],
#  [ -1.10080645,   4.17207792,  24.        ],
#  [ -3.0766129,    5.11904762,  18.        ],
#  [ -5.44758065,   4.09090909, 100.        ],
#  [  0.,           0.,         100.        ],
#  [  0.,           0.,         100.        ]]
#     )

# # Exceed bound
# cv = np.array(
# [[ 3.38709677,  4.57792208,  5.        ],
#  [ 0.56451613,  2.38636364,  2.5       ],
#  [-0.79032258,  7.41883117,  2.5       ],
#  [-2.90725806,  5.38961039,  4.5       ],
#  [-5.44758065,  4.09090909,  5.        ],
#  [ 0.,          0.,          5.        ],
#  [ 0.,          0.,          5.        ]]
#     )
# cv[:,2] = 100

# A case to prove my interpolation helps?
cv = np.array(
[[  3.38709677,   4.57792208, 200.        ],
 [  1.3266129,    7.04004329,   1.5       ],
 [ -1.15725806,   6.2012987,   24.5       ],
 [ -2.625,        5.30844156, 200.        ],
 [  0.,           0.,         200.        ],
 [  0.,           0.,         200.        ]]
    )
cv[:,2] = 5

# To test optimization to remove two peaks?
cv = np.array(
[[ 3.38709677,  4.57792208,  4.6       ],
 [ 1.01612903,  7.71645022,  4.6       ],
 [-1.3266129,   7.01298701,  4.6       ],
 [-2.625,       5.30844156,  4.6       ],
 [ 0.,          0.,          4.6       ],
 [ 0.,          0.,          4.6       ]]
    )

released = False

#cv[:,2] = 100

class Click():
    def __init__(self, ax, button=1):
        self.ax=ax
        self.button=button
        self.press=False
        self.move = False
        self.c1=self.ax.figure.canvas.mpl_connect('button_press_event', self.onpress)
        self.c2=self.ax.figure.canvas.mpl_connect('button_release_event', self.onrelease)
        self.c3=self.ax.figure.canvas.mpl_connect('motion_notify_event', self.onmove)
        self.c4=self.ax.figure.canvas.mpl_connect('scroll_event',self.onzoom)

    def onzoom(self, event):
        print("zoom")
        global cv
        lowest_dist = 1000
        lowest_index = -1
        for i in range(0, cv.shape[0]):
            dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
            if dist < lowest_dist:
                lowest_dist = dist
                lowest_index = i
        if lowest_dist < 2:
            if event.button == "up":
                cv[lowest_index, 2] += 0.5
            elif event.button == "down":
                if cv[lowest_index, 2] > 1.5:
                    cv[lowest_index, 2] -= 0.5
            # if event.button == "up":
            #     cv[:, 2] += 0.5
            # elif event.button == "down":
            #     cv[:, 2] -= 0.5

    def onclick(self,event):
        if event.inaxes == self.ax:
            global cv
            lowest_dist = 1000
            lowest_index = -1
            for i in range(0, cv.shape[0]):
                dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
                if dist < lowest_dist:
                    lowest_dist = dist
                    lowest_index = i
            if event.button == self.button:
                if lowest_dist > 1:
                    cv = np.vstack((np.array([event.xdata, event.ydata, 4.]),cv))

                    # Get Centre
                    center_x = (cv[0,0] + cv[1,0])/2
                    center_y = (cv[0,1] + cv[1,1])/2
                    cv[-2,0] = center_x
                    cv[-2,1] = center_y
            if event.button == 3:

                if lowest_dist < 1:
                    cv = np.delete(cv, (lowest_index), axis=0)

    def onpress(self,event):
        self.press=True
    def onmove(self,event):
        if self.press:
            self.move=True

            # Find the closest point to the click
            global cv
            lowest_dist = 1000
            lowest_index = -1
            for i in range(0, cv.shape[0]):
                dist = np.sqrt((cv[i,0]-event.xdata)**2 + (cv[i,1]-event.ydata)**2)
                if dist < lowest_dist:
                    lowest_dist = dist
                    lowest_index = i
            if lowest_dist < 2:
                cv[lowest_index, 0] = event.xdata
                cv[lowest_index, 1] = event.ydata

            print(cv)


    def onrelease(self,event):
        if self.press and not self.move:
            self.onclick(event)
        else:
            global released
            released = True
        self.press=False; self.move=False

import rospy
from curtain_maker.srv import ProcessPoints, ProcessPointsRequest, ProcessPointsResponse
from geometry_msgs.msg import PoseArray, Pose, Point

"""
I need a brute force solver to figure out what the ideal beta values are given a set of points
that ensures curve doesnt exceed limit and acceleration is limited

Another thing! Also even if it does exceed. Does the final result actually hit the point or at least minimizes to that
Also, velocity should stay positive or continuous


# Check. Going down being sharper is better
# Going up being smoother is better?

# SOLVE! Put less points between distances that are short!!

# SOLVE! Actually, optimize for beta so that i keep within velocity bound and acceleration is minimized. 
# and non negative
"""

def convert_forward(pts):
    poses = []
    for i in range(0, pts.shape[0]):
        pose = Pose()
        pose.position.x = pts[i, 0]
        pose.position.z = pts[i, 1]
        poses.append(pose)
    poseArray = PoseArray()
    poseArray.poses = poses
    return poseArray

def convert_back(pts, rays):
    np_pts = np.zeros((len(pts.poses),2))
    np_rays = np.zeros((len(rays.poses), 3))
    angles = []
    velos = []
    accels = []
    for i in range(0, len(pts.poses)):
        np_pts[i,0] = pts.poses[i].position.x
        np_pts[i,1] = pts.poses[i].position.z
        angles.append(pts.poses[i].orientation.w)
        velos.append(pts.poses[i].orientation.x)
        accels.append(pts.poses[i].orientation.y)
    for i in range(0, len(rays.poses)):
        np_rays[i,0] = rays.poses[i].position.x
        np_rays[i,1] = rays.poses[i].position.y
        np_rays[i,2] = rays.poses[i].position.z
    return np_pts, angles, velos, accels, np_rays

def process_points_client(camera_name, points):

    points = convert_forward(points)

    rospy.wait_for_service('/carla_lcfuse/process_points')
    try:
        process_points = rospy.ServiceProxy('/carla_lcfuse/process_points', ProcessPoints)
        output_msg = process_points(camera_name, points)
        output_pts, angles, velos, accels, np_rays = convert_back(output_msg.output_pts, output_msg.laser_rays)
        return output_pts, np.array(angles), np.array(velos), np.array(accels), np_rays

    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

import curses, time
from scipy import signal
import scipy.signal


def peak_finding(x, N=5, threshold=0.12):

    array = []

    # Find change in direction
    for i in range(N, x.shape[0]-N):
        left_pt = x[i-N]
        mid_pt = x[i]
        right_pt = x[i+N]
        left_dist = mid_pt - left_pt
        right_dist = right_pt - mid_pt

        if(np.sign(left_dist) != np.sign(right_dist)):
            dist = min(np.abs(left_dist), np.abs(right_dist))
            if dist > threshold:
                array.append([i, dist])


    # Consolidate
    clusters = []
    curr_cluster = []
    for i in range(0, len(array)):
        if len(curr_cluster) == 0:
            curr_cluster.append(array[i])
        elif abs(curr_cluster[-1][0] - array[i][0]) < N:
            curr_cluster.append(array[i])
        else:

            # Choose the max in the cluster
            maxsize = 0
            best_item = None
            for item in curr_cluster:
                if item[1] > maxsize:
                    maxsize = item[1]
                    best_item = item

            clusters.append(best_item)
            curr_cluster = []

    if len(clusters):
        
        return np.array(clusters)
    else:
        return np.zeros((2,2))


def optimize():
    global cv


    best_b = None
    lowest_cost = 1000000000.

    costs = []
    for b in np.arange(1.5, 10, 0.5):

        cv[:,2] = b

        cvT = cv.T
        spline = bspline.process(cvT[0],cvT[1],cvT[2],plt.gca(), 1, draw=False)
        spline = np.flip(spline, 0)
        output_pts, angles, velos, accels, rays = process_points_client("camera01", spline)


        velos = velos[np.isfinite(velos)]
        accels = accels[np.isfinite(accels)]

        # Accels
        N = 5
        accels = running_mean(accels, N)
        peaks = peak_finding(accels)

        summed_peak = np.sum(peaks[:,1]**2)
        avg_peak = np.mean(peaks[:,1])
        maximum_peak = np.max(peaks[:,1])
        max_velo = np.max(velos)
        # print(peaks.shape)
        # print(np.max(peaks[:,1]))

        


        delt = 0.01
        cost = (1-delt)*float(summed_peak) + delt*max_velo

        print((b, cost))

        costs.append(cost)

        #cost += summed_peak



        #print((b, maximum_peak))

        # # Velocity
        # max_velo = np.max(velos)
        # if(max_velo > 135000): cost+=10000

        #cost += np.max(np.abs(accels))

        # CHeck cost
        if cost < lowest_cost:
            lowest_cost = cost
            best_b = b

        # **
        #print("Velo Max: " + str(np.max(velos)))
        #print("Accel Min: " + str(np.min(accels)) + " " +  str(np.min(accels)))
        print(b)

    print(best_b)
    cv[:,2] = best_b

    # plt.figure(2)
    # plt.plot(costs)
    # plt.pause(5)


def running_mean(x, N):
    cumsum = np.cumsum(np.insert(x, 0, 0)) 
    return (cumsum[N:] - cumsum[:-N]) / float(N)


if __name__ == "__main__":

    # # Test
    # pts = np.ones((5,2))*5
    # for i in range(0, pts.shape[0]):
    #     pts[i, 0] = i

    # process_points_client("camera01", pts)

    # stop

    import matplotlib.pyplot as plt
    import matplotlib.lines as mlines
    first_run = True
    click  = None


    bspline = BSpline()

    def test(closed):
        global click
        global first_run
        global cv
        global released

        plt.figure(1)
        plt.cla()

        if(first_run):
            first_run = False
            click = Click(plt.gca(), button=1)

        cvT = cv.T
        #ax2 = plt.gca()
        spline = bspline.process(cvT[0],cvT[1],cvT[2],plt.gca())

        spline = np.flip(spline, 0)

        output_pts, angles, velos, accels, rays = process_points_client("camera01", spline)


        if released:

            print("Optimize")
            optimize()
            released = False

        # # Plot Rays
        # for i in range(0, rays.shape[0], 1):
        #     x1 = 0
        #     y1 = 0
        #     x2 = rays[i,0]*10
        #     y2 = rays[i,2]*10

        #     l = mlines.Line2D([x1,x2], [y1,y2])
        #     l.set_alpha(.5)
        #     plt.gca().add_line(l)


        N = 5
        velos = velos[np.isfinite(velos)]
        accels = accels[np.isfinite(accels)]
        accels = running_mean(accels, N)
        peaks = peak_finding(accels)
        summed_peak = str(np.sum(peaks[:,1]**2))
        max_velo = np.max(velos)
        print("Peak Accel: " + str(summed_peak))
        print("Velos Max: " + str(max_velo))
        delt = 0.01
        print("Cost: " + str((1-delt)*float(summed_peak) + delt*max_velo))


        print(peaks)
        # **
        plt.scatter(output_pts[:, 0], output_pts[:, 1], s=1.0, c='r')


        # Visualize
        # plt.plot(x,y,'k-',label='Curve')
        plt.minorticks_on()
        plt.legend()
        plt.xlabel('x')
        plt.ylabel('y')
        plt.xlim(-7, 7)
        plt.ylim(0, 10)
        #plt.gca().set_aspect('equal', adjustable='box-forced')

        #plt.figure(2)

        # # **
        # plt.pause(0.001)
        # return

        plt.figure(2)
        plt.ylim(5000, 20000)
        plt.plot(velos)

        plt.figure(3)
        plt.plot(accels)

        plt.figure(2)
        plt.pause(0.001)
        plt.figure(3)
        plt.pause(0.001)

        #while 1: plt.pause(0.001)

        plt.figure(2)
        plt.cla()
        plt.figure(3)
        plt.cla()




    while 1:
        test(False)