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

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

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.hqwc.cn/news/700220.html

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

相关文章

AuroraFOC使用指南一(STM32F405双路FOC)

AuroraFOC使用指南一(STM32F405双路FOC)

一. 简介 哈喽&#xff0c;感谢各位选择AuroraFOC开发板&#xff0c;在这里将对其进行一个详细的介绍&#xff0c;方便大家使用。并且对提供的工程文件和上位机的操作也进行了详细的说明。 有什么疑问或者好的建议 可以微信联系: WU1356742146 最后再次感谢大家的支持。 Aur…
阅读更多...
选择法（数值排序）（C语言）

选择法（数值排序）（C语言）

一、运行结果&#xff1b; 二、源代码&#xff1b; # define _CRT_SECURE_NO_WARNINGS # include <stdio.h>//声明排序函数sort; void sort(int a[], int n);int main() {//初始化变量值&#xff1b;int i, a[10];//填充数组&#xff1b;printf("请输入10个整数\n&…
阅读更多...
Windows安装Linux子系统

Windows安装Linux子系统

WSL 是 Windows Subsystem for Linux 的简称&#xff0c;意思是 linux 版的 window 子系统。适用于 Linux 的 Windows 子系统可让开发人员按原样运行 GNU/Linux 环境 - 包括大多数命令行工具、实用工具和应用程序 - 且不会产生传统虚拟机或双启动设置开销。 Linux 分发版可以在…
阅读更多...
AI图书推荐：ChatGPT全面指南—用AI帮你更健康、更富有、更智慧

AI图书推荐：ChatGPT全面指南—用AI帮你更健康、更富有、更智慧

你是否在努力改善你的健康&#xff1f; 你是否长期遭受财务困难&#xff1f; 你想丰富你的思想、身体和灵魂吗&#xff1f; 如果是这样&#xff0c;那么这本书就是为你准备的。 《ChatGPT全面指南—用AI帮你更健康、更富有、更智慧》&#xff08;CHATGPT Chronicles AQuick…
阅读更多...
【GD32F470紫藤派使用手册】第十讲 USART-中断串口收发实验

【GD32F470紫藤派使用手册】第十讲 USART-中断串口收发实验

10.1 实验内容 通过本实验主要学习以下内容&#xff1a; 使用中断进行串口收发 10.2 实验原理 10.2.1 串口寄存器介绍 串口有几个非常重要的寄存器需要读者理解。 数据寄存器&#xff08;USART_DATA&#xff09; 该寄存器虽然只有一个&#xff0c;但内部是映射为发送和接…
阅读更多...
供水设备数据采集

供水设备数据采集

随着城市化进程的加快&#xff0c;供水系统作为城市基础设施的重要组成部分&#xff0c;其运行效率和稳定性直接关系到市民的日常生活。在这个信息化、智能化的时代&#xff0c;如何利用先进技术提升供水系统的管理水平&#xff0c;成为了摆在我们面前的重要课题。HiWoo Cloud平…
阅读更多...
ROS2+TurtleBot3+Cartographer+Nav2实现slam建图和导航

ROS2+TurtleBot3+Cartographer+Nav2实现slam建图和导航

0 引言 入门机器人最常见的应用就是slam建图和导航&#xff0c;本文将详细介绍这一流程&#xff0c; 便于初学这快速上手。 首先对需要用到的软件包就行简单介绍。 turtlebot3: 是一个小型的&#xff0c;基于ros的移动机器人。 学习机器人的很多示例程序都是基于turtlebot3。 …
阅读更多...
AIGC数字人视频创作平台，赋能企业常态化制作数字内容营销

AIGC数字人视频创作平台，赋能企业常态化制作数字内容营销

随着数字人技术不断发展&#xff0c;AIGC、元宇宙等相关产业迅速发展&#xff0c;企业通过3D虚拟数字人定制&#xff0c;打造出专属的数字人作为企业与用户沟通的新桥梁。 作为3D、AI数字人技术服务商及方案提供商&#xff0c;广州虚拟动力一直致力于为各领域企业通过3D虚拟数字…
阅读更多...
Fabric实现多GPU运行

Fabric实现多GPU运行

官方的将pytorch转换为fabric简单分为五个步骤&#xff1a; 步骤 1&#xff1a; 在训练代码的开头创建 Fabric 对象 from lightning.fabric import Fabricfabric Fabric() 步骤 2&#xff1a; 如果打算使用多个设备&#xff08;例如多 GPU&#xff09;&#xff0c;就调用…
阅读更多...
搞什么副业可以月入过万？

搞什么副业可以月入过万？

月入过万的副业不是一件容易的事情&#xff0c;它需要你付出很多努力和时间。以下是一些可能能够实现月入过万的副业 1. 自媒体运营 通过开设自己的公众号、博客或YouTube频道&#xff0c;积累一定的粉丝和流量&#xff0c;然后通过广告、赞助、商品销售等方式赚取收入。 2. …
阅读更多...
Python入门系列-02 pip的安装

Python入门系列-02 pip的安装

目录 一、pip介绍二、pip安装检查三、pip安装 一、pip介绍 pip 是 Python 包管理工具&#xff0c;该工具提供了对Python 包的查找、下载、安装、卸载的功能。 二、pip安装检查 你可以通过以下命令来判断是否已安装。 pip --version # Python2.x 版本命令 pip3 --versio…
阅读更多...
Redis分布式缓存

Redis分布式缓存

分布式缓存 引入&#xff1a; 一&#xff1a;持久化&#xff1a; 1.1.RDB持久化&#xff1a; 1.2.AOF文件&#xff1a; 记得关闭RDB&#xff0c;开启AOF。 注意&#xff0c;AOF默认是详细的记录每一条命令&#xff0c;即使是对同一个key的多次修改&#xff0c;RDB只会记录最…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023