3.2-建图与导航

1. 实训目的

使用gmapping与navigation功能包实现小车的建图与导航操作，了解建图导航仿真环境的配置与运用，编写launch文件实现所需的建图与导航功能。此外，基于actionlib通信实现小车在gazebo环境下自动巡航，根据给定目标点序列依次执行路径规划。

1. 实训操作步骤

为构建小车实现建图与导航功能，我们需要完成以下步骤：

• 安装环境依赖。

• 调用gmapping功能包，设置相应参数。

• 配置小车AMCL、move_base功能包，了解功能构成与参数配置。

• 通过launch启动navigation，实现小车自主导航避障功能。

• 通过编写python脚本实现小车自动巡航。

1. 环境搭建

• 安装 gmapping 包(用于构建地图):

$ sudo apt install ros-melodic-gmapping

• 安装地图服务包(用于保存与读取地图):

$ sudo apt install ros-melodic-map-server

• 安装 navigation 包(用于定位以及路径规划):

$ sudo apt install ros-melodic-navigation

进入工作空间的src目录下，通过如下命令创建一个命名为proj03_slam_nav的功能包：

$ catkin_create_pkg proj03_slam_nav gmapping map_server amcl move_base rospy std_msgs actionlib actionlib_msgs

1. Gmapping的使用

在proj03_slam_nav目录下新建/launch文件夹，创建gmapping.launch文件。

gmapping.launch：

<launch>

    <!-- 仿真环境下将 use_sim_time 设置为true -->

    <param name="use_sim_time" value="true"/>

 

    <!-- 指定机器人模型 -->

    <arg name="model" default="waffle_pi" />

 

    <!-- 在参数服务器载入xacro文件 -->

     <param name="robot_description" command="$(find xacro)/xacro $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />

 

    <!-- 启动rviz 可以保存 rviz 配置并后期直接使用-->

    <node name="rviz" pkg="rviz" type="rviz" />

    <!--<node pkg="rviz" type="rviz" name="rviz" args="-d $(find proj03_slam_nav)/rviz/turtlebot3_house.rviz"/>-->

 

    <!-- 启动机器人状态和关节状态发布节点 -->

    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />

    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" />

 

    <!--Gmapping相关参数配置-->

    <!-- 根据自己发布的雷达话题名称进行修改 -->

    <arg name="scan_topic" default="/scan" />                 

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">

        <!-- 根据自己的基座标系名称进行修改 -->

        <param name="base_frame" value="/base_footprint"/> 

        <!-- 根据自己的里程计坐标系名称进行修改 -->

        <param name="odom_frame" value="/odom"/>   

        <!--地图更新的间隔 -->

        <param name="map_update_interval" value="4.0"/>

        <param name="maxRange" value="5.0"/>

        <param name="maxUrange" value="4.5"/>

        <param name="sigma" value="0.05"/>

        <param name="kernelSize" value="1"/>

        <param name="lstep" value="0.05"/>

        <param name="astep" value="0.05"/>

        <param name="iterations" value="5"/>

        <param name="lsigma" value="0.075"/>

        <param name="ogain" value="3.0"/>

        <param name="lskip" value="0"/>

        <param name="srr" value="0.01"/>

        <param name="srt" value="0.02"/>

        <param name="str" value="0.01"/>

        <param name="stt" value="0.02"/>

        <param name="linearUpdate" value="0.5"/>

        <param name="angularUpdate" value="0.436"/>

        <param name="temporalUpdate" value="-1.0"/>

        <param name="resampleThreshold" value="0.5"/>

        <!-- 粒子个数 -->

        <param name="particles" value="80"/> 

        <!--map初始化的大小 -->

        <param name="xmin" value="-1.0"/>               

        <param name="ymin" value="-1.0"/>

        <param name="xmax" value="1.0"/>

        <param name="ymax" value="1.0"/>

        <param name="delta" value="0.05"/>

        <param name="llsamplerange" value="0.01"/>

        <param name="llsamplestep" value="0.01"/>

        <param name="lasamplerange" value="0.005"/>

        <param name="lasamplestep" value="0.005"/>

        <remap from="scan" to="$(arg scan_topic)"/>

    </node>

</launch>

分别在终端中输入如下命令：

