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=自己的目录apolloazel-bin。所以就是以非挂断方式执行自己的目录apolloazel-binmodulescanbuscanbus这个编译后的可执行文件，并且后面带上--flagfile和--log_dir这些参数。
canbus模块的入口是其main函数，也就是启动canbus模块首先自动执行其main函数，其main函数就位于自己的目录apollomodulescanbusmain.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 ") 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 " % 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后发布给下面的控制模块计算具体的控制量。