ROS系统SLAM基础学习：gazebo仿真机器人自主导航

ROS中的导航框架有move_base和amcl两种，
使用下载导航功能包

sudo apt-get install ros-kinetic-navigation

move_base节点配置

1、在ROS工作空间下的mbot_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 mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mbot_navigation)/config/mbot/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find mbot_navigation)/config/mbot/base_local_planner_params.yaml" command="load" />
  </node>
  
</launch>

补充：
在ROS工作空间下的mbot_navigation功能包下的rviz文件夹中建立nav.rviz文件，内容如下：

Panels:
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /RobotModel1/Links1/base_footprint1
        - /Pose Array1
      Splitter Ratio: 0.652661026
    Tree Height: 691
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
    Name: Tool Properties
    Splitter Ratio: 0.428570986
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Experimental: false
    Name: Time
    SyncMode: 0
    SyncSource: LaserScan
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 0.5
      Class: rviz/Grid
      Color: 0; 0; 0
      Enabled: true
      Line Style:
        Line Width: 0.0299999993
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 80
      Reference Frame: odom
      Value: true
    - Angle Tolerance: 0.100000001
      Class: rviz/Odometry
      Covariance:
        Orientation:
          Alpha: 0.5
          Color: 255; 255; 127
          Color Style: Unique
          Frame: Local
          Offset: 1
          Scale: 1
          Value: true
        Position:
          Alpha: 0.300000012
          Color: 204; 51; 204
          Scale: 1
          Value: true
        Value: true
      Enabled: false
      Keep: 100
      Name: Odometry
      Position Tolerance: 0.100000001
      Shape:
        Alpha: 1
        Axes Length: 1
        Axes Radius: 0.100000001
        Color: 255; 25; 0
        Head Length: 0.300000012
        Head Radius: 0.100000001
        Shaft Length: 1
        Shaft Radius: 0.0500000007
        Value: Arrow
      Topic: /odom
      Unreliable: false
      Value: false
    - Angle Tolerance: 0.100000001
      Class: rviz/Odometry
      Covariance:
        Orientation:
          Alpha: 0.5
          Color: 255; 255; 127
          Color Style: Unique
          Frame: Local
          Offset: 1
          Scale: 1
          Value: true
        Position:
          Alpha: 0.300000012
          Color: 204; 51; 204
          Scale: 1
          Value: true
        Value: true
      Enabled: false
      Keep: 100
      Name: Odometry EKF
      Position Tolerance: 0.100000001
      Shape:
        Alpha: 1
        Axes Length: 1
        Axes Radius: 0.100000001
        Color: 255; 25; 0
        Head Length: 0.300000012
        Head Radius: 0.100000001
        Shaft Length: 1
        Shaft Radius: 0.0500000007
        Value: Arrow
      Topic: /odom
      Unreliable: false
      Value: false
    - Alpha: 1
      Class: rviz/RobotModel
      Collision Enabled: false
      Enabled: true
      Links:
        All Links Enabled: true
        Expand Joint Details: false
        Expand Link Details: false
        Expand Tree: false
        Link Tree Style: Links in Alphabetic Order
        back_caster_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_footprint:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        base_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        front_caster_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        laser_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        left_wheel_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
        right_wheel_link:
          Alpha: 1
          Show Axes: false
          Show Trail: false
          Value: true
      Name: RobotModel
      Robot Description: robot_description
      TF Prefix: ""
      Update Interval: 0
      Value: true
      Visual Enabled: true
    - Alpha: 0.699999988
      Class: rviz/Map
      Color Scheme: map
      Draw Behind: true
      Enabled: true
      Name: Map
      Topic: /map
      Unreliable: false
      Use Timestamp: false
      Value: true
    - Alpha: 1
      Buffer Length: 1
      Class: rviz/Path
      Color: 255; 0; 0
      Enabled: true
      Head Diameter: 0.300000012
      Head Length: 0.200000003
      Length: 0.300000012
      Line Style: Lines
      Line Width: 0.0299999993
      Name: Local Plan
      Offset:
        X: 0
        Y: 0
        Z: 0
      Pose Color: 255; 85; 255
      Pose Style: None
      Radius: 0.0299999993
      Shaft Diameter: 0.100000001
      Shaft Length: 0.100000001
      Topic: /move_base/TrajectoryPlannerROS/local_plan
      Unreliable: false
      Value: true
    - Alpha: 1
      Buffer Length: 1
      Class: rviz/Path
      Color: 0; 213; 0
      Enabled: true
      Head Diameter: 0.300000012
      Head Length: 0.200000003
      Length: 0.300000012
      Line Style: Lines
      Line Width: 0.0299999993
      Name: Global Plan
      Offset:
        X: 0
        Y: 0
        Z: 0
      Pose Color: 255; 85; 255
      Pose Style: None
      Radius: 0.0299999993
      Shaft Diameter: 0.100000001
      Shaft Length: 0.100000001
      Topic: /move_base/TrajectoryPlannerROS/global_plan
      Unreliable: false
      Value: true
    - Alpha: 1
      Arrow Length: 0.300000012
      Axes Length: 0.300000012
      Axes Radius: 0.00999999978
      Class: rviz/PoseArray
      Color: 170; 255; 127
      Enabled: true
      Head Length: 0.0700000003
      Head Radius: 0.0299999993
      Name: Pose Array
      Shaft Length: 0.230000004
      Shaft Radius: 0.00999999978
      Shape: Arrow (Flat)
      Topic: /particlecloud
      Unreliable: false
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 0.30399999
        Min Value: 0.30399999
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/LaserScan
      Color: 255; 0; 0
      Color Transformer: FlatColor
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: LaserScan
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.00999999978
      Style: Points
      Topic: /scan
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Axes Length: 1
      Axes Radius: 0.100000001
      Class: rviz/Pose
      Color: 0; 255; 0
      Enabled: true
      Head Length: 0.100000001
      Head Radius: 0.150000006
      Name: Goal Pose
      Shaft Length: 0.5
      Shaft Radius: 0.0299999993
      Shape: Arrow
      Topic: /move_base_simple/goal
      Unreliable: false
      Value: true
    - Alpha: 0.699999988
      Class: rviz/Map
      Color Scheme: costmap
      Draw Behind: false
      Enabled: true
      Name: Inflated Obstacles
      Topic: /move_base/local_costmap/costmap
      Unreliable: false
      Use Timestamp: false
      Value: true
    - Class: rviz/Marker
      Enabled: true
      Marker Topic: /waypoint_markers
      Name: Marker
      Namespaces:
        {}
      Queue Size: 100
      Value: true
  Enabled: true
  Global Options:
    Background Color: 0; 0; 0
    Default Light: true
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/MoveCamera
    - Class: rviz/Interact
      Hide Inactive Objects: true
    - Class: rviz/Select
    - Class: rviz/SetInitialPose
      Topic: /initialpose
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
  Value: true
  Views:
    Current:
      Angle: -6.3000164
      Class: rviz/TopDownOrtho
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.0599999987
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.00999999978
      Scale: 52.4497948
      Target Frame: <Fixed Frame>
      Value: TopDownOrtho (rviz)
      X: 0.412709981
      Y: -2.02176332
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 904
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000342fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000198000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000342000000d700ffffff000000010000010f00000270fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000270000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004280000034200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1432
  X: 2298
  Y: 239