启动机器人仿真环境：

$ export TURTLEBOT3_MODEL=waffle_pi

$ roslaunch turtlebot3_gazebo turtlebot3_house.launch

运行gmapping.launch文件：

$ roslaunch proj03_slam_nav gmapping.launch

启动键盘控制节点：

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

在rviz中添加如下订阅：雷达话题、地图、机器人模型，并指定相应topic。

LaserScan--/scan

Map--/map

使用键盘控制小车建图，在proj03_slam_nav目录下创建/map文件夹，用于存放地图文件，cd进入map文件夹下，打开终端输入如下命令保存地图：

$ rosrun map_server map_saver -f <map_name> 

进入map文件夹下可看到turtlebot3_house.pgm和turtlebot3_house.yaml文件。

建图结果：

1. 使用Navigation功能包

ROS navigation为移动机器人导航相关包的集合，实现定位规划避障等相关功能。通过map_server加载现有地图，通过AMCL提供map->odom的变换，move_base为导航的核心框架，通过预设接口加载代价地图（costmap）和规划避障（global_planner, local_planner）相关插件，读取设置参数（机器人外形，规划避障设置等），并发布相关话题和服务器。

1. map_server配置

map_server是一个ROS 节点，用于保存slam建图及加载保存好的静态地图，便于导航。

在/launch目录下新建/include目录，添加map_server.launch，用于启动地图服务节点。

map_server.launch：

<launch>

<!--  default中填写slam保存的地图名称 -->

  <arg  name = "map" default = "turtlebot3_house.yaml"/>

  <node pkg="map_server" type ="map_server" name = "map_load" args="$(find proj03_slam_nav)/map/$(arg map) "/>       

</launch>

在终端中输入如下命令运行map_server.launch：

$ roslaunch proj03_slam_nav map_server.launch map:=<slam保存的地图名称>.yaml

在Rviz中订阅地图话题可看到地图已经加载。

1. AMCL配置

AMCL是一个基于概率的定位系统，主要针对二维移动机器人。它实现了自适应蒙特卡罗滤波的定位方法，并使用粒子滤波器去跟踪在已知地图中机器人的位置。

在/launch/include下添加amcl.launch文件，用于AMCL节点的配置。

amcl.launch:

<launch>

  <!-- 雷达话题 -->

  <arg name="scan_topic"     default="scan"/>

  <!-- 初始位置 -->

  <arg name="initial_pose_x" default="-3.0"/>

  <arg name="initial_pose_y" default="1.0"/>

  <arg name="initial_pose_a" default="0.0"/>

 

  <!-- AMCL参数 -->

  <node pkg="amcl" type="amcl" name="amcl">

 

    <param name="min_particles"             value="500"/>

    <param name="max_particles"             value="3000"/>

    <param name="kld_err"                   value="0.02"/>

    <param name="update_min_d"              value="0.20"/>

    <param name="update_min_a"              value="0.20"/>

    <param name="resample_interval"         value="1"/>

    <param name="transform_tolerance"       value="0.5"/>

    <param name="recovery_alpha_slow"       value="0.00"/>

    <param name="recovery_alpha_fast"       value="0.00"/>

    <param name="initial_pose_x"            value="$(arg initial_pose_x)"/>

    <param name="initial_pose_y"            value="$(arg initial_pose_y)"/>

    <param name="initial_pose_a"            value="$(arg initial_pose_a)"/>

    <param name="gui_publish_rate"          value="50.0"/>

    <remap from="scan"                      to="$(arg scan_topic)"/>

    <param name="laser_max_range"           value="3.5"/>

    <param name="laser_max_beams"           value="180"/>

    <param name="laser_z_hit"               value="0.5"/>

    <param name="laser_z_short"             value="0.05"/>

    <param name="laser_z_max"               value="0.05"/>

    <param name="laser_z_rand"              value="0.5"/>

    <param name="laser_sigma_hit"           value="0.2"/>

    <param name="laser_lambda_short"        value="0.1"/>

    <param name="laser_likelihood_max_dist" value="2.0"/>

    <param name="laser_model_type"          value="likelihood_field"/>

    <param name="odom_model_type"           value="diff"/>

    <param name="odom_alpha1"               value="0.1"/>

    <param name="odom_alpha2"               value="0.1"/>

    <param name="odom_alpha3"               value="0.1"/>

    <param name="odom_alpha4"               value="0.1"/>

    <!-- 里程计坐标系 -->

    <param name="odom_frame_id"             value="odom"/>

    <!-- 地图坐标系 -->

    <param name="base_frame_id"             value="base_footprint"/>

  </node>

