def getMapPrimePoint():
# 临时试验直走一次跑道,即两条直线
mapEndPoint, numGPSallPoint = findMapEndPoint(numAllLaneGLB, distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
point0 = mapEndPoint[0][0]
point1 = mapEndPoint[0][1]
point2 = mapEndPoint[1][1]
point3 = mapEndPoint[1][0]
MapPrimePoint = [point0, point1, point2, point3]
return MapPrimePoint
def plotMapLine():
# 绘制直线地图
mapEndPoint, numGPSallPoint = findMapEndPoint(numAllLaneGLB,distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
for i in range(len(mapEndPoint)):
start_x, start_y = mapEndPoint[i][0]
end_x, end_y = mapEndPoint[i][1]
c_line = np.polyfit([start_x,end_x],[start_y,end_y],1)
x_line = np.linspace(start_x,end_x,100)
y_line = np.polyval(c_line,x_line)
plt.plot(x_line, y_line, 'b')
plt.plot(x_line[0],y_line[0],'ro') # 绘制起点
def __init__(self):
self.logPosition = []
self.logPositionAll = []
self.flagLogPosition = 0
self.flagIsLogging = False
self.indOptimalLast = 10
self.desireDirDrive = FORWARD # desireDirDrive是为了触发底层换挡
self.curDirDrive = self.desireDirDrive # curDirDrive是为了协调内部程序，如航向变换
self.flagIsShifting = 0
self.distLog = 0
self.countSingleLane = 0
self.countLaneFinish = 0
self.flagLaneFinish = 0
self.flagMCUIsShifting = 0
self.flagChangesPre = True
self.flagLogPointStart = True
self.pointStart = np.array([0,0])
self.newStraightPathFlag = False # 准备错距到新的直线碾压路径，用来合并换道规划路径
self.cloudTaskID = 0 # 云端接收到新任务，ID增1，通过此变量判断是否有新任务，有新任务后，重置相关标志位和地图
self.mapEndPoint, self.numGPSallPoint = findMapEndPoint(numAllLaneGLB,distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
np.save(os.path.join(cur_path,'../log/mapEndPoint.npy'),self.mapEndPoint)
self.indexCircleMap = 0
if mapTypeGLB == 'raw':
self.GPSmap = np.load(os.path.join(cur_path,'../map/rawMap.npy'))
else:
if taskTypeGLB == 'csdCircle':
genCurveMap()
self.GPSmap = np.load(os.path.join(cur_path,''.join(['../map/curveMap', str(self.indexCircleMap), '.npy'])))
elif taskTypeGLB == 'csdLine':
self.GPSmap = genMapPoint(self.mapEndPoint[self.countLaneFinish][0],self.mapEndPoint[self.countLaneFinish][1],self.numGPSallPoint)
while not flagPositionReceivedGLB:
print 'is waiting vehState topic'
if rospy.is_shutdown():
sys.exit()
time.sleep(0.02)
self.indLastClosest, self.flagClosestPointSearched = commonFcn.findClosestPointInit(recv, self.GPSmap)
self.indLastPre = self.indLastClosest # 检测是否换地图
self.dataPlanning = {'xCtrPath':[], 'yCtrPath':[], \
'xGlbPath':[], 'yGlbPath':[], \
'pathCandidate':np.array([[0,0],]), 'indOptimal':0, \
'modeDrive':NORMAL, 'vbrMode':0,'distObst':1000.}
self.distYClose = 0.
self.modeDrive = NORMAL
self.vbrMode = 0
self.prePoint = []
self.toCloudStartedFlag = False # 0: 未开始碾压任务； 1: 已按需开始云端任务
self.toCloudEndTaskFlag = False # 0: 任务未结束； 1: 当前碾压任务已完成
def __init__(self):
# 求取矩形区域四个顶点，四条边
# if taskTypeGLB == 'csdLine':
mapEndPoint, numGPSallPoint = findMapEndPoint(
numAllLaneGLB,
distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
print 'mapEndPoint', mapEndPoint
# 向distBetweenLaneGLB反方向一侧平移安全距离，获取1 2点
# 向左偏移，得到最左边
startPoint = mapEndPoint[0][0]
endPoint = mapEndPoint[0][1]
offsetDist = -np.sign(distBetweenLaneGLB) * SAFEDIST
self.point0, self.point1 = commonFcn.findOffsetLinePoint(
startPoint, endPoint, offsetDist)
# 按原偏移方向继续平移一次安全距离，获取3 4点
# 向右偏移得到最右边
startPoint = mapEndPoint[-1][0]
endPoint = mapEndPoint[-1][1]
offsetDist = np.sign(distBetweenLaneGLB) * SAFEDIST
self.point3, self.point2 = commonFcn.findOffsetLinePoint(
startPoint, endPoint, offsetDist)
# 再将上述两条边的四个顶点组成的前后两条边，进行前后偏移一定距离
# 以前边为参考，则应对其采取右偏
startPoint = self.point0
endPoint = self.point3
offsetDist = SAFEDIST
if taskTypeGLB == 'csdCircle':
offsetDist = SAFEDIST + distBetweenLaneGLB / 2.
self.point0, self.point3 = commonFcn.findOffsetLinePoint(
startPoint, endPoint, offsetDist)
# 以后边为参考，则应对其采取左偏
startPoint = self.point1
endPoint = self.point2
offsetDist = -SAFEDIST
self.point1, self.point2 = commonFcn.findOffsetLinePoint(
startPoint, endPoint, offsetDist)
plt.plot(self.point0, 'ro')
plt.plot(self.point1, 'ro')
plt.plot(self.point2, 'ro')
plt.plot(self.point3, 'ro')
# elif taskTypeGLB == 'csdCircle':
# pass
self.stopSafeArea = False
def globalMap():
mapEndPoint, numGPSallPoint = findMapEndPoint(
numAllLaneGLB,
distBetweenLaneGLB) # (numAllLaneGLB,distOffsetStep=1.5)
marker = Marker()
marker.header.frame_id = "gps_marker"
marker.header.stamp = rospy.Time.now()
marker.ns = "campus_driving"
marker.id = 0
marker.type = Marker.LINE_LIST
for i in xrange(numAllLaneGLB):
p = Point()
route_x_st, route_y_st = mapEndPoint[i][0]
p.x = route_x_st
p.y = route_y_st
p.z = 0.
marker.points.append(p)
p = Point() # note: without it, line_list can not display in rviz
route_x_end, route_y_end = mapEndPoint[i][1]
p.x = route_x_end
p.y = route_y_end
p.z = 0.
marker.points.append(p)
marker.action = Marker.ADD
marker.pose.orientation.w = 1.0
marker.scale.x = 0.1
# marker.scale.y = 0.1
# marker.scale.z = 0.1
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.lifetime = rospy.Duration()
pubMap.publish(marker)
def callbackCAN_RX(msg):
global allow_route, CAN_is_normal
allow_route = msg.allow_route
CAN_is_normal = msg.is_normal
if __name__ == '__main__':
# Loading Parameters
base_path = os.path.dirname(__file__)
param_file = base_path + '/../params/param.yaml'
with open(param_file, 'r') as f:
param = yaml.load(f)
numAllLane = param["map"]["numAllLane"]
distBetweenLane = param["map"]["distBetweenLane"]
mapEndPoint, numGPSallPoint = findMapEndPoint(
numAllLane, distBetweenLane) # (numAllLane,distOffsetStep=1.5)
rospy.init_node('CAN_Driver', anonymous=True)
fd = libtest.open_can("can1", 250)
rospy.Subscriber("vehState", VehState, Callback)
rospy.Subscriber("/mcu_flag", mcuFlag, callbackCAN_RX)
mode = Subscriber('/planOutput', PlanOP)
steer = Subscriber('/steerAngleCmd', stm32TX)
tss1 = ApproximateTimeSynchronizer([mode, steer], 10, 0.1)
tss1.registerCallback(CallbackSteer)
rospy.spin()
libtest.close_can(fd)