Last active
October 8, 2019 22:32
-
-
Save soulslicer/439960753bfe144ade3bace78fbc5ca2 to your computer and use it in GitHub Desktop.
test
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment