slam_gmapping 是使用粒子滤波算法进行建图的一种方式,它严重依赖里程计,所以使用前面用stm32搭建的底层建图效果并不理想,接下来考虑进行线速度与角速度的标定,或使用谷歌的gartographer建图,为了方便使用串口连接,所有串口号都使用软连接,参考rplidar_ros/scripts/rulidar.rules,使用lsusb-vvv查看idVendor和idProduct,然后在/etc/udev/rule.d下新建软连接的设备的.rules文件
# set the udev rule , make the device_port be fixed by rplidar # KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar"
安装必要的slam_gmapping包,接下来就是配置rplidar和gmapping的launch 文件,以下直接给出
my_robot_gmapping.launch:
<launch> <arg name="scan_topic" default="scan" /> <!--laser的topic名称,与自己的激光的topic相对应--> <arg name="base_frame" default="base_link"/> <arg name="odom_frame" default="odom"/> <!--param name="use_sim_time" value="true"/--> <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" clear_params="true"> <!--remap from="scan" to="base_scan"/--> <param name="base_frame" value="/base_link"/> <!--***机器人的坐标系--> <param name="odom_frame" value="/odom" /> <!--***世界坐标系--> <param name="map_frame" value="/map" /> <!--***地图坐标系--> <param name="map_update_interval" value="20"/> <!--************地图更新间隔,两次scanmatch的间隔,地图更新也受scanmach的影响,如果scanmatch没有成功的话,是不会更新地图的--> <!-- Set maxUrange < actual maximum range of the Laser --> <param name="maxRange" value="5.0"/> <!--探测最大可用范围,计光束能到的范围default80.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"/> <!--icp的迭代次数--> <param name="lsigma" value="0.075"/> <!--波束的sigma,用来计算似然估计每次扫描跳过的波束--> <param name="ogain" value="3.0"/> <!--用来平滑重采样的影响--> <param name="lskip" value="0"/> <!--为0,表示所有的激光都处理,尽可能为零,每次扫描跳过的波束评估似然的增益,用来平滑重采样的影响--> <param name="minimumScore" value="50"/> <!--************************避免在大空间范围内使用有限距离的激光仪出现的jumping pose estimates问题,决定对激光的一个置信度,越高说明对激光匹配算法的要求越高,激光的匹配也越容易失败转而去使用里程计数据,太低又会使地图出现大量噪声,所以需要权衡--> <param name="srr" value="0.01"/> <!--以下四个参数是运动模型的噪声参数--><!--平移时里程误差作为平移函数0.1--> <param name="srt" value="0.02"/> <!--平移时里程误差作为旋转函数0.2--> <param name="str" value="0.01"/> <!--旋转时里程误差作为平移函数0.1--> <param name="stt" value="0.02"/> <!--旋转时里程误差作为旋转函数0.1--> <param name="linearUpdate" value="0.5"/> <!--机器人移动linearUpdate距离,进行scanmatch--> <param name="angularUpdate" value="0.436"/> <!--机器人选装angularUpdate角度,进行scanmatch--><!--机器人每旋转这么远处理一次扫描1--> <param name="temporalUpdate" value="-1.0"/> <!--如果最新扫描处理比更新慢,则处理1次扫描,该值为负数时关闭基于时间的更新--> <param name="resampleThreshold" value="0.5"/> <!--基于重采样门限的Neff--> <param name="particles" value="50"/> <!--********************很重要,粒子个数80--> <!-- <param name="xmin" value="-50.0"/> <param name="ymin" value="-50.0"/> <param name="xmax" value="50.0"/> <param name="ymax" value="50.0"/> make the starting size small for the benefit of the Android client's memory... --> <param name="xmin" value="-1.0"/> <!--map初始化的大小--> <!--地图初始尺寸-100,-100,100,100--> <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"/> <param name="transform_publish_period" value="0.1"/><!--添加--> <remap from="scan" to="$(arg scan_topic)"/> </node> </launch>
my_robot_rplidar_laser.launch:
<launch> <node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen"> <param name="serial_port" type="string" value="/dev/rplidar"/> <param name="serial_baudrate" type="int" value="115200"/> <param name="frame_id" type="string" value="laser"/> <param name="inverted" type="bool" value="false"/> <param name="angle_compensate" type="bool" value="true"/> </node> <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.0 0.0 0.375 0.0 0.0 0.0 base_link laser 100"/> </launch>