首页 > 其他分享 >障碍物地图(三)写一张障碍物地图

障碍物地图(三)写一张障碍物地图

时间:2024-07-23 10:41:45浏览次数:12  
标签:障碍物 一张 map ite 地图 _. second points cloud

花了不少时间看完了障碍物地图的大致思路,这里简单根据前面的思路来写一个简易版的障碍物地图。

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

相关文章

  • 障碍物地图(二)
    上一篇大致看完了障碍物地图的初始化内容以及对于传感器数据的处理,我们知道在该部分算法维护了一个ObservationBuffer,其中存储了一段时间内的点云数据。每次新的数据进来后,还会根据设定的时间参数observation_keep_time抛弃比较久远的障碍物点云。但是在看的过程中,我们也产生了一......
  • 障碍物地图
    前面我们看完了栅格地图,知道了地图的基本数据结构,今天进一步的看一下障碍物地图。障碍物地图的存在更多是用于局部路径规划中所使用,因为大部分时候全局地图都是比较大的,那么很难保证其始终是一成不变的,所以如果我们只是按照全局地图进行路径规划,很可能会出现原有的地图中没有障碍......
  • 栅格地图
    在ROS中,地图是非常基本的元素,特别对于2D激光SLAM而言,栅格地图可以说是必不可少的元素。机器人在需要前往目标点时,需要在栅格地图中找到一条合适的路径从当前点到达目标点,这部分内容在move_base中有了详细的接口,可以直接调用并返回路径。但是作为一名工程师,不仅要知其然更要知其所......
  • openlayers在地图上使用Overlay渲染图标无法操作地图问题
    ol对于在地图上渲染图标,并且图标可以随着地图的缩放层级自适应,跟随地图移动,ol是提供了一个很好用的方法的---overlay代码如下://定义标注对象letlable_div=document.createElement('div')lable_div.className=[s.labelDiv]letoffsetY=0let_classname=''_classn......
  • 全地图商家采集工具
    1.引言地图信息采集软件在商业分析、城市规划和交通管理等领域发挥着重要作用。通过实时采集地图上的商户信息,该软件能够帮助用户快速获取所需的地理数据和商业信息。2.系统功能2.1地图数据采集多平台支持:软件支持百度地图、高德地图、腾讯地图等主流电子地图服务。......
  • 全地图商家采集工具
    1.引言地图信息采集软件在商业分析、城市规划和交通管理等领域发挥着重要作用。通过实时采集地图上的商户信息,该软件能够帮助用户快速获取所需的地理数据和商业信息。2.系统功能2.1地图数据采集多平台支持:软件支持百度地图、高德地图、腾讯地图等主流电子地图服务。实......
  • 注意!使用高德地图授权但还未缴费的用户,账号将被强制关闭
    这回高德是来真的了,以前三大图商的催收策略是不停地进行电话轰炸,限流、关闭账号、法律起诉等都是停留在口头的告知,并未真正地行动。现在是来真的了,前面的文章我也提到过,高德在今年7月将会出行新的政策,现在已经在其平台公布,且限流、关闭账号、等一些列催......
  • VUE diff 算法:为了直观展示,画了一张图来直观展示
      上图直观展示了Vue的Diff算法流程:3种方式比较根节点:图中左侧的"OldVNode"和右侧的"NewVNode"表示旧的和新的虚拟DOM根节点。箭头表示比较过程,如果根节点不同,直接替换整个节点。比较子节点:当根节点相同时,递归比较子节点。左侧"OldChild1"和"O......
  • GIS地图可视化怎么做?这款免费工具帮你轻松搞定
    GIS地图可视化怎么做?山海鲸可视化这款免费可视化工具帮你轻松搞定。从三维GIS地图可视化需求出发,山海鲸可视化提供了强大的GIS场景编辑功能,包括支持添加倾斜摄影和地形编辑。无论是复杂的地形调整还是细致的倾斜摄影添加,这款工具都能轻松实现。山海鲸可视化是一款非常易用的软件......
  • 使用高德/百度/腾讯SDK,不购买地图商用授权APP无法上架?
    随着国内三大图商联合针对地图位置服务收取5万/年的商业授权费,许多使用三大图商地图或定位sdk的应用开发企业遇到了两难:没有地图商业授权,无法上架!其实大家信息了解有误区,并非都无法上线。目前国内主流的上架商城:华为商城、小米商城、应用宝、vivo、oppo等。上架华为商城通常......