def __init__(self):
# init node subs pubs
rospy.init_node('loc_est', anonymous=True)
self.pathlocalpub = rospy.Publisher('pathlocal', Path, queue_size=10)
self.dynamic_param_pub = rospy.Publisher('dynamic_vehicle_params',
DynamicVehicleParams,
queue_size=10)
self.statepub = rospy.Publisher('state', State, queue_size=10)
self.vehicle_out_sub = rospy.Subscriber("vehicle_out", VehicleOut,
self.vehicle_out_callback)
self.dt = 0.1
self.rate = rospy.Rate(1 / self.dt) # 10hz
# set static vehicle params
self.setStaticParams()
# init local vars
self.loadPathGlobalFromFile()
self.pathlocal = Path()
self.state = State()
self.vehicle_out = VehicleOut()
# init dynamic params
self.dynamic_params = DynamicVehicleParams()
self.dynamic_params.mu_alg = 0.7
self.dynamic_params.mu_real = 0.7
self.dynamic_params.Fz = self.sp.m * self.sp.g
self.dynamic_params.Fzf = self.dynamic_params.Fz * self.sp.lr / (
self.sp.lf + self.sp.lr)
# moment balance at 0 acc
self.dynamic_params.Fzr = self.dynamic_params.Fz * self.sp.lf / (
self.sp.lf + self.sp.lr)
self.dynamic_params.theta = 0.0
self.dynamic_params.phi = 0.0
# wait for messages before entering main loop
while (not self.vehicle_out.X):
print("waiting for vehicle_out")
self.rate.sleep()
# Main loop
while not rospy.is_shutdown():
# update dynamic params
self.dynamic_params.header.stamp = rospy.Time.now()
self.dynamic_param_pub.publish(self.dynamic_params)
# update local path around new state
self.updateLocalPath()
self.pathlocal.header.stamp = rospy.Time.now()
self.pathlocalpub.publish(self.pathlocal)
# compute vehicle state from vehicle_out info
self.updateState()
self.statepub.publish(self.state)
self.rate.sleep()
def __init__(self):
# init node subs pubs
rospy.init_node('state_est', anonymous=True)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path,
self.pathglobal_callback)
self.vehicle_out_sub = rospy.Subscriber(
"/fssim/base_pose_ground_truth", fssimState,
self.vehicle_out_callback)
self.statepub = rospy.Publisher('state', saartiState, queue_size=10)
# rqt debug
self.debugpub = rospy.Publisher('/state_est_debug',
Float32,
queue_size=1)
self.debug_val = Float32()
# init local vars
self.pathglobal = Path()
self.state_out = saartiState()
self.state_in = fssimState()
self.passed_halfway = False
self.lapcounter = 0
# node params
self.dt = 0.01
self.rate = rospy.Rate(1 / self.dt) # 100hz
self.received_vehicle_out = False
self.received_pathglobal = False
# wait for messages before entering main loop
while (not self.received_pathglobal):
print "state est: waiting for pathglobal"
self.rate.sleep()
# compute length of track
stot_global = self.pathglobal.s[-1]
dist_sf = np.sqrt((self.pathglobal.X[0] - self.pathglobal.X[-1])**2 +
(self.pathglobal.Y[0] - self.pathglobal.Y[-1])**2)
self.s_lap = stot_global + dist_sf
while (not self.received_vehicle_out):
print "state est: waiting for vehicle_out"
self.rate.sleep()
print "state est: running main "
print "state est: lap count = ", self.lapcounter
# Main loop
while not rospy.is_shutdown():
self.updateState()
self.statepub.publish(self.state_out)
# rqt debug
self.debug_val = self.state_out.deltapsi
self.debugpub.publish(self.debug_val)
self.rate.sleep()
def __init__(self):
# params
self.robot_name = rospy.get_param('/robot_name')
# init node subs pubs
rospy.init_node('ctrl_interface', anonymous=True)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.pathlocalsub = rospy.Subscriber("pathlocal", Path,
self.pathlocal_callback)
self.state_sub = rospy.Subscriber("/state", State, self.state_callback)
self.ctrlmodesub = rospy.Subscriber("ctrl_mode", Int16,
self.ctrl_mode_callback)
self.vehicleinpub = rospy.Publisher('/fssim/cmd', Cmd, queue_size=10)
self.lhptpub = rospy.Publisher('/lhpt_vis', Marker, queue_size=1)
self.vx_errorpub = rospy.Publisher('/vx_error_vis',
Float32,
queue_size=1)
self.rate = rospy.Rate(100)
# set static vehicle params
self.setStaticParams()
# init msgs
self.state = State()
self.vehicle_in = Cmd()
self.trajstar = Trajectory()
self.pathlocal = Path()
self.ctrl_mode = 0 # 0: stop, 1: cruise_ctrl, 2: tamp
self.trajstar_received = False
self.pathlocal_received = False
self.state_received = False
# ctrl errors
self.vx_error = Float32()
# get cc setpoint
self.cc_vxref = rospy.get_param('/cc_vxref')
self.cc_dref = rospy.get_param('/cc_dref')
# delay sim variable
self.delta_out_FIFO = []
# misc vars
self.delta_out_last = 0
self.dc_out_last = 0
# wait for messages before entering main loop
while (not self.state_received):
rospy.loginfo_throttle(1, "waiting for state")
self.rate.sleep()
while (not self.pathlocal_received):
rospy.loginfo_throttle(1, "waiting for pathlocal")
self.rate.sleep()
while (self.ctrl_mode == 0):
rospy.loginfo_throttle(1,
"waiting for activation from exp manager")
self.rate.sleep
# main loop
while not rospy.is_shutdown():
if (self.ctrl_mode == 0
): # STOP (set by exp manager if outside of track)
if (self.state.vx > 0.1):
rospy.loginfo_throttle(1, "in stop mode")
delta_out = self.delta_out_last
dc_out = -200000 #self.dc_out_last
else:
delta_out = 0
dc_out = 0
elif (self.ctrl_mode == 1): # CRUISE CTRL
delta_out, dc_out, Xlh, Ylh = self.cc_ctrl()
elif (self.ctrl_mode == 2): # TAMP
while (not self.trajstar_received):
print("waiting for trajstar, stopping")
delta_out = 0
dc_out = 0
self.rate.sleep()
delta_out, dc_out, Xlh, Ylh = self.tamp_ctrl()
else:
print "invalid ctrl_mode! ctrl_mode = ", self.ctrl_mode
# publish ctrl cmd
self.vehicle_in.delta = delta_out
#print "dc_out published = ", dc_out
self.vehicle_in.dc = dc_out
self.vehicleinpub.publish(self.vehicle_in)
# publish tuning info
self.vx_errorpub.publish(self.vx_error)
if (self.ctrl_mode in [1, 2]):
m = self.getlhptmarker(Xlh, Ylh)
self.lhptpub.publish(m)
# store latest controls
self.delta_out_last = delta_out
self.dc_out_last = dc_out
self.rate.sleep()
def __init__(self):
# timing params
self.dt_sim = 0.01 # timestep of the simulation
self.t_activate = rospy.get_param('/t_activate')
self.t_final = rospy.get_param('/t_final')
# pop-up scenario params
self.s_ego_at_popup = rospy.get_param('/s_ego_at_popup')
self.s_obs_at_popup = rospy.get_param('/s_obs_at_popup')
self.d_obs_at_popup = rospy.get_param('/d_obs_at_popup')
# track params
self.track_name = rospy.get_param('/track_name')
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
self.mu_segment_idx = 0
# vehicle params
self.vehicle_width = rospy.get_param('/car/kinematics/l_width')
# algo params (from acado)
N = 40
dt_algo = 0.1
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path,
self.pathglobal_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.carinfosub = rospy.Subscriber("/fssim/car_info", CarInfo,
self.fssim_carinfo_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.obspub = rospy.Publisher('/obs', Obstacles, queue_size=1)
self.obsvispub = rospy.Publisher('/obs_vis', Marker, queue_size=1)
self.tireparampub = rospy.Publisher('/tire_params',
TireParams,
queue_size=1)
self.ctrl_mode_pub = rospy.Publisher('/ctrl_mode', Int16, queue_size=1)
self.statetextmarkerpub = rospy.Publisher('/state_text_marker',
Marker,
queue_size=1)
# init logging vars
self.explog_activated = False
self.explog_iterationcounter = 0
self.stored_pathglobal = False
self.stored_trajstar = False
self.stored_trajcl = False
self.explog_saved = False
# init misc internal variables
self.pathglobal = Path()
self.received_pathglobal = False
self.state = State()
self.received_state = False
self.trajstar = Trajectory()
self.received_trajstar = False
self.fssim_carinfo = CarInfo()
while (not self.received_pathglobal):
print("waiting for pathglobal")
time.sleep(self.dt_sim * 100)
# init experiment variables
self.scenario_id = rospy.get_param('/scenario_id')
self.tireparams = TireParams()
self.obs = Obstacles()
self.obs.s = [self.s_obs_at_popup]
self.obs.d = [self.d_obs_at_popup]
self.obs.R = [0.5]
wiggleroom = 0.5
self.obs.Rmgn = [
0.5 * self.obs.R[0] + 0.5 * self.vehicle_width + wiggleroom
]
Xobs, Yobs = ptsFrenetToCartesian(np.array(self.obs.s), \
np.array(self.obs.d), \
np.array(self.pathglobal.X), \
np.array(self.pathglobal.Y), \
np.array(self.pathglobal.psi_c), \
np.array(self.pathglobal.s))
self.ctrl_mode = 0 # # 0: stop, 1: cruise_ctrl, 2: tamp
# Main loop
#print 'running experiment: '
#print 'track: ', self.track_name
self.exptime = 0
while (not rospy.is_shutdown()) and self.exptime < self.t_final:
if (self.exptime >= self.t_activate):
rospy.loginfo_throttle(1, "Running experiment")
# HANDLE TRACTION IN SIMULATION
s_ego = self.state.s % self.s_lap
for i in range(self.N_mu_segments - 1):
if (self.s_begin_mu_segments[i] <= s_ego <=
self.s_begin_mu_segments[i + 1]):
self.mu_segment_idx = i
break
if (s_ego >= self.s_begin_mu_segments[-1]):
self.mu_segment_idx = self.N_mu_segments - 1
mu = self.mu_segment_values[self.mu_segment_idx]
#print "s_ego = ", s_ego
#print "state.s = ", self.state.s
#print "self.N_mu_segments = ", self.N_mu_segments
#print "mu_segment_idx = ", self.mu_segment_idx
#print "mu in this section = ", self.mu_segment_values[self.mu_segment_idx]
# set tire params of sim vehicle
if (0.0 <= mu < 0.3): # ice
B = 4.0
C = 2.0
D = mu
E = 1.0
elif (0.3 <= mu < 0.5): # snow
B = 5.0
C = 2.0
D = mu
E = 1.0
elif (0.5 <= mu < 0.9): # wet
B = 12.0
C = 2.3
D = mu
E = 1.0
elif (0.9 <= mu < 1.5): # dry
B = 10.0
C = 1.9
D = mu
E = 0.97
elif (1.5 <= mu <
2.5): # dry + racing tires (gotthard default)
B = 12.56
C = 1.38
D = mu
E = 1.0
else:
rospy.loginfo_throttle(1, "Faulty mu value in exp manager")
self.tireparams.tire_coefficient = 1.0
#self.tireparams.B = 10
#self.tireparams.C = 1.9
#self.tireparams.D = -mu
#self.tireparams.E = 1.0
self.tireparams.B = B
self.tireparams.C = C
self.tireparams.D = -D
self.tireparams.E = E
self.tireparams.header.stamp = rospy.Time.now()
self.tireparampub.publish(self.tireparams)
# POPUP SCENARIO
if (self.scenario_id == 1):
self.ctrl_mode = 1 # cruise control
m_obs = self.getobstaclemarker(Xobs, Yobs, self.obs.R[0])
m_obs.color.a = 0.3 # transparent before detect
if (self.state.s >= self.s_ego_at_popup):
self.obspub.publish(self.obs)
m_obs.color.a = 1.0 # non-transparent after detect
self.ctrl_mode = 2 # tamp
self.obsvispub.publish(m_obs)
# REDUCED MU TURN
elif (self.scenario_id == 2):
self.ctrl_mode = 1 # pp cruise control from standstill
if (self.state.vx > 5): # todo
self.ctrl_mode = 2 # tamp cruise control when we get up to speed
# RACING
else:
self.ctrl_mode = 2 # tamp
# SEND STOP IF EXIT TRACK
dlb = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dlb)
dub = np.interp(self.state.s, self.pathglobal.s,
self.pathglobal.dub)
if (self.state.d < dlb or self.state.d > dub):
self.ctrl_mode = 0 # stop
self.ctrl_mode_pub.publish(self.ctrl_mode)
# publish text marker (state info)
state_text = "vx: " + "%.3f" % self.state.vx + "\n" \
"mu: " + "%.3f" % mu
self.statetextmarkerpub.publish(self.gettextmarker(state_text))
# save data for plot
filename = "/home/larsvens/ros/tamp__ws/src/saarti/common/logs/explog_latest.npy" # todo get from param
self.s_begin_log = 195 # 190 todo get from param
if (self.state.s >= self.s_begin_log
and not self.explog_activated):
print "STARTED EXPLOG"
t_start_explog = copy.deepcopy(self.exptime)
self.explog_activated = True
# store planned traj
self.trajstar_dict = {
"X": np.array(self.trajstar.X),
"Y": np.array(self.trajstar.Y),
"psi": np.array(self.trajstar.psi),
"s": np.array(self.trajstar.s),
"d": np.array(self.trajstar.d),
"deltapsi": np.array(self.trajstar.deltapsi),
"psidot": np.array(self.trajstar.psidot),
"vx": np.array(self.trajstar.vx),
"vy": np.array(self.trajstar.vy),
"ax": np.array(self.trajstar.ax),
"ay": np.array(self.trajstar.ay),
"Fyf": np.array(self.trajstar.Fyf),
"Fxf": np.array(self.trajstar.Fxf),
"Fyr": np.array(self.trajstar.Fyr),
"Fxr": np.array(self.trajstar.Fxr),
"Fzf": np.array(self.trajstar.Fzf),
"Fzr": np.array(self.trajstar.Fzr),
"kappac": np.array(self.trajstar.kappac),
"Cr": np.array(self.trajstar.Cr),
"t": np.arange(0, (N + 1) * dt_algo, dt_algo),
}
self.stored_trajstar = True
# initialize vars for storing CL traj
self.X_cl = []
self.Y_cl = []
self.psi_cl = []
self.s_cl = []
self.d_cl = []
self.deltapsi_cl = []
self.psidot_cl = []
self.vx_cl = []
self.vy_cl = []
self.ax_cl = []
self.ay_cl = []
self.t_cl = []
self.Fyf_cl = []
self.Fyr_cl = []
self.Fx_cl = []
if (self.state.s >= self.s_begin_log and self.explog_activated
and self.exptime >= t_start_explog +
self.explog_iterationcounter * dt_algo):
# build CL traj
self.X_cl.append(self.state.X)
self.Y_cl.append(self.state.Y)
self.psi_cl.append(self.state.psi)
self.s_cl.append(self.state.s)
self.d_cl.append(self.state.d)
self.deltapsi_cl.append(self.state.deltapsi)
self.psidot_cl.append(self.state.psidot)
self.vx_cl.append(self.state.vx)
self.vy_cl.append(self.state.vy)
self.ax_cl.append(self.state.ax)
self.ay_cl.append(self.state.ay)
self.t_cl.append(self.exptime)
self.Fyf_cl.append(self.fssim_carinfo.Fy_f_l +
self.fssim_carinfo.Fy_f_r)
self.Fyr_cl.append(self.fssim_carinfo.Fy_r)
self.Fx_cl.append(self.fssim_carinfo.Fx)
# store CL traj as dict
if (self.explog_iterationcounter == N
): # store N+1 values, same as trajstar
self.trajcl_dict = {
"X": np.array(self.X_cl),
"Y": np.array(self.Y_cl),
"psi": np.array(self.psi_cl),
"s": np.array(self.s_cl),
"d": np.array(self.d_cl),
"deltapsi": np.array(self.deltapsi_cl),
"psidot": np.array(self.psidot_cl),
"vx": np.array(self.vx_cl),
"vy": np.array(self.vy_cl),
"ax": np.array(self.ax_cl),
"ay": np.array(self.ay_cl),
"t": np.array(self.t_cl),
"Fyf": np.array(self.Fyf_cl),
"Fyr": np.array(self.Fyr_cl),
"Fx": np.array(self.Fx_cl),
}
self.stored_trajcl = True
self.explog_iterationcounter += 1
# save explog
# debug
#print "stored_pathglobal: ", self.stored_pathglobal
#print "stored_trajstar: ", self.stored_trajstar
#print "stored_trajcl: ", self.stored_trajcl
if (self.stored_pathglobal and self.stored_trajstar
and self.stored_trajcl and not self.explog_saved):
explog = {
"pathglobal": self.pathglobal_dict,
"trajstar": self.trajstar_dict,
"trajcl": self.trajcl_dict,
}
np.save(filename, explog)
self.explog_saved = True
print "SAVED EXPLOG"
else: # not reached activation time
rospy.loginfo_throttle(
1, "Experiment starting in %i seconds" %
(self.t_activate - self.exptime))
# handle exptime
self.exptime += self.dt_sim
msg = Clock()
t_rostime = rospy.Time(self.exptime)
msg.clock = t_rostime
#self.clockpub.publish(msg)
time.sleep(self.dt_sim)
print 'simulation finished'
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
def __init__(self):
# init node subs pubs
rospy.init_node('perception', anonymous=True)
self.pathglobalsub = rospy.Subscriber("pathglobal", Path, self.pathglobal_callback)
self.state_sub = rospy.Subscriber("state", State, self.state_callback)
self.pathlocalpub = rospy.Publisher('pathlocal', Path, queue_size=10)
#self.pathlocalvispub_old = rospy.Publisher('pathlocal_vis', navPath, queue_size=10)
self.pathlocalvispub = rospy.Publisher('pathlocal_vis', PolygonArray, queue_size=1)
# node params
self.dt = 0.1
self.rate = rospy.Rate(1/self.dt) # 10hz
# params of local path
self.N = 100
self.ds = 1.0 #0.5
# set static vehicle params
self.setRosParams()
# init local vars
self.pathglobal = Path()
self.pathrolling = Path() # used to run several laps
self.pathlocal = Path()
self.state = State()
# msg receive checks
self.received_pathglobal = False
self.received_state = False
# wait for messages before entering main loop
while(not self.received_pathglobal):
print "perception: waiting for pathglobal"
self.rate.sleep()
while(not self.received_state):
print "perception: waiting for state"
self.rate.sleep()
print "perception: running main with: "
print "perception: lap length: ", self.s_lap
print "perception: length of local path: ", self.N*self.ds
# Main loop
while not rospy.is_shutdown():
# check timing wrt dt
start = time.time()
# update pathrolling to handle multiple laps
if (self.state.s > self.pathrolling.s[0]+self.s_lap + 25): # if we're on the second lap of pathrolling
self.pathrolling.s = self.pathrolling.s + self.s_lap
# update local path
self.updateLocalPath()
print self.pathlocal.s
self.pathlocalpub.publish(self.pathlocal)
# visualize local path in rviz
pathlocal_vis = navPath()
pathlocal_vis.header.stamp = rospy.Time.now()
pathlocal_vis.header.frame_id = "map"
for i in range(self.N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = self.pathlocal.X[i]
pose.pose.position.y = self.pathlocal.Y[i]
pathlocal_vis.poses.append(pose)
#self.pathlocalvispub_old.publish(pathlocal_vis)
pa = self.pathLocalToPolArr()
self.pathlocalvispub.publish(pa)
end = time.time()
comptime = end-start
#print("perception: compute took ", comptime)
if (comptime > self.dt):
rospy.logwarn("perception: compute time exceeding dt!")
self.rate.sleep()
def __init__(self):
# params
self.dt = 0.01 # timestep of the simulation
t_final = rospy.get_param('/t_final')
do_liveplot = rospy.get_param('/do_liveplot')
save_logfile = rospy.get_param('/save_logfile')
# init node subs pubs
rospy.init_node('experiment_manager', anonymous=True)
#rospy.init_node('experiment_manager', anonymous=True, disable_signals=True)
self.clockpub = rospy.Publisher('/clock', Clock, queue_size=10)
self.pathlocalsub = rospy.Subscriber("pathlocal", Path,
self.pathlocal_callback)
self.obstaclesub = rospy.Subscriber("obstacles", Obstacles,
self.obstacles_callback)
self.trajhatsub = rospy.Subscriber("trajhat", Trajectory,
self.trajhat_callback)
self.trajstarsub = rospy.Subscriber("trajstar", Trajectory,
self.trajstar_callback)
self.statesub = rospy.Subscriber("state", State, self.state_callback)
self.dynamicparamsub = rospy.Subscriber("dynamic_vehicle_params",
DynamicVehicleParams,
self.dynamicparams_callback)
# load global path
self.loadPathGlobalFromFile()
# set static vehicle params
self.setStaticParams()
# init internal variables
self.counter = 0 # use this to reduce plot update rate
self.pathlocal = Path()
self.obstacles = Obstacles()
self.trajhat = Trajectory()
self.trajstar = Trajectory()
self.state = State()
self.dp = DynamicVehicleParams()
# init lists for logging
self.tvec = []
self.states = []
if (do_liveplot):
# init plot window
plt.ion()
self.f, (self.a0, self.a1) = plt.subplots(
1, 2, gridspec_kw={'width_ratios': [3, 1]})
self.a0.axis("equal")
self.a1.axis("equal")
# Main loop
self.t = 0
while (not rospy.is_shutdown()) and self.t < t_final:
# store data from subscribed topics
if (save_logfile):
self.stack_data()
# handle simtime
self.t += self.dt
msg = Clock()
t_rostime = rospy.Time(self.t)
msg.clock = t_rostime
self.clockpub.publish(msg)
if (do_liveplot):
self.liveplot()
slowdown_factor = 1
else:
slowdown_factor = 1
print 'simtime t =', t_rostime.to_sec()
time.sleep(self.dt * slowdown_factor)
print 'simulation finished'
if (save_logfile):
self.save_log()
# send shutdown signal
message = 'run finished, shutting down'
print message
rospy.signal_shutdown(message)
def __init__(self):
rospy.init_node('track_interface', anonymous=True)
self.pathglobalpub = rospy.Publisher('pathglobal', Path, queue_size=10)
self.pathglobalvispub = rospy.Publisher('pathglobal_vis', navPath, queue_size=10)
self.dubvispub = rospy.Publisher('dubglobal_vis', navPath, queue_size=10)
self.dlbvispub = rospy.Publisher('dlbglobal_vis', navPath, queue_size=10)
self.track_sub = rospy.Subscriber("/fssim/track", Track, self.track_callback)
self.track = Track()
self.pathglobal = Path()
self.received_track = False
self.rate = rospy.Rate(1)
# set params
ds = 1.0 # step size in s # 0.5 nominal
plot_track = False
plot_orientation = False
plot_dlbdub = False
plot_mu = False
self.s_begin_mu_segments = rospy.get_param('/s_begin_mu_segments')
self.mu_segment_values = rospy.get_param('/mu_segment_values')
self.N_mu_segments = len(self.s_begin_mu_segments)
# wait for track
while(not self.received_track):
print "waiting for track"
self.rate.sleep()
# get cone positions left and right as numpy arrays
cl_X = []
cl_Y = []
for i in range(len(self.track.cones_left)):
cl_X.append(self.track.cones_left[i].x)
cl_Y.append(self.track.cones_left[i].y)
cl_X = np.array(cl_X)
cl_Y = np.array(cl_Y)
print "nr of cones left: ", cl_X.size
cr_X = []
cr_Y = []
for i in range(len(self.track.cones_right)):
cr_X.append(self.track.cones_right[i].x)
cr_Y.append(self.track.cones_right[i].y)
cr_X = np.array(cr_X)
cr_Y = np.array(cr_Y)
print "nr of cones right: ", cr_X.size
# compute rough centerline
ccl_X = []
ccl_Y = []
for i in range(cl_X.size):
pt_left = {"X": cl_X[i], "Y": cl_Y[i]}
# find closest right cone
dist = np.sqrt((cr_X-pt_left["X"])**2 + (cr_Y-pt_left["Y"])**2)
idx = np.argmin(dist)
pt_right = {"X": cr_X[idx], "Y": cr_Y[idx]}
pt_mid = {"X": 0.5*(pt_left["X"]+pt_right["X"]), "Y": 0.5*(pt_left["Y"]+pt_right["Y"])}
ccl_X.append(pt_mid["X"])
ccl_Y.append(pt_mid["Y"])
ccl_X = np.array(ccl_X)
ccl_Y = np.array(ccl_Y)
# get approximate length of track
stot = 0
for i in range(ccl_X.size-1):
stot += np.sqrt((ccl_X[i+1]-ccl_X[i])**2 + (ccl_Y[i+1]-ccl_Y[i])**2)
stot = (stot//ds)*ds
print "length of track: stot = ", str(stot)
# set s
s = np.arange(0, stot, ds)
# parametric spline interpolation
print "spline interpolation of centerline"
N = int(stot/ds)
unew = np.arange(0, 1.0, 1.0/N) # N equidistant pts
# center
tck, u = interpolate.splprep([ccl_X, ccl_Y], s=0)
out = interpolate.splev(unew, tck)
fcl_X = out[0]
fcl_Y = out[1]
# left
tck, u = interpolate.splprep([cl_X, cl_Y], s=0)
out = interpolate.splev(unew, tck)
fll_X = out[0]
fll_Y = out[1]
# right
tck, u = interpolate.splprep([cr_X, cr_Y], s=0)
out = interpolate.splev(unew, tck)
frl_X = out[0]
frl_Y = out[1]
# set psic
print "computing psic"
dX = np.diff(fcl_X)
dY = np.diff(fcl_Y)
psic = np.arctan2(dY,dX)
psic_final = np.arctan2(fcl_Y[0]-fcl_Y[-1],fcl_X[0]-fcl_X[-1])
psic = np.append(psic,psic_final) # assuming closed track
# separate in pieces (for 2pi flips)
idx_low = 0
idx_high = 0
psic_piecewise = []
s_piecewise = []
for i in range(psic.size-1):
if(np.abs(psic[i+1] - psic[i]) > np.pi ):
# if single pt, remove
if(np.abs(psic[i+2] - psic[i]) < np.pi ):
print("removing single flip")
psic[i+1] = psic[i]
# otherwise make a piece
else:
print("making pieces (psic flips)")
idx_high = i+1
psic_piece = psic[idx_low:idx_high]
psic_piecewise.append(psic_piece)
s_piece = s[idx_low:idx_high]
s_piecewise.append(s_piece)
idx_low = i+1
# add final piece
psic_piece = psic[idx_low:psic.size]
psic_piecewise.append(psic_piece)
s_piece = s[idx_low:psic.size]
s_piecewise.append(s_piece)
# shift pieces to make continous psi_c
for j in range(len(psic_piecewise)-1):
while(psic_piecewise[j][-1] - psic_piecewise[j+1][0] > np.pi):
psic_piecewise[j+1] = psic_piecewise[j+1] + 2*np.pi
while(psic_piecewise[j][-1] - psic_piecewise[j+1][0] <= -np.pi):
psic_piecewise[j+1] = psic_piecewise[j+1] - 2*np.pi
# concatenate pieces
psic_cont = psic_piecewise[0]
for j in range(len(psic_piecewise)-1):
psic_cont = np.concatenate((psic_cont,psic_piecewise[j+1]))
# downsample for smoother curve
step = 5 #11
print "interpolating downsampled psic with step size ", str(step)
s_ds = s[0::step]
s_ds = np.append(s_ds,s[-1]) # append final value for closed circuit
psic_ds = psic_cont[0::step]
psic_ds = np.append(psic_ds,psic_cont[-1]) # append final value for closed circuit
# interpolate
t, c, k = interpolate.splrep(s_ds, psic_ds, s=0, k=4)
psic_spl = interpolate.BSpline(t, c, k, extrapolate=False)
# compute derrivatives (compare with naive numerical)
print "computing derivatives of psic"
kappac_spl = psic_spl.derivative(nu=1)
kappacprime_spl = psic_spl.derivative(nu=2)
# put psi_c back on interval [-pi,pi]
psic_out = psic_spl(s)
#plt.plot(psic_out)
psic_out = angleToInterval(psic_out)
# tmp test helpers
#plt.plot(psic_out,'*')
#psic_out = angleToContinous(psic_out)
#plt.plot(psic_out,'.')
#plt.show()
# set kappac
kappac_out = kappac_spl(s)
kappacprime_out = kappacprime_spl(s)
# set dlb and dub
print "setting dlb and dub"
dub = []
dlb = []
for i in range(N):
X = fcl_X[i]
Y = fcl_Y[i]
# left
dub_ele = np.amin(np.sqrt((fll_X-X)**2 + (fll_Y-Y)**2))
# right
dlb_ele = -np.amin(np.sqrt((frl_X-X)**2 + (frl_Y-Y)**2))
# correction if dlb or dub w.r.t zero division in dynamics
th = 0.3
while(abs(1.0-dub_ele*kappac_out[i]) < th):
dub_ele=dub_ele-0.01
print "adjusting dub"
while(abs(1.0-dlb_ele*kappac_out[i]) < th):
dlb_ele=dlb_ele+0.01
print "adjusting dub"
dub.append(dub_ele)
dlb.append(dlb_ele)
dub = np.array(dub)
dlb = np.array(dlb)
# set mu
mu = []
for i in range(N):
mu_ele = self.mu_segment_values[-1]
for j in range(self.N_mu_segments-1):
if(self.s_begin_mu_segments[j]-0.01 <= s[i] <= self.s_begin_mu_segments[j+1]):
mu_ele = self.mu_segment_values[j]
break
mu.append(mu_ele)
mu = np.array(mu)
# plot to see what we're doing
if plot_orientation:
fig, axs = plt.subplots(3,1)
axs[0].plot(s,psic,'k*')
axs[0].set_title('psic')
#for j in range(len(psic_piecewise)):
# ax.plot(s_piecewise[j],psic_piecewise[j],'.')
#ax.plot(s,psic_cont,'r')
axs[0].plot(s,psic_out,'m.')
axs[1].plot(s,kappac_out,'.m')
axs[1].set_title('kappac')
plt.show()
if plot_track:
fig, ax = plt.subplots()
ax.axis("equal")
ax.plot(fcl_X,fcl_Y, '.b') # fine centerline
ax.plot(fll_X,fll_Y, '.b') # fine left line
ax.plot(frl_X,frl_Y, '.b') # fine right line
ax.plot(cl_X,cl_Y, '*k') # cones left
ax.plot(cr_X,cr_Y, '*k') # cones right
ax.plot(ccl_X,ccl_Y, '*r') # coarse centerline
plt.show()
if plot_dlbdub:
fig, axs = plt.subplots(3,1)
axs[0].plot(s,dlb,'.b')
axs[1].plot(s,dub,'.b')
axs[2].plot(s,1.0/kappac_out,'r.')
#axs[2]([np.min(dlb),np.max(dub)])
plt.show()
if plot_mu:
fig, ax = plt.subplots()
ax.plot(s,mu,'.b')
plt.show()
# put all in message and publish
self.pathglobal.X = fcl_X
self.pathglobal.Y = fcl_Y
self.pathglobal.s = s
self.pathglobal.psi_c = psic_out
self.pathglobal.kappa_c = kappac_out
self.pathglobal.kappaprime_c = kappacprime_out
self.pathglobal.theta_c = np.zeros(N) # grade/bank implement later
self.pathglobal.mu = mu
self.pathglobal.dub = dub
self.pathglobal.dlb = dlb
print "publishing pathglobal"
self.pathglobalpub.publish(self.pathglobal)
# publish paths for rviz
pathglobalvis = navPath()
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = fcl_X[i]
pose.pose.position.y = fcl_Y[i]
quaternion = tf.transformations.quaternion_from_euler(0, 0, psic[i])
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
pathglobalvis.poses.append(pose)
pathglobalvis.header.stamp = rospy.Time.now()
pathglobalvis.header.frame_id = "map"
print "publishing pathglobal visualization"
self.pathglobalvispub.publish(pathglobalvis)
# test correctness of dub and dlb in rviz
Xleft,Yleft = ptsFrenetToCartesian(s,dub,fcl_X,fcl_Y,psic,s)
pathleft = navPath()
pathleft.header.stamp = rospy.Time.now()
pathleft.header.frame_id = "map"
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = Xleft[i]
pose.pose.position.y = Yleft[i]
pathleft.poses.append(pose)
self.dubvispub.publish(pathleft)
Xright,Yright = ptsFrenetToCartesian(s,dlb,fcl_X,fcl_Y,psic,s)
pathright = navPath()
pathright.header.stamp = rospy.Time.now()
pathright.header.frame_id = "map"
for i in range(N):
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "map"
pose.pose.position.x = Xright[i]
pose.pose.position.y = Yright[i]
pathright.poses.append(pose)
self.dlbvispub.publish(pathright)