栅格地图、障碍物地图与膨胀地图（障碍物地图（三）写一张障碍物地图）

news2025/4/6 9:40:25

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

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内的激光点云的，所以在运动的时候会看到边界会显得更加粗一点，部分动态障碍物还存在托尾。
放一段动图：

简单障碍物地图实现

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1675152.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

一文读懂 Pencil 积分，打开 Pencils Protocol 生态权益大门

一文读懂 Pencil 积分，打开 Pencils Protocol 生态权益大门

近日&#xff0c;Scroll 生态项目 Penpad 近期将品牌全新升级为 Pencils Protocol &#xff0c;在升级后&#xff0c;其从一个 Scroll 生态的原生 LaunchPad 平台进一步拓展为集 Staking、Vault 以及 Shop 等功能于一体的全新生态。全新的品牌升级不仅让 Pencils Protocol 生态…
阅读更多...
鸿蒙内核源码分析 (内核启动篇) | 从汇编到 main ()

鸿蒙内核源码分析 (内核启动篇) | 从汇编到 main ()

这应该是系列篇最难写的一篇&#xff0c;全是汇编代码&#xff0c;需大量的底层知识&#xff0c;涉及协处理器&#xff0c;内核镜像重定位&#xff0c;创建内核映射表&#xff0c;初始化 CPU 模式栈&#xff0c;热启动&#xff0c;到最后熟悉的 main() 。 内核入口 在链接文件…
阅读更多...
nginx配置域名与IP访问服务冲突问题

nginx配置域名与IP访问服务冲突问题

在最近的一次开发中遇到一个问题&#xff0c;我在云服务器上部署了两个服务&#xff0c;A服务和B服务&#xff0c; A服务在服务器中用的端口是80端口&#xff0c;所以我在浏览器访问的地址就是 B服务在服务器中用的是9818端口&#xff0c;所以我在浏览器访问的是 现在我给B服务…
阅读更多...
Network Compression

Network Compression

听课&#xff08;李宏毅老师的&#xff09;笔记&#xff0c;方便梳理框架&#xff0c;以作复习之用。本节课主要讲了Network Compression&#xff0c;包括为什么要压缩&#xff0c;压缩的主要手段&#xff08;pruning,knowledge distillation,parameter quantization,architect…
阅读更多...
Linux线程(四) 生产者消费者模型

Linux线程(四) 生产者消费者模型

目录 一、什么是生产者消费者模型 基本概念 优点以及应用场景 二、 基于阻塞队列的生产者消费者模型 三、POSIX信号量 四、基于环形队列的生产消费模型 一、什么是生产者消费者模型 Linux下的生产者消费者模型是一种经典的多线程或多进程编程设计模式&#xff0c;它用于解…
阅读更多...
2024年3月 电子学会青少年等级考试机器人理论真题六级

2024年3月 电子学会青少年等级考试机器人理论真题六级

202403 青少年等级考试机器人理论真题六级 第 1 题 下列选项中&#xff0c;属于URL的是&#xff1f;&#xff08; &#xff09; A&#xff1a;192.168.1.10 B&#xff1a;www.baidu.com C&#xff1a;http://www.kpcb.org.cn/h-col-147.html D&#xff1a;fe80::7998:ffc8…
阅读更多...
【MIT6.S081】Lab7: Multithreading(详细解答版)

【MIT6.S081】Lab7: Multithreading(详细解答版)

实验内容网址：https://xv6.dgs.zone/labs/requirements/lab7.html 本实验的代码分支：https://gitee.com/dragonlalala/xv6-labs-2020/tree/thread2/ Uthread: switching between threads 关键点：线程切换、swtch 思路： 本实验完成的任务为用户级线程系统设计上下文切换机制…
阅读更多...
x264 帧类型代价计算原理：slicetype_frame_cost 函数分析

x264 帧类型代价计算原理：slicetype_frame_cost 函数分析

slicetype_frame_cost 函数 函数功能 这个函数的核心是计算编码一系列帧（从 p0 到p1，以 b 为当前帧）的代价 cost，并根据这个代价 cost来辅助帧类型决策。它考虑了运动搜索的结果、帧间和帧内预测的成本，并且可以并行处理以提高效率。该函数在帧类型决策、MBtree 分析、场…
阅读更多...
有一个21年的前端vue项目,死活安不上依赖

有一个21年的前端vue项目,死活安不上依赖

在公司开发的时候遇到的一个很玄幻的问题,这个项目是21年开发的,现在我是24年中途二开增加新功能 这个项目经过多人之手,现在已经出现了问题------项目依赖安不上,我能启动完全是因为在23年的时候写这个项目的时候将依赖费九牛二虎之力下载好后打成了压缩包发给另外一个安不上依…
阅读更多...
【Java学习笔记10 Java Web 应用——JSP

【Java学习笔记10 Java Web 应用——JSP

