从gazebo环境仿真建图定位到真实P3DX机器人建图定位

从gazebo环境仿真建图定位到真实P3DX机器人建图定位

杭州电子科技大学-自动化学院-智能系统和机器人研究中心-Jolen Xie

一、gazebo下建图，定位仿真

先看我仿真一共用到的包

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-6T1S1oYm-1596701464933)(/media/xjl/数据/我的资料/Typera图片备份/image-20200805201411151.png)]

1.gazebo构建环境

打开gazebo

gazebo

进入gazebo后使用快捷键Ctrl+B或者菜单栏选择Edit->building editor,进入创建建筑的界面，坐标可以选择简单的墙和窗户和门（墙就足够了）

拖动wall进行简易的环境搭建

点击左上角菜单栏file->save然后点击Exit building editor进入gazebo主界面，然后继续点击file->save world as保存的.world文件后面要使用。到这里gazebo的环境搭建完成。

2.创建工作空间

创建工作空间不多说，如下创建了一个工作空间

3.机器人模型

机器人模型我选用的ROS-Academy-for-Beginners中的机器人模型，github可以下载https://github.com/sychaichangkun/ROS-Academy-for-Beginners.git。我只提出其中的robot_sim_demo这个包，里面包含了机器人的模型。

把上面保存的.world文件，放到robot_sim_demo/worlds文件下。

然后在robot_sim_demo/launch/robot_spawn.launch文件修改机器人运行的环境，文件的代码如下

<launch>

  <arg name="robot" default="xbot-u"/>
  <arg name="debug" default="false"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>

  <!-- Start Gazebo with a blank world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <!--在这里修改世界地图的文件-->
    <arg name="world_name" value="$(find robot_sim_demo)/worlds/fangzhen.world"/>
    <!--arg name="world_name" value="$(find turtlebot_gazebo)/worlds/playground.world"/-->gmapping建图
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- Oh, you wanted a robot? -->
  <include file="$(find robot_sim_demo)/launch/include/$(arg robot).launch.xml" />

  <!--node    name="rviz"    pkg="rviz"    type="rviz"    args="-d $(find robot_sim_demo)/urdf_gazebo.rviz" /-->
  
</launch>

上面的fangzhen.world是我自建的世界。

除此之外，机器人的位置可能在地图之外，需要修改机器人的位置，修改robot_sim_demo/launch/include/xbot-u.launch.xml文件，该文件内容如下

<launch>
<!--下面的x、y、z和yaw是机器人的初始位姿-->
  <arg name="x" default="0.0" />
  <arg name="y" default="0.0" />
  <arg name="z" default="0.0" />
  <arg name="yaw" default="0.0" />

  <!-- Setup controllers -->
  <!-- rosparam file="$(find fetch_gazebo)/param/freight_controllers.yaml" command="load" / -->

  <!-- URDF and TF support -->
  <param name="robot_description" command="$(find xacro)/xacro.py $(find robot_sim_demo)/urdf/robot.xacro" />
  <!-- Put a robot in gazebo, make it look pretty -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" 
        args="-urdf -x $(arg x) -y $(arg y) -z $(arg z) -Y $(arg yaw) -model xbot-u -param robot_description"/>

  <!--node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="use_gui" value="false"/>
  </node-->

  <!--Load the joint controllers to param server-->
  <rosparam file="$(find robot_sim_demo)/param/xbot-u_control.yaml" command="load"/>

  <!--Load controllers-->
  <node name="spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/xbot" args="joint_state_controller
    yaw_platform_position_controller
    pitch_platform_position_controller
    "/>  <!--mobile_base_controller-->


  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" ns="/xbot" respawn="false" output="screen">
    <param name="publish_frequency" value="20.0"/>
  </node>

  <!-- Publish base_scan_raw if anything subscribes to it -->
  <!--node name="publish_base_scan_raw" pkg="topic_tools" type="relay" args="base_scan base_scan_raw" >
    <param name="lazy" type="bool" value="True"/>
  </node-->

  <!-- Start a mux between application and teleop, but the switch must be called by service rather than automatically -->
  <!--<node pkg="topic_tools" type="mux" name="cmd_vel_mux" respawn="true" args="/cmd_vel /cmd_vel_mux/input/teleop /cmd_vel_mux/input/navi">
    <remap from="mux" to="cmd_vel_mux"/>
  </node>-->

  <!-- To make the interface of simulation identical to real XBot -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"  args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find robot_sim_demo)/param/mux.yaml"/>
    <remap from="cmd_vel_mux/output/cmd_vel" to="cmd_vel"/>
  </node>


</launch>

4.用gmapping进行建图

slam gmapping可以在github上获取，https://github.com/ros-perception/slam_gmapping.git

也可以通过以下命令安装

sudo apt-get install ros-kinetic-slam-gmapping

通过把下载好的slam gmapping放到工作空间下编译。

在robot_sim_demo/launch文件夹下建立gmapping.launch文件用于配置gmapping的启动文件，内容如下

<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>

启动gazebo和小车模型

roslaunch robot_sim_demo robot_spawn.launch

启动gmapping

