【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】
我们学习了很多的开源包，比如hector、gmapping。但其实我们也可以自己编写一个slam包。这么做最大的好处，主要还是可以帮助自己更好地去了解slam、掌握slam以及用好slam。就像学习rtos一样，使用好别人提供的api是一回事，自己会写rtos又是另外一回事。一旦我们自己会写rtos之后，那么其他所有的实时操作系统都是很容易掌握的。slam也是一样。
前面我们也知道，怎么构建一个slam包了？一般来说，它就是画图-》定位-》画图循环迭代的过程。今天可以做的简单一点。直接从cmd_vel-》laser-》画图，虽然内容简单了一点，但是效果出来的时候，还是很有成就感的。另外本文代码参考了现有的ros书籍，再次表示感谢。
1、编写slam_tfpub文件
代码的主要功能就是接收到cmd_vel消息之后，将自己的tf信息发送出去。头文件slam_tfpub.h如下所示，
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>#define pi 3.1415926class TfMove
{public:TfMove(ros::NodeHandle& nh, ros::Rate& r);void VelCallback(const geometry_msgs::TwistPtr& vel);void init_sub();private:ros::NodeHandle& nh_;ros::Subscriber sub_;tf::TransformBroadcaster tfbrd_;ros::Rate rate;double x,y,z,roll,pit,yaw;
};
而源文件slam_tfpub.cpp如下所示，
#include "slam_tfpub.h"TfMove::TfMove(ros::NodeHandle& nh, ros::Rate& r):nh_(nh), rate(r)
{x = 0;y = 0;z = 0;roll = 0;pit = 0;yaw = 0;init_sub();
}void TfMove::VelCallback(const geometry_msgs::TwistPtr& vel)
{x += vel->linear.x;y += vel->linear.y;z += vel->linear.z;roll += vel->angular.x/pi * 180;pit += vel->angular.y/pi * 180;yaw += vel->angular.z/pi * 180;tf::Transform trans;trans.setOrigin(tf::Vector3(x,y,z));tf::Quaternion q;q.setRPY(roll, pit, yaw);trans.setRotation(q);tfbrd_.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "map", "base_link"));rate.sleep();
}void TfMove::init_sub()
{sub_ = nh_.subscribe("cmd_vel", 1, &TfMove::VelCallback, this);ros::spin();
}int main(int argc, char* argv[])
{ros::init(argc, argv, "myslam_tfpub");ros::NodeHandle nh;ros::Rate rate(10);TfMove tfmove(nh, rate);return 0;}
2、编写slam_laser文件
slam_laser主要是模拟lidar的传感器数据。它的头文件slam_laser.h是这样的，
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>class LaserScanPub
{public:LaserScanPub(ros::NodeHandle& nh, double minAngle, double maxAngle, double scanTime,double minRange, double maxRange, double scanNums);~LaserScanPub();void scanpub_init();void laserdata_init();private:ros::NodeHandle nh_;ros::Publisher scanpub_;sensor_msgs::LaserScan laserdata_;double minAngle;double maxAngle;double minRange;double maxRange;double scanTime;double scanNums;
};而源文件slam_laser.cpp是这样的，
#include "slam_laser.h"LaserScanPub::LaserScanPub(ros::NodeHandle& nh, double min_angle, double maxAngle,double scanTime, double minRange, double maxRange, double scanNums):nh_(nh),minAngle(minAngle), maxAngle(maxAngle),minRange(minRange),maxRange(maxRange), scanNums(scanNums), scanTime(scanTime)
{scanpub_init();
}LaserScanPub::~LaserScanPub()
{
}void LaserScanPub::laserdata_init()
{ros::Time scantime = ros::Time::now();laserdata_.header.stamp = scantime;laserdata_.header.frame_id = "base_link";laserdata_.range_min = minRange;laserdata_.range_max = maxRange;laserdata_.scan_time = scanTime;laserdata_.angle_increment = (maxAngle - minAngle)/scanNums; // angle resolutionlaserdata_.time_increment = scanTime/scanNums; // time resolutionlaserdata_.ranges.resize(scanNums);laserdata_.intensities.resize(scanNums);for(int i = 0; i < scanNums; i++){laserdata_.ranges[i] = 5;laserdata_.intensities[i] = 100;}
}void LaserScanPub::scanpub_init()
{scanpub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 100);ros::Rate rate(10);while(nh_.ok()){laserdata_init();scanpub_.publish(laserdata_);rate.sleep();}
}int main(int argc, char* argv[])
{ros::init(argc, argv, "myslam_laser");ros::NodeHandle nh;LaserScanPub scanpub(nh, 0, 1.57, 0.01, 0, 10, 100);return 0;
}3、编写book_myslam文件
前面准备好了tf和laser，接下来就是最终要的制图工作的。它的基本原理就是在laser触发回调的时候，利用tf信息，计算出lidar坐标在地图上的实际位置。中间绘制的方法使用到了bresenham算法，这个前面也提及过。book_myslam.h头文件是这样的，
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>#include <tf/tf.h>
#include <vector>
#include <fstream>
#include <math.h>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>using namespace std;struct MapPoint
{int x;int y;MapPoint(){x = 0;y = 0;}MapPoint(int x0, int y0){x = x0;y = y0;}
};class MySlam
{public:MySlam(ros::NodeHandle& nh, double mapreso, double mposx, double mposy,double mposz, double morientx, double morienty, double orientz,double morientw, int mwidth, int mheight);~MySlam();void mappub_init();void lasersub_init();void lasercallback(const sensor_msgs::LaserScanConstPtr& laserdata);void mapdata_init();vector<MapPoint> bresenham(int x0, int y0, int x1, int y1);private:ros::NodeHandle nh_;ros::Subscriber lasersub_;ros::Publisher mappub_;tf::TransformListener tflistener_;nav_msgs::OccupancyGrid mapdata_;double mapreso;double mposx;double mposy;double mposz;double morientx;double morienty;double morientz;double morientw;int mwidth;int mheight;vector<MapPoint> endpoints;MapPoint endpoint;vector<MapPoint> mappoints;tf::StampedTransform base2map;tf::Quaternion quat;double theta;tf::Vector3 trans_base2map;double tx, ty;int basex0, basey0;double basex, basey;double mapx, mapy;double beamsAngle;int mapxn, mapyn;int laserNum;int nx,ny;int idx;ofstream fopen;int scan_count;int scan_reso;boost::mutex map_mutex;
};
它的实现文件book_myslam.cpp是这样的，
#include "book_myslam.h"MySlam::MySlam(ros::NodeHandle& nh, double mapreso, double mposx, double mposy,double mposz, double morientx, double morienty, double morientz,double morientw, int mwidth, int mheight):nh_(nh), mapreso(mapreso),mposx(mposx), mposy(mposy), mposz(mposz), morientx(morientx),morienty(morienty), morientz(morientz), morientw(morientw),mwidth(mwidth), mheight(mheight)
{mapdata_init();mappub_init();lasersub_init();
}MySlam::~MySlam()
{
}void MySlam::lasercallback(const sensor_msgs::LaserScanConstPtr& laserdata)
{if(scan_count % scan_reso == 0){try {tflistener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));tflistener_.lookupTransform("map", "base_link", ros::Time(0), base2map);}catch(tf::TransformException& ex){ROS_INFO("%s", ex.what());ros::Duration(1.0).sleep();}boost::mutex::scoped_lock map_lock(map_mutex);quat = base2map.getRotation();theta = quat.getAngle();trans_base2map = base2map.getOrigin();tx = trans_base2map.getX();ty = trans_base2map.getY();basex0 = int(tx/mapreso);basey0 = int(ty/mapreso);laserNum = laserdata->ranges.size();fopen.open("data.txt", ios::app);if(fopen.is_open()){cout << "open file successful!" << endl;}else{cout << "open file fail" << endl;}for(int i = 0; i < laserNum; i++){beamsAngle = laserdata->angle_min + i * laserdata->angle_increment;basex = laserdata->ranges[i] * cos(beamsAngle);basey = laserdata->ranges[i] * sin(beamsAngle);mapx = basex * cos(theta) + basey * sin(theta) + tx;mapy = basey * cos(theta) - basex * sin(theta) + ty;nx = int(mapx/mapreso);ny = int(mapy/mapreso);mapxn = nx + 1;mapyn = ny + 1;endpoint.x = mapxn;endpoint.y = mapyn;fopen << endpoint.x << " " << endpoint.y << std::endl;endpoints.push_back(endpoint);}fopen.close();for(vector<MapPoint>::iterator iter = endpoints.begin(); iter != endpoints.end(); iter++){mappoints = MySlam::bresenham(basex0, basey0, (*iter).x, (*iter).y);cout << "scan numbers: " << endpoints.size() << endl;cout << "bresenham point nums are: " << mappoints.size() << endl;cout << "x0, y0 is " << basex0 << " " << basey0 << std::endl;cout << "angle is " << theta << std::endl;for(vector<MapPoint>::iterator iter1 = mappoints.begin(); iter1 != mappoints.end(); iter1 ++){idx = mwidth * (*iter1).y + (*iter1).x;cout << "idx is " << (*iter1).x << " " << (*iter1).y << std::endl;mapdata_.data[idx] = 0;}mappoints.clear();}endpoints.clear();mappub_.publish(mapdata_);}scan_count ++;
}vector<MapPoint> MySlam::bresenham(int x0, int y0, int x1, int y1)
{vector<MapPoint> pp;MapPoint p;int dx, dy, h, a, b, x, y, flag, t;dx = abs(x1-x0);dy = abs(y1-y0);if(x1 > x0) a = 1; else a = -1;if(y1 > y0) b = 1; else b = -1;x = x0;y = y0;if(dx >= dy){flag = 0;}else{t = dx;dx = dy;dy = t;flag = 1;}h = 2 * dy - dx;for(int i = 1; i <= dx; ++i){p.x = x, p.y = y;pp.push_back(p);if(h >= 0){if(flag == 0) y = y+b;else x = x+a;h =h - 2*dx;}if(flag ==0) x = x+a;else y = y+b;h = h + 2*dy;}return pp;
}void MySlam::mappub_init()
{mappub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 100);
}void MySlam::lasersub_init()
{lasersub_ = nh_.subscribe("scan", 1, &MySlam::lasercallback, this);
}void MySlam::mapdata_init()
{scan_count = 0;scan_reso = 1;ros::Time currtime = ros::Time::now();mapdata_.header.stamp = currtime;mapdata_.header.frame_id = "map";mapdata_.info.resolution = mapreso;mapdata_.info.width = mwidth;mapdata_.info.height = mheight;mapdata_.info.origin.position.x = mposx;mapdata_.info.origin.position.y = mposy;mapdata_.info.origin.position.z = mposz;mapdata_.info.origin.orientation.x = morientx;mapdata_.info.origin.orientation.y = morienty;mapdata_.info.origin.orientation.z = morientz;mapdata_.info.origin.orientation.w = morientw;int datasize = mwidth * mheight;mapdata_.data.resize(datasize);for(int i = 0; i < datasize; i++){mapdata_.data[i] = -1;}
}int main(int argc, char* argv[])
{int debug_flag = 0;//while(debug_flag == 0) sleep(10);ros::init(argc, argv, "MySlam");ros::NodeHandle nh;double mapreso = 0.05;double mposx = 0;double mposy = 0;double mposz = 0;double morientx = 0;double morienty = 0;double morientz= 0;double morientw= 1;int mwidth = 300;int mheight = 300;MySlam myslam(nh, mapreso, mposx, mposy, mposz, morientx, morienty, morientz, morientw, mwidth, mheight);ros::spin();return 0;
}4、准备编译脚本
上面3个文件其实就是3个程序，所以我们在CMakeLists.txt里面做好三个程序的编译脚本就可以了。需要调试的话，可以添加上-Wall -g选项。
add_executable(slam_tfpub src/slam_tfpub.cpp)
target_link_libraries(slam_tfpub ${catkin_LIBRARIES})
add_dependencies(slam_tfpub beginner_tutorials_generate_messages_cpp)add_executable(slam_laser src/slam_laser.cpp)
target_link_libraries(slam_laser ${catkin_LIBRARIES})
add_dependencies(slam_laser beginner_tutorials_generate_messages_cpp)add_definitions("-Wall -g")add_executable(book_myslam src/book_myslam.cpp)
target_link_libraries(book_myslam ${catkin_LIBRARIES})
add_dependencies(book_myslam beginner_tutorials_generate_messages_cpp)5、编译
编译就很简单了，直接输入catkin_make即可。
6、构建launch文件
因为启动的程序比较多，这里可以编写一个myslam.launch文件，使用起来方便一点。脚本文件注意放在launch目录下面。
<launch><node pkg="beginner_tutorials" type="slam_tfpub" name="tf_pub"/><node pkg="beginner_tutorials" type="slam_laser" name="laser_pub"/><node pkg="beginner_tutorials" type="book_myslam" name="myslam"/>
</launch>
7、实验步骤
实验步骤稍微复杂一点，主要分成四步。第一，打开roscore；第二，用rostopic发送cmd_vel信息，
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '[0.003, 0.0, 0.0]' '[0.0, 0.0, 0.0]'第三，启动myslam.launch文件，
roslaunch beginner_tutorials myslam.launch第四，就是输入rosrun rviz rviz命令，创建map，选中map之后进一步查看建图效果。这四个步骤需要严格按顺序执行，不然缺少了某个步骤，很有可能程序会发生闪退，主要是book_myslam这个程序。这样，不出意外的话，我们就可以在rviz上面看到这样的建图效果了，
