ros 雷达 slam 导航文件分析

- robot_slam_laser.launch
- - robot_lidar.launch
  - lidar.launch
  - raplidar.launch
  - karto.launch
  - gmapping.launch
  - cartographer.launch
- robot_navigation.launch
- - map.yaml
  - map.pgm
  - amcl_params.yaml
  - move_base.launch
  - costmap_common_params.yaml
  - local_costmap_params.yaml
  - global_costmap_params.yaml
  - move_base_params.yaml：
  - dwa_local_planner_params.yaml
  - teb_local_planner_params.yaml
- 多点导航
- - multi_points_navigation.launch
  - multi_navigation.launch
  - multi_goal_point.py：
- 多点全自动巡航
- - way_point.launch
  - multi_goal_point.py

lidar文件夹下面有多个雷达功能包。

还包括一个udev规则转换脚本。雷达使用cp2102usb转串口芯片。

多个设备通过usb连主机，很难分辨各个端口名所对应的设备。

给USB 转串口的芯片设定规则，根据芯片的 Vender ID 和 Product ID 进行筛选。读取到芯片后做一个符号链接，链接到对应端口。取名为rplidar等。

initenv.sh
#!/bin/bash
echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="hldslidar"' >/etc/udev/rules.d/hldslidar.rulesecho  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rulesecho  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="iiilidar"' >/etc/udev/rules.d/iiilidar.rulesecho  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="rplidar"' >/etc/udev/rules.d/rplidar.rulesecho  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout",  SYMLINK+="sclidar"' >/etc/udev/rules.d/sclidar.rulessystemctl daemon-reload
service udev reload
sleep 1
service udev restart

robot_navigation这个文件夹很重要。

robot:
roslaunch robot_navigation lidar.launch  启动雷达
PC：
roslaunch robot_navigation lidar_rviz.launch  运行rviz工具robot:
roslaunch robot_navigation robot_slam_laser.launch  激光雷达slam建图
pc：
roslaunch robot_navigation slam_rviz.launchrobot：保存地图
roscd robot_navigation/maps
rosrun map_server map_saver -f maprobot：
roslaunch robot_navigation robot_navigation.launch   导航和避障
pc：
roslaunch robot_navigation navigation_rviz.launch  robot：
roslaunch robot_navigation robot_slam_laser.launch slam_methods:=hector 建图算法切换
pc：
roslaunch robot_navigation navigation_rviz.launchrobot：
roslaunch robot_navigation robot_navigation.launch planner:=dwa 路径规划算法切换robot：
roslaunch robot_navigation robot_slam_laser.launch planner:=teb 导航建图
pc：
roslaunch robot_navigation slam_rviz.launch

文件里面包含：

rviz文件夹：rviz相关配置文件
maps文件夹：地图文件和地图配置信息相关的文件
param文件夹：yaml文件
launch文件夹
script文件夹:脚本文件

launch文件夹里面：

includes文件夹：建图算法的launch文件
lidar文件夹：雷达的launch文件
rviz文件夹：rviz的launch文件

robot_slam_laser.launch

robot_slam_laser.launch：

这里面主要就是launch文件的循环调用，比如，通过robot_slam_laser.launch文件调用了robot_lidar.launch文件。

这样做的好处是具有模块化。可以避免重复启动多个节点。

<launch><!-- Arguments --><arg name="slam_methods" default="gmapping" doc="slam type [gmapping, hector, karto, cartographer]"/><!-- 可传入参数slam_methods，选择SLAM建图算法 --><arg name="open_rviz" default="false"/><arg name="simulation" default= "false"/> <!-- 判断启动实体机器人还是 stage 仿真器 --><arg name="planner"  default="" doc="opt: dwa, teb"/> <!-- 无地图环境下边建图边导航，可选参数为dwa、teb局部路径规划器 --><param name="/use_sim_time" value="$(arg simulation)" />  <!-- simulation robot with lidar and map --><!-- simulation为true 时启动仿真软件运行仿真机器人--><group if="$(arg simulation)"><include file="$(find robot_simulation)/launch/simulation_one_robot.launch"/></group><!-- robot with lidar --><!--simulation为false 启动launch文件夹下的robot_lidar.launch--><group unless="$(arg simulation)"><include file="$(find robot_navigation)/launch/robot_lidar.launch"/></group><!-- SLAM: Gmapping, Cartographer, Hector, Karto --><!-- 根据 slam_methods 参数选用不同的建图算法 --><include file="$(find robot_navigation)/launch/includes/$(arg slam_methods).launch"><arg name="simulation"            value="$(arg simulation)"/></include><!-- move_base 无地图环境下导航建图功能，传入planner参数为dwa或teb，就会启动导航堆栈--><group unless="$(eval planner == '')"><!--启动move_base.launch--><include file="$(find robot_navigation)/launch/move_base.launch" unless="$(eval planner == '')"><arg name="planner"            value="$(arg planner)"/></include></group><!--因为gmapping会发布map话题，这里不用启动地图服务器从本地读取地图也可以正常导航--><!-- rviz open_rviz 参数为 true时，打开rviz--><group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find robot_navigation)/rviz/slam.rviz"/><!-- 使用rviz文件夹中预置的配置文件slam.rviz --></group></launch>