roslaunch robot_sim_demo gmapping.launch

启动小车键盘控制

rosrun robot_sim_demo robot_keyboard_teleop.py

启动rviz进行地图观察

rviz

需要添加map话题，即可看到。

通过键盘控制小车，可以在rviz中看到地图构建的过程

当地图构建完成，进行地图保存，在要保存到的路径下打开终端，输入

rosrun map_server map_saver -f map

所在目录下会保存下面两个文件

5.ROS navigation包下载与编译

一般ros自带了amcl的包，也可以自己下载编译。

下载地址：https://github.com/ros-planning/navigation.git

在这里选择对应的ros版本，然后下载。

把下载下来的压缩包解压到你的工作工作空间下如：～/catkin_ws/src，然后回到～/catkin_ws进行编译

catkin_make

6.amcl定位与效果展示

写一个启动机器人和amcl包的launch文件，代码如下

<launch>
    <!--启动机器人模型-->
	<include file="$(find robot_sim_demo)/launch/robot_spawn.launch"/>
    <!--读取地图信息-->
	<node pkg="map_server" type="map_server" name="map_server" args="$(find robot_sim_demo)/map/map.yaml"/>
    <!--启动amcl包-->
	<include file="$(find amcl)/examples/amcl_diff.launch"/>
	<!--node name="lidar" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /base_link /laser 100"/-->
</launch>

打开rviz，分别从add/By topic中启动

1.map
2.PoseArray
3.PoseWithCovriance
4.LaserScan

打开控制机器人控制的包

rosrun robot_sim_demo robot_keyboard_teleop.py

控制机器人运动，即可看见机器人定位以及粒子的变化

二、通过主机和从机通信完成P3DX机器人的建图和定位

先展示一下工作空间下的用到的功能包，工作空间最好加入系统的全局变量中，方便使用，命令如下

sudo echo "source ~/工作空间名/devel/setup.bash" >> ~/.bashrc

1.分布式多机通信

1.1.设置IP地址

所有机器要处于同一网络下，通过ifconfig命令查看局域网IP地址

从机信息如下：ip为192.168.0.114

同理，主机的ip为192.168.0.107

然后分别在两台计算机系统的/etc/hosts文件中加入对方的IP地址和对应的计算机名，从机下这样记录主机的ip，xjl2为主机

然后用ping命令检测两台计算机网络是否通畅，下图为从机测试到主机

1.2.设置ROS_MASTER_URI

因为系统只能存在一个Master，所以从机需要直到Master的位置。可以使用环境变量ROS_MASTER_URI进行定义，在从机上使用如下命令设置ROS_MASTER_URI

export ROS_MASTER_URI=http://xjl2:11311
export ROS_HOSTNAME=xjl1

为了在多个终端能使用，需要输入到.bashrc中

sudo echo "export ROS_MASTER_URI=http://xjl2:11311" >> ~/.bashrc
sudo echo "export ROS_HOSTNAME=xjl1" >> ~/.bashrc

1.3.多机通信

在主机上打开roscore

roscore

在从机上查看话题

rostopic list

上述的方式可以实现从机能获取主机上的消息，但是主机上功能包的启动要使用ssh连接到从机进行启动和控制

在从机进行主机控制

ssh xjl2

然后输入密码，就可以对主机进行功能包启动，以及小车的控制，下图展示了在从机上用rviz查看主机上的话题信息(先不管这个图怎么来的，后面就知道啦)

2.P3DX机器人和rplidar A2驱动下载和编译

在主机上下载机器人和雷达的驱动包，可以在github上获取

机器人的驱动包：https://github.com/amor-ros-pkg/rosaria.git

机器人键盘控制的驱动包：https://github.com/pengtang/rosaria_client.git

雷达运行的驱动包：https://github.com/robopeak/rplidar_ros.git

机器人和雷达的运行需要以上三个功能包

除此之外，还要为机器人和雷达的端口赋予权限，一般先插入接口的为他ttyUSB0,后插入的为ttyUSB1，我喜欢把雷达作为第二个，这里需要修改雷达下载功能包中的启动文件rplidar_ros/launch/rplidar.launch,修改里面如下的端口配置：

<param name="serial_port"         type="string" value="/dev/ttyUSB1"/>

然后赋予权限：

sudo chmod 777 /dev/ttyUSB0
sudo chmod 777 /dev/ttyUSB1

遇到的问题：

机器人功能包和键盘控制的包不能一起编译，要先编译机器人驱动包，然后再编译键盘控制的驱动包

3.gmapping建图

3.1.编译slam_gmapping，建立启动文件

下载gmapping的功能包：https://github.com/ros-perception/slam_gmapping.git，也可以通过以下命令获取

sudo apt-get install ros-kinetic-slam-gmapping

并在gmapping/launch路径下新建一个launch文件rosaria_gmapping.launch，里面的内容可以通过复制该路径下一个launch文件slam_gmapping_pr2.launch得到，然后再修改2个参数既可使用，修改后的内容如下