JSP(Java Script Pages)技术是一种网站开发技术&#xff0c;可以让Web开发人员快速、高效的开发出易于维护的动态网页。使用JSP技术开发的Web应用程序具有跨平台性&#xff0c;不需要修改程序&#xff0c;发布后即可在Windows、Linux等不同的操作系统中运行。 10.1 JSP技术概述…
阅读更多...
【JavaWeb】前后端分离SpringBoot项目快速排错指南

【JavaWeb】前后端分离SpringBoot项目快速排错指南

1 发起业务请求 打开浏览器开发者工具&#xff0c;同时显示网络&#xff08;Internet&#xff09;和控制台&#xff08;console&#xff09; 接着&#xff0c;清空控制台和网络的内容&#xff0c;如下图 然后&#xff0c;点击你的业务按钮&#xff0c;发起请求。 首先看控制台…
阅读更多...
nginx 配置域名SSL证书HTTPS服务

nginx 配置域名SSL证书HTTPS服务

下载 上传根目录 /home/wwwroot/xx.com/ssl 从新执行 添加域名命令 选择添加SSL SSL Certificate file: 填写 完整目录 PEM文件地址 SSL Certificate Key file:填写 完整目录 key文件地址
阅读更多...
OmniDrive：具有 3D 感知推理和规划功能的自动驾驶整体 LLM-智体框架

OmniDrive：具有 3D 感知推理和规划功能的自动驾驶整体 LLM-智体框架

24年5月北理工、Nvidia和华中科大的论文“OmniDrive&#xff1a;A Holistic LLM-Agent Framework for Autonomous Driving with 3D Perception Reasoning and Planning”。 多模态大语言模型&#xff08;MLLMs&#xff09;的进展导致了对基于LLM的自动驾驶的兴趣不断增长&…
阅读更多...
QT状态机10-QKeyEventTransition和QMouseEventTransition的使用

QT状态机10-QKeyEventTransition和QMouseEventTransition的使用

1、QMouseEventTransition的使用 首先明白 QMouseEventTransition 继承自 QEventTransition类。 关于QEventTransition类的使用，可参考 QT状态机9-QEventTransition和QSignalTransition的使用 回顾 QT状态机9-QEventTransition和QSignalTransition的使用 中的状态切换代码，如…
阅读更多...
NSS【web】刷题

NSS【web】刷题

[SWPUCTF 2021 新生赛]jicao 类型&#xff1a;PHP、代码审计、RCE 主要知识点&#xff1a;json_decode()函数 json_decode()&#xff1a;对JSON字符串解码&#xff0c;转换为php变量 用法&#xff1a; <?php $json {"ctf":"web","question"…
阅读更多...
《Fundamentals of Power Electronics》——阻抗和传递函数的图解构造

《Fundamentals of Power Electronics》——阻抗和传递函数的图解构造

通常&#xff0c;我们可以通过观察画出近似的波德图&#xff0c;而不需要大量杂乱的代数和不可避免的相关代数错误。使用这种方法可以对电路的工作原理有很大的了解。在不同频率下&#xff0c;哪些元件主导电路响应变得很清楚&#xff0c;因此合适的近似变得很明显。可以直接得…
阅读更多...
JAVA毕业设计141—基于Java+Springboot+Vue的物业管理系统(源代码+数据库)

JAVA毕业设计141—基于Java+Springboot+Vue的物业管理系统(源代码+数据库)

毕设所有选题&#xff1a; https://blog.csdn.net/2303_76227485/article/details/131104075 基于JavaSpringbootVue的物业管理系统(源代码数据库)141 一、系统介绍 本项目前后端分离&#xff0c;分为管理员、员工、用户三种角色(角色权限可自行分配) 1、用户&#xff1a; …
阅读更多...
《看漫画学C++》背后的故事6：死循环

《看漫画学C++》背后的故事6：死循环

C 中的死循环是指一个循环结构没有终止条件&#xff0c;导致程序永远无法跳出该循环&#xff0c;从而陷入无限循环的状态。这种情况通常是由于逻辑错误或编程错误导致的。 在《看漫画学C》中我们用这样一副漫画描述死循环。 购书链接&#xff1a;https://item.jd.com/144188…
阅读更多...
AQS应用--CountDownLatch

AQS应用--CountDownLatch

一、是什么 顾名思义&#xff0c;Latch是门闩的意思&#xff0c;把到达门闩的线程都阻塞住&#xff0c;CountDown是减少计数的意思。 所以CountDownLatch是当每个线程到达某个状态就将计数减一&#xff0c;计数为0时所有被阻塞线程全部被唤醒。 二、内部实现 CountDownLatch…
阅读更多...
[牛客网]——C语言刷题day2

[牛客网]——C语言刷题day2

答案&#xff1a;B 解析&#xff1a; char *p[10] 是指针数组,数组里存放了10个指针,在64位系统下指针占8个字节,所以sizeof(p) 10 * 8 80. char (*p1)[10]是数组指针,p1是一个指向存放10个char类型的数组的指针,所以sizeof(p1) 8. 答案&#xff1a;B 解析&#xff1a…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023