robot_lidar.launch

robot_lidar.launch:启动底盘和雷达这两个硬件

<launch><!-- config param --><arg name="pub_imu"       default="False" /><arg name="sub_ackermann"       default="False" /><arg name="lidar_frame" default="base_laser_link"/>  <!-- include 标签包含了两个 launch --><include file="$(find base_control)/launch/base_control.launch"><arg name="pub_imu"            value="$(arg pub_imu)"/>  <arg name="sub_ackermann"            value="$(arg sub_ackermann)"/>  </include><include file="$(find robot_navigation)/launch/lidar.launch"><arg name="lidar_frame"            value="$(arg lidar_frame)"/>  </include></launch>

lidar.launch

lidar.launch:这里面启动了雷达的launch文件

<launch><!--robot bast type use different tf value--><!-- 读取机器人底盘类型 --><arg name="base_type"       default="$(env BASE_TYPE)" /><!-- robot frame --><arg name="base_frame"       default="/base_footprint" />    <!-- 读取机器人雷达类型 --><arg name="lidar_type"       default="$(env LIDAR_TYPE)" />   <arg name="lidar_frame" default="base_laser_link"/>  <!-- 根据lidar_type类型启动节点，如果是rplidar，就启动rplidar.launch --><include file="$(find robot_navigation)/launch/lidar/$(arg lidar_type).launch"><arg name="lidar_frame"            value="$(arg lidar_frame)"/></include><!-- TF 坐标转换 --><group if="$(eval base_type == 'NanoRobot')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="-0.01225 0.0 0.18 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == 'NanoRobot_Pro')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="-0.0515 0.0 0.18 -1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == '4WD')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == '4WD_OMNI')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == 'NanoCar')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="0.1037 0.0 0.115 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == 'NanoCar_Pro')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="0.1037 0.0 0.165 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group><group if="$(eval base_type == 'NanoCar_SE')"><node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"args="0.0955 0.0 0.115 1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20"></node></group></launch>

raplidar.launch

raplidar.launch 雷达数据

端口名称使用了udev规则的名称。

<param name="serial_port"         type="string" value="/dev/rplidar"/>

<launch><arg name="lidar_frame" default="lidar"/>   <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen"><param name="serial_port"         type="string" value="/dev/rplidar"/><param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 --><!--param name="serial_baudrate"     type="int"    value="256000"--><!--A3 --><param name="frame_id"            type="string" value="$(arg lidar_frame)"/><param name="inverted"            type="bool"   value="false"/><param name="angle_compensate"    type="bool"   value="true"/></node>
</launch>

karto.launch

karto.launch文件，配置参数是通过yaml文件载入的。

<launch><arg name="simulation" default= "false"/> <param name="/use_sim_time" value="$(arg simulation)" />  <!-- slam_karto --><node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen"><rosparam command="load" file="$(find robot_navigation)/config/karto_params.yaml" /></node></launch>

karto_params.yaml

# General Parametersbase_frame: base_footprintmap_update_interval: 5use_scan_matching: trueuse_scan_barycenter: trueminimum_travel_distance: 0.12 minimum_travel_heading: 0.174             #in radiansscan_buffer_size: 70scan_buffer_maximum_scan_distance: 3.5link_match_minimum_response_fine: 0.12link_scan_maximum_distance: 3.5loop_search_maximum_distance: 3.5do_loop_closing: trueloop_match_minimum_chain_size: 10loop_match_maximum_variance_coarse: 0.4   # gets squared laterloop_match_minimum_response_coarse: 0.8loop_match_minimum_response_fine: 0.8# Correlation Parameters - Correlation Parameterscorrelation_search_space_dimension: 0.3correlation_search_space_resolution: 0.01correlation_search_space_smear_deviation: 0.03# Correlation Parameters - Loop Closure Parametersloop_search_space_dimension: 8.0loop_search_space_resolution: 0.05loop_search_space_smear_deviation: 0.03# Scan Matcher Parametersdistance_variance_penalty: 0.3      # gets squared laterangle_variance_penalty: 0.349       # in degrees (gets converted to radians then squared)fine_search_angle_offset: 0.00349   # in degrees (gets converted to radians)coarse_search_angle_offset: 0.349   # in degrees (gets converted to radians)coarse_angle_resolution: 0.0349     # in degrees (gets converted to radians)minimum_angle_penalty: 0.9minimum_distance_penalty: 0.5use_response_expansion: false

与上面对比的话，就是通过launch文件直接传参数。他们两个效果其实是一样的。

gmapping.launch

<launch><arg name="set_base_frame" default="base_footprint"/><arg name="set_odom_frame" default="odom"/><arg name="set_map_frame"  default="map"/><arg name="scan_topic" default="/scan" /><arg name="simulation" default= "false"/> <param name="/use_sim_time" value="$(arg simulation)" />  <!-- Gmapping --><node pkg="gmapping" type="slam_gmapping" name="gmapping" output="screen"><param name="base_frame" value="$(arg set_base_frame)"/><param name="odom_frame" value="$(arg set_odom_frame)"/><param name="map_frame"  value="$(arg set_map_frame)"/><param name="map_update_interval" value="2.0"/><param name="maxUrange" value="5.0"/><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="minimumScore" value="50"/><param name="srr" value="0.1"/><param name="srt" value="0.2"/><param name="str" value="0.1"/><param name="stt" value="0.2"/><param name="linearUpdate" value="1.0"/><param name="angularUpdate" value="0.2"/><param name="temporalUpdate" value="0.5"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="100"/><param name="xmin" value="-10.0"/><param name="ymin" value="-10.0"/><param name="xmax" value="10.0"/><param name="ymax" value="10.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>

