wp网站开发,个人小程序开发流程,室内设计难学吗,网站建设添加视频【 声明#xff1a;版权所有#xff0c;欢迎转载#xff0c;请勿用于商业用途。 联系信箱#xff1a;feixiaoxing 163.com】 我们学习了很多的开源包#xff0c;比如hector、gmapping。但其实我们也可以自己编写一个slam包。这么做最大的好处#xff0c;主要还是可以帮助…【 声明版权所有欢迎转载请勿用于商业用途。 联系信箱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.hTfMove::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.hclass 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.hLaserScanPub::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_.advertisesensor_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.hppusing 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();vectorMapPoint 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;vectorMapPoint endpoints;MapPoint endpoint;vectorMapPoint 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.hMySlam::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(vectorMapPoint::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(vectorMapPoint::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 ;
}vectorMapPoint MySlam::bresenham(int x0, int y0, int x1, int y1)
{vectorMapPoint 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 yb;else x xa;h h - 2*dx;}if(flag 0) x xa;else y yb;h h 2*dy;}return pp;
}void MySlam::mappub_init()
{mappub_ nh_.advertisenav_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目录下面。
launchnode pkgbeginner_tutorials typeslam_tfpub nametf_pub/node pkgbeginner_tutorials typeslam_laser namelaser_pub/node pkgbeginner_tutorials typebook_myslam namemyslam/
/launch7、实验步骤 实验步骤稍微复杂一点主要分成四步。第一打开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上面看到这样的建图效果了