</launch>

检测amcl是否配置成功，可以编写一个amcl_test.launch

<launch>

    <!-- 启动rviz -->

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />

    <node pkg="robot_state_publisher"name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz" />

    <!-- 加载地图 -->

    <include file="$(find proj03_slam_nav)/launch/map_server.launch"/>

    <!-- amcl -->

    <include file="$(find proj03_slam_nav)/launch/amcl.launch"/>

</launch>

启动仿真环境模型，终端输入roslaunch proj03_slam_nav amcl_test.launch测试：

添加订阅PoseArry、map、robotModel：

可以看到小车周围有红色箭头，说明amcl定位服务启动，particlecloud是位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

点击Add添加一个Axes坐标系，在参考系的下拉列表中选择map选项，将map坐标系可视化，如下图所示：

map坐标系的原点与朝向如上图所示，后续AMCL的所有定位结果都将参考这个坐标系。

此时在终端中输入如下命令订阅AMCL的定位结果话题/amcl_pose：

$  rostopic echo /amcl_pose

可以看到AMCL给出的小车目前的定位结果是(-2.46,0.86)，姿态四元数接近于(0,0,0,1)，即是说小车目前的姿态与map坐标系朝向基本一致，符合我们对gazebo中小车位姿的判断。

1. move_base配置

move_base 功能包订阅传感器话题以及tf信息，在SLAM建立的地图中进行路径规划，速度发布。因此可以根据给定的目标点，控制机器人底盘运动至目标位置，在运动过程中连续反馈机器人自身的姿态与目标点的状态信息。

然而在导航过程中，障碍物的数量和位置有可能发生变化，机器人需要实时获取障碍物数据，同时为了防止机器人由于惯性或形体不规则在转弯时与障碍物发生碰撞，需要在SLAM构建的静态地图上叠加膨胀层。

因此，机器人导航需要依赖如下地图图层，称为代价地图：

• Static Map Layer：静态地图层，SLAM构建的静态地图。

• bstacle Map Layer：障碍地图层，传感器感知的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap。

规划机器人当前位置与目标位置之间的路径称为全局路径规划，在全局规划的过程中遇到了障碍物，需要做动态调整时，就需要局部路径规划。

综上所述，move_base配置涉及到的参数文件主要有如下几个：

• costmap_common_params_waffle_pi.yaml 代价地图公共参数

• global_costmap_params.yaml 全局代价地图的设置

• local_costmap_params.yaml 局部代价地图的设置

• move_base_params.yaml 控制器move_base 本身的设置

• dwa_local_planner_params_waffle_pi.yaml局部规划器的设置，使用DWA算法进行局部规划

此外，全局规划(global_planner)采用默认规划器navfn/NavfnROS。

在功能包下新建/param目录，新建上述参数文件，编辑内容。

costmap_common_params_waffle_pi.yaml：

obstacle_range: 3.0

raytrace_range: 3.5

 

footprint: [[-0.205, -0.155], [-0.205, 0.155], [0.077, 0.155], [0.077, -0.155]]

#robot_radius: 0.17

# 膨胀半径

inflation_radius: 0.4

# 代价比例系数,越大代价值越小

cost_scaling_factor: 3.0

# 地图类型

map_type: costmap

# 导航包使用的传感器

observation_sources: scan

scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml：

global_costmap:

  global_frame: map

  robot_base_frame: base_footprint

  # 膨胀半径

  inflation_radius: 0.4

  # 代价比例系数,越大代价值越小

  cost_scaling_factor: 6.0

  update_frequency: 10.0

  publish_frequency: 10.0

  transform_tolerance: 0.5

  static_map: true

 

local_costmap_params.yaml:

local_costmap:

  global_frame: odom

  robot_base_frame: base_footprint

  # 膨胀半径

  inflation_radius: 0.3

  # 代价比例系数,越大代价值越小

  cost_scaling_factor: 3.0

  update_frequency: 10.0

  publish_frequency: 10.0

  transform_tolerance: 0.5

  static_map: false 

  rolling_window: true

  width: 3

  height: 3

  resolution: 0.05

move_base_params.yaml：

shutdown_costmaps: false

controller_frequency: 10.0

planner_patience: 5.0

controller_patience: 15.0

conservative_reset_dist: 3.0

planner_frequency: 5.0

oscillation_timeout: 10.0

oscillation_distance: 0.2

dwa_local_planner_params_waffle_pi.yaml：

DWAPlannerROS:

# 机器人配置参数

  max_vel_x: 0.26

  min_vel_x: -0.26

 

  max_vel_y: 0.0

  min_vel_y: 0.0

 

# 机器人直线速度

  max_vel_trans:  0.26

  min_vel_trans:  0.13

 

  max_vel_theta: 1.82

  min_vel_theta: 0.9

 

  acc_lim_x: 2.5

  acc_lim_y: 0.0

  acc_lim_theta: 3.2

 

# 全局容忍度参数

  xy_goal_tolerance: 0.05

  yaw_goal_tolerance: 0.17

  latch_xy_goal_tolerance: false

 

  sim_time: 2.0

  vx_samples: 20

  vy_samples: 0

  vth_samples: 40

  controller_frequency: 10.0

 

# Trajectory Scoring Parameters

  path_distance_bias: 32.0

  goal_distance_bias: 20.0

  occdist_scale: 0.02

  forward_point_distance: 0.325

  stop_time_buffer: 0.2

  scaling_speed: 0.25

  max_scaling_factor: 0.2

 

# Oscillation Prevention Parameters

  oscillation_reset_dist: 0.05

 

# Debugging

  publish_traj_pc : true

  publish_cost_grid_pc: true

在/launch/include下新建move_base.launch文件，将move_base相关参数上传参数服务器。

move_base.launch：

DWAPlannerROS:

# 机器人配置参数

  max_vel_x: 0.26

  min_vel_x: -0.26

 

  max_vel_y: 0.0

  min_vel_y: 0.0

 

# 机器人直线速度

  max_vel_trans:  0.26

  min_vel_trans:  0.13

 

  max_vel_theta: 1.82

  min_vel_theta: 0.9

 

  acc_lim_x: 2.5

  acc_lim_y: 0.0

  acc_lim_theta: 3.2

 

# 全局容忍度参数

  xy_goal_tolerance: 0.05

  yaw_goal_tolerance: 0.17

  latch_xy_goal_tolerance: false

 

  sim_time: 2.0

  vx_samples: 20

  vy_samples: 0

  vth_samples: 40

  controller_frequency: 10.0

 

# Trajectory Scoring Parameters

  path_distance_bias: 32.0

  goal_distance_bias: 20.0

  occdist_scale: 0.02

  forward_point_distance: 0.325

  stop_time_buffer: 0.2

  scaling_speed: 0.25

  max_scaling_factor: 0.2

 

# Oscillation Prevention Parameters

  oscillation_reset_dist: 0.05

 

# Debugging

  publish_traj_pc : true

  publish_cost_grid_pc: true

1. navigation启动文件编写

做好了全部的准备工作，现在可以编写navigation的launch文件，在launch目录下新建navigation.launch，用于集成前述launch文件。

navigation.launch：

<launch>

    <!-- 地图服务 -->

    <include file="$(find proj03_slam_nav)/launch/include/map_server.launch"/>

    <!-- amcl -->

    <include file="$(find proj03_slam_nav)/launch/include/amcl.launch"/>

    <!-- move_base -->

    <include file="$(find proj03_slam_nav)/launch/include/move_base.launch"/>

    <!-- rviz -->

    <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />

    <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />

    <node pkg="rviz" type="rviz" name="rviz"/>

    <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find proj03_slam_nav)/rviz/turtlebot3_nav.rviz" /> -->

</launch>

完成前述所有编辑后，启动机器人仿真环境。

$ export TURTLEBOT3_MODEL=waffle_pi

$ roslaunch turtlebot3_gazebo turtlebot3_house.launch