cartographer.launch

它里面使用了lua脚本语言载入配置文件。

-configuration_basename revo_lds_$(arg version).lua"

revo_lds_melodic.lua

include "map_builder.lua"
include "trajectory_builder.lua"options = {map_builder = MAP_BUILDER,trajectory_builder = TRAJECTORY_BUILDER,map_frame = "map",
-- tracking_frame = "base_laser_link",
-- published_frame = "base_laser_link",
-- odom_frame = "odom",
-- provide_odom_frame = true,
-- use_odometry = false,tracking_frame = "base_footprint",published_frame = "odom",odom_frame = "odom",provide_odom_frame = false,publish_frame_projected_to_2d = false,use_odometry = true,use_nav_sat = false,use_landmarks = false,num_laser_scans = 1,num_multi_echo_laser_scans = 0,num_subdivisions_per_laser_scan = 1,num_point_clouds = 0,lookup_transform_timeout_sec = 0.2,submap_publish_period_sec = 0.3,pose_publish_period_sec = 5e-3,trajectory_publish_period_sec = 30e-3,rangefinder_sampling_ratio = 1.,odometry_sampling_ratio = 1.,fixed_frame_pose_sampling_ratio = 1.,imu_sampling_ratio = 1.,landmarks_sampling_ratio = 1.,
}MAP_BUILDER.use_trajectory_builder_2d = trueTRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65return options

cartographer.launch

<launch>  <arg name="simulation" default= "true"/> <arg name="version" default="$(env ROS_DISTRO)"/><!-- 读取环境变量ROS_DISTRO的版本 --><param name="/use_sim_time" value="$(arg simulation)" />  <!-- cartographer_node --><node name="cartographer_node" pkg="cartographer_ros"  type="cartographer_node" args="  -configuration_directory $(find robot_navigation)/config  -configuration_basename revo_lds_$(arg version).lua"  output="screen">  <!--根据version的参数选择配置文件--></node>  <group if="$(eval version == 'melodic')"><!--对于melodic版本，手动启动下面这个节点--><node pkg="cartographer_ros" type="cartographer_occupancy_grid_node" name="cartographer_occupancy_grid_node"/></group>
</launch>

robot_navigation.launch

两个group，每个都，实现了机器人、雷达、地图、amcl四个节点的启动。

<launch><!-- Arguments --><!--传入地图文件所在的路径--><arg name="map_file" default="$(find robot_navigation)/maps/map.yaml"/><!--参数map_file传入maps文件下的map.yaml文件 --><!--如果地图名称和保存路径是自定义的，只需修改传入的map_file的值--><arg name="simulation" default= "false"/> <arg name="planner"  default="teb" doc="opt: dwa, teb"/> <arg name="open_rviz" default="false"/><arg name="use_dijkstra" default= "true"/> <group if="$(arg simulation)"><!-- simulation robot with lidar and map--><!-- simulation 参数为 true，在那个launch文件中启动地图服务 --><include file="$(find robot_simulation)/launch/simulation_one_robot_with_map.launch"/></group><group unless="$(arg simulation)"><!-- robot with lidar 启动底盘和雷达--><include file="$(find robot_navigation)/launch/robot_lidar.launch"><!--<arg name="robot_name"            value="$(arg robot_name)"/>--></include><!-- Map server 启动地图服务器读入map_file参数获取地图文件--><node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"><param name="frame_id" value="map"/></node><!-- AMCL，启动一个定位节点 amcl --><node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- 通过yaml文件读取配置文件--><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" /><!-- 通过下面的设置，机器人启动时候默认就是位于地图零点 --><!-- 如果机器人从一个固定位置启动且不是原点，可以修改参数 --><param name="initial_pose_x"            value="0.0"/><param name="initial_pose_y"            value="0.0"/><param name="initial_pose_a"            value="0.0"/><!--航向--></node></group><!-- move_base 导航堆栈--><include file="$(find robot_navigation)/launch/move_base.launch" ><!--传入的参数--><arg name="planner"            value="$(arg planner)"/><arg name="simulation"            value="$(arg simulation)"/><arg name="use_dijkstra"     value="$(arg use_dijkstra)"/></include><!-- rviz --><group if="$(arg open_rviz)"> <node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find robot_navigation)/rviz/navigation.rviz"/></group></launch>

map.yaml

包括了地图的一些基本配置信息。

image: map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

里面的参数含义：可以在维基里面找对应的功能包，有英文介绍里面的参数。

