在9中我们已经实现了机器人的模块仿真，现在要在这个基础上实现SLAM建图，地图服务，机器人定位，路径规划

1.   还是在上述机器人的工作空间下，新建功能包（nav），导入依赖

gmapping map_server amcl move_base

2.   导航实现SLAM建图（gmapping）

在功能包下新建文件夹（launch）---> 新建文件（map.launch）写入

<launch>
<param name="use_sim_time" value="true"/><node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"><remap from="scan" to="scan"/><param name="base_frame" value="base_footprint"/><!--底盘坐标系--><param name="odom_frame" value="odom"/> <!--里程计坐标系--><param name="map_update_interval" value="5.0"/><param name="maxUrange" value="16.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="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.5"/><param name="temporalUpdate" value="3.0"/><param name="resampleThreshold" value="0.5"/><param name="particles" value="30"/><param name="xmin" value="-50.0"/><param name="ymin" value="-50.0"/><param name="xmax" value="50.0"/><param name="ymax" value="50.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"/></node><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" /><!-- 可以保存 rviz 配置并后期直接使用--><!--<node pkg="rviz" type="rviz" name="rviz" args="-d $(find my_nav_sum)/rviz/gmapping.rviz"/>-->
</launch>

关键代码解释：
<remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

执行

1.先启动 Gazebo 仿真环境

source ./devel/setup.bash
roslaunch car car.launch

2.然后再启动地图绘制的 launch 文件

source ./devel/setup.bash
roslaunch nav map.launch

3.在 rviz 中添加组件，显示栅格地图

        保存组件（避免下次再添加）可以做以下操作

File -> Save Config As

然后再launch文件中替换

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

替换为

<node pkg="rviz" type="rviz" name="rviz" args="-d $(find your_bag_name )/rviz/XXX.rviz"/>

下次再启动时就保存上次添加的组件

4.启动键盘键盘控制节点，用于控制机器人运动建图 

rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.5 _turn:=0.3

绘制完成后的地图

 

3.   导航实现----地图服务

保存地图：在launch文件夹--->新建launch文件（map_saver）,写入

<launch><arg name="filename" value="$(find your_bag_name)/map/nav" /><node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
</launch>

需要修改的地方

1）your_bag_name

2）nav--->your_saver_map_name,当然不修改，就保存为nav.pgm和nav.yaml

读取地图：在launch文件夹--->新建launch文件（map_server）,写入

<launch><!-- 设置地图的配置文件 --><arg name="map" default="nav.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find your_bag_name)/map/$(arg map)"/>
</launch>

需要修改的地方

1）your_bag_name

2）nav--->your_saver_map_name,当然不修改，就读取为nav.yaml

执行该launch文件，该节点会发布题:map(nav_msgs/OccupancyGrid)  

rviz

rviz 中使用 map 组件订阅/map话题就可以显示栅格地图

4.   导航实现----定位

 在功能包下新建文件夹（launch）---> 新建文件（amcl.launch）写入

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen"><!-- Publish scans from best pose at a max of 10 Hz --><param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 --><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="30"/><param name="min_particles" value="500"/><param name="max_particles" value="5000"/><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="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 --><param name="global_frame_id" value="map"/><!-- 添加地图坐标系 --><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"/>
</node>
</launch>

amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件，在功能包下新建文件夹（launch）---> 新建文件（test_amcl.launch）写入

内容示例如下:

<launch><!-- 设置地图的配置文件 --><arg name="map" default="nav1.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/><!-- 启动AMCL节点 --><include file="$(find nav)/launch/amcl.launch" /><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" />--> <!-- 可以保存 rviz 配置并后期直接使用--><node pkg="rviz" type="rviz" name="rviz" />
</launch>

执行

1.先启动 Gazebo 仿真环境

2.启动键盘控制节点：

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

3.启动上一步中集成地图服务、amcl 与 rviz 的 launch 文件；

source ./devel/setup.bash
roslaunch nav test_amcl.launch 

4.在启动的 rviz 中，添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；

5.通过键盘控制机器人运动，会发现 posearray 也随之而改变。

5.   导航实现----路径规划

在功能包下新建文件夹（launch）---> 新建文件（move_base.launch）写入

<launch><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"><rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="global_costmap" /><rosparam file="$(find 功能包)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /><rosparam file="$(find 功能包)/param/local_costmap_params.yaml" command="load" /><rosparam file="$(find 功能包)/param/global_costmap_params.yaml" command="load" /><rosparam file="$(find 功能包)/param/base_local_planner_params.yaml" command="load" /></node></launch>

新建文件夹（params）--->新建文件（costmap_common_params.yaml，global_costmap_params.yaml，local_costmap_params.yaml，base_local_planner_params.yaml）

分别写入：

costmap_common_params.yaml

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

global_costmap:global_frame: map #地图坐标系robot_base_frame: base_footprint #机器人坐标系# 以此实现坐标变换update_frequency: 1.0 #代价地图更新频率publish_frequency: 1.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

local_costmap_params.yaml

local_costmap:global_frame: odom #里程计坐标系robot_base_frame: base_footprint #机器人坐标系update_frequency: 10.0 #代价地图更新频率publish_frequency: 10.0 #代价地图的发布频率transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间static_map: false  #不需要静态地图，可以提升导航效果rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化width: 3 # 局部地图宽度 单位是 mheight: 3 # 局部地图高度 单位是 mresolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

base_local_planner_params.yaml

TrajectoryPlannerROS:# Robot Configuration Parametersmax_vel_x: 0.5 # X 方向最大速度min_vel_x: 0.1 # X 方向最小速速max_vel_theta:  1.0 # min_vel_theta: -1.0min_in_place_vel_theta: 1.0acc_lim_x: 1.0 # X 加速限制acc_lim_y: 0.0 # Y 加速限制acc_lim_theta: 0.6 # 角速度加速限制# Goal Tolerance Parameters，目标公差xy_goal_tolerance: 0.10yaw_goal_tolerance: 0.05# Differential-drive robot configuration
# 是否是全向移动机器人holonomic_robot: false# Forward Simulation Parameters，前进模拟参数sim_time: 0.8vx_samples: 18vtheta_samples: 20sim_granularity: 0.05

在新建test_move_base.launch

<launch><!-- 设置地图的配置文件 --><arg name="map" default="map.yaml" /><!-- 运行地图服务器，并且加载设置的地图--><node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/><!-- 启动AMCL节点 --><include file="$(find nav)/launch/amcl.launch" /><!-- 运行move_base节点 --><include file="$(find nav)/launch/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" />--> <!-- 可以保存 rviz 配置并后期直接使用 args="-d $(find nav)/config/move_base.rviz"--><node pkg="rviz" type="rviz" name="rviz" /></launch>

执行：

启动 Gazebo 仿真环境

source ./devel/setup.bash
roslaunch car car.launch

启动上一步中集成地图服务、amcl 与 rviz,move_base 的 launch 文件

source ./devel/setup.bash
roslaunch nav test_move_base.launch

在Rviz中添加组件

RobotModel,Map(/map)

PoseArray(/particlecloud)

LaserScan(/scan),Map_Global(//move_base/global_costmap/costmap和修改Color Scheme为costmap)

Map_Local(/move_base/local_costmap/costmap和修改Color Scheme为costmap)

Path_Global(/move_base/TrajectoryPlannerROS/global_plan)

path_Local(/move_base/TrajectoryPlannerROS/local_plan)

Odometry(/odom)

通过2D Nav Goal设置终点，机器人实现导航