花了不少时间看完了障碍物地图的大致思路,这里简单根据前面的思路来写一个简易版的障碍物地图。
1.订阅一张地图
首先,我们需要一张静态地图作为原始数据,这个我们可以订阅当前的map来获取:
void map_test1::MapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map) { map_origin = map->info.origin; ROS_INFO("map_origin.x:%f,y:%f",map_origin.position.x,map_origin.position.y); map_resolution = map->info.resolution; ROS_INFO("map_resolution:%f",map_resolution); map_width = map->info.width; map_height = map->info.height; ROS_INFO("map_width:%d",map_width); ROS_INFO("map_height:%d",map_height); raw_data.clear(); for(int i=0;i<map->data.size();i++) { raw_data.push_back(map->data[i]); } first_receive = true; ROS_INFO("get raw map"); map_sub.shutdown(); }
这里代码的处理比较简单,只是简单的存储了当前地图的基本信息数据,以给到后续代码使用。
2.订阅激光数据
第一部分我们获取到的只是最基本的原始地图数据,然后我们需要将障碍物添加到地图中去,也就是当前的激光点云,在原代码中,对于来自传感器的点云是存储了一定时间内的,这里我们按照原代码的思路也需要存储一定时间下的点云:
-
void map_test1::ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in) { sensor_msgs::PointCloud mapcloud; if(scan_in->header.frame_id.find("laser")==string::npos) return; if(!tf_listener.waitForTransform( scan_in->header.frame_id, "/map", scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment), ros::Duration(1))){ //ROS_INFO("timestamp error"); return; } try { projector_.transformLaserScanToPointCloud("/map",*scan_in,mapcloud,tf_listener); } catch(const std::exception& e) { ROS_ERROR("%s", e.what()); } mapcloud.header.frame_id = scan_in->header.frame_id; tf::StampedTransform transform; try{ tf_listener.lookupTransform("/map", "/base_link", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); return; } Obstacle obstacle_cloud; obstacle_cloud.origin_.x = transform.getOrigin().getX(); obstacle_cloud.origin_.y = transform.getOrigin().getY(); obstacle_cloud.obstacle_range_ = 25.0; obstacle_cloud.cloud_.points = mapcloud.points; //processCloud(mapcloud); double now_time = ros::Time::now().toSec(); laser_points.insert(std::make_pair(now_time, obstacle_cloud)); }
简单解析一下上述代码,首先第一部分是调用的ROS下的开源包transformLaserScanToPointCloud将激光点云在传感器坐标系转换到map坐标系下,然后同步存储该时刻下的机器人位姿(参考Observation类函数的形式),最后统一存储到obstacle_cloud下。注意这里我没有使用类函数的形式去保存,因为代码写的比较简单,直接使用了结构体进行的数据存储,然后再使用unordered_map的格式存储的一系列数据。通过这种方式实现了一个类似于Observation类以及ObservationBuffer的处理方式。
3.删除时间过久的数据
对于那些超过一定时间的点云,我们这里也是同样需要对其进行删除的,删除的方式也很简单,直接从unordered_map中删除掉这一条数据就可以了:
void map_test1::DeletePoint() { if (laser_points.empty()) { return; } double now_time = ros::Time::now().toSec(); auto ite = laser_points.begin(); while (ite != laser_points.end()) { if (now_time - ite->first > dely_time) { // 删除当前元素,并预取下一个元素的迭代器 laser_points.erase(ite++); } else { ++ite; // 正常移动到下一个元素 } } }
4.将激光点云添加到地图中
这里就用到了上一章中看过的东西了:
void map_test1::AddScanData() { //拷贝原始数据 obstacle_map_data = raw_data; //遍历激光点云 unordered_map<double, Obstacle>::iterator ite; //ROS_INFO("MAP.SIZE:%zu",laser_points.size()); for (ite = laser_points.begin(); ite != laser_points.end(); ite++) { //ROS_INFO("point.size:%ld",ite->second.cloud_.points.size()); for(int i=0;i<ite->second.cloud_.points.size();i++) { //舍弃地图外的点 if(!InMap(ite->second.cloud_.points[i].x,ite->second.cloud_.points[i].y)) { continue; } //舍弃距离过远的点 if(sqrt((ite->second.cloud_.points[i].x-ite->second.origin_.x)*(ite->second.cloud_.points[i].x-ite->second.origin_.x)+ (ite->second.cloud_.points[i].y-ite->second.origin_.y)*(ite->second.cloud_.points[i].y-ite->second.origin_.y))>ite->second.obstacle_range_) { continue; } ModifyMap(ite->second.cloud_.points[i].x,ite->second.cloud_.points[i].y,obstacle_map_data); } } }
首先,我们需要遍历之前存储的每一帧激光点云中的点,判断这个点是否在地图上:
bool map_test1::InMap(double x,double y) { if(x>(map_origin.position.x+map_resolution*map_width) || x<map_origin.position.x) { //ROS_INFO("point not in map,point position:%f,%f",x,y); //ROS_INFO("map max_x:%f,min_x:%f",map_origin.position.x+map_resolution*map_width,map_origin.position.x); return false; } else if(y>(map_origin.position.y+map_resolution*map_height) || y<map_origin.position.y) { //ROS_INFO("point not in map,point position:%f,%f",x,y); //ROS_INFO("map max_y:%f,min_y:%f",map_origin.position.y+map_resolution*map_height,map_origin.position.y); return false; } return true; }
对于那些不在地图中的数据我们就没有必要再进行后续的处理了。然后我们根据之前设置的obstacle_range_参数判断这个点到当时机器人本体所在位置的距离是否足够近,由此排除掉一些非常远的点。
//舍弃距离过远的点 if(sqrt((ite->second.cloud_.points[i].x-ite->second.origin_.x)*(ite->second.cloud_.points[i].x-ite->second.origin_.x)+ (ite->second.cloud_.points[i].y-ite->second.origin_.y)*(ite->second.cloud_.points[i].y-ite->second.origin_.y))>ite->second.obstacle_range_) { continue; }
然后,我们就可以对剩下的点调用ModifyMap函数修改对应点所在的地图栅格值:
void map_test1::ModifyMap(double x,double y,std::vector<int> &map) { int mx,my; if(!worldToMap(x,y,mx,my)) { return; } map[my * map_width + mx] = 100; }
注意这里调用了worldToMap是根据源代码来写的,输入地图点的坐标返回的是栅格地图的XY索引。最后转化成一维坐标并修改对应的值:
bool map_test1::worldToMap(double wx, double wy, int& mx, int& my) { if (wx < map_origin.position.x || wy < map_origin.position.y) return false; mx = (int)((wx - map_origin.position.x) / map_resolution); my = (int)((wy - map_origin.position.y) / map_resolution); if (mx < map_width && my < map_height) return true; return false; }
到此,一个简单的障碍物地图就差不多实现了,运行这个代码并给定一些地图以及激光数据,我们大概就能得到类似于这样子的情况:
原始地图:
加入障碍物点云:
上述图片中是保留了2s内的激光点云的,所以在运动的时候会看到边界会显得更加粗一点,部分动态障碍物还存在托尾。
放一段动图:
栅格地图、障碍物地图与膨胀地图(障碍物地图(三)写一张障碍物地图)_栅格地图的膨胀处理-CSDN博客
简单障碍物地图实现
标签:障碍物,一张,map,ite,地图,_.,second,points,cloud From: https://www.cnblogs.com/Gaowaly/p/18317761