image：地图名称。
resolution：地图分辨率，米/像素
origin：地图中左下角像素的二维姿态，为x，y，yaw。偏航为逆时针旋转，偏航=0表示无旋转
occupied_thresh：大于此阈值的像素，完全占用。
free_thresh：小于此阈值的像素，完全空闲。
negate：
像素的COLOR值x在[0,255]范围内。
首先，将整数x转换为一个浮点数p，转换公式具体取决于yaml中negate标志的定义。
如果negate=0，则p =（255 - x）/ 255.0。这意味着黑色（0）现在具有最高值（1.0）而白色（255）具有最低值（0.0）。
如果negate=1，则p = x / 255.0。

map.pgm

pgm是一个图像的格式。

在这里插入图片描述

amcl_params.yaml

可以通过amcl的ros wiki介绍页面查看参数信息。

use_map_topic: trueodom_frame_id: "odom"base_frame_id: "base_footprint"global_frame_id: "map"## Publish scans from best pose at a max of 10 Hzodom_model_type: "diff"odom_alpha5: 0.1gui_publish_rate: 10.0laser_max_beams: 60laser_max_range: 12.0min_particles: 500max_particles: 2000kld_err: 0.05kld_z: 0.99odom_alpha1: 0.2odom_alpha2: 0.2## translation std dev, m odom_alpha3: 0.2odom_alpha4: 0.2laser_z_hit: 0.5aser_z_short: 0.05laser_z_max: 0.05laser_z_rand: 0.5laser_sigma_hit: 0.2laser_lambda_short: 0.1laser_model_type: "likelihood_field" # "likelihood_field" or "beam"laser_likelihood_max_dist: 2.0update_min_d: 0.25update_min_a: 0.2resample_interval: 1## Increase tolerance because the computer can get quite busy transform_tolerance: 1.0recovery_alpha_slow: 0.001recovery_alpha_fast: 0.1

move_base.launch

move_base.launch：ros下的导航堆栈

默认传入的 planner 参数是 dwa，但是robot_navigation.launch调用move_base.launch时，传入的参数是teb。所以这里planner参数置为teb。

<launch><!-- Arguments --><arg name="cmd_vel_topic" default="cmd_vel" /><arg name="odom_topic" default="odom" /><arg name="planner"  default="dwa" doc="opt: dwa, teb"/> <!--默认传入的 planner 参数是 dwa--><arg name="simulation" default= "false"/> <arg name="use_dijkstra" default= "true"/>  <arg name="base_type" default= "$(env BASE_TYPE)"/> <!--不同车型不同配置--><arg name="move_forward_only" default="false"/><!--使用深度相机导航和建图，只有前向视野没有后向视野--><!--禁止车的后向运行保证运行安全--><!-- 使用了两个 group 区分DWA和TEB两个不同的局部路径规划算法 --><!-- move_base use DWA planner--><group if="$(eval planner == 'dwa')"><!-- 启动move_base节点 --><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm--><!--设置传入节点的参数--><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /><!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" />       --><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/dwa_local_planner_params.yaml" command="load" /><remap from="cmd_vel" to="$(arg cmd_vel_topic)"/><remap from="odom" to="$(arg odom_topic)"/><!-- move_forward_only时，不让有负速度（后向运动） --><param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" /><!--default is True,use dijkstra algorithm;set to False,usd A* algorithm--><param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />    </node></group><!-- move_base use TEB planner--><group if="$(eval planner == 'teb')"><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen"><!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm--><param name="base_global_planner" value="global_planner/GlobalPlanner"/><param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /><!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" /> --><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" /><rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/teb_local_planner_params.yaml" command="load" /><remap from="cmd_vel" to="$(arg cmd_vel_topic)"/><remap from="odom" to="$(arg odom_topic)"/><param name="TebLocalPlannerROS/max_vel_x_backwards" value="0.0" if="$(arg move_forward_only)" /><!--default is True,use dijkstra algorithm;set to False,usd A* algorithm--><param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" /><!--stage simulator takes the angle instead of the rotvel as input (twist message)--></node></group><!-- move_base -->
</launch>

costmap_common_params.yaml

其中一个车型的costmap_common_params.yaml文件如下。

#---(in meters)---
#单位是米footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ]
#机器人的矩形轮廓尺寸
#以机器人中心为坐标轴原点，这四个坐标点(x,y)分别对应机器人的四个角
#路径规划器获取到机器人轮廓大小从而规划相应的路径#参数含义通过move_base的ros wiki查看
transform_tolerance: 0.2obstacle_layer:enabled: trueobstacle_range: 2.5raytrace_range: 3.0inflation_radius: 0.05track_unknown_space: falsecombination_method: 1observation_sources: laser_scan_sensorlaser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}inflation_layer:enabled:              truecost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)inflation_radius:     0.1  # max. distance from an obstacle at which costs are incurred for planning paths.static_layer:enabled:              truemap_topic:            "/map"

local_costmap_params.yaml

机器人避障功能通过local costmap实现。全局地图不存在障碍，通过局部地图发现障碍然后重新规划路线实现。

#指定发布频率和更新频率
local_costmap:global_frame: maprobot_base_frame: base_footprintupdate_frequency: 5.0publish_frequency: 5.0rolling_window: true#滑动窗口#以机器人所在位置为中心的 3*3 滑动窗口#这个框设置得越小路径规划效果越差，越大机器人负担的运算就越大width: 3height: 3resolution: 0.05transform_tolerance: 0.5plugins:#设置了一个地图的层#静态层- {name: static_layer,        type: "costmap_2d::StaticLayer"}#障碍物层，地图中不存在而实际扫描到的障碍物，会被识别在障碍物层。- {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}

