ROS学习初探之自建小车模型并进行仿真（六）

小车导航仿真

上一节我们通过gmapping实现了SLAM，现在我们可以在上一节构建好的地图中导航了。

一、导航框架

导航主要分为机器人定位和路径规划两大部分。ROS分别提供了功能包
1.move_base:实现机器人导航中的最优路径规划。
2.amcl:实现二维地图中的机器人定位。
我们先用命令下载导航功能包

 sudo apt-get install ros-kinetic-navigation

move_base功能包和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
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

全局规划配置文件

全局规划配置文件用于存储配置全局代价地图的参数，命名为global_costmap_params.yaml，代码如下:

global_costmap:
   global_frame: map
   robot_base_frame: base_footprint
   update_frequency: 1.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   resolution: 0.01
   transform_tolerance: 1.0
   map_type: costmap

本地规划配置文件

本地规划配置文件用于存储配置本地代价地图的参数，命名为local_costmap_params.yaml，代码如下:

local_costmap:
   global_frame: odom
   robot_base_frame: base_footprint
   update_frequency: 3.0
   publish_frequency: 1.0
   static_map: true
   rolling_window: false
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 1.0

三、本地规划器配置

本地规划器base_local_planner的主要作用是，根据规划的全局路径计算发布给机器人的速度控制指令。该规划器要根据机器人的规格配置相关参数，在tianbot_navigation/config/tianbot文件夹创名为base_local_planner_params.yaml的配置文件，代码如下:

controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

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: false
   yaw_goal_tolerance: 0.1 # about 6 degrees
   xy_goal_tolerance: 0.1  # 10 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 0.9
   gdist_scale: 0.6
   meter_scoring: true

   heading_lookahead: 0.325
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0.1
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   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: false

该配置文件声明机器人本地规划采用Trajectory Rollout算法，并且设置算法中需要用到的机器人速度、加速度阈值等参数。

四、在gazebo中仿真机器人导航

创建launch文件

move_base.launch文件

先在tianbot_navigation/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 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/base_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

move_base.launch文件里调用了上面的配置文件。

amcl.launch文件

在tianbot_navigation/launch文件夹创建amcl.launch文件，代码如下:

<launch>
    <arg name="use_map_topic" default="false"/>
    <arg name="scan_topic" default="scan"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <!-- 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="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.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="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_model_type" value="beam"/> -->
        <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="odom"/>
        <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"/>
        <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_server" pkg="map_server" type="map_server" args="$(find tianbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find tianbot_navigation)/launch/move_base.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_laser_gazebo.launch
roslaunch tianbot_navigation nav_cloister_demo.launch 

如下图所示，我们点击rviz中的2D Nav Goal图标，在点击rviz中的一个导航目标点，机器人可自动导航到该位置。如果你在机器人导航的时候，在它要经过的路径上放置一些障碍物，机器人会绕过障碍物继续向目标点运动，这是由move_base功能包实现的。

五、自主探索SLAM

我们可以创建一个launch文件，同时调动gmapping和move_base的文件，就能实现自主探索SLAM了。
在tianbot_navigation/launch文件夹创建tianbot_exploring_slam.launch文件，代码如下:

<launch>

    <include file="$(find tianbot_navigation)/launch/gmapping.launch"/>

    <!-- 运行move_base节点 -->
    <include file="$(find tianbot_navigation)/launch/move_base.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 Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
from random import sample  
from math import pow, sqrt  

