全局路径与局部路径

1.全局路径的读取

waypoint_loader.py的作用是读取.csv文件中的路径点信息，并发布消息waypoints和base_path，其中waypoints包含了路径点的位置信息、小车的期望速度和表明小车是前进还是后退的状态位，base_path包含了实现Rviz可视化的路径点坐标。

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)

说明：
●os.path.isfile(path)用于判断某一对象是否为文件，其中path必须为绝对路径。
●self.load_waypoints(path)用于加载.csv文件，并以包含路径信息的消息作为返回值。
●self.publish(waypoints, base_path)用于发布话题消息。
●rospy.loginfo(‘Waypoint Loded’)用于输出INFO日志信息。
●rospy.logerr(‘%s is not a file’, path)当判断该对象不为文件时，输出ERROR日志信息。

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

说明：
●waypoints = []创建了一个空列表，命名为waypoint。
●base_path = Path()和base_path.header.frame_id = ‘world’定义了一个Path类的对象base_path，并以world作为frame id。
●with open(fname) as wfile的作用是打开文件（保证在文件出错后能够正常关闭文件），其中fname在此处是文件的绝对路径。
●reader = csv.DictReader(wfile, CSV_HEADER)以CSV_HEADER的构造格式，即‘x’、‘y’和‘yaw’格式读取文件内容。
●for wp in reader:在此处遍历文件中的每一行。

●p = Waypoint()定义了一个Waypoint类的对象，其中Waypoint类在styx_msgs文件夹的Waypoint.msg中进行了自定义，包含了geometry_msgs/PoseStamped pose、geometry_msgs/TwistStamped twist和bool forward，分别表示小车在环境中的x、y坐标、小车的期望速度以及表明小车是前进还是后退的标志位。

●p.pose.pose.position.x = float(wp[‘x’])和p.pose.pose.position.y = float(wp[‘y’])用于将文件中的x、y所对应的数据填充到相应的消息内容中。
●q = self.quaternion_from_yaw(float(wp[‘yaw’]))即调用tf.transformations.quaternion_from_euler(0., 0., yaw)该函数，用于将 (0., 0., yaw)欧拉角转换成四元数quaternion。
●p.pose.pose.orientation = Quaternion(*q)该操作相当于p.pose.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])，用于将元祖扩展成参数列表，并将四元数数据填充到相应的消息内容中。
●p.twist.twist.linear.x = float(self.velocity)用于将期望的线速度值填充到相应的消息内容中。
●waypoints.append(p)用于将Waypoint类的对象p，即路径信息添加到列表waypoints中。
●path_element = PoseStamped()定义了一个PoseStamped类的对象，用于在Rviz中可视化路径。
●waypoints = self.decelerate(waypoints)根据小车当前位置与终点位置来控制小车速度，并保证在终点位置速度为零。

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

说明：
●last = waypoints[-1]用于将waypoints列表中的最后一个值，即路径的终点信息赋给变量last。
●for wp in waypoints[:-1][::-1]:用于遍历waypoints列表中的所有值。
●dist = self.distance(wp.pose.pose.position, last.pose.pose.position)用于计算当前坐标点与终点坐标的距离。
●vel = math.sqrt(2 * MAX_DECEL * dist)根据小车当前位置距离终点的路程dist、人为设定的减速度MAX_DECEL和到达终点时的速度（速度为零）来计算得到小车当前的期望速度vel，其中所运用到的是高中物理公式Vt^2 - V0^2 = 2as。
●wp.twist.twist.linear.x = min(vel, wp.twist.twist.linear.x)表示在距离终点较远的位置，按照wp.twist.twist.linear.x的默认期望速度行驶，而在距离终点较近的位置，按照自定义的减速度开始减速行驶。

1
2
3

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)

说明：
●该函数用于计算两个坐标点之间的距离。

1 2	def quaternion_from_yaw(self, yaw): return tf.transformations.quaternion_from_euler(0., 0., yaw)

说明：
●该函数用于将欧拉角转换成四元数形式。

1 2	def kmph2mps(self, velocity_kmph): return (velocity_kmph * 1000.) / (60. * 60.)

说明：
●该函数用于将单位为km/h的速度值转换成以m/s为单位。

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)

说明：
●该函数用于发布话题消息waypoints和base_path。

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()

说明：
●rospy.init_node(‘waypoint_loader’, log_level=rospy.DEBUG)用于初始化ROS节点，在节点完全初始化之前，消息不会出现在/rosout话题上，因此可能看不到初始消息。要想在/rosout上看到logdebug消息，可以将log_level参数传递给rospy.init_node()。
●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)分别用于创建以/base_waypoints和/base_path为话题消息的发布者。其中latch=True将保存最后发布的消息并将其发送给连接的任何将来的订户，这对于诸如地图等缓慢变化的数据或静态数据很有用。
●self.velocity = self.kmph2mps(rospy.get_param(‘~velocity’))用于加载参数velocity的值，并转换成以m/s为单位。
●self.new_waypoint_loader(rospy.get_param(‘~path’))用于加载参数path所对应的文件路径，并发布相关的话题消息。

