#navigation

#navigation

1.安装相关依赖

sudo apt-get install ros-kinetic-navigation
sudo apt-get install ros-kinetic-move-base
sudo apt-get install ros-kinetic-amcl 
sudo apt-get install ros-kinetic-dwa-local-planner 
sudo apt-get install ros-kinetic-teb-local-planner 
sudo apt-get install ros-kinetic-navfn ros-kinetic-global-planner

2.创建功能包

cd ros_ws/src
catkin_create_pkg xqrobot_navigation

3.配置rviz

先在xqrobot_navigation功能包里面新建一个名为config的文件夹，用以存放.rviz文件。

 cd xqrobot_navigation
 mkdir config

（1） 运行主节点roscore：

（2）启动rviz：

（3）点击“Add”，添加“Map”（topic可先不改）

（4）点击“Add”，添加“RobotModel”

（5）点击“Add”，添加“LaserScan”（size改为0.03，topic先不改）

（6）点击“Add”，添加“Group”（添加两个，分别重命名为“Global”、“Local”）、

（7）点击“Add”，添加两个“Map”、两个“Path”，鼠标左键按住不动，分别将它们分别拖到“Global”、“Local”中，如下图。（其中一个“Path可以修改颜色，用以区分”）

（8）点击“Add”，添加“Pose”、“PointCloud2”、“PoseArry”

（9）将配置好的rviz保存到刚才新建的config文件夹下（在左上角）


点进去找到config目录保存好就行。

4.创建yaml参数文件

同样需要先在xqrobot_navigation功能包里面新建params文件夹存放下面6个.yaml文件。
新建文件用命令“touch”，打开文件用“gedit”命令。

cd xqrobot_navigation
mkdir params

① navfn 全局路径规划器（navfn_global_planner_params.yaml）

NavfnROS: 
  visualize_potential: false 
  allow_unknown: false 
  planner_window_x: 0.0 
  planner_window_y: 0.0 
  default_tolerance: 0.0

②DWA 局部路径规划器（dwa_local_planner_params.yaml）

DWAPlannerROS:
  max_vel_x: 0.5 # 0.55 
  min_vel_x: 0.0 
  max_vel_y: 0.0 
  min_vel_y: 0.0 
  max_trans_vel: 0.5 
  trans_stopped_vel: 0.2 
  max_rot_vel: 1.5 
  min_rot_vel: 0.3 
  rot_stopped_vel: 0.3 
  acc_lim_x: 2.0 
  acc_lim_theta: 3.0 
  acc_lim_y: 0.0 
  yaw_goal_tolerance: 0.3 
  xy_goal_tolerance: 0.15 
  latch_xy_goal_tolerance: false 
  sim_time: 1.7 
  sim_granularity: 0.04 
  vx_samples: 6 
  vy_samples: 1 
  vtheta_samples: 20 
  path_distance_bias: 30.0 
  goal_distance_bias: 24.0 
  occdist_scale: 0.2 
  forward_point_distance: 0.325 
  stop_time_buffer: 0.4 
  scaling_speed: 0.25 
  max_scaling_factor: 0.2 
  oscillation_reset_dist: 0.05 
  controller_frequency: 20.0 
# Debugging 
  publish_cost_grid_pc: true 
  global_frame_id: odom

③代价地图公有参数配置 （common_costmap_params.yaml）

# Obstacle Cost Shaping (http://wiki.ros.org/costmap_2d/hydro/inflation) 
robot_radius: 0.25 # distance a circular robot should be clear of the obstacle 
#footprint: [[x0, y0], [x1, y1], ... [xn, yn]] # if the robot is not circular 

map_type: voxel 
#map_type: costmap 
obstacle_layer: 
  #Global Filtering Parameters 
    max_obstacle_height: 0.7 
    obstacle_range: 3.0 
    raytrace_range: 4.0 
  #ObstacleCostmapPlugin 
    track_unknown_space: false 
    footprint_clearing_enabled: true 
  #VoxelCostmapPlugin 
    origin_z: 0 #(double, default: 0.0) 
    z_resolution: 0.2 #(double, default: 0.2) 
    z_voxels: 10 #(int, default: 10) 
    unknown_threshold: 10 #(int, default: ~<name>/z_voxels) 
    mark_threshold: 0 #(int, default: 0) 
    publish_voxel_map: false #(bool, default: false) 
    footprint_clearing_enabled: true #(bool, default: true) 
    combination_method: 1 
    enabled: true
  #Sensor management parameters 
    observation_sources: bump scan #ultrasonic 
    bump: 
        data_type: PointCloud2 
        topic: /kinect_camera/depth/points 
        sensor_frame: kinect_frame_optical 
        observation_persistence: 0 
        expected_update_rate: 0.0 #(double, default: 0.0) 
        marking: true 
        clearing: true 
        min_obstacle_height: 0.1 
        max_obstacle_height: 0.8 
        obstacle_range: 3.0 
        raytrace_range: 4.0 
        inf_is_valid: false 
    # for debugging only, let's you see the entire voxel grid 
    scan: 
        data_type: LaserScan 
        topic: scan 
        sensor_frame: laser_rada_Link 
        observation_persistence: 0 
        expected_update_rate: 0.0 #(double, default: 0.0) 
        marking: true 
        clearing: true 
        min_obstacle_height: 0.1 
        max_obstacle_height: 0.8 
        obstacle_range: 4.0 
        raytrace_range : 5.0 
        inf_is_valid: false 
