waypoint_loader.py文件的功能是读取.csv文件中的路径点信息，并发布消息waypoints和base_path，其中waypoints包含了路径点的位置信息、小车的期望速度和表明小车是前进还是后退的状态位，base_path包含了实现Rviz可视化的路径点坐标。
(1) 主函数
def new_waypoint_loader(self, path):
if os.path.isfile(path):
waypoints, base_path = self.load_waypoints(path)
self.publish(waypoints, base_path)
rospy.loginfo('Waypoint Loded')
else:
rospy.logerr('%s is not a file', path)
说明：
(2) load_waypoints()路径加载函数
def load_waypoints(self, fname):
waypoints = []
base_path = Path()
base_path.header.frame_id = 'world'
with open(fname) as wfile:
reader = csv.DictReader(wfile, CSV_HEADER)
for wp in reader:
p = Waypoint()
p.pose.pose.position.x = float(wp['x'])
p.pose.pose.position.y = float(wp['y'])
q = self.quaternion_from_yaw(float(wp['yaw']))
p.pose.pose.orientation = Quaternion(*q)
p.twist.twist.linear.x = float(self.velocity)
p.forward = True
waypoints.append(p)
path_element = PoseStamped()
path_element.pose.position.x = p.pose.pose.position.x
path_element.pose.position.y = p.pose.pose.position.y
base_path.poses.append(path_element)
waypoints = self.decelerate(waypoints)
return waypoints,base_path
说明：
(3) decelerate()小车减速函数
def decelerate(self, waypoints):
last = waypoints[-1]
last.twist.twist.linear.x = 0.
for wp in waypoints[:-1][::-1]:
dist = self.distance(wp.pose.pose.position, last.pose.pose.position)
vel = math.sqrt(2 * MAX_DECEL * dist)
if vel < 1.:
vel = 0.
wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)
return waypoints
说明：
(4) distance()距离计算函数
def distance(self, p1, p2):
x, y, z = p1.x - p2.x, p1.y - p2.y, p1.z - p2.z
return math.sqrt(x*x + y*y + z*z)
说明：
(5) quaternion_from_yaw()四元数转换函数
def quaternion_from_yaw(self, yaw):
return tf.transformations.quaternion_from_euler(0., 0., yaw)
说明：
(6) kmph2mps()速度单位换算函数
def kmph2mps(self, velocity_kmph):
return (velocity_kmph * 1000.) / (60. * 60.)
说明：
(7) publish()消息发布函数
def publish(self, waypoints, base_path):
lane = Lane()
lane.header.frame_id = '/world'
lane.header.stamp = rospy.Time(0)
lane.waypoints = waypoints
self.pub.publish(lane)
self.pub_path.publish(base_path)
说明：
(8) 初始化函数
def __init__(self):
rospy.init_node('waypoint_loader', log_level=rospy.DEBUG)
self.pub = rospy.Publisher('/base_waypoints', Lane, queue_size=1, latch=True)
self.pub_path = rospy.Publisher('/base_path', Path, queue_size=1, latch=True)
self.velocity = self.kmph2mps(rospy.get_param('~velocity'))
self.new_waypoint_loader(rospy.get_param('~path'))
rospy.spin()
说明：
waypoint_updater.py文件的功能是通过KD-tree查询距离小车最近的路径点，以该点为起始点设置局部路径，并在Rviz可视化工具中显示局部路径。
(1) 初始化函数
def __init__(self):
rospy.init_node('waypoint_updater')
rospy.Subscriber('/smart/rear_pose', PoseStamped, self.pose_cb)
rospy.Subscriber('/base_waypoints', Lane, self.waypoints_cb)
self.final_waypoints_pub = rospy.Publisher('final_waypoints', Lane, queue_size=1)
self.final_path_pub = rospy.Publisher('final_path', Path, queue_size=1)
self.final_waypoints_pub = rospy.Publisher('real_waypoints', Lane, queue_size=1)
self.final_path_pub = rospy.Publisher('real_path', Path, queue_size=1)
# TODO: Add other member variables you need below
self.real_waypoints = []
self.base_waypoints = None
self.waypoints_2d = None
self.waypoint_tree = None
self.pose = None
# rospy.spin()
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if self.pose and self.base_waypoints and self.waypoint_tree:
# Get closest waypoint
closest_waypoint_idx = self.get_closest_waypoint_idx()
self.publish_waypoints(closest_waypoint_idx)
rate.sleep()
说明：
(2) 小车位置回调函数
def pose_cb(self, msg):
self.pose = msg
说明：
(3) 路径点回调函数
def waypoints_cb(self, waypoints):
self.base_waypoints = waypoints
if not self.waypoints_2d:
self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in waypoints.waypoints]
self.waypoint_tree = KDTree(self.waypoints_2d)
说明：
(4) get_closest_waypoint_idx()最近路径点获取函数
def get_closest_waypoint_idx(self):
x = self.pose.pose.position.x
y = self.pose.pose.position.y
q = Waypoint()
q.pose.pose.position.x = x
q.pose.pose.position.y = y
self.real_waypoints.append(q)
closest_idx = self.waypoint_tree.query([x,y],1)[1]
return closest_idx
说明：
(5) publish_waypoints()消息发布函数
def publish_waypoints(self, closest_idx):
# fill in final waypoints to publish
lane = Lane()
lane.header = self.base_waypoints.header
lane.waypoints = self.base_waypoints.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS]
real_lane = Lane()
real_lane.header = self.base_waypoints.header
real_lane.waypoints = self.real_waypoints
# fill in path for visulization in Rviz
path = Path()
path.header.frame_id = '/world'
for p in lane.waypoints:
path_element = PoseStamped()
path_element.pose.position.x = p.pose.pose.position.x
path_element.pose.position.y = p.pose.pose.position.y
path.poses.append(path_element)
real_path = Path()
real_path.header.frame_id = '/world'
for m in real_lane.waypoints:
real_path_element = PoseStamped()
real_path_element.pose.position.x = m.pose.pose.position.x
real_path_element.pose.position.y = m.pose.pose.position.y
real_path.poses.append(real_path_element)
self.final_waypoints_pub.publish(lane)
self.final_path_pub.publish(path)
self.final_waypoints_pub.publish(real_lane)
self.final_path_pub.publish(real_path)
说明：
打开一个新的终端，输入如下指令：
$ rosrun rviz rviz
在左菜单栏中加入两个Path的可视化插件，分别将话题信息Topic设置为/base_path和/final_path，即全局路径和局部路径。显示效果如下图。
在Gazebo中移动小车模型，由于小车当前位置发生变化，局部路径也因此发生变化，如下图中Rviz所显示。