class NavTest():  
    def __init__(self):  
        rospy.init_node('exploring_slam', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  

        # 在每个目标位置暂停的时间 (单位：s)
        self.rest_time = rospy.get_param("~rest_time", 2)  

        # 是否仿真？  
        self.fake_test = rospy.get_param("~fake_test", True)  

        # 到达目标的状态  
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
                       'SUCCEEDED', 'ABORTED', 'REJECTED',  
                       'PREEMPTING', 'RECALLING', 'RECALLED',  
                       'LOST']  
 
        # 设置目标点的位置  
        # 在rviz中点击 2D Nav Goal 按键，然后单击地图中一点  
        # 在终端中就会看到该点的坐标信息  
        locations = dict()  

        locations['1'] = Pose(Point(4.589, -0.376, 0.000),  Quaternion(0.000, 0.000, -0.447, 0.894))  
        locations['2'] = Pose(Point(4.231, -6.050, 0.000),  Quaternion(0.000, 0.000, -0.847, 0.532))  
        locations['3'] = Pose(Point(-0.674, -5.244, 0.000), Quaternion(0.000, 0.000, 0.000, 1.000))  
        locations['4'] = Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))  
        locations['5'] = Pose(Point(-4.701, -0.590, 0.000), Quaternion(0.000, 0.000, 0.340, 0.940))  
        locations['6'] = Pose(Point(2.924, 0.018, 0.000),   Quaternion(0.000, 0.000, 0.000, 1.000))  

        # 发布控制机器人的消息  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)  

        # 订阅move_base服务器的消息  
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  

        rospy.loginfo("Waiting for move_base action server...")  

        # 60s等待时间限制  
        self.move_base.wait_for_server(rospy.Duration(60))  
        rospy.loginfo("Connected to move base server")  
  
        # 保存机器人的在rviz中的初始位置  
        initial_pose = PoseWithCovarianceStamped()  

        # 保存成功率、运行时间、和距离的变量  
        n_locations = len(locations)  
        n_goals = 0  
        n_successes = 0  
        i = n_locations  
        distance_traveled = 0  
        start_time = rospy.Time.now()  
        running_time = 0  
        location = ""  
        last_location = ""    
 
        # 确保有初始位置  
        while initial_pose.header.stamp == "":  
            rospy.sleep(1)  

        rospy.loginfo("Starting navigation test")  

        # 开始主循环，随机导航  
        while not rospy.is_shutdown():  
            # 如果已经走完了所有点，再重新开始排序  
            if i == n_locations:  
                i = 0  
                sequence = sample(locations, n_locations)  
 
                # 如果最后一个点和第一个点相同，则跳过  
                if sequence[0] == last_location:  
                    i = 1  

            # 在当前的排序中获取下一个目标点  
            location = sequence[i]  

            # 跟踪行驶距离  
            # 使用更新的初始位置  
            if initial_pose.header.stamp == "":  
                distance = sqrt(pow(locations[location].position.x -   
                                    locations[last_location].position.x, 2) +  
                                pow(locations[location].position.y -   
                                    locations[last_location].position.y, 2))  
            else:  
                rospy.loginfo("Updating current pose.")  
                distance = sqrt(pow(locations[location].position.x -   
                                    initial_pose.pose.pose.position.x, 2) +  
                                pow(locations[location].position.y -   
                                    initial_pose.pose.pose.position.y, 2))  
                initial_pose.header.stamp = ""  

            # 存储上一次的位置，计算距离  
            last_location = location  

            # 计数器加1  
            i += 1  
            n_goals += 1  

            # 设定下一个目标点  
            self.goal = MoveBaseGoal()  
            self.goal.target_pose.pose = locations[location]  
            self.goal.target_pose.header.frame_id = 'map'  
            self.goal.target_pose.header.stamp = rospy.Time.now()  

            # 让用户知道下一个位置  
            rospy.loginfo("Going to: " + str(location))  

            # 向下一个位置进发  
            self.move_base.send_goal(self.goal)  

            # 五分钟时间限制  
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   

            # 查看是否成功到达  
            if not finished_within_time:  
                self.move_base.cancel_goal()  
                rospy.loginfo("Timed out achieving goal")  
            else:  
                state = self.move_base.get_state()  
                if state == GoalStatus.SUCCEEDED:  
                    rospy.loginfo("Goal succeeded!")  
                    n_successes += 1  
                    distance_traveled += distance  
                    rospy.loginfo("State:" + str(state))  
                else:  
                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  

            # 运行所用时间  
            running_time = rospy.Time.now() - start_time  
            running_time = running_time.secs / 60.0  

            # 输出本次导航的所有信息  
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
                          str(n_goals) + " = " +   
                          str(100 * n_successes/n_goals) + "%")  

            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  

            rospy.sleep(self.rest_time)  

    def update_initial_pose(self, initial_pose):  
        self.initial_pose = initial_pose  

    def shutdown(self):  
        rospy.loginfo("Stopping the robot...")  
        self.move_base.cancel_goal()  
        rospy.sleep(2)  
        self.cmd_vel_pub.publish(Twist())  
        rospy.sleep(1)  

def trunc(f, n):  
    slen = len('%.*f' % (n, f))  

    return float(str(f)[:slen])  

if __name__ == '__main__':  
    try:  
        NavTest()  
        rospy.spin()  

    except rospy.ROSInterruptException:  
        rospy.loginfo("Exploring SLAM finished.")

开始自主探索SLAM

现在使用以下命令，就能实现自主探索SLAM了:

roslaunch tianbot_mini_description tianbotmini_laser_gazebo.launch
roslaunch tianbot_navigation tianbot_exploring_slam.launch 
rosrun tianbot_navigation exploring_slam.py

如上图所示，小车开始自动规划路径进行SLAM了。

小结

本节算是小车篇的收尾了，后期内容基本是参考古月老师的书的，总之一步一个脚印，ROS算是初步入门了，从小车模型导出开始到最后的自主探索SLAM仿真，期间遇到了无数多的坑，也算是一点点爬出来了，当然以后还有更多的大坑等着我爬。我的功能包后期可能会开源出来。到这里想必大家都很好奇小车激光雷达上边的贴图是怎么贴的吧，那么下一节，我会教大家使用一个模型贴图神器，敬请期待。

参考资料

1.古月老师的<<ROS机器人开发实践>>