Apollo學習筆記（二）：循跡實現過程
Apollo開發者套件買來后，就可以按照官網循跡教程錄制軌跡，開始循跡。在此主要對Apollo循跡的實現過程進行學習。（注意代碼里面是有我寫的注釋的）
錄制循跡數據包
1 cd /apollo/scripts 2 bash rtk_recorder.sh setup 3 bash rtk_recorder.sh start （ 命令輸入后，開始開車走一段軌跡 ） 4 bash rtk_recorder.sh stop( 如果無法輸入就按Ctrl + C結束 )
setup
其中rtk_recorder.sh的setup函數如下
1 function setup() { 2 bash scripts/canbus.sh start 3 bash scripts/gps.sh start 4 bash scripts/localization.sh start 5 bash scripts/control.sh start 6 }
以canbus.sh為例
1 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"--獲取所執行腳本的絕對路徑 2 3 cd "${DIR}/.."-- 進入腳本所在路徑的上一層 4 5 source "$DIR/apollo_base.sh"--引入apollo_base.sh 6 7 # run function from apollo_base.sh 8 # run command_name module_name 9 run canbus "$@" --執行apollo_base.sh的run函數
對於
1 DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
更加詳細的分析可以參考https://blog.csdn.net/liuxiaodong400/article/details/85293670，但意義不大，這個相當於固定命令了。
可以發現canbus.sh內並沒有start函數，實際上這里調用了apollo_base.sh的run函數。run canbus "$@" 中"$@"是傳入的參數，展開來就是run canbus start，apollo_base.sh的run函數如下：
1 # run command_name module_name 2 function run() { 3 local module=$1 4 shift 5 run_customized_path $module $module "$@" 6 }
其中module的值為canbus，"$@"的值為start，因此展開為run_customized_path canbus canbus start，run_customized_path函數如下：
1 function run_customized_path() { 2 local module_path=$1 --module_path為canbus 3 local module=$2 --module為canbus 4 local cmd=$3 --cmd為start 5 shift 3 --向左移動參數3個，因為參數只有canbus canbus start這三個，因此參數為空 6 case $cmd in 7 start) -- 對應於此處 8 start_customized_path $module_path $module "$@"--因為上面向左移動參數， 9 -- 導致參數為空，所以"$@"為空，此處為start_customized_path canbus canbus 10 ;; 11 start_fe) 12 start_fe_customized_path $module_path $module "$@" 13 ;; 14 start_gdb) 15 start_gdb_customized_path $module_path $module "$@" 16 ;; 17 start_prof) 18 start_prof_customized_path $module_path $module "$@" 19 ;; 20 stop) 21 stop_customized_path $module_path $module 22 ;; 23 help) 24 help 25 ;; 26 *) 27 start_customized_path $module_path $module $cmd "$@" 28 ;; 29 esac 30 }
因此執行start_customized_path canbus canbus，start_customized_path函數如下：
1 function start_customized_path() { 2 MODULE_PATH=$1 --MODULE_PATH為canbus 3 MODULE=$2 --MODULE為canbus 4 shift 2 --向左移動參數2個 5 6 LOG="${APOLLO_ROOT_DIR}/data/log/${MODULE}.out" 7 is_stopped_customized_path "${MODULE_PATH}" "${MODULE}" 8 if [ $? -eq 1 ]; then 9 eval "nohup ${APOLLO_BIN_PREFIX}/modules/${MODULE_PATH}/${MODULE} \ 10 --flagfile=modules/${MODULE_PATH}/conf/${MODULE}.conf \ 11 --log_dir=${APOLLO_ROOT_DIR}/data/log $@ </dev/null >${LOG} 2>&1 &" 12 sleep 0.5 13 is_stopped_customized_path "${MODULE_PATH}" "${MODULE}" 14 if [ $? -eq 0 ]; then 15 echo "Launched module ${MODULE}." 16 return 0 17 else 18 echo "Could not launch module ${MODULE}. Is it already built?" 19 return 1 20 fi 21 else 22 echo "Module ${MODULE} is already running - skipping." 23 return 2 24 fi 25 }
其中is_stopped_customized_path函數如下：
1 function is_stopped_customized_path() { 2 MODULE_PATH=$1 3 MODULE=$2 4 NUM_PROCESSES="$(pgrep -c -f "modules/${MODULE_PATH}/${MODULE}")" 5 if [ "${NUM_PROCESSES}" -eq 0 ]; then 6 return 1 7 else 8 return 0 9 fi 10 }
pgrep是linux用於檢查在系統中活動進程的命令，-c 僅匹配列表中列出的ID的進程，-f 正則表達式模式將執行與完全進程參數字符串匹配。$(pgrep -c -f "modules/${MODULE_PATH}/${MODULE}")的作用是判斷canbus模塊是不是已經啟動了。如果沒啟動則返回1，已啟動則返回0。
start_customized_path根據is_stopped_customized_path的反饋選擇相應動作，如果canbus模塊沒啟動，則使用指令
1 nohup ${APOLLO_BIN_PREFIX}/modules/${MODULE_PATH}/${MODULE} \ 2 --flagfile=modules/${MODULE_PATH}/conf/${MODULE}.conf \ 3 --log_dir=${APOLLO_ROOT_DIR}/data/log $@ </dev/null >${LOG} 2>&1 &
以非掛斷方式啟動后台進程模塊canbus。其中APOLLO_BIN_PREFIX在determine_bin_prefix函數中確定
1 function determine_bin_prefix() { 2 APOLLO_BIN_PREFIX=$APOLLO_ROOT_DIR 3 if [ -e "${APOLLO_ROOT_DIR}/bazel-bin" ]; then 4 APOLLO_BIN_PREFIX="${APOLLO_ROOT_DIR}/bazel-bin" 5 fi 6 export APOLLO_BIN_PREFIX 7 }
APOLLO_ROOT_DIR是APOLLO_ROOT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )/.." && pwd )"，即當前文件夾。還記得之前canbus.sh內的cd "${DIR}/.."-- 進入腳本所在路徑的上一層嗎？所以此時的當前文件夾已經變為自己的目錄\apollo，所以APOLLO_ROOT_DIR=自己的目錄\apollo，APOLLO_BIN_PREFIX=自己的目錄\apollo\bazel-bin。所以就是以非掛斷方式執行自己的目錄\apollo\bazel-bin\modules\canbus\canbus這個編譯后的可執行文件，並且后面帶上--flagfile和--log_dir這些參數。
canbus模塊的入口是其main函數，也就是啟動canbus模塊首先自動執行其main函數，其main函數就位於自己的目錄\apollo\modules\canbus\main.cc中如下所示：
1 #include "modules/canbus/canbus.h" 2 #include "modules/canbus/common/canbus_gflags.h" 3 #include "modules/common/apollo_app.h" 4 5 APOLLO_MAIN(apollo::canbus::Canbus);
后續的過程在我另一篇博客Apollo學習筆記（一）：canbus模塊與車輛底盤之間的CAN數據傳輸過程詳細說明了，在此就不贅述了。
start
在啟動完成一些必備功能模塊后，執行bash rtk_recorder.sh start，rtk_recorder.sh的start函數如下：
1 function start() { 2 TIME=`date +%F_%H_%M` 3 if [ -e data/log/garage.csv ]; then 4 cp data/log/garage.csv data/log/garage-${TIME}.csv 5 fi 6 7 NUM_PROCESSES="$(pgrep -c -f "record_play/rtk_recorderpy")" 8 if [ "${NUM_PROCESSES}" -eq 0 ]; then 9 python modules/tools/record_play/rtk_recorder.py 10 fi 11 }
如果record_play/rtk_recorderpy進程沒有啟動，則運行python modules/tools/record_play/rtk_recorder.py，首先執行main(sys.argv)
rtk_recorder.py的main函數如下：
1 def main(argv): 2 """ 3 Main rosnode 4 """ 5 rospy.init_node('rtk_recorder', anonymous=True) 6 7 argv = FLAGS(argv) 8 log_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../../data/log/" 9 if not os.path.exists(log_dir): 10 os.makedirs(log_dir) 11 Logger.config( 12 log_file=log_dir + "rtk_recorder.log", 13 use_stdout=True, 14 log_level=logging.DEBUG) 15 print("runtime log is in %s%s" % (log_dir, "rtk_recorder.log")) 16 record_file = log_dir + "/garage.csv" 17 recorder = RtkRecord(record_file) 18 atexit.register(recorder.shutdown) 19 rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis, 20 recorder.chassis_callback) 21 22 rospy.Subscriber('/apollo/localization/pose', 23 localization_pb2.LocalizationEstimate, 24 recorder.localization_callback) 25 26 rospy.spin()
前面rospy.init_node('rtk_recorder', anonymous=True)是ros自帶的建ros節點的命令，接着log_dir和log_file是建文件夾和文件記錄log的。重要的是recorder = RtkRecord(record_file)，RtkRecord這個類的定義如下：
1 class RtkRecord(object): 2 """ 3 rtk recording class 4 """ 5 def __init__(self, record_file): 6 self.firstvalid = False 7 self.logger = Logger.get_logger("RtkRecord") 8 self.record_file = record_file 9 self.logger.info("Record file to: " + record_file) 10 11 try: 12 self.file_handler = open(record_file, 'w') 13 except: 14 self.logger.error("open file %s failed" % (record_file)) 15 self.file_handler.close() 16 sys.exit() 17 18 //向record_file中寫入數據，就是第一行寫上變量名 19 self.write("x,y,z,speed,acceleration,curvature,"\ 20 "curvature_change_rate,time,theta,gear,s,throttle,brake,steering\n") 21 22 //設置成員變量 23 self.localization = localization_pb2.LocalizationEstimate() 24 self.chassis = chassis_pb2.Chassis() 25 self.chassis_received = False 26 27 self.cars = 0.0 28 self.startmoving = False 29 30 self.terminating = False 31 self.carcurvature = 0.0 32 33 self.prev_carspeed = 0.0
后面atexit.register(recorder.shutdown)的作用是在腳本運行完后立馬執行一些代碼，關於atexit模塊這個python自帶的模塊的更詳細內容請查閱https://www.cnblogs.com/sigai/articles/7236494.html
接着節點訂閱/apollo/canbus/chassis和/apollo/localization/pose這兩個topic，對應的回調函數分別是recorder.chassis_callback和recorder.localization_callback
回調函數recorder.chassis_callback為
1 def chassis_callback(self, data): 2 """ 3 New message received 4 """ 5 if self.terminating == True: 6 self.logger.info("terminating when receive chassis msg") 7 return 8 9 self.chassis.CopyFrom(data) 10 #self.chassis = data 11 if math.isnan(self.chassis.speed_mps): 12 self.logger.warning("find nan speed_mps: %s" % str(self.chassis)) 13 if math.isnan(self.chassis.steering_percentage): 14 self.logger.warning( 15 "find nan steering_percentage: %s" % str(self.chassis)) 16 self.chassis_received = True
沒做啥，就是self.chassis.CopyFrom(data)把canbus模塊傳來的數據存到成員變量self.chassis
回調函數recorder.localization_callback為
1 def localization_callback(self, data): 2 """ 3 New message received 4 """ 5 if self.terminating == True: 6 self.logger.info("terminating when receive localization msg") 7 return 8 9 //首先要收到底盤canbus模塊傳來的數據 10 if not self.chassis_received: 11 self.logger.info( 12 "chassis not received when localization is received") 13 return 14 //將定位數據傳入成員變量self.localization 15 self.localization.CopyFrom(data) 16 #self.localization = data 17 //讀取本車位置的x,y,z坐標，x,y,z坐標是UTM坐標系下的，UTM坐標系簡單來說 18 //就是選定一個經緯度作為原點，其他點的經緯度與原點的距離就是其x,y,z坐標 19 carx = self.localization.pose.position.x 20 cary = self.localization.pose.position.y 21 carz = self.localization.pose.position.z 22 cartheta = self.localization.pose.heading 23 if math.isnan(self.chassis.speed_mps): 24 self.logger.warning("find nan speed_mps: %s" % str(self.chassis)) 25 return 26 if math.isnan(self.chassis.steering_percentage): 27 self.logger.warning( 28 "find nan steering_percentage: %s" % str(self.chassis)) 29 return 30 carspeed = self.chassis.speed_mps 31 caracceleration = self.localization.pose.linear_acceleration_vrf.y 32 33 speed_epsilon = 1e-9 34 //如果上次車速和本次車速都為0，則認為加速度為0 35 if abs(self.prev_carspeed) < speed_epsilon \ 36 and abs(carspeed) < speed_epsilon: 37 caracceleration = 0.0 38 39 //車輛轉角 40 carsteer = self.chassis.steering_percentage 41 //曲率 42 curvature = math.tan(math.radians(carsteer / 100 * 470) / 16) / 2.85 43 if abs(carspeed) >= speed_epsilon: 44 carcurvature_change_rate = (curvature - self.carcurvature) / ( 45 carspeed * 0.01) 46 else: 47 carcurvature_change_rate = 0.0 48 self.carcurvature = curvature 49 cartime = self.localization.header.timestamp_sec 50 //檔位 51 cargear = self.chassis.gear_location 52 53 if abs(carspeed) >= speed_epsilon: 54 if self.startmoving == False: 55 self.logger.info( 56 "carspeed !=0 and startmoving is False, Start Recording") 57 self.startmoving = True 58 59 if self.startmoving: 60 self.cars = self.cars + carspeed * 0.01 61 //往之前設置的文件內寫入數據 62 self.write( 63 "%s, %s, %s, %s, %s, %s, %s, %.4f, %s, %s, %s, %s, %s, %s\n" % 64 (carx, cary, carz, carspeed, caracceleration, self.carcurvature, 65 carcurvature_change_rate, cartime, cartheta, cargear, 66 self.cars, self.chassis.throttle_percentage, 67 self.chassis.brake_percentage, 68 self.chassis.steering_percentage)) 69 self.logger.debug( 70 "started moving and write data at time %s" % cartime) 71 else: 72 self.logger.debug("not start moving, do not write data to file") 73 74 self.prev_carspeed = carspeed
Ctrl + C結束后，apollo將會錄制一個軌跡數據包garage.csv，放在data/log/下（主要記錄了位置、剎車、油門、方向、速度等信息）。
stop
rtk_recorder.sh的stop函數如下：
1 function stop() { 2 pkill -SIGKILL -f rtk_recorder.py 3 }
很簡單，就是殺死線程。
回放數據包開始循跡
N檔，線控開啟，輸入以下命令：
1 bash scripts/rtk_player.sh setup 2 bash scripts/rtk_player.sh start （ 這個命令敲完，車還不會反應 ）
點擊Dreamview界面的start auto，這時候車子會出現反應，並且是大反應（司機注意接管）。bash scripts/rtk_player.sh start 這一步只是把record數據重新放出來，或者對路徑進行規划，即完成planning的過程。
下面我們看看代碼運行流程
首先是bash scripts/rtk_player.sh setup，rtk_player.sh的setup函數如下：
1 function setup() { 2 bash scripts/canbus.sh start 3 bash scripts/gps.sh start 4 bash scripts/localization.sh start 5 bash scripts/control.sh start 6 }
還是跟上面一樣啟動一些必備的模塊，具體啟動過程是一樣的。
接着bash scripts/rtk_player.sh start，rtk_player.sh的start函數如下：
1 function start() { 2 NUM_PROCESSES="$(pgrep -c -f "record_play/rtk_player.py")" 3 if [ "${NUM_PROCESSES}" -ne 0 ]; then 4 pkill -SIGKILL -f rtk_player.py 5 fi 6 7 python modules/tools/record_play/rtk_player.py 8 }
其他就不再重復了，我們看rtk_player.py的main函數
1 def main(): 2 """ 3 Main rosnode 4 """ 5 parser = argparse.ArgumentParser( 6 description='Generate Planning Trajectory from Data File') 7 parser.add_argument( 8 '-s', 9 '--speedmulti', 10 help='Speed multiplier in percentage (Default is 100) ', 11 type=float, 12 default='100') 13 parser.add_argument( 14 '-c', '--complete', help='Generate complete path (t/F)', default='F') 15 parser.add_argument( 16 '-r', 17 '--replan', 18 help='Always replan based on current position(t/F)', 19 default='F') 20 args = vars(parser.parse_args()) 21 22 rospy.init_node('rtk_player', anonymous=True) 23 24 Logger.config( 25 log_file=os.path.join(APOLLO_ROOT, 'data/log/rtk_player.log'), 26 use_stdout=True, 27 log_level=logging.DEBUG) 28 29 record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv') 30 player = RtkPlayer(record_file, args['speedmulti'], 31 args['complete'].lower(), args['replan'].lower()) 32 atexit.register(player.shutdown) 33 34 rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis, 35 player.chassis_callback) 36 37 rospy.Subscriber('/apollo/localization/pose', 38 localization_pb2.LocalizationEstimate, 39 player.localization_callback) 40 41 rospy.Subscriber('/apollo/control/pad', pad_msg_pb2.PadMessage, 42 player.padmsg_callback) 43 44 rate = rospy.Rate(10) 45 while not rospy.is_shutdown(): 46 player.publish_planningmsg() 47 rate.sleep()
前面使用python Argparse模塊輸入一些默認參數，具體使用參考https://www.jianshu.com/p/c4a66b5155d3?utm_campaign=maleskine&utm_content=note&utm_medium=seo_notes&utm_source=recommendation
接着player = RtkPlayer(record_file, args['speedmulti'], args['complete'].lower(), args['replan'].lower())實例化RtkPlayer，其中record_file是record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv')，也就是之前記錄的軌跡文件；args['speedmulti']是默認值100，args['complete'].lower()是默認值f；args['replan'].lower()也是默認值f。
RtkPlayer這個類的初始化函數如下：
1 class RtkPlayer(object): 2 """ 3 rtk player class 4 """ 5 6 def __init__(self, record_file, speedmultiplier, completepath, replan): 7 """Init player.""" 8 self.firstvalid = False 9 self.logger = Logger.get_logger(tag="RtkPlayer") 10 self.logger.info("Load record file from: %s" % record_file) 11 try: 12 file_handler = open(record_file, 'r') 13 except: 14 self.logger.error("Cannot find file: " + record_file) 15 file_handler.close() 16 sys.exit(0) 17 18 //從之前記錄的軌跡文件中提取數據 19 self.data = genfromtxt(file_handler, delimiter=',', names=True) 20 file_handler.close() 21 22 self.localization = localization_pb2.LocalizationEstimate() 23 self.chassis = chassis_pb2.Chassis() 24 self.padmsg = pad_msg_pb2.PadMessage() 25 self.localization_received = False 26 self.chassis_received = False 27 28 //創建發布topic為/apollo/planning的發布者， 29 //消息格式為planning_pb2.ADCTrajectory，隊列長度為1 30 self.planning_pub = rospy.Publisher( 31 '/apollo/planning', planning_pb2.ADCTrajectory, queue_size=1) 32 33 self.speedmultiplier = speedmultiplier / 100 34 self.terminating = False 35 self.sequence_num = 0 36 37 //對加速度acceleration進行濾波 38 //scipy.signal.butter(N, Wn, btype='low', analog=False, output='ba') 39 //輸入參數： 40 //N:濾波器的階數 41 //Wn：歸一化截止頻率。計算公式Wn=2*截止頻率/采樣頻率。（注意：根據采樣定理，采樣頻//率要大於兩倍的信號本身最大的頻率，才能還原信號。截止頻率一定小於信號本身最大的頻//率，所以Wn一定在0和1之間）。當構造帶通濾波器或者帶阻濾波器時，Wn為長度為2的列表。 42 //btype : 濾波器類型{‘lowpass’, ‘highpass’, ‘bandpass’, ‘bandstop’}, 43 //output : 輸出類型{‘ba’, ‘zpk’, ‘sos’}, 44 //輸出參數： 45 //b，a: IIR濾波器的分子（b）和分母（a）多項式系數向量。output='ba' 46 //z,p,k: IIR濾波器傳遞函數的零點、極點和系統增益. output= 'zpk' 47 //sos: IIR濾波器的二階截面表示。output= 'sos' 48 //具體參考https://blog.csdn.net/qq_38984928/article/details/89232898 49 50 b, a = signal.butter(6, 0.05, 'low') 51 self.data['acceleration'] = signal.filtfilt(b, a, self.data['acceleration']) 52 53 self.start = 0 54 self.end = 0 55 self.closestpoint = 0 56 self.automode = False 57 58 //因為輸入的replan和completepath都是f，因此self.replan和self.completepath都是false 59 self.replan = (replan == 't') 60 self.completepath = (completepath == 't') 61 62 self.estop = False 63 self.logger.info("Planning Ready")
隨后訂閱/apollo/canbus/chassis，/apollo/localization/pose和/apollo/control/pad這三個topic，對應的回調函數分別是player.chassis_callback，player.localization_callback和player.padmsg_callback
我們先看player.chassis_callback
1 def chassis_callback(self, data): 2 """ 3 New chassis Received 4 """ 5 self.chassis.CopyFrom(data) 6 //判斷是否是自動駕駛模式 7 self.automode = (self.chassis.driving_mode == 8 chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE) 9 self.chassis_received = True
接着player.localization_callback
1 def localization_callback(self, data): 2 """ 3 New localization Received 4 """ 5 //更新位置 6 self.localization.CopyFrom(data) 7 self.carx = self.localization.pose.position.x 8 self.cary = self.localization.pose.position.y 9 self.carz = self.localization.pose.position.z 10 self.localization_received = True
最后player.padmsg_callback
1 def padmsg_callback(self, data): 2 """ 3 New message received 4 """ 5 if self.terminating == True: 6 self.logger.info("terminating when receive padmsg") 7 return 8 //沒做啥，就是把消息中的數據拷貝至self.padmsg 9 self.padmsg.CopyFrom(data)
看到現在發現rtk_player.py里面沒啥東西，現在才到重點了player.publish_planningmsg()。我們看看publish_planningmsg函數里面究竟賣的什么貨：
1 def publish_planningmsg(self): 2 """ 3 Generate New Path 4 """ 5 if not self.localization_received: 6 self.logger.warning( 7 "locaization not received yet when publish_planningmsg") 8 return 9 10 //新建planning_pb2.ADCTrajectory消息，這是發布/apollo/planning所使用的消息格式 11 planningdata = planning_pb2.ADCTrajectory() 12 now = rospy.get_time() 13 planningdata.header.timestamp_sec = now 14 planningdata.header.module_name = "planning" 15 planningdata.header.sequence_num = self.sequence_num 16 self.sequence_num = self.sequence_num + 1 17 18 self.logger.debug( 19 "publish_planningmsg: before adjust start: self.start = %s, self.end=%s" 20 % (self.start, self.end)) 21 if self.replan or self.sequence_num <= 1 or not self.automode: 22 self.logger.info( 23 "trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s" 24 % (self.replan, self.sequence_num, self.automode)) 25 self.restart() 26 else: 27 timepoint = self.closest_time() 28 distpoint = self.closest_dist() 29 self.start = max(min(timepoint, distpoint) - 100, 0) 30 self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1) 31 32 xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2 33 ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2 34 if xdiff_sqr + ydiff_sqr > 4.0: 35 self.logger.info("trigger replan: distance larger than 2.0") 36 self.restart() 37 38 if self.completepath://此處completepath為false，因此不執行 39 self.start = 0 40 self.end = len(self.data) - 1 41 42 self.logger.debug( 43 "publish_planningmsg: after adjust start: self.start = %s, self.end=%s" 44 % (self.start, self.end)) 45 46 for i in range(self.start, self.end): 47 adc_point = pnc_point_pb2.TrajectoryPoint() 48 adc_point.path_point.x = self.data['x'][i] 49 adc_point.path_point.y = self.data['y'][i] 50 adc_point.path_point.z = self.data['z'][i] 51 adc_point.v = self.data['speed'][i] * self.speedmultiplier 52 adc_point.a = self.data['acceleration'][i] * self.speedmultiplier 53 adc_point.path_point.kappa = self.data['curvature'][i] 54 adc_point.path_point.dkappa = self.data['curvature_change_rate'][i] 55 56 time_diff = self.data['time'][i] - \ 57 self.data['time'][self.closestpoint] 58 59 adc_point.relative_time = time_diff / self.speedmultiplier - ( 60 now - self.starttime) 61 62 adc_point.path_point.theta = self.data['theta'][i] 63 adc_point.path_point.s = self.data['s'][i] 64 65 planningdata.trajectory_point.extend([adc_point]) 66 67 planningdata.estop.is_estop = self.estop 68 69 planningdata.total_path_length = self.data['s'][self.end] - \ 70 self.data['s'][self.start] 71 planningdata.total_path_time = self.data['time'][self.end] - \ 72 self.data['time'][self.start] 73 planningdata.gear = int(self.data['gear'][self.closest_time()]) 74 planningdata.engage_advice.advice = \ 75 drive_state_pb2.EngageAdvice.READY_TO_ENGAGE 76 77 self.planning_pub.publish(planningdata) 78 self.logger.debug("Generated Planning Sequence: " + 79 str(self.sequence_num - 1))
如果replan為true或者sequence_num<=1或者不是自動駕駛模式則調用restart()
1 def restart(self): 2 self.logger.info("before replan self.start=%s, self.closestpoint=%s" % 3 (self.start, self.closestpoint)) 4 5 self.closestpoint = self.closest_dist() 6 //先看下面對self.closest_dist()的分析 7 //基於對self.closest_dist()的假設 8 //假設self.closestpoint=50，則self.start仍為0,self.end=299 9 self.start = max(self.closestpoint - 100, 0) 10 self.starttime = rospy.get_time() 11 self.end = min(self.start + 1000, len(self.data) - 1) 12 self.logger.info("finish replan at time %s, self.closestpoint=%s" % 13 (self.starttime, self.closestpoint))
首先self.closest_dist()找到當前位置在上次記錄的軌跡中對應的是第幾條數據，所以循跡開始的時候需要將車開到以前的軌跡處才行，否則都找不到初始的點。當然循跡到中間出現問題退出自動駕駛模式，重啟自動駕駛模式后程序也能找到自己在原先軌跡中的位置，不必重頭開始，這也是restart()的意義所在吧。
1 def closest_dist(self): 2 shortest_dist_sqr = float('inf') 3 self.logger.info("before closest self.start=%s" % (self.start)) 4 5 //SEARCH_INTERVAL = 1000 6 //一開始的時候self.start=0，所以search_start=0；search_end=500和 7 //記錄的軌跡數據的長度中的最小值，假定上次記錄了300條數據， 8 //則search_end=300 9 search_start = max(self.start - SEARCH_INTERVAL / 2, 0) 10 search_end = min(self.start + SEARCH_INTERVAL / 2, len(self.data)) 11 start = self.start 12 for i in range(search_start, search_end): 13 dist_sqr = (self.carx - self.data['x'][i]) ** 2 + \ 14 (self.cary - self.data['y'][i]) ** 2 15 if dist_sqr <= shortest_dist_sqr: 16 start = i 17 shortest_dist_sqr = dist_sqr 18 //假設返回的是50 19 return start
如果不滿足（replan為true或者sequence_num<=1或者不是自動駕駛模式）則執行
1 timepoint = self.closest_time() 2 distpoint = self.closest_dist() 3 4 //先看下面的假設 5 //根據下面的假設，這里timepoint=51，distpoint=52，所以self.start=0 6 //同時結合上面len(self.data)=300的假設，所以self.end=299 7 self.start = max(min(timepoint, distpoint) - 100, 0) 8 self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1) 9 10 xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2 11 ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2 12 13 //如果時間最近的軌跡點跟當前位置的距離過大，則調用restart()重新找距離當前位置最近的軌跡點 14 if xdiff_sqr + ydiff_sqr > 4.0: 15 self.logger.info("trigger replan: distance larger than 2.0") 16 self.restart()
首先調用closest_time()找到在時間上距離我們前面假設找到的第50軌跡點最近的（時間差為正）軌跡點
1 def closest_time(self): 2 3 //self.starttime在上面restart()被設為當時的時刻 4 time_elapsed = rospy.get_time() - self.starttime 5 //根據上面的假設，這里closest_time = 0 6 closest_time = self.start 7 //此處就是time_diff=self.data['time'][0]-self.data['time'][50] 8 time_diff = self.data['time'][closest_time] - \ 9 self.data['time'][self.closestpoint] 10 11 //找到time_diff大於當前時刻與啟動時刻時差的那個軌跡點 12 while time_diff < time_elapsed and closest_time < (len(self.data) - 1): 13 closest_time = closest_time + 1 14 time_diff = self.data['time'][closest_time] - \ 15 self.data['time'][self.closestpoint] 16 //假設這個時間上的最近點為51 17 return closest_time
接着調用closest_dist()，跟前面restart()一樣就不再贅述了，也就是用來更新self.closestpoint，假設為52。（這個假設的編號貌似用處不大，先放着不管了）
最后在將序號在self.start, self.end之間的軌跡點都存入planningdata，最后self.planning_pub.publish(planningdata)發布出去，下面control模塊接收到消息后計算得到具體的油門、剎車、方向盤轉角傳遞給canbus模塊。
最后分析一通發現rtk_player.py也沒計算具體的控制量，只是根據時差和距離在上次記錄的軌跡點里面找到合適的范圍，將范圍內的軌跡點的數據都傳入planningdata后發布給下面的控制模塊計算具體的控制量。