def generate(self, event):
global key_list
global preX
global embeds
global main_ax
global bool_drawing
global pointSet
global Nav
if Nav == None:
print('\n>>>>>>>>>>>> prepare for navigating...')
Nav = navigate.Navigator(embeds, m)
if len(key_list) > 0:
print('\n>>>>>>>>>>>> generating shapes on a path...')
mu_fFull, var_fFull = m.predict_f_full_cov(key_list)
nametosave = []
for xx in key_list:
nametosave.append(str(xx[0])[:4] + '_' + str(xx[1])[:4])
thread_Generate.generateModels(mu_fFull, key_list, embeds,
pointSet, sess, model, nametosave)
key_list = []
else:
print('\n>>>>>>>>>>>> generating a single shape...')
newX = preX
mu_fFull, var_fFull = m.predict_f_full_cov(newX)
newxyz = np.reshape(mu_fFull, (1, D, 3)) #新的骨架
# new shapes : get horses here
nametosave = str(newX[:, 0])[:4] + '_' + str(newX[:, 1])[:4]
neighbor_horse = thread_Generate.find_neighbor(
newX, embeds, pointSet)
neighbor_horse = np.reshape(neighbor_horse, [1, 2048, 3])
horse_xyz = gen_horse2horse.gen_horses(sess, model, newxyz,
neighbor_horse, mustSavePly,
nametosave)
ax_skeleton.clear()
ax_horse.clear()
ax_skeleton.axis('off')
ax_horse.axis('off')
plot_skeleton(newxyz[0], ax_skeleton)
ax_horse.scatter(horse_xyz[:, 0],
horse_xyz[:, 2],
horse_xyz[:, 1],
s=1)
plt.draw()
bool_drawing = False
def navigate(self, event):
global Nav
global main_ax
global key_list
global nav_init
n_nav = 1
if Nav == None:
print('\n>>>>>>>>>>>> prepare for navigating...')
Nav = navigate.Navigator(embeds, m)
if len(nav_init) == 0:
print('\n>>>>>>>>>>>> set initial point...')
nav_init = preX
Nav.navigate_from(nav_init, n_nav)
bool_drawing = False
print('\n>>>>>>>>>>>> generating neighbors...')
'''res = Nav.optimize(method = tf.train.GradientDescentOptimizer(0.001))
p = res['x'].reshape([-1,2])
fetches = res['fun']
print('\t optimize result\n', res)'''
pnew, W, Wdir = Nav.navigate()
p = np.append(nav_init.reshape([-1, 2]), pnew.reshape([-1, 2]), axis=0)
print('\t new point:', p)
main_ax.plot(p[:, 0], p[:, 1], 'r*-', markersize=5)
dir1 = np.append(nav_init.reshape([-1, 2]),
(nav_init + W).reshape([-1, 2]),
axis=0)
dir2 = np.append(nav_init.reshape([-1, 2]),
(nav_init + Wdir).reshape([-1, 2]),
axis=0)
print('\t directions', W, Wdir)
main_ax.plot(dir1[:, 0],
dir1[:, 1],
color=colors['pink'],
markersize=5)
main_ax.plot(dir2[:, 0],
dir2[:, 1],
color=colors['lightblue'],
markersize=5)
plt.draw()
nav_init = pnew
import numpy as np
import rospy
from std_msgs.msg import Int8MultiArray
from time import sleep
import navigate
ANGLE_THRESHOLD = np.deg2rad(5) #degrees
DELTA_THRESHOLD = 50 # for lane delta, in pixels
DELTA_THRESHOLD_DEVIATION = 10
#rospy.init_node('Lane_Navigator', anonymous=True)
nav = navigate.Navigator()
def on_lanes(raw_data):
print 'On Lane'
angle = raw_data.data[0]
delta = raw_data.data[1]
if delta < DELTA_THRESHOLD - DELTA_THRESHOLD_DEVIATION or delta > DELTA_THRESHOLD + DELTA_THRESHOLD_DEVIATION:
# Too close, move right OR Too far, nove left
nav.turn(angle - np.pi)
subscriber = rospy.Subscriber('lanes', Int8MultiArray, on_lanes)
while True:
sleep(1)
print('\n>>>>>>>>>>>> computing shortest path...')
floyd_res = floyd.calculate_floyd(embeds)
dist = floyd_res[0]
parent = np.array(floyd_res[1], dtype=int)
edges = np.array(floyd_res[2])
print('\t size of edges', edges.shape)
###################################################### figure ##########################################
print('\n>>>>>>>>>>>> prepare plot inits...')
bool_drawing = False
key_spl = []
nav_init = []
Nav = Nav = navigate.Navigator(embeds, m)
print('\n>>>>>>>>>>>> ploting...')
fig = plt.figure(facecolor='silver', figsize=(10,5))
plt.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
grid = gridspec.GridSpec(3, 5, wspace=0.05)
## 主图
main_ax = fig.add_subplot(grid[:, 0:3])
#main_ax.patch.set_facecolor(colors['ghostwhite'])
main_ax.spines['top'].set_color('none')
main_ax.spines['right'].set_color('none')
main_ax.xaxis.set_ticks_position('bottom')
main_ax.spines['bottom'].set_position(('data', 0))
main_ax.spines['bottom'].set_color(colors['silver'])
main_ax.yaxis.set_ticks_position('left')