全国大学生智能汽车竞赛--讯飞组解读(七)

建图与导航

讯飞组比赛的主线就是多点导航，支线是穿插在各个导航点之间的识别、追踪等任务。这里先解释一下建图与导航的关系，建图是指使用键盘控制小车在场地中间跑一圈，同时开启建图节点，小车会结合激光雷达扫描数据和自己的位置构建出场地的二维栅格地图，如下图所示。

导航则是在建好的地图中选取若干个点，记录这些点的位置信息，然后在程序中依次发布这些点，使小车完成多点巡航。

推荐查看机器人阿杰关于导航的相关视频，讲解的比较清楚

建图初体验

官方的建图功能包为ucar_map，在其launch目录下，你可以看到：

launch/
├── cartographer_start.launch		# cartogpher算法建图
├── gmapping.launch							# gmapping算法建图启动文件，包含很多参数
├── gmapping_demo.launch				# 启动上面这个gmapping.launch
├── ucar_gmapping.launch				# 同时打开雷达、底盘、gmapping
└── ucar_mapping.launch					# 同时打开雷达、底盘、gmapping（同上）

直接在终端中启动ucar_mapping.launch

roslaunch ucar_map ucar_mapping.launch

接着打开rviz，Fixed Frame默认选择为map，然后添加名为map的显示插件，选择话题为map接着你就可以看到已经扫出的地图。

接着，使用键盘控制节点控制小车移动，你会发现地图正在不断刷新构建，同时模糊的边缘会变得更清晰。

保存地图可以在终端使用命令保存，这里我使用launch文件保存。在ucar_map/luanch中新建一个map_save.launch，导入以下代码：

<launch>
    <arg name="filename" value="$(find ucar_map)/maps/map_01" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

请注意每次建图后修改保存路径和地图名，如果地图名不修改会被覆盖。

运行该launch文件，成功后可以看到maps目录下多了一个.pgm和.yaml文件，可以打开.pgm查看建图效果。

导航初体验

官方的导航功能包为ucar_navi，请将ucar_navigation.launch中的地图路径改为保存地图的所在位置：

  <node name="map_server" pkg="map_server" type="map_server" args="$(find ucar_map)/maps/map0606.yaml" output="screen">
   <param name="frame_id" value="map" />
  </node>

将小车放置到建图开始位置，接着启动这个launch文件，并打开rviz，添加map显示，topic选择map；显示框中会出现你之前保存的地图。接着点击上方功能栏中的2D Nav Goal，用鼠标点击地图目标位置，并指定朝向。你会发现小车开始朝目标点移动。

同时，开启rviz的终端内会打印该点的坐标信息，后续我们要把这些记录下来的坐标写进程序中。

[INFO] [1769415618.386294061]: Setting goal: Frame:map, Position(1.620, 1.159, 0.000), Orientation(0.000, 0.000, 0.855, 0.518) = Angle: 2.053

程序导航

现在我们将刚才记录的若干个点放到程序中，自动执行多点导航。你将用到以下代码，注意替换掉你设置的坐标点。

#!/usr/bin/python3
# -*- coding: UTF-8 -*-

import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseActionResult

goal_pub = None     # 导航发布器
goal_pose = None    # 目标点
point = 0           # 目标点计数器

def goal_callback(msg):
    global point, goal_pub, goal_pose
    # msg.status.status == 3 表示到达目标点，point == 0 表示当前是第一个目标点
    if msg.status.status == 3 and point == 0:
        # 发送第二个目标点
        goal_pose.header.stamp = rospy.Time.now()
        # Position(1.047, 3.422, 0.000), Orientation(0.000, 0.000, 0.728, 0.686) = Angle: 1.630
        goal_pose.pose.position.x = 1.047
        goal_pose.pose.position.y = 3.422
        goal_pose.pose.position.z = 0.000
        goal_pose.pose.orientation.x = 0.000
        goal_pose.pose.orientation.y = 0.000
        goal_pose.pose.orientation.z = 0.728
        goal_pose.pose.orientation.w = 0.686
        goal_pub.publish(goal_pose)
        # 更新目标点计数器
        point += 1
        return
    
    # if msg.status.status == 3 and point == 1:
    #    ... 可以继续添加更多目标点的处理逻辑

if __name__ == "__main__":
    rospy.init_node("send_nav_goal")

    goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1)
    rospy.Subscriber("/move_base/result",MoveBaseActionResult,goal_callback)
    rospy.sleep(0.5)

    goal_pose = PoseStamped()
    goal_pose.header.frame_id = "map"
    goal_pose.header.stamp = rospy.Time.now()
    # Position(1.130, 0.621, 0.000), Orientation(0.000, 0.000, 1.000, 0.002) = Angle: 3.138
    goal_pose.pose.position.x = 1.130
    goal_pose.pose.position.y = 0.621
    goal_pose.pose.position.z = 0.000
    goal_pose.pose.orientation.x = 0.000
    goal_pose.pose.orientation.y = 0.000
    goal_pose.pose.orientation.z = 1.000
    goal_pose.pose.orientation.w = 0.002
    goal_pub.publish(goal_pose)
    rospy.spin()

启动ucar_navigation.launch之后，再启动该导航节点。会发现，小车到达第一个点位后又继续前往第二个点位…

导航测试效果很可能不好，因为官方使用的DWA规划器并且没有进行调参。后续可以换位TEB规划器，也可以自定义局部规划器。

小结

通过本章的建图与导航实践，你已经掌握了从 环境感知到自主移动 的完整流程。首先，通过激光雷达和底盘位姿信息，使用建图节点生成了场地的二维栅格地图；随后，在保存的地图中选取若干目标点，并通过程序或 RViz 发布导航指令，实现了小车的多点巡航。

更多关于全国大学生智能汽车的分享，请访问个人主页。期待你的评论与交流！

全国大学生智能汽车竞赛--讯飞组解读(七)

建图与导航

建图初体验

导航初体验

程序导航

小结

CATALOG

FEATURED TAGS