【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：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上面看到这样的建图效果了，
