Apollo学习笔记（二）：循迹实现过程
Apollo学习笔记（二）：循迹实现过程
Apollo开发者套件买来后，就可以按照官网循迹教程录制轨迹，开始循迹。在此主要对Apollo循迹的实现过程进行学习。（注意代码里面是有我写的注释的）
录制循迹数据包
cd /apollo/scripts
bash rtk_recorder.sh setup
bash rtk_recorder.sh start （ 命令输入后，开始开车走一段轨迹 ）
bash rtk_recorder.sh stop( 如果无法输入就按Ctrl + C结束 )
setup
其中rtk_recorder.sh的setup函数如下
function setup() {
bash scripts/canbus.sh start
bash scripts/gps.sh start
bash scripts/localization.sh start
bash scripts/control.sh start
}
以canbus.sh为例
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"--获取所执行脚本的绝对路径
cd "${DIR}/.."-- 进入脚本所在路径的上一层
source "$DIR/apollo_base.sh"--引入apollo_base.sh
# run function from apollo_base.sh
# run command_name module_name
run canbus "$@" --执行apollo_base.sh的run函数
对于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函数如下：
# run command_name module_name
function run() {
local module=$1
shift
run_customized_path $module $module "$@"
}
其中module的值为canbus，"$@"的值为start，因此展开为run_customized_path canbus canbus start，run_customized_path函数如下：
function run_customized_path() {
local module_path=$1 --module_path为canbus
local module=$2 --module为canbus
local cmd=$3 --cmd为start
shift 3 --向左移动参数3个，因为参数只有canbus canbus start这三个，因此参数为空
case $cmd in
start) -- 对应于此处
start_customized_path $module_path $module "$@"--因为上面向左移动参数，
-- 导致参数为空，所以"$@"为空，此处为start_customized_path canbus canbus
;;
start_fe)
start_fe_customized_path $module_path $module "$@"
;;
start_gdb)
start_gdb_customized_path $module_path $module "$@"
;;
start_prof)
start_prof_customized_path $module_path $module "$@"
;;
stop)
stop_customized_path $module_path $module
;;
help)
help
;;
*)
start_customized_path $module_path $module $cmd "$@"
;;
esac
}
因此执行start_customized_path canbus canbus，start_customized_path函数如下：
function start_customized_path() {
MODULE_PATH=$1 --MODULE_PATH为canbus
MODULE=$2 --MODULE为canbus
shift 2 --向左移动参数2个
LOG="${APOLLO_ROOT_DIR}/data/log/${MODULE}.out"
is_stopped_customized_path "${MODULE_PATH}" "${MODULE}"
if [ $? -eq 1 ]; then
eval "nohup ${APOLLO_BIN_PREFIX}/modules/${MODULE_PATH}/${MODULE} \
--flagfile=modules/${MODULE_PATH}/conf/${MODULE}.conf \
--log_dir=${APOLLO_ROOT_DIR}/data/log $@ ${LOG} 2>&1 &"
sleep 0.5
is_stopped_customized_path "${MODULE_PATH}" "${MODULE}"
if [ $? -eq 0 ]; then
echo "Launched module ${MODULE}."
return 0
else
echo "Could not launch module ${MODULE}. Is it already built?"
return 1
fi
else
echo "Module ${MODULE} is already running - skipping."
return 2
fi
}
其中is_stopped_customized_path函数如下：
function is_stopped_customized_path() {
MODULE_PATH=$1
MODULE=$2
NUM_PROCESSES="$(pgrep -c -f "modules/${MODULE_PATH}/${MODULE}")"
if [ "${NUM_PROCESSES}" -eq 0 ]; then
return 1
else
return 0
fi
}
pgrep是linux用于检查在系统中活动进程的命令，-c 仅匹配列表中列出的ID的进程，-f 正则表达式模式将执行与完全进程参数字符串匹配。$(pgrep -c -f "modules/${MODULE_PATH}/${MODULE}")的作用是判断canbus模块是不是已经启动了。如果没启动则返回1，已启动则返回0。
start_customized_path根据is_stopped_customized_path的反馈选择相应动作，如果canbus模块没启动，则使用指令
nohup ${APOLLO_BIN_PREFIX}/modules/${MODULE_PATH}/${MODULE} \
--flagfile=modules/${MODULE_PATH}/conf/${MODULE}.conf \
--log_dir=${APOLLO_ROOT_DIR}/data/log $@ </dev/null >${LOG} 2>&1 &
以非挂断方式启动后台进程模块canbus。其中APOLLO_BIN_PREFIX在determine_bin_prefix函数中确定
function determine_bin_prefix() {
APOLLO_BIN_PREFIX=$APOLLO_ROOT_DIR
if [ -e "${APOLLO_ROOT_DIR}/bazel-bin" ]; then
APOLLO_BIN_PREFIX="${APOLLO_ROOT_DIR}/bazel-bin"
fi
export APOLLO_BIN_PREFIX
}
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中如下所示：
#include "modules/canbus/canbus.h"
#include "modules/canbus/common/canbus_gflags.h"
#include "modules/common/apollo_app.h"
APOLLO_MAIN(apollo::canbus::Canbus);
后续的过程在我另一篇博客Apollo学习笔记（一）：canbus模块与车辆底盘之间的CAN数据传输过程详细说明了，在此就不赘述了。
start
在启动完成一些必备功能模块后，执行bash rtk_recorder.sh start，rtk_recorder.sh的start函数如下：
function start() {
TIME=`date +%F_%H_%M`
if [ -e data/log/garage.csv ]; then
cp data/log/garage.csv data/log/garage-${TIME}.csv
fi
NUM_PROCESSES="$(pgrep -c -f "record_play/rtk_recorderpy")"
if [ "${NUM_PROCESSES}" -eq 0 ]; then
python modules/tools/record_play/rtk_recorder.py
fi
}
如果record_play/rtk_recorderpy进程没有启动，则运行python modules/tools/record_play/rtk_recorder.py，首先执行main(sys.argv)
rtk_recorder.py的main函数如下：
def main(argv):
"""
Main rosnode
"""
rospy.init_node('rtk_recorder', anonymous=True)
argv = FLAGS(argv)
log_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../../data/log/"
if not os.path.exists(log_dir):
os.makedirs(log_dir)
Logger.config(
log_file=log_dir + "rtk_recorder.log",
use_stdout=True,
log_level=logging.DEBUG)
print("runtime log is in %s%s" % (log_dir, "rtk_recorder.log"))
record_file = log_dir + "/garage.csv"
recorder = RtkRecord(record_file)
atexit.register(recorder.shutdown)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
recorder.chassis_callback)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
recorder.localization_callback)
rospy.spin()
前面rospy.init_node('rtk_recorder', anonymous=True)是ros自带的建ros节点的命令，接着log_dir和log_file是建文件夹和文件记录log的。重要的是recorder = RtkRecord(record_file)，RtkRecord这个类的定义如下：
class RtkRecord(object):
"""
rtk recording class
"""
def __init__(self, record_file):
self.firstvalid = False
self.logger = Logger.get_logger("RtkRecord")
self.record_file = record_file
self.logger.info("Record file to: " + record_file)
try:
self.file_handler = open(record_file, 'w')
except:
self.logger.error("open file %s failed" % (record_file))
self.file_handler.close()
sys.exit()
//向record_file中写入数据，就是第一行写上变量名
self.write("x,y,z,speed,acceleration,curvature,"\
"curvature_change_rate,time,theta,gear,s,throttle,brake,steering\n")
//设置成员变量
self.localization = localization_pb2.LocalizationEstimate()
self.chassis = chassis_pb2.Chassis()
self.chassis_received = False
self.cars = 0.0
self.startmoving = False
self.terminating = False
self.carcurvature = 0.0
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为
def chassis_callback(self, data):
"""
New message received
"""
if self.terminating == True:
self.logger.info("terminating when receive chassis msg")
return
self.chassis.CopyFrom(data)
#self.chassis = data
if math.isnan(self.chassis.speed_mps):
self.logger.warning("find nan speed_mps: %s" % str(self.chassis))
if math.isnan(self.chassis.steering_percentage):
self.logger.warning(
"find nan steering_percentage: %s" % str(self.chassis))
self.chassis_received = True
没做啥，就是self.chassis.CopyFrom(data)把canbus模块传来的数据存到成员变量self.chassis
回调函数recorder.localization_callback为
def localization_callback(self, data):
"""
New message received
"""
if self.terminating == True:
self.logger.info("terminating when receive localization msg")
return
//首先要收到底盘canbus模块传来的数据
if not self.chassis_received:
self.logger.info(
"chassis not received when localization is received")
return
//将定位数据传入成员变量self.localization
self.localization.CopyFrom(data)
#self.localization = data
//读取本车位置的x,y,z坐标，x,y,z坐标是UTM坐标系下的，UTM坐标系简单来说
//就是选定一个经纬度作为原点，其他点的经纬度与原点的距离就是其x,y,z坐标
carx = self.localization.pose.position.x
cary = self.localization.pose.position.y
carz = self.localization.pose.position.z
cartheta = self.localization.pose.heading
if math.isnan(self.chassis.speed_mps):
self.logger.warning("find nan speed_mps: %s" % str(self.chassis))
return
if math.isnan(self.chassis.steering_percentage):
self.logger.warning(
"find nan steering_percentage: %s" % str(self.chassis))
return
carspeed = self.chassis.speed_mps
caracceleration = self.localization.pose.linear_acceleration_vrf.y
speed_epsilon = 1e-9
//如果上次车速和本次车速都为0，则认为加速度为0
if abs(self.prev_carspeed) < speed_epsilon \
and abs(carspeed) < speed_epsilon:
caracceleration = 0.0
//车辆转角
carsteer = self.chassis.steering_percentage
//曲率
curvature = math.tan(math.radians(carsteer / 100 * 470) / 16) / 2.85
if abs(carspeed) >= speed_epsilon:
carcurvature_change_rate = (curvature - self.carcurvature) / (
carspeed * 0.01)
else:
carcurvature_change_rate = 0.0
self.carcurvature = curvature
cartime = self.localization.header.timestamp_sec
//档位
cargear = self.chassis.gear_location
if abs(carspeed) >= speed_epsilon:
if self.startmoving == False:
self.logger.info(
"carspeed !=0 and startmoving is False, Start Recording")
self.startmoving = True
if self.startmoving:
self.cars = self.cars + carspeed * 0.01
//往之前设置的文件内写入数据
self.write(
"%s, %s, %s, %s, %s, %s, %s, %.4f, %s, %s, %s, %s, %s, %s\n" %
(carx, cary, carz, carspeed, caracceleration, self.carcurvature,
carcurvature_change_rate, cartime, cartheta, cargear,
self.cars, self.chassis.throttle_percentage,
self.chassis.brake_percentage,
self.chassis.steering_percentage))
self.logger.debug(
"started moving and write data at time %s" % cartime)
else:
self.logger.debug("not start moving, do not write data to file")
self.prev_carspeed = carspeed
Ctrl + C结束后，apollo将会录制一个轨迹数据包garage.csv，放在data/log/下（主要记录了位置、刹车、油门、方向、速度等信息）。
stop
rtk_recorder.sh的stop函数如下：
function stop() {
pkill -SIGKILL -f rtk_recorder.py
}
很简单，就是杀死线程。
回放数据包开始循迹
N档，线控开启，输入以下命令：
bash scripts/rtk_player.sh setup
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函数如下：
function setup() {
bash scripts/canbus.sh start
bash scripts/gps.sh start
bash scripts/localization.sh start
bash scripts/control.sh start
}
还是跟上面一样启动一些必备的模块，具体启动过程是一样的。
接着bash scripts/rtk_player.sh start，rtk_player.sh的start函数如下：
function start() {
NUM_PROCESSES="$(pgrep -c -f "record_play/rtk_player.py")"
if [ "${NUM_PROCESSES}" -ne 0 ]; then
pkill -SIGKILL -f rtk_player.py
fi
python modules/tools/record_play/rtk_player.py
}
其他就不再重复了，我们看rtk_player.py的main函数
def main():
"""
Main rosnode
"""
parser = argparse.ArgumentParser(
description='Generate Planning Trajectory from Data File')
parser.add_argument(
'-s',
'--speedmulti',
help='Speed multiplier in percentage (Default is 100) ',
type=float,
default='100')
parser.add_argument(
'-c', '--complete', help='Generate complete path (t/F)', default='F')
parser.add_argument(
'-r',
'--replan',
help='Always replan based on current position(t/F)',
default='F')
args = vars(parser.parse_args())
rospy.init_node('rtk_player', anonymous=True)
Logger.config(
log_file=os.path.join(APOLLO_ROOT, 'data/log/rtk_player.log'),
use_stdout=True,
log_level=logging.DEBUG)
record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv')
player = RtkPlayer(record_file, args['speedmulti'],
args['complete'].lower(), args['replan'].lower())
atexit.register(player.shutdown)
rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
player.chassis_callback)
rospy.Subscriber('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
player.localization_callback)
rospy.Subscriber('/apollo/control/pad', pad_msg_pb2.PadMessage,
player.padmsg_callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
player.publish_planningmsg()
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这个类的初始化函数如下：
class RtkPlayer(object):
"""
rtk player class
"""
def __init__(self, record_file, speedmultiplier, completepath, replan):
"""Init player."""
self.firstvalid = False
self.logger = Logger.get_logger(tag="RtkPlayer")
self.logger.info("Load record file from: %s" % record_file)
try:
file_handler = open(record_file, 'r')
except:
self.logger.error("Cannot find file: " + record_file)
file_handler.close()
sys.exit(0)
//从之前记录的轨迹文件中提取数据
self.data = genfromtxt(file_handler, delimiter=',', names=True)
file_handler.close()
self.localization = localization_pb2.LocalizationEstimate()
self.chassis = chassis_pb2.Chassis()
self.padmsg = pad_msg_pb2.PadMessage()
self.localization_received = False
self.chassis_received = False
//创建发布topic为/apollo/planning的发布者，
//消息格式为planning_pb2.ADCTrajectory，队列长度为1
self.planning_pub = rospy.Publisher(
'/apollo/planning', planning_pb2.ADCTrajectory, queue_size=1)
self.speedmultiplier = speedmultiplier / 100
self.terminating = False
self.sequence_num = 0
//对加速度acceleration进行滤波
//scipy.signal.butter(N, Wn, btype='low', analog=False, output='ba')
//输入参数：
//N:滤波器的阶数
//Wn：归一化截止频率。计算公式Wn=2*截止频率/采样频率。（注意：根据采样定理，采样频//率要大于两倍的信号本身最大的频率，才能还原信号。截止频率一定小于信号本身最大的频//率，所以Wn一定在0和1之间）。当构造带通滤波器或者带阻滤波器时，Wn为长度为2的列表。
//btype : 滤波器类型{‘lowpass’, ‘highpass’, ‘bandpass’, ‘bandstop’},
//output : 输出类型{‘ba’, ‘zpk’, ‘sos’},
//输出参数：
//b，a: IIR滤波器的分子（b）和分母（a）多项式系数向量。output='ba'
//z,p,k: IIR滤波器传递函数的零点、极点和系统增益. output= 'zpk'
//sos: IIR滤波器的二阶截面表示。output= 'sos'
//具体参考https://blog.csdn.net/qq_38984928/article/details/89232898
b, a = signal.butter(6, 0.05, 'low')
self.data['acceleration'] = signal.filtfilt(b, a, self.data['acceleration'])
self.start = 0
self.end = 0
self.closestpoint = 0
self.automode = False
//因为输入的replan和completepath都是f，因此self.replan和self.completepath都是false
self.replan = (replan == 't')
self.completepath = (completepath == 't')
self.estop = False
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
def chassis_callback(self, data):
"""
New chassis Received
"""
self.chassis.CopyFrom(data)
//判断是否是自动驾驶模式
self.automode = (self.chassis.driving_mode ==
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE)
self.chassis_received = True
接着player.localization_callback
def localization_callback(self, data):
"""
New localization Received
"""
//更新位置
self.localization.CopyFrom(data)
self.carx = self.localization.pose.position.x
self.cary = self.localization.pose.position.y
self.carz = self.localization.pose.position.z
self.localization_received = True
最后player.padmsg_callback
def padmsg_callback(self, data):
"""
New message received
"""
if self.terminating == True:
self.logger.info("terminating when receive padmsg")
return
//没做啥，就是把消息中的数据拷贝至self.padmsg
self.padmsg.CopyFrom(data)
看到现在发现rtk_player.py里面没啥东西，现在才到重点了player.publish_planningmsg()。我们看看publish_planningmsg函数里面究竟卖的什么货：
def publish_planningmsg(self):
"""
Generate New Path
"""
if not self.localization_received:
self.logger.warning(
"locaization not received yet when publish_planningmsg")
return
//新建planning_pb2.ADCTrajectory消息，这是发布/apollo/planning所使用的消息格式
planningdata = planning_pb2.ADCTrajectory()
now = rospy.get_time()
planningdata.header.timestamp_sec = now
planningdata.header.module_name = "planning"
planningdata.header.sequence_num = self.sequence_num
self.sequence_num = self.sequence_num + 1
self.logger.debug(
"publish_planningmsg: before adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
if self.replan or self.sequence_num <= 1 or not self.automode:
self.logger.info(
"trigger replan: self.replan=%s, self.sequence_num=%s, self.automode=%s"
% (self.replan, self.sequence_num, self.automode))
self.restart()
else:
timepoint = self.closest_time()
distpoint = self.closest_dist()
self.start = max(min(timepoint, distpoint) - 100, 0)
self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1)
xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
if self.completepath://此处completepath为false，因此不执行
self.start = 0
self.end = len(self.data) - 1
self.logger.debug(
"publish_planningmsg: after adjust start: self.start = %s, self.end=%s"
% (self.start, self.end))
for i in range(self.start, self.end):
adc_point = pnc_point_pb2.TrajectoryPoint()
adc_point.path_point.x = self.data['x'][i]
adc_point.path_point.y = self.data['y'][i]
adc_point.path_point.z = self.data['z'][i]
adc_point.v = self.data['speed'][i] * self.speedmultiplier
adc_point.a = self.data['acceleration'][i] * self.speedmultiplier
adc_point.path_point.kappa = self.data['curvature'][i]
adc_point.path_point.dkappa = self.data['curvature_change_rate'][i]
time_diff = self.data['time'][i] - \
self.data['time'][self.closestpoint]
adc_point.relative_time = time_diff / self.speedmultiplier - (
now - self.starttime)
adc_point.path_point.theta = self.data['theta'][i]
adc_point.path_point.s = self.data['s'][i]
planningdata.trajectory_point.extend([adc_point])
planningdata.estop.is_estop = self.estop
planningdata.total_path_length = self.data['s'][self.end] - \
self.data['s'][self.start]
planningdata.total_path_time = self.data['time'][self.end] - \
self.data['time'][self.start]
planningdata.gear = int(self.data['gear'][self.closest_time()])
planningdata.engage_advice.advice = \
drive_state_pb2.EngageAdvice.READY_TO_ENGAGE
self.planning_pub.publish(planningdata)
self.logger.debug("Generated Planning Sequence: " +
str(self.sequence_num - 1))
如果replan为true或者sequence_num<=1或者不是自动驾驶模式则调用restart()
def restart(self):
self.logger.info("before replan self.start=%s, self.closestpoint=%s" %
(self.start, self.closestpoint))
self.closestpoint = self.closest_dist()
//先看下面对self.closest_dist()的分析
//基于对self.closest_dist()的假设
//假设self.closestpoint=50，则self.start仍为0,self.end=299
self.start = max(self.closestpoint - 100, 0)
self.starttime = rospy.get_time()
self.end = min(self.start + 1000, len(self.data) - 1)
self.logger.info("finish replan at time %s, self.closestpoint=%s" %
(self.starttime, self.closestpoint))
首先self.closest_dist()找到当前位置在上次记录的轨迹中对应的是第几条数据，所以循迹开始的时候需要将车开到以前的轨迹处才行，否则都找不到初始的点。当然循迹到中间出现问题退出自动驾驶模式，重启自动驾驶模式后程序也能找到自己在原先轨迹中的位置，不必重头开始，这也是restart()的意义所在吧。
def closest_dist(self):
shortest_dist_sqr = float('inf')
self.logger.info("before closest self.start=%s" % (self.start))
//SEARCH_INTERVAL = 1000
//一开始的时候self.start=0，所以search_start=0；search_end=500和
//记录的轨迹数据的长度中的最小值，假定上次记录了300条数据，
//则search_end=300
search_start = max(self.start - SEARCH_INTERVAL / 2, 0)
search_end = min(self.start + SEARCH_INTERVAL / 2, len(self.data))
start = self.start
for i in range(search_start, search_end):
dist_sqr = (self.carx - self.data['x'][i]) ** 2 + \
(self.cary - self.data['y'][i]) ** 2
if dist_sqr <= shortest_dist_sqr:
start = i
shortest_dist_sqr = dist_sqr
//假设返回的是50
return start
如果不满足（replan为true或者sequence_num<=1或者不是自动驾驶模式）则执行
timepoint = self.closest_time()
distpoint = self.closest_dist()
//先看下面的假设
//根据下面的假设，这里timepoint=51，distpoint=52，所以self.start=0
//同时结合上面len(self.data)=300的假设，所以self.end=299
self.start = max(min(timepoint, distpoint) - 100, 0)
self.end = min(max(timepoint, distpoint) + 900, len(self.data) - 1)
xdiff_sqr = (self.data['x'][timepoint] - self.carx)**2
ydiff_sqr = (self.data['y'][timepoint] - self.cary)**2
//如果时间最近的轨迹点跟当前位置的距离过大，则调用restart()重新找距离当前位置最近的轨迹点
if xdiff_sqr + ydiff_sqr > 4.0:
self.logger.info("trigger replan: distance larger than 2.0")
self.restart()
首先调用closest_time()找到在时间上距离我们前面假设找到的第50轨迹点最近的（时间差为正）轨迹点
def closest_time(self):
//self.starttime在上面restart()被设为当时的时刻
time_elapsed = rospy.get_time() - self.starttime
//根据上面的假设，这里closest_time = 0
closest_time = self.start
//此处就是time_diff=self.data['time'][0]-self.data['time'][50]
time_diff = self.data['time'][closest_time] - \
self.data['time'][self.closestpoint]
//找到time_diff大于当前时刻与启动时刻时差的那个轨迹点
while time_diff < time_elapsed and closest_time < (len(self.data) - 1):
closest_time = closest_time + 1
time_diff = self.data['time'][closest_time] - \
self.data['time'][self.closestpoint]
//假设这个时间上的最近点为51
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后发布给下面的控制模块计算具体的控制量。