#cost_scaling_factor and inflation_radius were now moved to the inflation_layer ns
inflation_layer: 
    enabled: true 
    cost_scaling_factor: 5.0 # exponential rate at which the obstacle cost drops   off (default: 10) 
    inflation_radius: 0.3 # max. distance from an obstacle at which costs are incurred for planning paths. 
    
static_layer: 
    unknown_cost_value: -1 
    lethal_cost_threshold: 100 
    map_topic: map 
    first_map_only: false 
    subscribe_to_update: false 
    track_unknown_space: true 
    use_maximum: false 
    trinary_costmap: true 
    enabled: true

④ 全局代价地图参数配置（global_costmap_params.yaml）

global_costmap:
    global_frame: /map
    robot_base_frame: /base_footprint 
    update_frequency: 1.0 
    publish_frequency: 0.5 
    static_map: true 
    transform_tolerance: 0.5 
    plugins:     
     - {name: static_layer, type: "costmap_2d::StaticLayer"} 
     - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 
     - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

⑤ 局部代价地图参数配置（local_costmap_params.yaml）

local_costmap: 
    global_frame: odom 
    robot_base_frame: /base_footprint 
    update_frequency: 5.0 
    publish_frequency: 2.0 
    static_map: false 
    rolling_window: true 
    width: 4.0 
    height: 4.0 
    resolution: 0.05 
    transform_tolerance: 0.5 
    plugins: 
     - {name: obstacle_layer,type: "costmap_2d::VoxelLayer"} 
     - {name: inflation_layer,type: "costmap_2d::InflationLayer"}

⑥ move_base参数配置（move_base_navfn_dwa_params.yaml）

# Move base node parameters. For full documentation of the parameters in this  file, please see 
shutdown_costmaps: false 
controller_frequency: 5.0 
controller_patience: 3.0 
planner_frequency: 1.0 
planner_patience: 5.0 
oscillation_timeout: 10.0 
oscillation_distance: 0.2 

# local planner - default is trajectory rollout 
base_local_planner: "dwa_local_planner/DWAPlannerROS" 
#alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 
base_global_planner: "navfn/NavfnROS"
#We plan to integrate recovery behaviors for turtlebot but currently those  belong to gopher and still have to be adapted. 
## recovery behaviors; we avoid spinning, but we need a fall-back replanning 
#recovery_behavior_enabled: true 
#recovery_behaviors: 
 #- name: 'super_conservative_reset1' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'conservative_reset1' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'aggressive_reset1' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'clearing_rotation1' 
  #type: 'rotate_recovery/RotateRecovery' 
 #- name: 'super_conservative_reset2' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'conservative_reset2' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'aggressive_reset2' 
  #type: 'clear_costmap_recovery/ClearCostmapRecovery' 
 #- name: 'clearing_rotation2' 
  #type: 'rotate_recovery/RotateRecovery' 
#super_conservative_reset1: 
 #reset_distance: 3.0 
#conservative_reset1: 
 #reset_distance: 1.5 
#aggressive_reset1: 
 #reset_distance: 0.0 
#super_conservative_reset2: 
 #reset_distance: 3.0 
#conservative_reset2: 
 #reset_distance: 1.5 
#aggressive_reset2: 
 #reset_distance: 0.0

5.在launch目录中创建.launch文件

同样需要先在xqrobot_navigation功能包下新建launch文件夹存放下面4个.launch文件。
新建文件用命令“touch”，打开文件用“gedit”命令。
① amcl.launch(amcl启动文件)