<launch>
	<!--下面这个参数改为false-->
    <param name="use_sim_time" value="false"/>
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
	  <!--下面这个变为to scan-->
      <remap from="scan" to="scan"/>
      <param name="map_update_interval" value="5.0"/>
      <param name="maxUrange" value="16.0"/>
      <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.1"/>
      <param name="srt" value="0.2"/>
      <param name="str" value="0.1"/>
      <param name="stt" value="0.2"/>
      <param name="linearUpdate" value="1.0"/>
      <param name="angularUpdate" value="0.5"/>
      <param name="temporalUpdate" value="3.0"/>
      <param name="resampleThreshold" value="0.5"/>
      <param name="particles" value="30"/>
      <param name="xmin" value="-50.0"/>
      <param name="ymin" value="-50.0"/>
      <param name="xmax" value="50.0"/>
      <param name="ymax" value="50.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"/>
    </node>
</launch>

3.2.建立建图的启动文件

我在功能包rosaria里新建了一个launch文件夹，用于存放启动文件，建图大致需要启动如下结点

(1)启动小车驱动包

(2)启动雷达驱动包

(3)通过tf包建立雷达和机器人的固定位置关系（与仿真不同的地方）

(4)启动gmapping建图

在上面的launch文件夹下新建一个launch文件start_gmapping.launch,内容如下

<launch>
<!--启动小车-->
<node name="RosAria" pkg="rosaria" type="RosAria">
	<!--remap from="RosAria/cmd_vel" to="cmd_vel"/-->
 	<remap from="RosAria/pose" to="odom"/>
</node>

<!--启动雷达-->
<include file="$(find rplidar_ros)/launch/rplidar.launch"/>
<!--建立雷达和机器人的坐标关系-->
<node name="tf_static" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /base_link /laser 100"/>
<!--启动gmapping-->
<include file="$(find gmapping)/launch/rosaria_gmapping.launch"/>
</launch>

3.3.建图以及效果展示

在主机上启动roscore，接着就可以到从机上进行远程操控。

在从机上有2个操作：

1）通过ssh连接主机，进行远程控制

2）通过多机通信，用rviz进行数据观测

远程操作：

ssh xjl2 #远程控制主机
roslaunch rosaria start_gmapping.launch #启动建图
rosrun rosaria_client teteop #启动小车控制节点

从机观察只需打开rviz，然后通过上面的键盘控制即可获得地图，如下

保存地图

在远程控制窗口输入

roscd rosaria
mkdir map
rosrun map_server map_saver -f map

4.AMCL定位

amcl的包安装可见仿真部分

4.1.amcl的启动文件

在rosaria/launch中新建amcl.launch内容如下

<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
  <!-- 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="transform_tolerance" value="0.2" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="30"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <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.8"/>
  <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_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.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="odom_frame_id" value="odom"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>

  <param name="initial_pose_x" value="0.0"/>
  <param name="initial_pose_y" value="0.0"/>
  <param name="initial_pose_a" value="0.0"/>
  <param name="initial_cov_xx" value="2.0"/>
  <param name="initial_cov_yy" value="2.0"/>
  <param name="initial_cov_aa" value="2.0"/>
</node>
</launch>

4.2.机器人定位启动文件以及效果展示

在rosaria/launch中新建start_amcl.launch内容如下

<launch>
<node pkg="rosaria" type="RosAria" name="RosAria">
<remap from="RosAria/pose" to="odom"/>
</node>
<!--启动雷达-->
<include file="$(find rplidar_ros)/launch/rplidar.launch"/>
<!--建立雷达和机器人的坐标关系-->
<node name="tf_static" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /base_link /laser 100"/>

<!--读取地图信息-->
<arg name="map_name" default="map.yaml"/>
<node pkg="map_server" type="map_server" name="map_server" args="$(find rosaria)/map/$(arg map_name)"/>
<!--启动amcl定位包-->
<include file="$(find rosaria)/launch/amcl.launch"/>

</launch>

在主机上启动roscore，在从机上进行观察和控制

远程控制终端中：

ssh xjl2	# 远程控制主机
roslaunch rosaria start_amcl.launch	#启动所以节点
rosrun rosaria_client teleop	# 启动小车控制节点

从机终端中：

rviz

通过控制小车可以看到粒子聚集，实现定位，效果如下

4.3. amcl原始提供的launch启动文件，粒子定位的初始位置要与建图位置一样，否在会出现雷达的激光点和地图不能重合,定位不准的问题或需要大量时间进行自定位。

1. 通过launch文件修改(不建议)

   通过修改launch启动文件amcl.launch中的初始位置和初始状态协方差解决这个问题，如下参数会影响粒子的初始的粒子状态和分散度。

     <param name="initial_pose_x" value="0.0"/>
     <param name="initial_pose_y" value="0.0"/>
     <param name="initial_pose_a" value="0.0"/>
     <param name="initial_cov_xx" value="2.0"/>
     <param name="initial_cov_yy" value="2.0"/>
     <param name="initial_cov_aa" value="2.0"/>
   

2. 通过/initialpose话题进行位置均值和协方差初始化

3. 如果想在全局随机撒点，调用服务/global_localization即可

   rosservice call /global_localization