SLAM从入门到精通（构建自己的slam包）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：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.1415926

class 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 resolution
	laserdata_.time_increment = scanTime/scanNums; // time resolution
	laserdata_.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上面看到这样的建图效果了，