大学生智能汽车竞赛系列教程 第三章 导航功能包创建教程

一、导航指导

---

请按照先前教程完成ros的安装、工作空间(gazebo_test_ws)的创建和建图环境(gazebo_map)的搭建。

1.1 安装 ros-melodic-navigation 功能包

• 打开终端依次输入如下指令：

sudo apt-get install ros-melodic-navigation                           //编译功能包

1.2 在gazebo_test_ws/src下创建功能包gazebo_nav

• 打开终端输入如下指令：

cd gazebo_test_ws/src

catkin_create_pkg gazebo_nav      //功能包创建完毕

cd ..

catkin_make                                      //编译功能包

1.3 在gazebo_nav文件夹下创建launch,map两个文件夹

cd gazebo_test_ws/src/gazebo_nav

mkdir launch map

launch文件夹用于存储节点启动文件，map用于存储建图完成后的图片和配置文件(yaml)

• 打开终端依次输入如下指令：

cd launch 

mkdir config                                     //用于存放配置文件

touch navi_demo.launch           //创建节点启动文件

cd config

mkdir amcl move_base rviz		//创建amcl,move_base,rviz文件夹

//amcl 存储定位参数文件        move_base 存储路径规划参数文件      rviz 存储rviz配置文件

1.4 配置机器人定位启动文件

cd  amcl

touch amcl_omni.launch

将如下代码贴入到amcl_omni.launch文件中

<launch>
<node pkg="amcl" type="amcl" name="amcl">
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="omni"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="50"/>
  <param name="use_map_topic" value="false"/>  
  <!-- //当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说，当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。在navigation 1.4.2中新加入的参数。 -->
  <param name="first_map_only" value="true"/>  
    <!-- //当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图，在navigation 1.4.2中新加入的参数。 -->


  <param name="min_particles" value="1000"/>
  <param name="max_particles" value="10000"/>
  <param name="kld_err" value="0.05"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.2"/>
  <param name="odom_alpha2" value="0.2"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.8"/>
  <param name="odom_alpha4" value="0.2"/>
  <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_lambda_short" value="0.1"/>
  <!-- <param name="laser_model_type" value="likelihood_field"/> -->
  <param name="laser_model_type" value="beam"/>
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="odom_frame_id" value="odom"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>
  <!-- <param name="odom_frame_id" value="odom"/>   -->
  <!-- //里程计默认使用的坐标系 -->
  <!-- <param name="base_frame_id" value="base_link"/>   -->
  <!-- //用作机器人的基坐标系 -->
  <!-- <param name="global_frame_id" value="map"/>   -->
  <!-- //由定位系统发布的坐标系名称 -->
  <!-- <param name="tf_broadcast" value="false"/>   -->
  <!-- //设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换 -->
</node>
</launch>

1.5 配置导航配置文件

• 打开终端输入如下指令：

cd ..
cd move_base 
touch costmap_common_params.yaml  dwa_local_planner_params.yaml   global_costmap_params.yaml   global_planner_params.yaml   local_costmap_params.yaml

1.5.1 将如下代码贴入到costmap_common_params.yaml文件中

#Description:
# 代价地图通用参数配置文件,就是全局代价地图和局部代价地图
# 共同都需要配置的参数,各参数意义如下:
# robot_radius: 机器人的半径
# "obstacle_range" 参数确定最大范围传感器读数，这将导致障碍物被放入代价地图中。
# 在这里，我们把它设置在3米，这意味着机器人只会更新其地图包含距离移动基座3米以内的障碍物的信息。
# “raytrace_range”参数确定了用于清除指定范围外的空间。
# 将其设置为3.0米，这意味着机器人将尝试清除3米外的空间，在代价地图中清除3米外的障碍物。

#robot_radius: 0.2

footprint: [[0.171, -0.128], [0.171, 0.128],[-0.171, 0.128], [-0.171, -0.128]]

obstacle_layer:
  enabled: true
  combination_method: 1
  track_unknown_space: true
  obstacle_range: 2.0
  raytrace_range: 2.0
  observation_sources: laser_scan_sensor
  laser_scan_sensor: {
    sensor_frame: laser_frame,
    data_type: LaserScan,
    topic: scan,
    marking: true,
    clearing: true,
    inf_is_valid: true
  }
 
inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0
  inflation_radius: 0.03
#0.15
 
static_layer:
  enabled: true

1.5.2 将如下代码贴入到dwa_local_planner_params.yaml文件中

