ROS学习初探之⾃建⼩车模型并进⾏仿真（六）
⼩车导航仿真
上⼀节我们通过gmapping实现了SLAM，现在我们可以在上⼀节构建好的地图中导航了。
⼀、导航框架
导航主要分为机器⼈定位和路径规划两⼤部分。ROS分别提供了功能包
2.amcl：实现⼆维地图中的机器⼈定位。
我们先⽤命令下载导航功能包
sudo apt-get install ros-kinetic-navigation
move_ba功能包和amcl功能包的参数和发布的话题⼤家可以去古⽉⽼师的书⾥看看。
⼆、代价地图的配置
我们打开tianbot_navigation/config/tianbot⽂件夹，⾥⾯有三个配置代价地图的⽂件
通⽤配置⽂件
代价地图⽤来存储周围环境的障碍信息，其中需要声明地图关注的机器⼈传感器消息，以便于地图信息的更新。针对两种代价地图通⽤的配置选项，创建为costmap_common_params.yaml，代码如下:
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]
footprint_inflation: 0.01
robot_radius: 0.1
inflation_radius: 0.15
max_obstacle_height: 0.3
min_obstacle_height: 0.0
obrvation_sources: scan
scan: {data_type: LarScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}
全局规划配置⽂件
全局规划配置⽂件⽤于存储配置全局代价地图的参数，命名为global_costmap_params.yaml，代码如下:
global_costmap:
global_frame: map
robot_ba_frame: ba_footprint
update_frequency: 1.0
publish_frequency: 1.0
static_map: true
rolling_window: fal
团鱼resolution: 0.01
transform_tolerance: 1.0
map_type: costmap
本地规划配置⽂件
本地规划配置⽂件⽤于存储配置本地代价地图的参数，命名为local_costmap_params.yaml，代码如下:
local_costmap:
global_frame: odom
robot_ba_frame: ba_footprint
update_frequency: 3.0
publish_frequency: 1.0
static_map: true新生儿鼻子有鼻屎怎么弄出来
rolling_window: fal
width: 6.0
height: 6.0
汽车灯光标志图解
resolution: 0.01
transform_tolerance: 1.0
三、本地规划器配置
本地规划器ba_local_planner的主要作⽤是，根据规划的全局路径计算发布给机器⼈的速度控制指令。该规划器要根据机器⼈的规格配置相关参数，在tianbot_navigation/config/tianbot⽂件夹创名为ba_local_planner_params.yaml的配置⽂件，代码如下:
controller_frequency: 3.0
recovery_behavior_enabled: fal
clearing_rotation_allowed: fal
TrajectoryPlannerROS:
max_vel_x: 0.5
min_vel_x: 0.1
max_vel_y: 0.0 # zero for a differential drive robot
min_vel_y: 0.0
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.5
escape_vel: -0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # zero for a differential drive robot
acc_lim_theta: 1.2
holonomic_robot: fal
yaw_goal_tolerance: 0.1 # about 6 degrees
xy_goal_tolerance: 0.1 # 10 cm
latch_xy_goal_tolerance: fal
pdist_scale: 0.9
gdist_scale: 0.6
meter_scoring: true
heading_lookahead: 0.325
heading_scoring: fal
肉炒豆腐皮heading_scoring_timestep: 0.8
occdist_scale: 0.1
oscillation_ret_dist: 0.05
publish_cost_grid_pc: fal
春联的英文prune_plan: true
sim_time: 1.0
sim_granularity: 0.025
angular_sim_granularity: 0.025
vx_samples: 8
vy_samples: 0 # zero for a differential drive robot
vtheta_samples: 20
dwa: true
simple_attractor: fal
该配置⽂件声明机器⼈本地规划采⽤Trajectory Rollout算法，并且设置算法中需要⽤到的机器⼈速度、加速度阈值等参数。
四、在gazebo中仿真机器⼈导航
创建launch⽂件
move_ba.launch⽂件
先在tianbot_navigation/launch⽂件夹创建move_ba.launch⽂件，代码如下:
<launch>
<node pkg="move_ba" type="move_ba" respawn="fal" name="move_ba" output="screen" clear_params="true">
<rosparam file="$(find tianbot_navigation)/config/tianbot/costmap_common_params.yaml" command
="load" ns="global_costmap" /> <rosparam file="$(find tianbot_navigation)/config/tianbot/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find tianbot_navigation)/config/tianbot/local_costmap_params.yaml" command="load" />
<rosparam file="$(find tianbot_navigation)/config/tianbot/global_costmap_params.yaml" command="load" />
<rosparam file="$(find tianbot_navigation)/config/tianbot/ba_local_planner_params.yaml" command="load" />
</node>
</launch>
move_ba.launch⽂件⾥调⽤了上⾯的配置⽂件。
amcl.launch⽂件
在tianbot_navigation/launch⽂件夹创建amcl.launch⽂件，代码如下:
<launch>
<arg name="u_map_topic" default="fal"/>
<arg name="scan_topic" default="scan"/>水煮蛏子
<node pkg="amcl" type="amcl" name="amcl" clear_params="true">
<param name="u_map_topic" value="$(arg u_map_topic)"/>
<!-- Publish scans from best po at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="lar_max_beams" value="60"/>
<param name="lar_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.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="lar_z_hit" value="0.5"/>
<param name="lar_z_short" value="0.05"/>
<param name="lar_z_max" value="0.05"/>
<param name="lar_z_rand" value="0.5"/>
<param name="lar_sigma_hit" value="0.2"/>
<param name="lar_lambda_short" value="0.1"/>
<param name="lar_model_type" value="likelihood_field"/>
<!-- <param name="lar_model_type" value="beam"/> -->
<param name="lar_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="odom"/>
<param name="resample_interval" value="1"/>
<!-- Increa tolerance becau 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"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
nav_cloister_demo.launch⽂件
在tianbot_navigation/launch⽂件夹创建nav_cloister_demo.launch⽂件，代码如下:
<launch>
<!-- 设置地图的配置⽂件 -->
<arg name="map" default="map.yaml" />
<!-- 运⾏地图服务器，并且加载设置的地图-->
<node name="map_rver" pkg="map_rver" type="map_rver" args="$(find tianbot_navigation)/maps/$(arg map)"/>
<!-- 运⾏move_ba节点 -->
<include file="$(find tianbot_navigation)/launch/move_ba.launch"/>
<!-- 启动AMCL节点 -->
<include file="$(find tianbot_navigation)/launch/amcl.launch" />
<!-- 对于虚拟定位，需要设置⼀个/odom与/map之间的静态坐标变换 -->
<node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
<!-- 运⾏rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find tianbot_navigation)/rviz/nav.rviz"/>
</launch>
运⾏效果
接下来就能⽤如下命令在gazebo中仿真机器⼈导航功能了
roslaunch tianbot_mini_description tianbotmini_lar_gazebo.launch
roslaunch tianbot_navigation nav_cloister_demo.launch
如下图所⽰，我们点击rviz中的2D Nav Goal图标，在点击rviz中的⼀个导航⽬标点，机器⼈可⾃动导航到该位置。如果你在机器⼈导航的时候，在它要经过的路径上放置⼀些障碍物，机器⼈会绕过障碍物继续向⽬标点运动，这是由move_ba功能包实现的。
五、⾃主探索SLAM
我们可以创建⼀个launch⽂件，同时调动gmapping和move_ba的⽂件，就能实现⾃主探索SLAM了。
在tianbot_navigation/launch⽂件夹创建tianbot_exploring_slam.launch⽂件，代码如下:
<launch>
<include file="$(find tianbot_navigation)/launch/gmapping.launch"/>
<!-- 运⾏move_ba节点 -->
<include file="$(find tianbot_navigation)/launch/move_ba.launch" />
<!-- 运⾏rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find tianbot_navigation)/rviz/nav.rviz"/>
</launch>
接下去我们可以通过py代码设置⼀些关键点，让机器⼈随机导航，从⽽实现⾃主探索SLAM，我们在tianbot_navigation/scripts⽂件夹创建exploring_slam.py⽂件，代码如下:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import roslib;
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Po, PoWithCovarianceStamped, Point, Quaternion, Twist
from geometry_msgs.msg import Po, PoWithCovarianceStamped, Point, Quaternion, Twist from move_ba_msgs.msg import MoveBaAction, MoveBaGoal
from random import sample
from math import pow, sqrt
class NavTest():
def __init__(lf):
rospy.init_node('exploring_slam', anonymous=True)
<_shutdown(lf.shutdown)
# 在每个⽬标位置暂停的时间 (单位：s)
# 是否仿真？
lf.fake_test = _param("~fake_test", True)
# 到达⽬标的状态
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# 设置⽬标点的位置
# 在rviz中点击 2D Nav Goal 按键，然后单击地图中⼀点
# 在终端中就会看到该点的坐标信息
locations = dict()
locations['1'] = Po(Point(4.589, -0.376, 0.000), Quaternion(0.000, 0.000, -0.447, 0.894)) locations['2'] = Po(Point(4.231, -6.050, 0.000), Quaternion(0.000, 0.000, -0.847, 0.532)) locations['3'] = Po(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000)) locations['4'] = Po(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)) locations['5'] = Po(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940)) locations['6'] = Po(Point(2.924, 0.018, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))
# 发布控制机器⼈的消息
# 订阅move_ba服务器的消息
rospy.loginfo("Waiting for move_ba ")
# 60s等待时间限制
rospy.loginfo("Connected to move ba rver")
# 保存机器⼈的在rviz中的初始位置贵富
initial_po = PoWithCovarianceStamped()
# 保存成功率、运⾏时间、和距离的变量
n_locations = len(locations)
n_goals = 0
n_success = 0
i = n_locations
distance_traveled = 0
start_time = w()
running_time = 0
location = ""
last_location = ""
# 确保有初始位置
html格式while initial_po.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
