• 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如下所示，

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5.  
    6. #define pi 3.1415926
    7.  
    8. class TfMove
    9. {
    10. 	public:
    11. 		TfMove(ros::NodeHandle& nh, ros::Rate& r);
    12. 		void VelCallback(const geometry_msgs::TwistPtr& vel);
    13. 		void init_sub();
    14. 		
    15. 	private:
    16. 		ros::NodeHandle& nh_;
    17. 		ros::Subscriber sub_;
    18. 		tf::TransformBroadcaster tfbrd_;
    19. 		ros::Rate rate;
    20. 		double x,y,z,roll,pit,yaw;
    21. };

            而源文件slam_tfpub.cpp如下所示，

    1. #include "slam_tfpub.h"
    2.  
    3. TfMove::TfMove(ros::NodeHandle& nh, ros::Rate& r):nh_(nh), rate(r)
    4. {
    5. 	x = 0;
    6. 	y = 0;
    7. 	z = 0;
    8. 	roll = 0;
    9. 	pit = 0;
    10. 	yaw = 0;
    11. 	
    12. 	init_sub();
    13. }
    14.  
    15. void TfMove::VelCallback(const geometry_msgs::TwistPtr& vel)
    16. {
    17. 	x += vel->linear.x;
    18. 	y += vel->linear.y;
    19. 	z += vel->linear.z;
    20. 	
    21. 	roll += vel->angular.x/pi * 180;
    22. 	pit += vel->angular.y/pi * 180;
    23. 	yaw += vel->angular.z/pi * 180;
    24. 	
    25. 	tf::Transform trans;
    26. 	trans.setOrigin(tf::Vector3(x,y,z));
    27. 	tf::Quaternion q;
    28. 	q.setRPY(roll, pit, yaw);
    29. 	trans.setRotation(q);
    30. 	tfbrd_.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "map", "base_link"));
    31. 	rate.sleep();
    32. }
    33.  
    34. void TfMove::init_sub()
    35. {
    36. 	sub_ = nh_.subscribe("cmd_vel", 1, &TfMove::VelCallback, this);
    37. 	ros::spin();
    38. }
    39.  
    40. int main(int argc, char* argv[])
    41. {
    42. 	ros::init(argc, argv, "myslam_tfpub");
    43. 	ros::NodeHandle nh;
    44. 	ros::Rate rate(10);
    45. 	TfMove tfmove(nh, rate);
    46. 	return 0;
    47. 	
    48. }

    2、编写slam_laser文件

            slam_laser主要是模拟lidar的传感器数据。它的头文件slam_laser.h是这样的，

    1. #include 
    2. #include 
    3.  
    4. class LaserScanPub
    5. {
    6. 	public:
    7. 		LaserScanPub(ros::NodeHandle& nh, 
    8. 						double minAngle, double maxAngle, double scanTime,
    9. 						double minRange, double maxRange, double scanNums);
    10. 		~LaserScanPub();
    11. 		void scanpub_init();
    12. 		void laserdata_init();
    13. 		
    14. 	private:
    15. 		ros::NodeHandle nh_;
    16. 		ros::Publisher scanpub_;
    17. 		sensor_msgs::LaserScan laserdata_;
    18. 		double minAngle;
    19. 		double maxAngle;
    20. 		double minRange;
    21. 		double maxRange;
    22. 		double scanTime;
    23. 		double scanNums;
    24. };
    25.

            而源文件slam_laser.cpp是这样的，

    1. #include "slam_laser.h"
    2.  
    3. LaserScanPub::LaserScanPub(ros::NodeHandle& nh, double min_angle, double maxAngle,
    4. 									double scanTime, double minRange, double maxRange, double scanNums)
    5. 								:nh_(nh),minAngle(minAngle), maxAngle(maxAngle),minRange(minRange),
    6. 								maxRange(maxRange), scanNums(scanNums), scanTime(scanTime)
    7. {
    8. 	scanpub_init();
    9. }
    10.  
    11. LaserScanPub::~LaserScanPub()
    12. {
    13. }
    14.  
    15. void LaserScanPub::laserdata_init()
    16. {
    17. 	ros::Time scantime = ros::Time::now();
    18. 	laserdata_.header.stamp = scantime;
    19. 	laserdata_.header.frame_id = "base_link";
    20. 	laserdata_.range_min = minRange;
    21. 	laserdata_.range_max = maxRange;
    22. 	laserdata_.scan_time = scanTime;
    23. 	laserdata_.angle_increment = (maxAngle - minAngle)/scanNums; // angle resolution
    24. 	laserdata_.time_increment = scanTime/scanNums; // time resolution
    25. 	laserdata_.ranges.resize(scanNums);
    26. 	laserdata_.intensities.resize(scanNums);
    27. 	
    28. 	for(int i = 0; i < scanNums; i++)
    29. 	{
    30. 		laserdata_.ranges[i] = 5;
    31. 		laserdata_.intensities[i] = 100;
    32. 	}
    33. }
    34.  
    35. void LaserScanPub::scanpub_init()
    36. {
    37. 	scanpub_ = nh_.advertise<sensor_msgs::LaserScan>("scan", 100);
    38. 	ros::Rate rate(10);
    39. 	
    40. 	while(nh_.ok())
    41. 	{
    42. 		laserdata_init();
    43. 		scanpub_.publish(laserdata_);
    44. 		rate.sleep();
    45. 	}
    46. }
    47.  
    48. int main(int argc, char* argv[])
    49. {
    50. 	ros::init(argc, argv, "myslam_laser");
    51. 	ros::NodeHandle nh;
    52. 	LaserScanPub scanpub(nh, 0, 1.57, 0.01, 0, 10, 100);
    53. 	return 0;
    54. }
    55.

    3、编写book_myslam文件

            前面准备好了tf和laser，接下来就是最终要的制图工作的。它的基本原理就是在laser触发回调的时候，利用tf信息，计算出lidar坐标在地图上的实际位置。中间绘制的方法使用到了bresenham算法，这个前面也提及过。book_myslam.h头文件是这样的，

    1. #include 
    2. #include 
    3. #include 
    4. #include 
    5. #include 
    6.  
    7. #include 
    8. #include 
    9. #include 
    10. #include 
    11. #include 
    12. #include 
    13.  
    14. using namespace std;
    15.  
    16. struct MapPoint
    17. {
    18. 	int x;
    19. 	int y;
    20. 	
    21. 	MapPoint()
    22. 	{
    23. 		x = 0;
    24. 		y = 0;
    25. 	}
    26. 	
    27. 	MapPoint(int x0, int y0)
    28. 	{
    29. 		x = x0;
    30. 		y = y0;
    31. 	}
    32. };
    33.  
    34. class MySlam
    35. {
    36. 	public:
    37. 		MySlam(ros::NodeHandle& nh, double mapreso, double mposx, double mposy,
    38. 			double mposz, double morientx, double morienty, double orientz,
    39. 			double morientw, int mwidth, int mheight);
    40. 		~MySlam();
    41. 		void mappub_init();
    42. 		void lasersub_init();
    43. 		void lasercallback(const sensor_msgs::LaserScanConstPtr& laserdata);
    44. 		void mapdata_init();
    45. 		vector bresenham(int x0, int y0, int x1, int y1);
    46. 		
    47. 	private:
    48. 		ros::NodeHandle nh_;
    49. 		ros::Subscriber lasersub_;
    50. 		ros::Publisher mappub_;
    51. 		tf::TransformListener tflistener_;
    52. 		nav_msgs::OccupancyGrid mapdata_;
    53. 		
    54. 		double mapreso;
    55. 		double mposx;
    56. 		double mposy;
    57. 		double mposz;
    58. 		double morientx;
    59. 		double morienty;
    60. 		double morientz;
    61. 		double morientw;
    62. 		
    63. 		int mwidth;
    64. 		int mheight;
    65. 		vector endpoints;
    66. 		MapPoint endpoint;
    67. 		vector mappoints;
    68. 		tf::StampedTransform base2map;
    69. 		tf::Quaternion quat;
    70. 		double theta;
    71. 		tf::Vector3 trans_base2map;
    72. 		double tx, ty;
    73. 		int basex0, basey0;
    74. 		
    75. 		double basex, basey;
    76. 		double mapx, mapy;
    77. 		
    78. 		double beamsAngle;
    79. 		int mapxn, mapyn;
    80. 		int laserNum;
    81. 		int nx,ny;
    82. 		int idx;
    83. 		
    84. 		ofstream fopen;
    85. 		int scan_count;
    86. 		int scan_reso;
    87. 		boost::mutex map_mutex;	
    88. };

            它的实现文件book_myslam.cpp是这样的，

    1. #include "book_myslam.h"
    2.  
    3. MySlam::MySlam(ros::NodeHandle& nh, double mapreso, double mposx, double mposy,
    4. 					double mposz, double morientx, double morienty, double morientz,
    5. 					double morientw, int mwidth, int mheight):nh_(nh), mapreso(mapreso),
    6. 					mposx(mposx), mposy(mposy), mposz(mposz), morientx(morientx),
    7. 					morienty(morienty), morientz(morientz), morientw(morientw),
    8. 					mwidth(mwidth), mheight(mheight)
    9. {
    10. 	mapdata_init();
    11. 	mappub_init();
    12. 	lasersub_init();
    13. }
    14.  
    15. MySlam::~MySlam()
    16. {
    17. }
    18.  
    19. void MySlam::lasercallback(const sensor_msgs::LaserScanConstPtr& laserdata)
    20. {
    21. 	if(scan_count % scan_reso == 0)
    22. 	{
    23. 		try {
    24. 			
    25. 			tflistener_.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
    26. 			tflistener_.lookupTransform("map", "base_link", ros::Time(0), base2map);
    27. 		}
    28. 		catch(tf::TransformException& ex)
    29. 		{
    30. 			ROS_INFO("%s", ex.what());
    31. 			ros::Duration(1.0).sleep();
    32. 		}
    33. 		
    34. 		boost::mutex::scoped_lock map_lock(map_mutex);
    35. 		
    36. 		quat = base2map.getRotation();
    37. 		theta = quat.getAngle();
    38. 		
    39. 		trans_base2map = base2map.getOrigin();
    40. 		tx = trans_base2map.getX();
    41. 		ty = trans_base2map.getY();
    42. 		
    43. 		basex0 = int(tx/mapreso);
    44. 		basey0 = int(ty/mapreso);
    45. 		laserNum = laserdata->ranges.size();
    46. 		fopen.open("data.txt", ios::app);
    47. 		
    48. 		if(fopen.is_open())
    49. 		{
    50. 			cout << "open file successful!" << endl;
    51. 		}
    52. 		else
    53. 		{
    54. 			cout << "open file fail" << endl;
    55. 		}
    56. 		
    57. 		for(int i = 0; i < laserNum; i++)
    58. 		{
    59. 			beamsAngle = laserdata->angle_min + i * laserdata->angle_increment;
    60. 			
    61. 			basex = laserdata->ranges[i] * cos(beamsAngle);
    62. 			basey = laserdata->ranges[i] * sin(beamsAngle);
    63. 			
    64. 			mapx = basex * cos(theta) + basey * sin(theta) + tx;
    65. 			mapy = basey * cos(theta) - basex * sin(theta) + ty;
    66. 			
    67. 			nx = int(mapx/mapreso);
    68. 			ny = int(mapy/mapreso);
    69. 			mapxn = nx + 1;
    70. 			mapyn = ny + 1;
    71. 			endpoint.x = mapxn;
    72. 			endpoint.y = mapyn;
    73. 			
    74. 			fopen << endpoint.x << " " << endpoint.y << std::endl;
    75. 			endpoints.push_back(endpoint);
    76. 		}
    77. 		fopen.close();
    78. 		
    79. 		for(vector<MapPoint>::iterator iter = endpoints.begin(); iter != endpoints.end(); iter++)
    80. 		{
    81. 			mappoints = MySlam::bresenham(basex0, basey0, (*iter).x, (*iter).y);
    82. 			cout << "scan numbers: " << endpoints.size() << endl;
    83. 			cout << "bresenham point nums are: " << mappoints.size() << endl;
    84. 			cout << "x0, y0 is " << basex0 << " " << basey0 << std::endl;
    85. 			cout << "angle is " << theta << std::endl;
    86. 			
    87. 			for(vector<MapPoint>::iterator iter1 = mappoints.begin(); iter1 != mappoints.end(); iter1 ++)
    88. 			{
    89. 				idx = mwidth * (*iter1).y + (*iter1).x;
    90. 				cout << "idx is " << (*iter1).x << " " << (*iter1).y << std::endl;
    91. 				mapdata_.data[idx] = 0;
    92. 			}
    93. 			
    94. 			mappoints.clear();
    95. 		}
    96. 		
    97. 		endpoints.clear();
    98. 		mappub_.publish(mapdata_);
    99. 	}
    100. 	
    101. 	scan_count ++;
    102. }
    103.  
    104. vector<MapPoint> MySlam::bresenham(int x0, int y0, int x1, int y1)
    105. {
    106. 	vector<MapPoint> pp;
    107. 	MapPoint p;
    108. 	int dx, dy, h, a, b, x, y, flag, t;
    109. 	dx = abs(x1-x0);
    110. 	dy = abs(y1-y0);
    111. 	
    112. 	if(x1 > x0) 
    113. 		a = 1; 
    114. 	else 
    115. 		a = -1;
    116. 	
    117. 	if(y1 > y0) 
    118. 		b = 1; 
    119. 	else 
    120. 		b = -1;
    121. 	
    122. 	x = x0;
    123. 	y = y0;
    124. 	if(dx >= dy)
    125. 	{
    126. 		flag = 0;
    127. 	}
    128. 	else
    129. 	{
    130. 		t = dx;
    131. 		dx = dy;
    132. 		dy = t;
    133. 		flag = 1;
    134. 	}
    135. 	
    136. 	h = 2 * dy - dx;
    137. 	for(int i = 1; i <= dx; ++i)
    138. 	{
    139. 		p.x = x, p.y = y;
    140. 		
    141. 		pp.push_back(p);
    142. 		if(h >= 0)
    143. 		{
    144. 			if(flag == 0) 
    145. 				y = y+b;
    146. 			else 
    147. 				x = x+a;
    148. 			
    149. 			h =h - 2*dx;
    150. 		}
    151. 		
    152. 		if(flag ==0) 
    153. 			x = x+a;
    154. 		else 
    155. 			y = y+b;
    156. 		
    157. 		h = h + 2*dy;
    158. 	}
    159. 	
    160. 	return pp;
    161. }
    162.  
    163. void MySlam::mappub_init()
    164. {
    165. 	mappub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 100);
    166. }
    167.  
    168. void MySlam::lasersub_init()
    169. {
    170. 	lasersub_ = nh_.subscribe("scan", 1, &MySlam::lasercallback, this);
    171. }
    172.  
    173. void MySlam::mapdata_init()
    174. {
    175. 	scan_count = 0;
    176. 	scan_reso = 1;
    177. 	
    178. 	ros::Time currtime = ros::Time::now();
    179. 	mapdata_.header.stamp = currtime;
    180. 	mapdata_.header.frame_id = "map";
    181. 	mapdata_.info.resolution = mapreso;
    182. 	mapdata_.info.width = mwidth;
    183. 	mapdata_.info.height = mheight;
    184. 	mapdata_.info.origin.position.x = mposx;
    185. 	mapdata_.info.origin.position.y = mposy;
    186. 	mapdata_.info.origin.position.z = mposz;
    187. 	mapdata_.info.origin.orientation.x = morientx;
    188. 	mapdata_.info.origin.orientation.y = morienty;
    189. 	mapdata_.info.origin.orientation.z = morientz;
    190. 	mapdata_.info.origin.orientation.w = morientw;
    191. 	int datasize = mwidth * mheight;
    192. 	mapdata_.data.resize(datasize);
    193.  
    194. 	for(int i = 0;  i < datasize; i++)
    195. 	{
    196. 		mapdata_.data[i] = -1;
    197. 	}
    198. }
    199.  
    200. int main(int argc, char* argv[])
    201. {
    202. 	int debug_flag = 0;
    203. 	//while(debug_flag == 0) sleep(10);
    204.  
    205. 	ros::init(argc, argv, "MySlam");
    206. 	ros::NodeHandle nh;
    207. 	
    208. 	double mapreso = 0.05;
    209. 	double mposx = 0;
    210. 	double mposy = 0;
    211. 	double mposz = 0;
    212. 	double morientx = 0;
    213. 	double morienty = 0;
    214. 	double morientz= 0;
    215. 	double morientw= 1;
    216. 	
    217. 	int mwidth = 300;
    218. 	int mheight = 300;
    219. 	
    220. 	MySlam myslam(nh, mapreso, mposx, mposy, mposz, morientx, morienty, morientz, morientw, mwidth, mheight);
    221. 	ros::spin();
    222. 	return 0;
    223. }
    224.

    4、准备编译脚本

            上面3个文件其实就是3个程序，所以我们在CMakeLists.txt里面做好三个程序的编译脚本就可以了。需要调试的话，可以添加上-Wall -g选项。

    1. add_executable(slam_tfpub src/slam_tfpub.cpp)
    2. target_link_libraries(slam_tfpub ${catkin_LIBRARIES})
    3. add_dependencies(slam_tfpub beginner_tutorials_generate_messages_cpp)
    4.  
    5. add_executable(slam_laser src/slam_laser.cpp)
    6. target_link_libraries(slam_laser ${catkin_LIBRARIES})
    7. add_dependencies(slam_laser beginner_tutorials_generate_messages_cpp)
    8.  
    9. add_definitions("-Wall -g")
    10.  
    11. add_executable(book_myslam src/book_myslam.cpp)
    12. target_link_libraries(book_myslam ${catkin_LIBRARIES})
    13. add_dependencies(book_myslam beginner_tutorials_generate_messages_cpp)

    5、编译

            编译就很简单了，直接输入catkin_make即可。

    6、构建launch文件

            因为启动的程序比较多，这里可以编写一个myslam.launch文件，使用起来方便一点。脚本文件注意放在launch目录下面。

    1. <launch>
    2. 	<node pkg="beginner_tutorials" type="slam_tfpub" name="tf_pub"/>
    3. 	<node pkg="beginner_tutorials" type="slam_laser" name="laser_pub"/>
    4. 	<node pkg="beginner_tutorials" type="book_myslam" name="myslam"/>
    5. </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上面看到这样的建图效果了，

  • 相关阅读:
    已经有 MESI 协议，为什么还需要 volatile 关键字？
    记一次opencv安装过程
    leetcode347 前 K 个高频元素
    【计算机视觉 | 目标检测】目标检测常用数据集及其介绍（十六）
    Flutter开发实战之Google Play 最佳应用程序开发者分享Flutter经验与技巧
    Redis渐进式rehash小疑问
    游戏开发丨基于Tkinter的五子棋小游戏
    Java使用javah命令：‘javah‘ 不是内部或外部命令，也不是可运行的程序或批处理文件。
    【指针内功修炼】字符指针 + 指针数组 + 数组指针 + 指针参数（一）
    【C++】STL——list模拟实现
  • 原文地址：https://blog.csdn.net/feixiaoxing/article/details/133938333