global_costmap_params.yaml

#设置一些发布频率等信息
global_costmap:global_frame: maprobot_base_frame: base_footprintupdate_frequency: 1.0publish_frequency: 0.5# static_map: true transform_tolerance: 0.5plugins:- {name: static_layer,            type: "costmap_2d::StaticLayer"}- {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}- {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

move_base_params.yaml：

#进行一些频率设置#对底盘的控制频率
controller_frequency: 2.0
controller_patience: 15.0#路径规划频率
planner_frequency: 0.2
planner_patience: 5.0conservative_reset_dist: 3.0#机器人在一个点持续震荡时最大允许震荡时间
oscillation_timeout: 10.0
oscillation_distance: 0.2clearing_rotation_allowed: false #clearing_rotation_allowed为True时，机器人被困住，会原地 360 度转一圈重新扫描找到一条出路(激光雷达不是360才需要用)
#阿克曼结构，不具备原地转向的能力，需要禁用

dwa_local_planner_params.yaml

DWAPlannerROS:# Robot Configuration Parameters
#设置机器人导航的最大最小速度
#x方向最大速度的绝对值,单位m/smax_vel_x: 0.25
#x方向最小值绝对值,如果为负值表示可以后退min_vel_x: -0.25
#y轴速度，麦克纳姆轮才用max_vel_y: 0.0min_vel_y: 0.0# The velocity when robot is moving in a straight line
#最大角速度，单位rad/smax_vel_trans:  0.26min_vel_trans:  0.13max_vel_theta: 1.0min_vel_theta: 0.5#x方向的加速度绝对值，m/s2acc_lim_x: 2.5
#y方向的加速度绝对值,该值只有全向移动的机器人才需配置.acc_lim_y: 0.0
#旋转加速度的绝对值.rad/s2acc_lim_theta: 3.2 # Goal Tolerance Parametes
#允许机器人所到目标的坐标（以米为单位）偏差，过小可能导致机器人在目标位置附近不断调整到精确的目标位置xy_goal_tolerance: 0.1
#角度的容忍yaw_goal_tolerance: 0.2latch_xy_goal_tolerance: false# Forward Simulation Parameterssim_time: 2.0vx_samples: 20vy_samples: 0vtheta_samples: 40controller_frequency: 10.0# Trajectory Scoring Parameterspath_distance_bias: 32.0goal_distance_bias: 20.0occdist_scale: 0.02forward_point_distance: 0.325stop_time_buffer: 0.2scaling_speed: 0.25max_scaling_factor: 0.2# Oscillation Prevention Parametersoscillation_reset_dist: 0.05# Debuggingpublish_traj_pc : truepublish_cost_grid_pc: trueprune_plan: falseuse_dwa: truerestore_defaults: false

teb_local_planner_params.yaml

TebLocalPlannerROS:odom_topic: odom# Trajectoryteb_autosize: Truedt_ref: 0.3dt_hysteresis: 0.1max_samples: 500global_plan_overwrite_orientation: Trueallow_init_with_backwards_motion: Truemax_global_plan_lookahead_dist: 3.0global_plan_viapoint_sep: -1global_plan_prune_distance: 1exact_arc_length: Falsefeasibility_check_no_poses: 2publish_feedback: False# Robot       
#最大x向前速度max_vel_x: 0.2
#最大x后退速度max_vel_x_backwards: 0.2max_vel_y: 0.0
#最大转向角速度max_vel_theta: 0.53 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
#最大x加速度acc_lim_x: 0.5
#最大角速度acc_lim_theta: 0.5# ********************** Carlike robot parameters ********************
#设置最小转向半径的参数min_turning_radius: 0.375        # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
#轮距参数wheelbase: 0.145                 # Wheelbase of our robot#将收到的角度消息转换为操作上的角度变化cmd_angle_instead_rotvel: False  # stage simulator takes the angle instead of the rotvel as input (twist message)
#使用仿真器仿真阿克曼车型，将cmd_angle_instead_rotvel改为true，
#stage 仿真器中”Car”车型接受的 cmd_vel 话题里面的z轴角速度并非给定机器人角速度，而是指转向结构的转向角度。
#cmd_angle_instead_rotvel: False，角速度就是定机器人真实的角速度# ********************************************************************footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"type: "line"line_start: [0.05, 0.0] # for type "line"line_end: [0.10, 0.0] # for type "line"# GoalTolerancexy_goal_tolerance: 0.1yaw_goal_tolerance: 0.2free_goal_vel: Falsecomplete_global_plan: True# Obstaclesmin_obstacle_dist: 0.12 # This value must also include our robot's expansion, since footprint_model is set to "line".inflation_dist: 0.6include_costmap_obstacles: Truecostmap_obstacles_behind_robot_dist: 1.5obstacle_poses_affected: 15dynamic_obstacle_inflation_dist: 0.6include_dynamic_obstacles: True costmap_converter_plugin: ""costmap_converter_spin_thread: Truecostmap_converter_rate: 5# Optimizationno_inner_iterations: 5no_outer_iterations: 4optimization_activate: Trueoptimization_verbose: Falsepenalty_epsilon: 0.1obstacle_cost_exponent: 4weight_max_vel_x: 2weight_max_vel_theta: 1weight_acc_lim_x: 1weight_acc_lim_theta: 1weight_kinematics_nh: 1000weight_kinematics_forward_drive: 1weight_kinematics_turning_radius: 1weight_optimaltime: 1 # must be > 0weight_shortest_path: 0weight_obstacle: 100weight_inflation: 0.2weight_dynamic_obstacle: 10 # not in use yetweight_dynamic_obstacle_inflation: 0.2weight_viapoint: 1weight_adapt_factor: 2# Homotopy Class Plannerenable_homotopy_class_planning: Falseenable_multithreading: Truemax_number_classes: 4selection_cost_hysteresis: 1.0selection_prefer_initial_plan: 0.95selection_obst_cost_scale: 1.0selection_alternative_time_cost: Falseroadmap_graph_no_samples: 15roadmap_graph_area_width: 5roadmap_graph_area_length_scale: 1.0h_signature_prescaler: 0.5h_signature_threshold: 0.1obstacle_heading_threshold: 0.45switching_blocking_period: 0.0viapoints_all_candidates: Truedelete_detours_backwards: Truemax_ratio_detours_duration_best_duration: 3.0visualize_hc_graph: Falsevisualize_with_time_as_z_axis_scale: False# Recoveryshrink_horizon_backup: Trueshrink_horizon_min_duration: 10oscillation_recovery: Falseoscillation_v_eps: 0.1oscillation_omega_eps: 0.1oscillation_recovery_min_duration: 10oscillation_filter_duration: 10