#Description:
#  dwa_local_planner提供一个能够驱动底座的控制器,该控制器连接了路径规划器和机器人.
#  使用地图,规划器产生从起点到目标点的运动轨迹,在移动时,规划器在机器人周围产生一个函数,
#  用网格地图表示。控制器的工作就是利用这个函数来确定发送给机器人的速度dx, dy, dtheta
#
#       >> DWA算法的基本思想 <<
#  1.在机器人控制空间离散采样(dx, dy, dtheta)
#  2.对每一个采样的速度进行前向模拟,看看在当前状态下,使用该采样速度移动一小段时间后会发生什么.
#  3.评价前向模拟得到的每个轨迹,是否接近障碍物,是否接近目标,是否接近全局路径以及速度等等.舍弃非法路径
#  4.选择得分最高的路径,发送对应的速度给底座
#
#  DWA与Trajectory Rollout的区别主要是在机器人的控制空间采样差异.Trajectory Rollout采样点来源于整个
#  前向模拟阶段所有可用速度集合,而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合.这意味着相比之下
#  DWA是一种更加有效算法,因为其使用了更小采样空间;然而对于低加速度的机器人来说可能Trajectory Rollout更好,
#  因为DWA不能对常加速度做前向模拟。
#
#  下面来依次介绍下每个参数的意义:
#   * acc_lim_x:x方向的加速度绝对值
#   * acc_lim_y:y方向的加速度绝对值,该值只有全向移动的机器人才需配置.
#   * acc_lim_th:旋转加速度的绝对值.
#
#   * max_trans_vel:平移速度最大值绝对值
#   * min_trans_vel:平移速度最小值的绝对值
#
#   * max_vel_x:x方向最大速度的绝对值
#   * min_vel_x:x方向最小值绝对值,如果为负值表示可以后退.
#   * max_vel_y:y方向最大速度的绝对值.
#   * min_vel_y:y方向最小速度的绝对值.
#
#   *trans_stopped_vel:停止的时候，最大的平移速度
#   *theta_stopped_vel:停止的时候，最大角速度
#
#   * max_rot_vel:最大旋转速度的绝对值.
#   * min_rot_vel:最小旋转速度的绝对值.
#   *
#   * yaw_goal_tolerance:到达目标点时偏行角允许的误差，单位弧度.
#   * xy_goal_tolerance:到达目标点时,在xy平面内与目标点的距离误差.
#   * latch_xy_goal_tolerance:设置为true,如果到达容错距离内,机器人就会原地
#        旋转,即使转动是会跑出容错距离外.
#
#   * sim_time:向前仿真轨迹的时间.
#   * sim_granularity:步长,轨迹上采样点之间的距离,轨迹上点的密集程度.
#   * vx_samples:x方向速度空间的采样点数.
#   * vy_samples:y方向速度空间采样点数.
#   * vth_samples:旋转方向的速度空间采样点数.
#   * controller_frequency:发送给底盘控制移动指令的频率.
#
#   * path_distance_bias:定义控制器与给定路径接近程度.
#   * goal_distance_bias:定义控制器与局部目标点的接近程度
#   * occdist_scale:定义控制器躲避障碍物的程度.
#   * stop_time_buffer:为防止碰撞,机器人必须提前停止的时间长度.
#   * scaling_speed:启动机器人底盘的速度.
#   * max_scaling_factor:最大缩放参数.
#
#   * publish_cost_grid:是否发布规划器在规划路径时的代价网格.如果设置为true,
#       那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.
#       每个点云代表代价网格,并且每个单独的评价函数都有一个字段及其每个单元
#       的总代价，并考虑评分参数.
#   
#   * oscillation_reset_dist:机器人运动多远距离才会重置振荡标记.
#   
#   * prune_plan:机器人前进是是否清楚身后1m外的轨迹.
latch_xy_goal_tolerance: true

DWAPlannerROS:
# Robot Configuration Parameters - stdr robot
  acc_lim_x: 0.5
  acc_lim_y: 0.0
  acc_lim_th: 0.3

#  max_trans_vel: 0.4#choose slightly less than the base's capability
#  min_trans_vel: 0.1 #this is the min trans velocity when there is negligible rotational velocity

  max_vel_trans: 1.0 #choose slightly less than the base's capability
  min_vel_trans: -0.1 #this is the min trans velocity when there is negligible rotational velocity

  trans_stopped_vel: 0.1
  theta_stopped_vel: 0.1

  max_vel_x: 1.2  #  0.8
  min_vel_x: -0.2
  max_vel_y: 1.2  #   0.2  diff drive robot,don't need set vel_y
  min_vel_y: -0.2

#  max_rot_vel: 0.5  #choose slightly less than the base's capability
#  min_rot_vel: 0.1  #this is the min angular velocity when there is negligible translational velocity

  max_vel_theta: 0.6  #choose slightly less than the base's capability
  min_vel_theta: -0.6  #this is the min angular velocity when there is negligible translational velocity


# Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1  # 0.1 rad = 5.7 degree
  xy_goal_tolerance: 0.3
  latch_xy_goal_tolerance: true

# Forward Simulation Parameters
  sim_time: 1.0    # 1.7
  sim_granularity: 0.025
  vx_samples: 3    # default 3
  vy_samples: 3    # diff drive robot, there is only one sample
  vth_samples: 7  # 20
  controller_frequency: 5.0

# Trajectory Scoring Parameters
  path_distance_bias: 32.0  # 32.0  -weighting for how much it should stick to the global path plan
  goal_distance_bias: 20.0  # 24.0  -wighting for how much it should attempt to reach its goal
  occdist_scale: 0.2        # 0.01  -weighting for how much the controller should avoid obstacles
  forward_point_distance: 0.125 # 0.325 -how far along to place an additional scoring point
  stop_time_buffer: 0.2     # 0.2   -amount of time a robot must stop in before colliding for a valid traj.
  scaling_speed: 0.20       # 0.25  -absolute velocity at which to start scaling the robot's footprint
  max_scaling_factor: 0.2   # 0.2   -how much to scale the robot's footprint when at speed.
  publish_cost_grid: false

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # default 0.05

  hdiff_scale: 1.0 #全局和局部角度判断
  heading_points: 1

