Ubuntu14.04下sick LMS5xx的gmapping建图

    本文是一篇笔记，记录Ubuntu.14.04下使用sick LMS500的gmapping建图。业余时间整理，仅供参考。

功能：使用移动平台(如pioneer3dx)以及激光sicklms5xx在ubuntu操作系统下，利用ros的gmapping package建立环境地图。

要求：

• 硬件：PC一台，sick LMS500激光一个
• 软件：Ubuntu14.04， ROS for indigo，网口驱动，laser_node驱动，gmapping

步骤：

Step1: 将LMS5xx接24V电源供电，通过网线插入PC。

Step2：在Ubuntu14.04上装网口通讯的驱动。由于PC可以连网线上网，但接入lms5xx的网口时却无反应，因此需要装入新的网口驱动程序。在3台电脑上试过，都不行，可能原因是ubuntu系统对网口的支持并不好。

驱动程序：CH9200DRV_130607（自行下载）

进入CH9200DRV_130607/linux文件夹

安装命令：$sudo make

                   $sudo make load

卸载命令：$sudo make unload

Step3:配置LMS500网口的IP地址。

利用Windows下的SOPAS软件查看LMS500的IP地址，在Ubuntu14.04的网络连接进行编辑，将IP地址设为192.168.203.10，保存。如果成功，可以看到LMS500已经可以网口通讯了。

$ping 192.168.203.10 ,有数据接收。

Step4:安装LMS500的laser_driver。

在网站https://github.com/NatanBiesmans/lms5xx下载lms5xx的驱动程序，进行安装。安装步骤按照安装说明进行，如下。

Step5:安装gmapping package

安装命令：$sudo apt-get installros-indigo-slam-gmapping

Step6：安装pioneer3DX的驱动程序（此处可类比自己的平台，发送速度指令可移动），发布机器人的里程计tf变换。

(1)安装pioneer3DX驱动，通过串口进行通讯（可能需要配置端口如$sudo chmod a+wr /dev/ttyUSB0）。

安装命令：$sudo apt-get install ros-indigo-p2os-driver

(2)此处关键的是发布odometry的变换，因为gmapping需要里程计的输入，需要通过ros的tf变换发布出去。可以用虚拟的数据。

附：odom_tf.cpp文件