多点导航

多点导航：
普通的导航只能发布一个目标点。
在抵达目标点前，如果发布了新的目标点，则会舍弃原本的目标点直接前往新的目标点。
多目标点导航则是按照目标点发布顺序依次前往。stage 仿真：roslaunch robot_navigation robot_navigation.launch simulation:=true
实体机器人上运行则无需传入 simulation:=true启动多点导航工具：roslaunch robot_navigation multi_points_navigation.launch
启动rviz：roslaunch robot_navigation multi_navigation.launchrviz多出了一个2D Nav Goal按钮，可以连续给机器人指定多个目标点，机器人会依次去往各个目标点

multi_points_navigation.launch

multi_points_navigation.launch，多点导航

<launch><!--<node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher" />--><!--启动了一个 multi_goal_point.py 的节点--><node name="multi_mark" pkg="robot_navigation" type="multi_goal_point.py" output="screen" /></launch>

multi_navigation.launch

<launch><!-- rviz --><node pkg="rviz" type="rviz" name="rviz" required="true"args="-d $(find robot_navigation)/rviz/multi_navigation.rviz"/>
</launch>

multi_goal_point.py：

核心思路：

通过rviz按钮，发布goal话题，而不是发布/move_base_simple/goal话题，因为/move_base_simple/goal话题会直接发送给movebase 直接产生一个新的导航目标。

通过markerArray把goal缓存起来，通过move_base对每一个点导航的结果，触发status callback，执行下一个目标点。

循环往复。

