ROS下移动机器人导航仿真

作者:杭州电子科技大学智能机器人实验室
环境:ROS+Kinetic16.04
参考:<<ROS机器人开发实践>>----胡春旭

准备工作

新建一个工作空间(或者已有的workspace),然后在src目录下导入中科大ROS-Academy-for-Beginners包中的robot_sim_demopackage,本实验将采用该模型中的移动机器人作为仿真对象.

搭建仿真环境

使用gazebo搭建仿真环境

gazebo如何进行搭建这里不再赘述,本文参考这篇博客进行了一个简单的地图构建,如下图所示:

构建地图生成的world文件保存在robot_sim_demo下的worlds文件夹下面,如下图第三个文件.

然后对部分文件进行修改:
(1)修改launch文件夹下的robot_spawn.launch文件,主要用我们自己仿真的world环境替换原仿真环境;

<arg name="world_name" value="$(find robot_sim_demo)/worlds/square.world"/>

(2)修改robot_sim_demo/launch/include/xbot-u.launch.xml文件,主要是修改机器人的初始坐标,让其在环境范围之内,我的修改内容如下:
小技巧:建图时可以围绕原点建,这样可直接设置初始点为(0, 0, 0);

建图

(1)安装gmapping

sudo apt-get install ros-kinetic-gmapping

(2)在launch文件夹下新建一个 gmapping.launch文件,具体内容如下:

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

    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true">
        <param name="odom_frame" value="odom"/>
        <param name="map_update_interval" value="5.0"/>
        <!-- Set maxUrange < actual maximum range of the Laser -->
        <param name="maxRange" value="5.0"/>
        <param name="maxUrange" value="4.5"/>
        <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.01"/>
        <param name="srt" value="0.02"/>
        <param name="str" value="0.01"/>
        <param name="stt" value="0.02"/>
        <param name="linearUpdate" value="0.5"/>
        <param name="angularUpdate" value="0.436"/>
        <param name="temporalUpdate" value="-1.0"/>
        <param name="resampleThreshold" value="0.5"/>
        <param name="particles" value="80"/>
        <param name="xmin" value="-1.0"/>
        <param name="ymin" value="-1.0"/>
        <param name="xmax" value="1.0"/>
        <param name="ymax" value="1.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"/>
        <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

(3)为了控制机器人移动,分别在robot_sim_demo下的launch文件夹下新建mbot_teleop.launch文件以及在robot_sim_demo下面新建一个scripts文件夹,具体内容如下:
launch文件:mbot_teleop.launch

<launch>
  <node name="mbot_teleop" pkg="mbot_teleop" type="mbot_teleop.py" output="screen">
    <param name="scale_linear" value="0.1" type="double"/>
    <param name="scale_angular" value="0.4" type="double"/>
  </node>
</launch>

scripts文件:mbot_teleop.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty

msg = """
Control mbot!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .

q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
space key, k : force stop
anything else : stop smoothly

CTRL-C to quit
"""

moveBindings = {
        'i':(1,0),
        'o':(1,-1),
        'j':(0,1),
        'l':(0,-1),
        'u':(1,1),
        ',':(-1,0),
        '.':(-1,1),
        'm':(-1,-1),
           }

speedBindings={
        'q':(1.1,1.1),
        'z':(.9,.9),
        'w':(1.1,1),
        'x':(.9,1),
        'e':(1,1.1),
        'c':(1,.9),
          }

def getKey():
    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

speed = .2
turn = 1

def vels(speed,turn):
    return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__=="__main__":
    settings = termios.tcgetattr(sys.stdin)
    
    rospy.init_node('mbot_teleop')
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

    x = 0
    th = 0
    status = 0
    count = 0
    acc = 0.1
    target_speed = 0
    target_turn = 0
    control_speed = 0
    control_turn = 0
    try:
        print msg
        print vels(speed,turn)
        while(1):
            key = getKey()
            # 运动控制方向键（1：正方向，-1负方向）
            if key in moveBindings.keys():
                x = moveBindings[key][0]
                th = moveBindings[key][1]
                count = 0
            # 速度修改键
            elif key in speedBindings.keys():
                speed = speed * speedBindings[key][0]  # 线速度增加0.1倍
                turn = turn * speedBindings[key][1]    # 角速度增加0.1倍
                count = 0

                print vels(speed,turn)
                if (status == 14):
                    print msg
                status = (status + 1) % 15
            # 停止键
            elif key == ' ' or key == 'k' :
                x = 0
                th = 0
                control_speed = 0
                control_turn = 0
            else:
                count = count + 1
                if count > 4:
                    x = 0
                    th = 0
                if (key == '\x03'):
                    break

            # 目标速度=速度值*方向值
            target_speed = speed * x
            target_turn = turn * th

            # 速度限位，防止速度增减过快
            if target_speed > control_speed:
                control_speed = min( target_speed, control_speed + 0.02 )
            elif target_speed < control_speed:
                control_speed = max( target_speed, control_speed - 0.02 )
            else:
                control_speed = target_speed

            if target_turn > control_turn:
                control_turn = min( target_turn, control_turn + 0.1 )
            elif target_turn < control_turn:
                control_turn = max( target_turn, control_turn - 0.1 )
            else:
                control_turn = target_turn

            # 创建并发布twist消息
            twist = Twist()
            twist.linear.x = control_speed; 
            twist.linear.y = 0; 
            twist.linear.z = 0
            twist.angular.x = 0; 
            twist.angular.y = 0; 
            twist.angular.z = control_turn
            pub.publish(twist)

    except:
        print e

    finally:
        twist = Twist()
        twist.linear.x = 0; twist.linear.y = 0; twist.linear.z = 0
        twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = 0
        pub.publish(twist)

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

至此,建图准备工作完成:
分别运行下述命令:

roslaunch robot_sim_demo robot_spawn.launch//启动仿真环境
roslaunch robot_sim_demo gmapping.launch//启动gmapping
rviz//启动rviz,添加map话题
roslaunch robot_sim_demo mbot_teleop.launch//启动控制机器人节点

开始控制机器人移动进行建图,将建好的地图使用如下命令进行保存:

rosrun map_server map_saver -f map

保存会产生两个文件,以map名字命名:

然后将这两个文件copy到robot_sim_demo文件夹下面的maps下面,建图完成.

导航

(1)安装导航功能包:

sudo apt-get install ros-kinetic-navigation

(2)配置代价地图文件
在config文件夹下新建mbot文件夹,同时新建下图5个文件:
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_params.yaml:

shutdown_costmaps: false
controller_frequency: 10.0
planner_patience: 5.0
controller_patience: 15.0
conservative_reset_dist: 3.0
planner_frequency: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2

(3)launch文件夹下分别添加 move_base.launch amcl.launch nav_cloister_demo.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 robot_sim_demo)/config/mbot/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot_sim_demo)/config/mbot/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robot_sim_demo)/config/mbot/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_sim_demo)/config/mbot/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_sim_demo)/config/mbot/move_base_params.yaml" command="load" />
    <rosparam file="$(find robot_sim_demo)/config/mbot/base_local_planner_params.yaml" command="load" />
  </node>
  
</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:
注意:该文件下面的map配置信息yaml文件替换成自己建好的地图.,launch文件路径也要换成自己的.

<launch>

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

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

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

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

</launch>

文件配置完成,开始进行导航仿真:
依次输入以下命令:

roslaunch robot_sim_demo robot_spawn.launch//启动仿真环境
roslaunch robot_sim_demo nav_cloister_demo.launch//启动导航节点

如下图:

然后在rviz上方的2D Nav Goal 控制,让机器人朝着某个方向移动


避障


至此,导航完成.