def process_vision_log(vision_pipe_filename='/tmp/pipe_run.npz'):
#path_fname = '/home/poine/work/overlay_ws/src/two_d_guidance/paths/demo_z/track_trr_real.npz'
path_fname = '/home/poine/work/overlay_ws/src/two_d_guidance/paths/vedrines/track_trr_0.npz'
_path = trr_u.TrrPath(path_fname)
#plot_path(p)
print('loading vision log from {}'.format(vision_pipe_filename))
data = np.load(vision_pipe_filename)
lane_times, lane_mod_coefs = data['times'], data['lane_mod_coefs']
odom_times, odom_vlins, odom_vangs = data['odom_times'], data[
'odom_vlins'], data['odom_vangs']
if 0:
plot_lane_coefs(lane_times, lane_mod_coefs)
plt.show()
if 0:
plot_odom(odom_times, odom_vlins, odom_vangs)
if 0:
#test_vision_curvature(lane_mod_coefs, 390)
#test_vision_curvature(lane_mod_coefs, 601)
#test_vision_curvature(lane_mod_coefs, 756)
test_vision_curvature(lane_mod_coefs, 1300)
plt.show()
Cs, cs = get_vision_measurements(lane_mod_coefs, force_recompute=True)
if 0:
plot_frames(0, len(lane_mod_coefs), lane_mod_coefs, Cs, cs)
dists, curv_odom_times, path_curv_shifted_odom_times, distn_odom_times = test_state_est(
-1, odom_times, odom_vlins, path_fname)
#plot_run(lane_times, lane_mod_coefs, Cs, cs, _path)
plot_chronogram(lane_times, cs, odom_times, curv_odom_times,
path_curv_shifted_odom_times, distn_odom_times)
plt.show()
def __init__(self): #/caroline/camera_road_front
robot_name = rospy.get_param('~robot_name', '')
def prefix(robot_name, what): return what if robot_name == '' else '{}/{}'.format(robot_name, what)
cam_name = rospy.get_param('~camera', prefix(robot_name, 'camera_road_front'))
path_filename = rospy.get_param('~path_filename')
self.path = trr_u.TrrPath(path_filename) #'/home/poine/work/overlay_ws/src/two_d_guidance/paths/demo_z/track_trr_real.npz'
rospy.loginfo('### se_display_node loaded {}'.format(path_filename))
self.im_pub = ImgPublisher(self.path, cam_name)
self.state_est_sub = trr_rpu.TrrStateEstimationSubscriber(what='state est display')
def make_z_room_path(res=0.01):
#x0, x1 = -0.75, 2.25
x0, x1 = 0., 2.25 # moved start of path to center
c1, r = [x1, 0], 1.25
line1 = tdg.make_line_path([x0, r], [x1, r], res=res)
circle1 = tdg.make_circle_path(c1, -r, np.pi / 2, np.pi, res=res)
x2, y2 = 1.581, -0.35
line2 = tdg.make_line_path([x1, -r], [x2, -r], res=0.01)
c2, r2, th2 = [x2, y2], 0.9, np.deg2rad(61.8)
circle2 = tdg.make_circle_path(c2, -r2, -np.pi / 2, th2, res=res)
c3, r3 = [0, -1.21], 0.9
circle3 = tdg.make_circle_path(c3, r3, np.pi / 2 - th2, 2 * th2, res=res)
c4, r4 = [-x2, y2], 0.9
circle4 = tdg.make_circle_path(c4, -r4, -(np.pi / 2 - th2), th2, res=res)
line3 = tdg.make_line_path([-x2, -r], [-x1, -r], res=res)
circle5 = tdg.make_circle_path([-x1, 0], -r, -np.pi / 2, np.pi, res=res)
line4 = tdg.make_line_path([-x1, r], [x0, r], res=res)
line1.append(
[circle1, line2, circle2, circle3, circle4, line3, circle5, line4])
p = line1
fname = '/tmp/path_tmp.npz'
p.save(fname)
# add landmarks and velocity profile
p1 = trr_u.TrrPath(fname, v=1.)
p1.lm_s = [
p.dists[-1] - 1.25, 1.25
] # start and finish are located -1.25 and 1.25 from path origin
vel_prof_id = 0
vels = [[1., 1., 1., 1., 1., 1., 1., 1., 1.],
[1.2, 1., 1., 1., 1., 1., 1., 1., 1.2],
[1.4, 1.2, 1.1, 1., 1., 1., 1.1, 1.2, 1.4],
[1.5, 1.3, 1.2, 1.0, 1.0, 1.0, 1.2, 1.3, 1.5],
[1.75, 1.4, 1.2, 1.0, 1.0, 1.0, 1.2, 1.4, 1.75],
[2.0, 1.4, 1.4, 1., 1., 1., 1.4, 1.4, 2.0]]
make_vel_profile(p1,
vels[vel_prof_id],
a_max=0,
omega_ref=5.5,
braking_delay=100)
p1.report()
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
#fname = os.path.join(tdg_dir, 'paths/demo_z/track_trr_sim_{}.npz'.format(vel_prof_id))
fname = os.path.join(
tdg_dir, 'paths/demo_z/track_trr_real_{}.npz'.format(vel_prof_id))
p1.save(fname)
return fname, p1
def make_curvature_based_vel_profile(fname, v0=1.75, kc=0.75, _plot=False):
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
fname = os.path.join(tdg_dir, 'paths/demo_z/track_trr_real.npz')
path = trr_u.TrrPath(fname)
for i, c in enumerate(path.curvatures):
path.vels[i] = v0 - kc * abs(c) # why not ...
fltrd, delayed_curv = trr_u.filter(path.vels, path.dists)
if _plot:
plt.subplot(3, 1, 1)
plt.plot(fltrd[:, 0])
plt.plot(delayed_curv, alpha=0.5)
plt.plot(path.vels)
plt.subplot(3, 1, 2)
plt.plot(fltrd[:, 1])
plt.subplot(3, 1, 3)
plt.plot(fltrd[:, 2])
plt.show()
path.vels = fltrd[:, 0]
fname = os.path.join(tdg_dir, 'paths/demo_z/track_trr_real_vel.npz')
path.save(fname)
p2 = trr_u.TrrPath(fname)
return p2
def main():
path_filename = '/home/poine/work/two_d_guidance/paths/demo_z/track_trr_sim_2.npz'
if len(sys.argv) > 1: path_filename = sys.argv[1]
print('loading {}'.format(path_filename))
rospy.init_node('load_vel_profile')
_path = trr_u.TrrPath(path_filename)
_path.report()
# guidance
call_load_path_service('GuidanceLoadPath', path_filename)
# State estimator
call_load_path_service('StateEstimatorLoadPath', path_filename)
# Race Manager
call_load_path_service('RaceManagerLoadPath', path_filename)
def load_path(self):
#path_filename= '/home/poine/work/two_d_guidance/paths/vedrines/track_trr_0.npz'
path_filename= '/tmp/ph.npz' #TODO remove
#tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
#fname = os.path.join(tdg_dir, 'paths/vedrines/track_trr_{}.npz'.format(idx_velp))
self.trr_path = trr_u.TrrPath(path_filename)
self.node.set_trr_path(self.trr_path)
self.node.update_track(self.trr_path.track_points)
self.gui.set_path_length((len(self.trr_path.points) - 2) / 100)
self.points = []
for position, speed in self.trr_path.speed_points:
self.create_point(Point.TYPE_SPEED, position=position, speed=speed)
for position, offset_size, offset_length in self.trr_path.offset_points:
self.create_point(Point.TYPE_OFFSET, position=position, offset_size=offset_size, offset_length=offset_length)
node.set_points(self.points)
def load_path(self, path_filename):
rospy.loginfo(' loading path: {}'.format(path_filename))
self.path = trr_u.TrrPath(path_filename)
self.path.report()
def load_profile(self, path_fname):
self.path = trr_u.TrrPath(path_fname)
rospy.loginfo('Loaded path file: {}'.format(path_fname))
self.path.report()
def make_vedrines_path(res=0.01):
# origin of path is at the center of the long straight line
lw = 1.5 # lane width
x0, x1 = 0., 22.5 # start and end of first line (step0)
c1, r1 = [x1, 0], 1.75 # center and radius of first circle (step1)
line1 = tdg.make_line_path([x0, r1], [x1, r1], res=res)
circle1 = tdg.make_circle_path(c1, -r1, np.pi / 2, np.pi, res=res)
d1 = 3.83
r2 = 2
x2, y2 = 2 * d1, r2 - r1
line2 = tdg.make_line_path([x1, -r1], [x2, -r1], res=0.01)
c2, th2 = [x2, y2], np.deg2rad(51.1) # 51 < th2 < 51.15
circle2 = tdg.make_circle_path(c2, -r2, -np.pi / 2, th2, res=res)
r3 = 2.927
x3, y3 = d1, -r1 - 1.1
c3, th3, dth3 = [x3, y3], np.pi / 2 - th2, 2 * th2
circle3 = tdg.make_circle_path(c3, r3, th3, dth3, res=res)
r4, c4, th4, dth4 = r2, [0, y2], -np.pi / 2 + th2, 2 * th2
circle4 = tdg.make_circle_path(c4, -r4, th4, dth4, res=res)
r5, c5, th5, dth5 = r3, [-x3, y3], th3, dth3
circle5 = tdg.make_circle_path(c5, r5, th5, dth5, res=res)
r6, c6, th6, dth6 = r2, [-x2, y2], -np.pi / 2 + th2, th2
circle6 = tdg.make_circle_path(c6, -r6, th6, dth6, res=res)
line3 = tdg.make_line_path([-x2, -r1], [-x1, -r1], res=0.01)
r7, c7, th7, dth7 = r1, [-x1, 0], -np.pi / 2, np.pi
circle7 = tdg.make_circle_path(c7, -r7, th7, dth7, res=res)
line4 = tdg.make_line_path([-x1, r1], [-x0, r1], res=res)
line1.append([
circle1, line2, circle2, circle3, circle4, circle5, circle6, line3,
circle7, line4
])
p = line1
fname = '/tmp/path_tmp.npz'
p.save(fname)
# add landmarks and velocity profile
p1 = trr_u.TrrPath(fname, v=1.)
p1.lm_s = [
0., 16.66
] # start and finish are located at 0 and 16.66 m from path origin
idx_velp = 1
vels = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1], [3, 2, 3, 2, 2, 2, 2, 2, 3, 2, 3],
[4, 2, 4, 2, 2, 2, 2, 2, 4, 2, 4],
[6, 2.5, 6, 2, 2.5, 2, 2.5, 2, 6, 2.5, 6],
[7, 2.5, 7, 2, 2.5, 2, 2.5, 2, 7, 2.5, 7],
[8, 2.5, 8, 2, 2.5, 2, 2.5, 2, 8, 2.5, 8]]
for idx_velp in range(len(vels)):
make_vel_profile(p1,
vels[idx_velp],
a_max=0,
omega_ref=3.,
braking_delay=100)
p1.report()
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
fname = os.path.join(
tdg_dir, 'paths/vedrines/track_trr_{}.npz'.format(idx_velp))
p1.save(fname)
return fname, p1
def plot_vedrines():
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
fname = os.path.join(tdg_dir, 'paths/vedrines/track_trr.npz')
plot_path(trr_u.TrrPath(fname))