#!/usr/bin/pythonfrom visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
from geometry_msgs.msg import PointStamped,PoseStamped
import actionlib
from move_base_msgs.msg import *
import tfdef status_callback(msg):#收到move base导航结果时，判断是读取下一个点继续发布；或是当前已是最后一个点做标记标识global goal_pub, index,markerArrayglobal add_more_point,try_againif(msg.status.status == 3):#状态是3，导航成功到目标点try_again = 1if add_more_point == 0:print 'Goal reached'if index < count:pose = PoseStamped()pose.header.frame_id = "map"pose.header.stamp = rospy.Time.now()pose.pose.position.x = markerArray.markers[index].pose.position.x#给pose一个新的值#从markerArray读取一个新的目标点pose.pose.position.y = markerArray.markers[index].pose.position.ypose.pose.orientation.w = 1goal_pub.publish(pose)#发布给move baseindex += 1elif index == count:#执行到 最后一个点add_more_point = 1else:# status值及其含义# uint8 PENDING         = 0   # The goal has yet to be processed by the action server# uint8 ACTIVE          = 1   # The goal is currently being processed by the action server# uint8 PREEMPTED       = 2   # The goal received a cancel request after it started executing#                             #   and has since completed its execution (Terminal State)# uint8 SUCCEEDED       = 3   # The goal was achieved successfully by the action server (Terminal State)# uint8 ABORTED         = 4   # The goal was aborted during execution by the action server due#                             #    to some failure (Terminal State)# uint8 REJECTED        = 5   # The goal was rejected by the action server without being processed,#                             #    because the goal was unattainable or invalid (Terminal State)# uint8 PREEMPTING      = 6   # The goal received a cancel request after it started executing#                             #    and has not yet completed execution# uint8 RECALLING       = 7   # The goal received a cancel request before it started executing,#                             #    but the action server has not yet confirmed that the goal is canceled# uint8 RECALLED        = 8   # The goal received a cancel request before it started executing#                             #    and was successfully cancelled (Terminal State)# uint8 LOST            = 9   # An action client can determine that a goal is LOST. This should not be#                             #    sent over the wire by an action server#如果是失败print 'Goal cannot reached has some error :',msg.status.status," try again!!!!"if try_again == 1:#把目标点再发给move base，让它重试pose = PoseStamped()pose.header.frame_id = "map"pose.header.stamp = rospy.Time.now()pose.pose.position.x = markerArray.markers[index-1].pose.position.xpose.pose.position.y = markerArray.markers[index-1].pose.position.ypose.pose.orientation.w = 1goal_pub.publish(pose)try_again = 0else:#已经重试过但还是到不了，就跳过它执行下个点if index < len(markerArray.markers):pose = PoseStamped()pose.header.frame_id = "map"pose.header.stamp = rospy.Time.now()pose.pose.position.x = markerArray.markers[index].pose.position.xpose.pose.position.y = markerArray.markers[index].pose.position.ypose.pose.orientation.w = 1goal_pub.publish(pose)index += 1def click_callback(msg):#rviz给定一个多目标点，会触发global markerArray,countglobal goal_pub,indexglobal add_more_pointmarker = Marker()marker.header.frame_id = "map"marker.header.stamp = rospy.Time.now()# marker.type = marker.TEXT_VIEW_FACINGmarker.type = marker.CYLINDERmarker.action = marker.ADDmarker.scale.x = 0.2marker.scale.y = 0.2marker.scale.z = 0.5marker.color.a = 1.0marker.color.r = 0.0marker.color.g = 1.0marker.color.b = 0.0marker.pose.orientation.x = 0.0marker.pose.orientation.y = 0.0marker.pose.orientation.z = 0.0marker.pose.orientation.w = 1.0marker.pose.position.x = msg.pose.position.x#读出msg中的位置信息，传到marker消息中marker.pose.position.y = msg.pose.position.ymarker.pose.position.z = msg.pose.position.zmarkerArray.markers.append(marker)# Renumber the marker IDsid = 0for m in markerArray.markers:m.id = idid += 1# Publish the MarkerArraymark_pub.publish(markerArray)#发布#first goal，第一次发布多目标点的时候，直接把话题发布出去if count==0:#没有目标点的时候pose = PoseStamped()#把话题读到pose消息pose.header.frame_id = "map"pose.header.stamp = rospy.Time.now()pose.pose.position.x = msg.pose.position.xpose.pose.position.y = msg.pose.position.ypose.pose.position.z = msg.pose.position.zpose.pose.orientation.x = msg.pose.orientation.xpose.pose.orientation.y = msg.pose.orientation.ypose.pose.orientation.z = msg.pose.orientation.zpose.pose.orientation.w = msg.pose.orientation.wgoal_pub.publish(pose)#通过goal_pub发布#goal_pub发布的就是move base需要的goal话题。包含了目标点的位置和角度信息index += 1#add_more_point=1时，在ros发布一个status=3，触发status_callback，这样能再次发布新的目标点#为解决当所有导航点完成后想要再发布新的导航点的问题if add_more_point and count > 0:add_more_point = 0move =MoveBaseActionResult()move.status.status = 3move.header.stamp = rospy.Time.now()goal_status_pub.publish(move)quaternion = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)theta = tf.transformations.euler_from_quaternion( quaternion)[2]count += 1#每点击一次，count加1# print 'add a path goal point %f %f %f'%(msg.pose.position.x,msg.pose.position.y,theta*180.0/3.14)markerArray = MarkerArray()#定义一个变量count = 0       #total goal num 统计发布的目标点数量
index = 0       #current goal point index 统计当前执行到哪个目标点
add_more_point = 0 # after all goal arrive, if add some more goal 标志位
try_again = 1  # try the fail goal once again 尝试次数rospy.init_node('multi_goal_point_demo') #创建节点mark_pub = rospy.Publisher('/path_point_array', MarkerArray,queue_size=100)#发布器发布MarkARRAY类型数据
click_goal_sub = rospy.Subscriber('/goal',PoseStamped,click_callback)#订阅器订阅goal话题，在rviz中发布多目标点所发布的话题，每次接收到goal进入click_callback
goal_pub = rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size=1)#发布器发布/move_base_simple/goal话题。
#触发move_base进行导航的话题
goal_status_sub = rospy.Subscriber('/move_base/result',MoveBaseActionResult,status_callback)#订阅movebase导航的结果。
#after all goal arrive, if add some more goal
#we deleberate pub a topic to trig the goal sent
goal_status_pub = rospy.Publisher('/move_base/result',MoveBaseActionResult,queue_size=1)
rospy.spin()#在节点中发布/move_base/result
#

多点全自动巡航