<launch> 
<arg name="use_map_topic" default="false"/> 
<arg name="scan_topic" default="/scan"/> 
<arg name="initial_pose_x" default="0"/> 
<arg name="initial_pose_y" default="0"/> 
<arg name="initial_pose_a" default="0"/>
<arg name="odom_frame_id" default="/odom"/> 
<arg name="base_frame_id" default="/base_footprint"/> <arg name="global_frame_id" default="/map"/>
<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.04"/>
<param name="odom_alpha2" value="0.6"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.3"/>
<param name="odom_alpha4" value="0.04"/>
<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_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<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)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="amcl_pose" to="/amcl_pose"/>
<remap from="particlecloud" to="/particlecloud"/>
</node>
</launch>

② map_server.launch(map_server启动文件)

<launch> 
<arg name="map_file" default=" $(find xqrobot_gmapping)/maps/gmapping_sim.yaml"/> 
<!-- ****** Maps ***** --> 
<node name="map_server" pkg="map_server" type="map_server" args="$(arg 
map_file)"> 
<param name="frame_id" value="/map"/> 
</node> 
</launch>

③ move_base_navfn_dwa.launch (navfn+dwa版move_base启动文件)

<launch> 
<arg name="odom_frame_id" default="/odom"/> 
<arg name="base_frame_id" default="/base_footprint"/> 
<arg name="global_frame_id" default="/map"/> 
<arg name="odom_topic" default="/odom" /> 
<arg name="laser_topic" default="/scan" /> 

<node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"> 
<rosparam file="$(find xqrobot_navigation)/params/common_costmap_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find xqrobot_navigation)/params/common_costmap_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find xqrobot_navigation)/params/local_costmap_params.yaml" command="load" />
<rosparam file="$(find xqrobot_navigation)/params/global_costmap_params.yaml" command="load" />
<rosparam file="$(find xqrobot_navigation)/params/dwa_local_planner_params.yaml" command="load" />
<rosparam file="$(find xqrobot_navigation)/params/move_base_navfn_dwa_params.yaml" command="load" />
<rosparam file="$(find xqrobot_navigation)/params/navfn_global_planner_params.yaml" command="load" />
<!-- reset frame_id parameters using user input data -->
<param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
<param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
<param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>

<remap from="odom" to="$(arg odom_topic)"/>
<remap from="scan" to="$(arg laser_topic)"/>
<remap from="cmd_vel" to="/cmd_vel"/>
</node>
</launch>

④xqrobot_navigation_navfn_dwa.launch(navfn+dwa版导航总启动文件)

<launch> 
<!--gazebo--> 
<include file="$(find xqrobot_description)/launch/display_xqrobot_gazebo.launch"/> 
<!--map_server--> 
<include file="$(find xqrobot_navigation)/launch/map_server.launch"/> 
<!--move_base--> 
<include file="$(find xqrobot_navigation)/launch/move_base_navfn_dwa.launch"/> 
<!--amcl--> 
<include file="$(find xqrobot_navigation)/launch/amcl.launch"/>
<!-- **************** Visualisation **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find xqrobot_navigation)/config/navigation.rviz"/>
</launch>

6.maps

将上次Gmapping保存地图时生成的两个文件调整一下位置。
原位置：

新位置：
在xqrobot_gmapping功能包下新建一个名为“maps”的文件夹，然后把map.pgm和“map.yaml”放进去，并将“map.yaml”改名为“gmapping_sim.yaml”。

7.运行仿真结果

（1）安装依赖

sudo apt-get install ros-kinetic-global-planner 
sudo apt-get install ros-kinetic-navfn 
sudo apt-get install ros-kinetic-dwa-local-planner 
sudo apt-get install ros-kinetic-teb-local-planner

（2）编译（catkin_make）
（3）运行
回到工作空间运行以下命令，roslaunch那句注意巧用“Tab”键补齐，补不齐的，可能会报错。

export SVGA_VGPU10=0
source devel/setup.bash
roslaunch xqrobot_navigation xqrobot_navigation_navfn_dwa.launch

运行起来之后，rviz中的Map、LaserScan等的“topic”可以下拉选择，大概就如下图所示了，出不来就就就，摆烂吧 ~ （上图是经过我缩放、摆正过的，并不是运行roslaunch就能像我的一样！）

8.仿真环境开始自主导航效果

① gazebo物理仿真环境中截图

② rviz可视化界面截图

点击“2D Nav Goal”之后画个箭头，机器人就能自动形成轨迹过去了。

③ 使用QT可视化工具，查看topic之间的关系

9.拜拜 ~

如果遇到以下错误的，不要找我，（因为我也不会）我只能告诉你去重新装虚拟机。第一个是安装不了相关依赖，第二个问题我也看不懂，貌似是move_base和Map_server的问题。除了这两个错误，其他的错误可以私信我。


附：实践报告七（仅供参考，请勿他用！）