运行navigation.launch文件：

$ source devel/setup.bash

$ roslaunch proj03_slam_nav navigation.launch

在RViz中添加如下订阅，分别用于显示机器人模型、局部规划路径、全局规划路径、静态地图、全局代价地图、局部代价地图：

添加完成后可保存RViz配置文件到proj03_slam_nav/rviz目录下，修改launch文件相关位置，后续可以直接打开已保存好配置的RViz界面。

Rviz加载完成后显示如下：

选择2D Nav Goal选项在地图中发布目标点，小车规划路径如下：

在小车路途中设置障碍物，小车成功规划避让路线。

可以发现在雷达扫描侦测到前方障碍物后，小车实现了自主避让。

4、基于actionlib的自主巡航

actionlib是ROS中一个很重要的功能包集合，尽管在ROS中已经提供了service机制来满足请求—响应式的使用场景，但是假如某个请求执行时间很长，在此期间用户想查看执行的进度或者取消这个请求的话，service机制就不能满足了，此时使用actionlib更为合适。

actionlib使用client-server工作模式，客户端和服务端以ROS消息方式进行通信。

接下来，我们编写一个Action Client与move_base的Action Server进行通信，即是说将goal消息发送给move_base，通过回调函数接收move_base返回的机器人状态信息。

move_base的goal消息定义如下： 

move_base_msgs/MoveBaseGoal goal

  geometry_msgs/PoseStamped target_pose # ros内置的带有时间戳和参考系的位姿消息

    std_msgs/Header header    # 消息头 包含序号、时间戳和坐标系id

      uint32 seq       # 消息序号 默认自增

      time stamp       # 时间戳 使用ros当前时间

      string frame_id  # 坐标系id 由于是目标点的参考系，因此设置为map

    geometry_msgs/Pose pose   # 目标位姿 包含目标位置和目标姿态

      geometry_msgs/Point position  # 目标位置

        float64 x      # 通过参数配置目标点x值

        float64 y      # 通过参数配置目标点y值

        float64 z      # 平面移动机器人无法指定z方向位置，置0

      geometry_msgs/Quaternion orientation # 目标姿态 四元数形式，yaw角转换而来

        float64 x

        float64 y

        float64 z

        float64 w

通过goal的消息定义，可以看到goal消息包含一个PoseStamped类型的对象target_pose，表示带有时间戳和参考系的位姿，其中我们需要配置的数据是目标点的x坐标，y坐标以及航向角yaw。

目标点的坐标值均参考map坐标系，yaw代表目标点处机器人的目标姿态相对于map坐标系的z轴旋转角，称为航向角/偏航角。

将我们设定的目标点（x，y，yaw）封装成goal消息发送给move_base，就完成了运动目标规划的任务。

首先，我们使用第三节navigation的相关知识，在地图中采集几个巡航点。

开启navigation.launch后，分别使用2D Nav Goal工具点击上列点，在终端订阅/amcl_pose，待机器人到达上述目标位置后，从/amcl_pose中获取目标点AMCL定位结果，充作目标点坐标值。

点1、2、3坐标值分别为：(-6.25,-0.50)，(1.00,2.50)，(6.00,-1.00)

接下来在功能包下新建/scripts文件夹，新建navigation_multi_goals.py文件。

navigation_multi_goals.py：

#!/usr/bin/env python

# -*- coding: UTF-8 -*-

import rospy

 

import actionlib

from actionlib_msgs.msg import *

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

from nav_msgs.msg import Path

from geometry_msgs.msg import PoseWithCovarianceStamped

from tf_conversions import transformations

from math import pi

from std_msgs.msg import String

 

class navigation:

    def __init__(self):

        # 类的初始化函数,初始化导航订阅与发布的主题

        self.arrive_pub = rospy.Publisher('/robot_voice/tts_topic',String,queue_size=10)

        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

        self.move_base.wait_for_server(rospy.Duration(60))

 

    # 回调函数,反馈导航过程中的信息(开始导航/抵达终点)  

    def _done_cb(self, status, result):

        rospy.loginfo("navigation done! status:%d result:%s"%(status, result))

        arrive_str = "arrived"

        self.arrive_pub.publish(arrive_str)

 

    def _active_cb(self):

        rospy.loginfo("[Navi] navigation has be actived")

 

    def _feedback_cb(self, feedback):

        rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)

 

    # 发送目标点给 move_base,由 move_base 全程接管路径规划,运动控制,这里只发布命令,接收回调。

    def goto(self, p):

