def toPath(self):
path = navPath()
path.header = make_header("/map")
for p in self.points:
pose = PoseStamped()
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = 0
pose.pose.orientation = angle_to_quaternion(p[2])
path.poses.append(pose)
return path
def toPath(self):
path = navPath()
path.header = make_header("/map")
q = angle_to_quaternion(np.arctan((self.points[-1][1]-self.points[0][1])/(self.points[-1][0]-self.points[0][0])))
for p in self.points:
pose = PoseStamped()
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = 0
pose.pose.orientation = q
path.poses.append(pose)
return path
def publish_path(self, duration=0):
if not self.visualize:
print "Cannot visualize path, not initialized with visualization enabled"
return
if self.pub_path.get_num_connections() > 0:
path = navPath()
path.header = make_header("/map")
for p in self.points:
pose = PoseStamped()
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = 0
pose.pose.orientation = angle_to_quaternion(0)
path.poses.append(pose)
self.pub_path.publish(path)
elif self.pub_path.get_num_connections() == 0:
print "Not publishing recorded path, no subscriber."
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):
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)