2、在mbot_navigation功能包下创建config文件夹，并在其文件夹下创建mbot文件夹，在文件夹下创建四个yaml文件

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

costmap_common_params.yaml文件内容如下：

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
footprint_inflation: 0.01
robot_radius: 0.175
inflation_radius: 0.15
max_obstacle_height: 0.6
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

move_base节点配置完成
参数含义参考：http://wiki.ros.org/move_base.

amcl节点配置

在ROS工作空间下的mbot_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>

导航仿真

1、ROS工作空间下的mbot_gazebo功能包下launch文件夹里的mbot_laser_nav_gazebo.launch，参考我上一篇博客：https://blog.csdn.net/A981012/article/details/105332233.
2、在ROS工作空间下的mbot_navigation功能包的launch文件夹下创建nav_cloister_demo.launch文件，内容如下：

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="maps.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_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 mbot_navigation)/rviz/nav.rviz"/>

</launch>

其中第四行的maps.yaml为之前保存地图的时候产生的文件名称。具体保存地图参考我上一篇博客：https://blog.csdn.net/A981012/article/details/105332233.
3、新建终端运行roscore命令开启ROS系统
新建终端，cd到ROS工作空间下，编译功能包，并注册程序

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash
roslaunch src/mbot_gazebo/launch/mbot_laser_nav_gazebo.launch