仿真：roslaunch robot_navigation robot_navigation.launch simulation:=true
实体机器人上运行则无需传入 simulation:=true 参数rviz: roslaunch robot_navigation navigation_rviz.launchpc开终端：rostopic echo /move_base_simple/goal
(订阅/move_base_simple/goal 话题)通过“2D Nav Goal”按钮设置巡航的点，记录输出的内容。
pose:position:x:y:zorizntation:xvz:打开robot_navigation 功能包下的 way_point.launch 文件修改goallistx，y，z巡航次数：
roslaunch robot_navigation way_point.launch loopTimes loopTimes:=2
loopTimes参数为巡航次数，2，机器人在巡航点来回巡航两遍后停止；0，一直巡航。巡航中每到达一个巡航点，输出下一巡航点的目标ID，巡航次数到达后会输出Loop: 2 Times Finshed 信息。

way_point.launch

<launch><!-- For Simulation --><arg name="sim_mode" default="false" /><param name="/use_sim_time" value="$(arg sim_mode)"/><arg name="loopTimes"       default="0" /><!-- move base --><!--launch 文件启动 way_point.py节点--><node pkg="robot_navigation" type="way_point.py" respawn="false" name="way_point" output="screen"><!-- params for move_base 编写好预设的地点--><param name="goalListX" value="[2.0, 4.0, 3.0]" /><param name="goalListY" value="[3.0, 2.0, 2.0]" /><param name="goalListZ" value="[0.0, 0.0, 0.0]" /><param name="loopTimes" value="$(arg loopTimes)"/><param name="map_frame" value="map" /></node></launch>

multi_goal_point.py

核心思路：

先预置一系列路径点，然后依次发布，只有在上一个点到达后，再发下一个点。

#!/usr/bin/pythonimport rospy
import string
import math
import time
import sysfrom std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
import tf
class MultiGoals:def __init__(self, goalListX, goalListY, goalListZ,loopTimes, map_frame):#订阅器，订阅move_base/result，move base导航结果话题发布，每次收到导航结果话题就会进入statusCB回调self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)#发布器，发布move_base_simple/goal，move_base真正接收的目标点话题self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)   # params & variablesself.goalListX = goalListXself.goalListY = goalListYself.goalListZ = goalListZself.loopTimes = loopTimesself.loop = 1self.wayPointFinished = False self.goalId = 0#执行当前目标点的序列号self.goalMsg = PoseStamped()self.goalMsg.header.frame_id = map_frameself.goalMsg.pose.orientation.z = 0.0self.goalMsg.pose.orientation.w = 1.0# Publish the first goaltime.sleep(1)#把goalList中xyz分别付给goalMsg消息self.goalMsg.header.stamp = rospy.Time.now()self.goalMsg.pose.position.x = self.goalListX[self.goalId]self.goalMsg.pose.position.y = self.goalListY[self.goalId]self.goalMsg.pose.orientation.x = 0.0self.goalMsg.pose.orientation.y = 0.0if abs(self.goalListZ[self.goalId]) > 1.0:self.goalMsg.pose.orientation.z = 0.0self.goalMsg.pose.orientation.w = 1.0else:w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]self.goalMsg.pose.orientation.w = wself.pub.publish(self.goalMsg) #赋值完goalMsg，通过发布器，发布给move aserospy.loginfo("Current Goal ID is: %d", self.goalId)  #输出日志self.goalId = self.goalId + 1 #目标点发步出去就会等待statusCB回调#机器人到达一个目标点就进入statusCB函数def statusCB(self, data):if self.loopTimes and (self.loop > self.loopTimes):rospy.loginfo("Loop: %d Times Finshed", self.loopTimes)self.wayPointFinished = Trueif data.status.status == 3 and (not self.wayPointFinished): # reached，而且路径巡航没结束self.goalMsg.header.stamp = rospy.Time.now()                self.goalMsg.pose.position.x = self.goalListX[self.goalId]self.goalMsg.pose.position.y = self.goalListY[self.goalId]if abs(self.goalListZ[self.goalId]) > 1.0:self.goalMsg.pose.orientation.z = 0.0self.goalMsg.pose.orientation.w = 1.0else:w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]self.goalMsg.pose.orientation.w = wself.pub.publish(self.goalMsg) #读取下一个点然后发布出去rospy.loginfo("Current Goal ID is: %d", self.goalId)              if self.goalId < (len(self.goalListX)-1):self.goalId = self.goalId + 1else:self.goalId = 0self.loop += 1if __name__ == "__main__":try:    # ROS Init    rospy.init_node('way_point', anonymous=True)# Get params 获取launch文件中传入的goalListXYZgoalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]')map_frame = rospy.get_param('~map_frame', 'map' )loopTimes = int(rospy.get_param('~loopTimes', '0')) goalListX = goalListX.replace("[","").replace("]","")goalListY = goalListY.replace("[","").replace("]","")goalListZ = goalListZ.replace("[","").replace("]","")goalListX = [float(x) for x in goalListX.split(",")]goalListY = [float(y) for y in goalListY.split(",")]goalListZ = [float(z) for z in goalListZ.split(",")]#处理成数组的形式if len(goalListX) == len(goalListY) == len(goalListZ) & len(goalListY) >=2:  #判断三个数组长度等# Constract MultiGoals Obj rospy.loginfo("Multi Goals Executing...")mg = MultiGoals(goalListX, goalListY, goalListZ, loopTimes, map_frame)#循环次数 地图基座标rospy.spin()else:rospy.errinfo("Lengths of goal lists are not the same")except KeyboardInterrupt:print("shutting down")