# Global Plan Parameters
  prune_plan: true

  publish_traj_pc : false
  publish_cost_grid_pc: false
  global_frame_id: map

1.5.3 将如下代码贴入到global_costmap_params.yaml文件中

#Description:
#  全局代价地图参数配置文件,各参数的意义如下：
#  global_frame:在全局代价地图中的全局坐标系;
#  robot_base_frame:机器人的基坐标系;
#
global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 10
  track_unknown_space: true
  plugins:
    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

1.5.4 将如下代码入贴到global_planner_params.yaml文件中

#Description:
#   * allow_unknown:是否允许规划器规划穿过未知区域的路径,
#        只设计该参数为true还不行,还要在costmap_commons_params.yaml
#        中设置track_unknown_space参数也为true才行.
#   * default_tolerance:当设置的目的地被障碍物占据时,需要以该参数
#        为半径寻找到最近的点作为新目的地点.
#   * visualize_potential:是否显示从PointCloud2计算得到的势区域.
#   * use_dijkstra:设置为true,将使用dijkstra算法,否则使用A*算法.
#   * use_quadratic:设置为true,将使用二次函数近似函数,否则使用更加
#      简单的计算方式,这样节省硬件计算资源.
#   * use_grid_path:如果设置为true,则会规划一条沿着网格边界的路径,
#      偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
#   * old_navfn_behavior:若在某些情况下,想让global_planner完全复制navfn
#      的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,
#      现在已经都用global_planner代替navfn了,所以不建议设置为true.
#   * lethal_cost:致命代价值,默认是设置为253,可以动态来配置该参数.
#   * neutral_cost:中等代价值,默认设置是50,可以动态配置该参数.
#   * cost_factor:代价地图与每个代价值相乘的因子.
#   * publish_potential:是否发布costmap的势函数.
#   * orientation_mode: 如何设置每个点的方向（None = 0,Forward = 1,
#       Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,
#       Leftward = 5,Rightward = 6）（可动态重新配置）
#   * orientation_window_size:根据orientation_mode指定的位置积分来得到
#      使用窗口的方向.默认值1,可以动态重新配置.
GlobalPlanner:
  allow_unknown: false
  default_tolerance: 0.2
  visualize_potential: false
  use_dijkstra: false
  use_quadratic: true
  use_grid_path: true
  old_navfn_behavior: false

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  publish_potential: true
  orientation_mode: 0
  orientation_window_size: 1

1.5.5 将如下代码贴入到local_costmap_params.yaml文件中

#Description:
#  本地代价地图需要配置的参数，各参数意义如下:
#  global_frame:在本地代价地图中的全局坐标系;
#  robot_base_frame:机器人本体的基坐标系;

local_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 3.0
  static_map: false
  rolling_window: true
  width: 3.0
  height: 3.0
  resolution: 0.05
  transform_tolerance: 10
  plugins:
#    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

导航配置文件创建完毕，退出config文件夹。

1.5.6 配置navi_demo.launch文件， 将如下代码贴入到demo_demo.launch文件中

<launch>
  <node name="map_server" pkg="map_server" type="map_server" args="$(find gazebo_nav)/map/mymap1.yaml" output="screen">
   <param name="frame_id" value="map" />
  </node>

  <include file="$(find gazebo_nav)/launch/config/amcl/amcl_omni.launch" > </include>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS"/>

    <rosparam file="$(find gazebo_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find gazebo_nav)/launch/config/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find gazebo_nav)/launch/config/move_base/global_planner_params.yaml" command="load" />
    <rosparam file="$(find gazebo_nav)/launch/config/move_base/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find gazebo_nav)/launch/config/move_base/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find gazebo_nav)/launch/config/move_base/global_costmap_params.yaml" command="load" />
  </node>

<node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find gazebo_nav)/launch/config/rviz/demo2.rviz" />
</launch>

1.6 将存于gazebo_map中的地图文件复制到gazebo_nav/map目录下

若要修改读取的地图文件，修改navi_demo.launch即可

  <node name="map_server" pkg="map_server" type="map_server" args="$(find gazebo_nav)/map/mapname.yaml" output="screen">        //将mymap1修改即可

关闭所有终端，重新执行

roslaunch gazebo_pkg race.launch

roslaunch gazebo_nav navi_demo.launch

1.7 保存Rviz配置

rviz启动后点击左下角add，在topic中选择/laserscan,/map,/ PointStamped/move_base//GlobalPlanner/plan/path,在display中选择robotmodel。

添加完毕后，点击左上角file—>save config as文件命名为demo2.rviz，存储在gazebo_nav/launch/config/rviz目录下

在rviz中选择navi goal在地图中选定地点即可开始导航

此导航包只用于示例功能不完整，请同学们自行修改参数进行调试。