得到的界面是：这是在自己创建的world中添加了带有激光雷达的机器人

4、新建终端运行导航仿真

roslaunch src/mbot_navigation/launch/nav_cloister_demo.launch

这个是手动规划路线，点击rviz上方的2D Nav Goal，然后就可以规划路线了，要使用两个箭头相连之后机器人才能根据箭头运动。

这是路线规划后，机器人所走的路线

导航SLAM仿真

1、在ROS工作空间下的mbot_navigation功能包的launch文件夹下创建，exploring_slam_demo.launch文件，内容如下：

<launch>

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

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>

</launch>

2、新建终端,cd到ROS工作空间下，运行命令，

roslaunch src/mbot_gazebo/launch/mbot_laser_nav_gazebo.launch

3、新建终端，cd到ROS工作空间下，运行命令：

roslaunch src/mbot_navigation/launch/exploring_slam_demo.launch

启动SLAM导航仿真，点击rviz上方的2D Nav Goal进行规划路线，机器人就可以运动到规划地点上去

自主探索SLAM仿真

自主探索仿真就是在导航SLAM仿真的基础上，加一个py程序控制机器人自己规划路线并运动到目标点上。
1、在mbot_navigation功能包下创建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.")

2、新建终端，cd到ROS工作空间下，运行以下命令

catkin_make
source ~/catkin_ws/devel/setup.bash
roslaunch src/mbot_gazebo/launch/mbot_laser_nav_gazebo.launch

3、新建终端，cd到ROS工作空间下，运行命令

roslaunch src/mbot_navigation/launch/exploring_slam_demo.launch

4、新建终端，cd到ROS工作空间下，运行命令

rosrun mbot_navigation exploring_slam.py

机器人就会根据目标点以及提供的地图，自动规划路线，并根据路线运动。图中的绿线为机器人根据目标点，规划的路线，

上下两幅图对比可以发现，机器人在根据运动情况实时调制规划路线。

从这个可以看出，我们提供给机器人的图还是有瑕疵的，导致规划的路线从强中间穿过了。

rosrun py文件的终端显示的是机器人是否到达过我们在py文件里设定的那六个点，以及几分钟运动了多少米的这些信息

而roslaunch exploring_slam_demo.launch文件的终端显示的是机器人自主导航时的实时运动位置。
补充：

自主导航：避障

这是我之后重新建立world文件并运行的图片，只是在自主导航的基础上增加了一个障碍物，来判断机器人自主导航过程中能不能避开障碍物，命令运行就不重复了，就是自主导航的命令。
这是自主导航时没有添加障碍物的时候的情况：可以看到，只要点击了上方的2D Nav Goal之后，在地图里随便定一个目标点，可以自动规划出到目标点的路线。

然后在仿真环境中放置障碍物，再进行自主导航，可以看到，规划路线的时候，机器人的激光雷达会将障碍物轮廓扫描出来，然后绕开这个障碍来规划路线。

遇到的问题及解决方法和总结

每次新建完文件都要跳到ROS工作空间下进行编译注册
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

错误提示找不到move_base功能包，使用

sudo apt-get install ros-kinetic-move-base

这个错误是由于我保存了地图之后将图片移动了位置，所以找不到，我将图片移动回原来的位置就可以了。

从机器人的自主规划路线可以看出，机器人规划路线的误差比较大，即使在将图片中的框线都加粗封好了以后，一个很小的缺口，机器人都为认为这是有路的，可以从那里出去，其实那里就是一堵墙，所以这个图片的框线加固要很仔细小心。而且使用激光雷达扫描出来的图片中建筑物的框线其实有很大的缺陷，要人为的加固一下。

如有错误请指正！

最后

以上就是炙热流沙为你收集整理的ROS系统SLAM基础学习：gazebo仿真机器人自主导航的全部内容，希望文章能够帮你解决ROS系统SLAM基础学习：gazebo仿真机器人自主导航所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。