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

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

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.mzph.cn/pingmian/12176.shtml

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

相关文章

Ruby on Rails（Rails）

Ruby on Rails（Rails）

Ruby on Rails&#xff08;通常简称为 Rails&#xff09;无疑是Web开发领域中最受欢迎的Ruby框架之一。它深受开发者喜爱&#xff0c;原因在于其遵循的“约定优于配置”&#xff08;Convention over Configuration, CoC&#xff09;和“不重复自己”&#xff08;Dont Repeat Yo…
阅读更多...
财务分析软件怎么选？看这篇就够了

财务分析软件怎么选？看这篇就够了

怎么才能选到一款真正能够用于财务分析的软件&#xff1f;且是能够又快又直观地将财务数据情况分析清楚&#xff0c;从不同角度灵活分析财务数据背后的关联的软件&#xff0c;简单来说那得选一款BI财务分析软件。具体哪款BI财务分析软件更合适&#xff1f;那就要从以下几个方面…
阅读更多...
如何使用EasyExcel导入百万数据

如何使用EasyExcel导入百万数据

摘要&#xff1a; 本文将详细探讨如何利用EasyExcel库&#xff0c;以及结合Java编程&#xff0c;高效地导入大规模数据至应用程序中。我们将逐步介绍导入流程、代码实现细节&#xff0c;并提供性能优化建议&#xff0c;旨在帮助读者在处理百万级别数据时&#xff0c;提高效率与…
阅读更多...
VSCOde安装node.js环境

VSCOde安装node.js环境

Visual Studio Code (VSCode) 本身并不直接安装 Node.js 环境&#xff0c;但 VSCode 提供了对 Node.js 开发的优秀支持&#xff0c;包括语法高亮、智能感知、调试等特性。要使用 VSCode 进行 Node.js 开发&#xff0c;你需要先独立安装 Node.js。以下是安装 Node.js 的步骤&…
阅读更多...
几款打工人必备的AI绘图软件工具分享给你!

几款打工人必备的AI绘图软件工具分享给你!

随着人工智能技术的不断进步&#xff0c;AI绘图软件工具成为了设计师和打工人提升工作效率的得力助手。这些工具不仅能够帮助我们快速完成复杂的绘图任务&#xff0c;还能激发我们的创意灵感。在本文中&#xff0c;我将为大家介绍几款打工人必备的AI绘图软件工具&#xff0c;其…
阅读更多...
历史影像的下载办法总结

历史影像的下载办法总结

最近想要下黄河口的历史影像&#xff0c;试验了几个办法&#xff1a; 1&#xff09;参考文献1中的办法&#xff0c;用Global Mapper下载World Imagery Wayback网站的历史数据&#xff0c;能下载从2014年至现在的&#xff1b; 2&#xff09;参考文献1中的办法&#xff0c;用SA…
阅读更多...
vue3中教你如何使用指令解决文本的溢出提示

vue3中教你如何使用指令解决文本的溢出提示

在我们项目开发中,经常会有超长文本溢出提示,未溢出则不提示的场景。 在项目开发中遇到了比较复杂的场景,在一个组织树中,我们使用了el-tree来显示组织树,文字长度不一,太长的显示不全&#xff0c;刚开始我们使用滚动条&#xff0c;结果不好看 后来我们就直接再el-tree中添加el…
阅读更多...
iOS copy的正确姿势

iOS copy的正确姿势

参考文章 知识准备&#xff08;理解堆栈&#xff09; 堆区&#xff1a; 程序员管理 若程序员不释放&#xff0c;由os释放不同于数据结构中的堆&#xff0c;堆区的结构类似于数据结构中的链表栈区&#xff1a; 由编译器来管理 存放函数参数值&#xff0c;局部变量的值等结构类似…
阅读更多...
Facebook消息群发脚本的制作思路!

Facebook消息群发脚本的制作思路!

在数字化社交日益盛行的今天&#xff0c;Facebook作为全球最大的社交平台之一&#xff0c;为企业和个人提供了广阔的交流与合作空间。 然而&#xff0c;手动向大量用户发送消息既耗时又低效&#xff0c;因此&#xff0c;开发一款能够自动群发消息的脚本成为了许多人的需求&…
阅读更多...
MySQL数据库的详解（2）

MySQL数据库的详解（2）

1、一对多 定义 案例&#xff1a;员工表为子表&#xff0c;部门表为父表一对多关系实现&#xff1a;在数据库表中多的一方&#xff0c;添加字段&#xff0c;来关联一的一方的主键。 外键语法 -- 创建表时指定 create table 表名(字段名 数据类型,...[constraint] [外键…
阅读更多...
【正点原子Linux连载】 第四十二章 多点电容触摸屏实验摘自【正点原子】ATK-DLRK3568嵌入式Linux驱动开发指南

