从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启动文件，粒子定位的初始位置要与建图位置一样，否在会出现雷达的激光点和地图不能重合,定位不准的问题或需要大量时间进行自定位。
-
通过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"/> -
通过
/initialpose话题进行位置均值和协方差初始化 -
如果想在全局随机撒点，调用服务
/global_localization即可rosservice call /global_localization
扫一扫
1146
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