#  目标点p为[x,y,yaw]形式

        rospy.loginfo("[Navi] goto %s"%p)

        arrive_str = "going to next point"

        self.arrive_pub.publish(arrive_str)

        goal = MoveBaseGoal()

      

# 发布导航目标点的封装,目标点由 position 和 orientation 组成。

        # Position 表示目标点的三维坐标(参考ros坐标系)

        # orientation 代表目标点的朝向

        goal.target_pose.header.frame_id = 'map'

        goal.target_pose.header.stamp = rospy.Time.now()

        goal.target_pose.pose.position.x = p[0]

        goal.target_pose.pose.position.y = p[1]

        q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)

# 欧拉角(roll,pitch,yaw)中，平面移动机器人仅关心yaw角，因此roll与pitch均置0。

# p[2]是角度制的yaw角，需要p[2]/180.0*pi转换为弧度制，再通过quaternion_from_euler转换为四元数。

        goal.target_pose.pose.orientation.x = q[0]

        goal.target_pose.pose.orientation.y = q[1]

        goal.target_pose.pose.orientation.z = q[2]

        goal.target_pose.pose.orientation.w = q[3]

 

        self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)

        result = self.move_base.wait_for_result(rospy.Duration(60))

        if not result:

            self.move_base.cancel_goal()

            rospy.loginfo("Timed out achieving goal")

        else:

            state = self.move_base.get_state()

            if state == GoalStatus.SUCCEEDED:

                rospy.loginfo("reach goal %s succeeded!"%p)

        return True

 

    # 取消导航

    def cancel(self):

        self.move_base.cancel_all_goals()

        return True

 

if __name__ == "__main__":

    rospy.init_node('navigation',anonymous=True)

    # 读取目标点集合,可在 launch 文件中进行修改。

goalListX = rospy.get_param('~goalListX', '2.0, 2.0')

    goalListY = rospy.get_param('~goalListY', '2.0, 4.0')

    goalListYaw = rospy.get_param('~goalListYaw', '0, 90.0')

 

    goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","),goalListY.split(","),goalListYaw.split(","))]

    print ('Please 1 to start navigation: ')

    input = raw_input()

    print (goals)

    r = rospy.Rate(1)

    r.sleep()

    navi = navigation()

    if (input == '1'):

        while not rospy.is_shutdown():

            for goal in goals:

                navi.goto(goal)

                rospy.sleep(1)

 

    while not rospy.is_shutdown():

        r.sleep()

保存后，输入如下命令添加可执行权限：

$  chmod 777 navigation_multi_goals.py  

目标点的参数化有助于目标点的调试与修改。在/launch目录下添加如下文件，将我们采集的目标点坐标值(-6.25,-0.50)，(1.00,2.50)，(6.00,-1.00)添加到相应位置：

navigation_multi.launch：

<launch>

    <node pkg="proj03_slam_nav" type="navigation_multi_goals.py" respawn="false" name="navigation_multi_goals" output="screen">

        <param name="goalListX" value="-6.25, 1.00, 6.00" />

        <param name="goalListY" value="-0.50, 2.50, -1.00" />

        <param name="goalListYaw" value="90.0, 0.0, 90.0" /><!--degree-->

    </node>

</launch>

完成前述所有编辑后，启动机器人仿真环境，运行navigation.launch文件，配置RViz。

另起终端，输入如下命令启动launch文件：

$ source devel/setup.bash

$ roslaunch proj03_slam_nav navigation_multi.launch

终端返回action通信的feedback回调信息： 

同时可以看到小车三个目标点之间巡航。

1. 实训结果

1. 使用launch文件启动gmapping、navigation功能包，了解其参数配置。

2. 了解move_base节点的插件及参数配置。

3. 实现基于gmapping与navigation的已知地图下小车自主导航避障功能。

4. 了解actionlib通信特点，编写python脚本实现actionlib通信发布航点，通过move_base驱动机器人进行目标点路径规划并实现巡航。