在9中我們已經實現了機器人的模塊仿真，現在要在這個基礎上實現SLAM建圖，地圖服務，機器人定位，路徑規劃
1.? ?還是在上述機器人的工作空間下，新建功能包（nav），導入依賴
gmapping map_server amcl move_base2.? ?導航實現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.launch2.然后再啟動地圖繪制的 launch 文件
source ./devel/setup.bash
roslaunch nav map.launch3.在 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) ?
rvizrviz 中使用 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.py3.啟動上一步中集成地圖服務、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設置終點，機器人實現導航
 環境安裝)