#include <ros/ros.h>#include <tf/transform_broadcaster.h>#include <nav_msgs/Odometry.h>#include <geometry_msgs/Twist.h>  //forcmd_vel publication#include<geometry_msgs/PoseWithCovarianceStamped.h>#include <math.h>#include <stdio.h>#include <vector>#include <iostream>using namespace std;ros::Subscriber odom_sub;ros::Publisher odom_pub;nav_msgs::Odometry odom;boost::shared_ptr<tf::TransformBroadcaster>odom_broadcaster;geometry_msgs::TransformStamped odom_trans;//tf::TransformBroadcasterodom_broadcaster;geometry_msgs::Twist cmdvel;ros::Time odom_stamp;double odom_poseYaw, odom_poseX,odom_poseY;ros::Time current_time, last_time;void odom_cb(const nav_msgs::Odometry&odom_data){     odom_stamp = odom_data.header.stamp;   //convert from quaternion into radium   odom_poseX = odom_data.pose.pose.position.x;       odom_poseY = odom_data.pose.pose.position.y;       odom_poseYaw = tf::getYaw(odom_data.pose.pose.orientation);   //   //cap to (-2PI, 2PI)   while (odom_poseYaw >= 2*M_PI)   odom_poseYaw -= 2*M_PI;   while (odom_poseYaw <   0)        odom_poseYaw += 2*M_PI;   ROS_INFO("odom %f %f %f", odom_poseX, odom_poseY,odom_poseYaw);   current_time = ros::Time::now();    odom_trans.header.stamp = current_time;   odom_trans.header.frame_id = "odom";   odom_trans.child_frame_id = "base_link";    odom_trans.transform.translation.x = odom_poseX;   odom_trans.transform.translation.y = odom_poseY;   odom_trans.transform.translation.z = 0.0;   //since all odometry is 6DOF we'll need a quaternion created from yaw   geometry_msgs::Quaternion odom_quat =tf::createQuaternionMsgFromYaw(odom_poseYaw);   odom_trans.transform.rotation = odom_quat;   //send the transform   odom_broadcaster->sendTransform(odom_trans);   //next, we'll publish the odometry message over ROS   odom.header.stamp = current_time;   odom.header.frame_id = "odom";   odom_trans.child_frame_id = "base_link";   //set the position   odom.pose.pose.position.x = odom_poseX;   odom.pose.pose.position.y = odom_poseY;   odom.pose.pose.position.z = 0.0;   odom.pose.pose.orientation = odom_quat;   //set the velocity   odom.child_frame_id = "base_link";   odom.twist.twist.linear.x = odom_data.twist.twist.linear.x;   odom.twist.twist.linear.y = odom_data.twist.twist.linear.y;   odom.twist.twist.angular.z = odom_data.twist.twist.angular.z;   //publish the message   odom_pub.publish(odom);   last_time = ros::Time::now();}int main(int argc, char **argv) { ros::init(argc, argv, "odom_tf");   ros::NodeHandle prv_nh("~"); odom_broadcaster.reset(new tf::TransformBroadcaster); odom_sub = prv_nh.subscribe( "/pose", 50, &odom_cb); odom_pub = prv_nh.advertise<nav_msgs::Odometry>("odom",50); ros::Rate r(10.0); while(ros::ok())    {      ros::spinOnce();               //check for incoming messages      r.sleep();   }   }

Step7：写gmapping_test.launch文件进行启动，移动小车，在rviz中可以看到正在建立地图。

思路：

a)      首先启动移动平台（如p3dx），然后启动激光发布topic /scan，于是得到激光数据。

b)      接着发布/laser 与/base_link的静态tf变换，发布odometry的tf变换。

c)       最后是启动gmapping node 进行建图，移动机器人建立环境地图，通过rviz进行显示。

附：gmapping_test.launch文件：

<launch>         <!-- p2os driver -->        <node pkg="p2os_driver"type="p2os_driver" name="p2os_driver">                <paramname="pulse" value="0.5"/>                <param name="port"value="/dev/ttyUSB0" />        </node>        <!-- 2D laser -->        <node pkg="laser_node"type="laser_node" name="laser_node"/>         <!-- static tf -->               <node pkg="tf"type="static_transform_publisher" name="base2laser"args="0.2 0 0.25 0 0 0 base_link laser 100" />         <node name="odom_tf"pkg="sick_nav" type="odom_tf"/>        <!--run Gmappping -->        <node pkg="gmapping"type="slam_gmapping" name="slam_gmapping"args="/scan" output="screen"/>         <!-- static tf -->               <node pkg="tf"type="static_transform_publisher" name="map2tag"args="0 0 0 0 0 0 /map /odom 100"/>            <!-- Run rviz -->        <node name="rviz"pkg="rviz" type="rviz" output="screen"/>  </launch>

Step8：地图保存，最终得到robotlab.pgm和robotlab.yaml参数文件， 供其他node使用。

$rosrun map_server map_saver –f robotlab

效果就不在此处展示，由于sick lms5xx的精度很高，距离远，建立的地图效果非常好。缺点是sick lms5xx价格昂贵，一般场合用不起。（此处独白：有一个能玩一玩，算是抱老师大腿了。）

小结：

本文记录了gmapping建图的过程，很久之前也做过，但那时对此还不太懂，现在算是一个补充。主要遇到的问题有：laser的网口连接/laser的驱动/odometry的tf变换/平台移动建图的技巧（平移和旋转）。

 

个人经验，仍需继续思考，如有好的想法，欢迎给我发邮件[e-mail]，多多交流。