【正点原子Linux连载】 第四十二章 多点电容触摸屏实验摘自【正点原子】ATK-DLRK3568嵌入式Linux驱动开发指南

1&#xff09;实验平台&#xff1a;正点原子ATK-DLRK3568开发板 2&#xff09;平台购买地址&#xff1a;https://detail.tmall.com/item.htm?id731866264428 3&#xff09;全套实验源码手册视频下载地址&#xff1a; http://www.openedv.com/docs/boards/xiaoxitongban 第四十…
阅读更多...
汇昌联信：拼多多网店该如何开店?

汇昌联信：拼多多网店该如何开店?

拼多多网店的开设流程并不复杂&#xff0c;但需要细心和耐心去完成每一步。下面将详细阐述如何开设一家拼多多网店。 一、选择商品与定位 开设拼多多网店的第一步是确定你要销售的商品类型&#xff0c;这决定了你的目标客户群体和市场定位。你需要了解这些商品的市场需求、竞争…
阅读更多...
面向对象 06：三大特性之——多态，多态的基本概念和相关使用，关键字 instanceof，以及对象间的类型转换

面向对象 06：三大特性之——多态，多态的基本概念和相关使用，关键字 instanceof，以及对象间的类型转换

一、前言 记录时间 [2024-05-14] 系列文章简摘&#xff1a; 面向对象 02&#xff1a;区分面向过程与面向对象&#xff0c;类和对象的关系 面向对象 03&#xff1a;类与对象的创建、初始化和使用&#xff0c;通过 new 关键字调用构造方法&#xff0c;以及创建对象过程的内存分析…
阅读更多...
CAST: Cross-Attention in Space and Time for Video Action Recognition

CAST: Cross-Attention in Space and Time for Video Action Recognition

标题&#xff1a;CAST: 时空交叉注意力网络用于视频动作识别 原文链接&#xff1a;2311.18825v1 (arxiv.org)https://arxiv.org/pdf/2311.18825v1 源码链接&#xff1a;GitHub - KHU-VLL/CASThttps://github.com/KHU-VLL/CAST 发表&#xff1a;NeurIPS-2023&#xff08;CCF A…
阅读更多...
【打字】打字训练之针对性键盘区域练习

【打字】打字训练之针对性键盘区域练习

本文章的核心点是&#xff1a;使用代码生成自己想要训练的键位的词汇&#xff0c;然后导入到打字软件针对性练习 一个程序员突然想纠正打字习惯源于腱鞘炎&#xff0c;虽然使用双拼打字已经不慢了&#xff0c;但是姿势不是很正确&#xff0c;导致了腱鞘炎。 所以想着好好纠正指…
阅读更多...
Golang——http包

Golang——http包

Go语言内置的net/http包十分优秀&#xff0c;提供了http客户端和服务器的实现。 超文本传输协议(HTTP&#xff0c;HyperText Transfer Protocol)是互联网上应用最为广泛的一种网络传输协议&#xff0c;所有的www文件都必须遵循这个标准。设计HTTP最初的目的是为了提供一种发布和…
阅读更多...
消息队列选型

消息队列选型

一、要解决的问题 1.1 异步 分析&#xff1a; 需要根据场景来判断。若整体链路的逻辑中&#xff0c;某些逻辑是不需要强实时的&#xff0c;滞后一段时间是允许的&#xff0c;同时又不会对用户带来不好的体验&#xff0c;那么可以使用MQ完成异步操作。 例如&#xff1a;秒杀场…
阅读更多...
解锁客户需求密码：银行业数据分析在业务决策中的关键作用

解锁客户需求密码：银行业数据分析在业务决策中的关键作用

一、引言 在数字化和大数据时代的浪潮下&#xff0c;银行业正经历着前所未有的变革。作为数据分析领域的资深专家&#xff0c;我深知数据分析在银行业务发展中的重要性和价值。本文将从银行业数据分析的角度出发&#xff0c;深入探讨相关业务场景下的数据分析应用&#xff0c;…
阅读更多...
UVa11419 SAM I AM

UVa11419 SAM I AM

UVa11419 SAM I AM 题目链接题意分析AC 代码 题目链接 UVA - 11419 SAM I AM 题意 给出一个 RC 大小的网格&#xff0c;网格上面放了一些目标。可以在网格外发射子弹&#xff0c;子弹会沿着垂直或者水平方向飞行&#xff0c;并且打掉飞行路径上的所有目标&#xff0c;如下图所…
阅读更多...
Java 环境变量未生效

Java 环境变量未生效

在配置 Java 环境变量的时候&#xff0c;有可能会出现修改了JDK的路径&#xff0c;但是Java的环境变量没有相应切换的情况。比如&#xff1a; 但此时在控制台使用java -version命令输出的JDK版本不是新配置的版本&#xff0c;依然是之前的&#xff0c;甚至提示找不到java命令&a…
阅读更多...
最新文章

Copyright @ 2022~2023