2.局部路径和历史路径的更新

waypoint_updater.py的作用是通过KD-tree查询距离小车最近的路径点，以该点为起始点设置局部路径，并在Rviz可视化工具中显示局部路径。

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()

说明：
●rospy.init_node(‘waypoint_updater’)用于初始化ROS节点。
●rospy.Subscriber(‘/smart/rear_pose’, PoseStamped, self.pose_cb)和rospy.Subscriber(‘/base_waypoints’, Lane, self.waypoints_cb)分别用于创建以/smart/rear_pose和/base_waypoints为话题消息的订阅者，其中/smart/rear_pose话题消息内容为小车后轮中心点的坐标，/base_waypoints 话题消息内容为全局路径点坐标及小车期望速度等，pose_cb和waypoints_cb分别为/smart/rear_pose和/base_waypoints的消息回调函数。
●self.final_waypoints_pub = rospy.Publisher(‘final_waypoints’, Lane, queue_size=1)和self.final_path_pub = rospy.Publisher(‘final_path’, Path, queue_size=1)分别创建以final_waypoints和final_path为话题消息的发布者，用于发布局部路径信息。
●self.final_waypoints_pub = rospy.Publisher(‘real_waypoints’, Lane, queue_size=1)和self.final_path_pub = rospy.Publisher(‘real_path’, Path, queue_size=1)分别创建以real_waypoints和real_path为话题消息的发布者，用于发布历史路径信息。
●rate = rospy.Rate(20)用于设置ROS节点的更新频率为20Hz。
●while not rospy.is_shutdown():表示在ROS节点未关闭的条件下，执行下面的操作。
●if self.pose and self.base_waypoints and self.waypoint_tree:表示在pose、base_waypoints和waypoint_tree话题消息内容非空的条件下，执行下面的操作。
●closest_waypoint_idx = self.get_closest_waypoint_idx()的作用是返回一个距离当前小车位置最近的路径点的横坐标x。
●self.publish_waypoints(closest_waypoint_idx)用于发布话题消息closest_waypoint_idx。

1 2	def pose_cb(self, msg): self.pose = msg

说明：
●该函数为/smart/rear_pose话题消息的回调函数，作用是将该话题消息内容赋给self.pose。

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)

说明：
●该函数为/base_waypoints话题消息的回调函数。
●self.waypoints_2d = [[waypoint.pose.pose.position.x, waypoint.pose.pose.position.y] for waypoint in waypoints.waypoints]将waypoints话题消息中的x、y轴坐标提取到一个二维列表self.waypoints_2d中。二维列表self.waypoints_2d的内容如下：

●self.waypoint_tree = KDTree(self.waypoints_2d)的作用是构建一个二维的KD-tree。

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

说明：
●q = Waypoint()的作用是创建一个Waypoint类型的对象q，包含坐标点信息。
●self.real_waypoints.append(q)的作用是将最新的实时位置信息添加到列表中。
●closest_idx = self.waypoint_tree.query([x,y],1)[1]的作用是在KD-tree中查询距离当前小车位置最近的一个路径点的横坐标x。

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)

说明：
●lane = Lane()的作用是创建一个Lane类型的对象lane。
●lane.waypoints = self.base_waypoints.waypoints[closest_idx:closest_idx + LOOKAHEAD_WPS]的作用是将距离小车最近的路径点以及之后的LOOKAHEAD_WPS个路径点信息填充到final_waypoints话题消息中，其中LOOKAHEAD_WPS是自定义的局部路径点个数。
●path.header.frame_id = ‘/world’设置/world为局部路径可视化的参考系坐标。
●for p in lane.waypoints:的作用是遍历上述的局部路径点。
●path_element.pose.position.x = p.pose.pose.position.x和path_element.pose.position.y = p.pose.pose.position.y是将局部路径点的坐标信息填充到final_path话题信息中，用于在Rviz可视化工具中显示局部路径。
●self.final_waypoints_pub.publish(lane)和self.final_path_pub.publish(path)的作用是发布话题消息final_waypoints和final_path。
●历史路径消息real_path和real_lane的创建和定义同理。

3.路径可视化

打开一个新的终端，输入如下指令：

1	$ rosrun rviz rviz

在左菜单栏中加入两个Path的可视化插件，分别将Topic设置为/base_path和/final_path，即全局路径和局部路径。显示效果如下图。

在Gazebo中移动小车模型，由于小车当前位置发生变化，局部路径也因此发生变化，如下图中Rviz所显示。