北达科他大学( North Dakota State University)cloud_to_map学习

2023-10-21 00:30

本文主要是介绍北达科他大学( North Dakota State University)cloud_to_map学习,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

一、ROS 动态参数

cfg文件:

#!/usr/bin/env python
#ROS动态参数文件
PACKAGE = "cloud_to_map"import roslib;roslib.load_manifest(PACKAGE)
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
#以上代码初始化ROS并导入参数生成器。gen.add("frame", str_t, 0, "Frame the Occupancy Grid should connect to", "map")
gen.add("search_radius", double_t, 0, "Search radius for approximating the normal vectors of a point",0.06, 0.0001, 1)
gen.add("deviation", double_t, 0, "Allowable deviation of the normal vector of a point from the normal vector of the ground plane before the point is considered an obstacle (in Radians)", 0.78539816339, 0, 1.57079632679)
gen.add("buffer", int_t, 0, "Number of points that must register as past the allowable amount of deviation before the corresponding cell is considered an obstacle (modifying search radius is generally more effective)", 0, 0, 100)
gen.add("loop_rate", double_t, 0, "Rate in Hz the node will attempt to run", 10, 0, 1000)
gen.add("cell_resolution", double_t, 0, "Resolution of the Occupancy Grid output (in m/cell)", 0.05, 0, 0.1)"""以上代码为加入不同的参数。其中gen.add(...)格式如下:
gen.add(name, type, level, description, default, min, max)
name: 参数的名称
type: 参数类型
level:一个传递给回调的位掩码
description: 一个描述参数
default: 节点启动的初始值
min: 参数最小值
max: 参数最大值"""exit(gen.generate(PACKAGE, "cloud_to_map", "cloud_to_map_node"))
#生成必要的文件并退出。
#上述的cfg中的代码为python代码。

参数调试:

frame:应该修改为和建图frame一致

search_radius:近似求解点的法向量的搜索半径

deviation:在该点被认为是障碍之前,点的法向量与地平面的法向量的允许偏差(以弧度表示):越小图越密集

buffer:在相应单元被视为障碍之前必须注册为超过允许偏差量的点数(修改搜索半径通常更有效):越大噪点越少

loop_rate:节点运行的频率(HZ)

cell_resolution:输出占据栅格地图的分辨率(m/cell)

只有在检索到参数服务器的参数值时才使用参数服务器的参数,如果服务器没有某个参数则使用程序的默认参数。

在点云地图有明显的上下边界限制(屋顶和地面都是平面的室内)修改buffer效果不错,可以通过修改deviation改变生成的地图的密度。

动态参数调整界面:

 

 

二、初始化栅格地图 initGrid

grid->header.seq = 1;

grid->header.frame_id = param.frame; //参考frame

grid->info.origin.position.z = 0;

grid->info.origin.orientation.w = 1;

grid->info.origin.orientation.x = 0;

grid->info.origin.orientation.y = 0;

grid->info.origin.orientation.z = 0;

OccupancyGrid具体结构在这里。

三、计算时间

四、Populate map with cost values

挨个对比点云中每个点的法向量和地面法向量的偏差deviation,当deviation大于某个值将此点作为障碍。代码如下:

  double deviation = param.deviation;for (size_t i = 0; i < currentPC->size(); i++) //遍历每个点{double x = currentPC->points[i].x;double y = currentPC->points[i].y;double z = cloud_normals->points[i].normal_z;double phi = acos(fabs(z));  //值域 0--phiint xCell, yCell;xCell = (int) ((x - xMin) / cellResolution);//取整后默认按照cellReso将点分配到cellyCell = (int) ((y - yMin) / cellResolution);      if (phi > deviation) countGrid[yCell * xCells + xCell]++;  //统计一个cell中垂直方向满足条件的点数

z = cloud_normals->points[i].normal_z,根据pcl::Normal的定义,当法向量是垂直向上的时候,points[i].normal_z的值是1,acos是0,为函数的最小值。根据acos函数的递减性质,非地面点的值应该都比地面点大。可以设置deviation值,决定障碍物点的阈值。然后统计一个cell中垂直方向满足条件的点数,用做下一步生成Occupancy Grid。

// ---------------------------------
// -----Generate Occupancy Grid-----
// ---------------------------------
void genOccupancyGrid(std::vector<signed char> &ocGrid, std::vector<int> &countGrid, int size) 
{int buf = param.buffer;for (int i = 0; i < size; i++)  //size:xCells * yCells{if (countGrid[i] < buf ) ocGrid[i] = 0;else if (countGrid[i] > buf) ocGrid[i] = 100;else if (countGrid[i] == 0) ocGrid[i] = 0; // TODO Should be -1/* if (countGrid[i] < buf && countGrid[i]>0) ocGrid[i] = 0;else if (countGrid[i] > buf) ocGrid[i] = 100;else if (countGrid[i] == 0) ocGrid[i] = -1; // TODO Should be -1  */    }
}
// -----------------------------------
// -----Update Occupancy Grid Msg-----
// -----------------------------------
void updateGrid(nav_msgs::OccupancyGridPtr grid, double cellRes, int xCells, int yCells,double originX, double originY, std::vector<signed char> *ocGrid) 
{grid->header.seq++;grid->header.stamp.sec = ros::Time::now().sec;grid->header.stamp.nsec = ros::Time::now().nsec;grid->info.map_load_time = ros::Time::now();grid->info.resolution = cellRes;grid->info.width = xCells;grid->info.height = yCells;grid->info.origin.position.x = originX;  //minxgrid->info.origin.position.y = originY;  //minygrid->data = *ocGrid;
}

 

 

这篇关于北达科他大学( North Dakota State University)cloud_to_map学习的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/250696

相关文章

JavaScript中的Map用法完全指南

《JavaScript中的Map用法完全指南》:本文主要介绍JavaScript中Map用法的相关资料,通过实例讲解了Map的创建、常用方法和迭代方式,还探讨了Map与对象的区别,并通过一个例子展... 目录引言1. 创建 Map2. Map 和对象的对比3. Map 的常用方法3.1 set(key, v

Golang中map缩容的实现

《Golang中map缩容的实现》本文主要介绍了Go语言中map的扩缩容机制,包括grow和hashGrow方法的处理,具有一定的参考价值,感兴趣的可以了解一下... 目录基本分析带来的隐患为什么不支持缩容基本分析在 Go 底层源码 src/runtime/map.go 中,扩缩容的处理方法是 grow

Spring Cloud Hystrix原理与注意事项小结

《SpringCloudHystrix原理与注意事项小结》本文介绍了Hystrix的基本概念、工作原理以及其在实际开发中的应用方式,通过对Hystrix的深入学习,开发者可以在分布式系统中实现精细... 目录一、Spring Cloud Hystrix概述和设计目标(一)Spring Cloud Hystr

Spring Boot 3 整合 Spring Cloud Gateway实践过程

《SpringBoot3整合SpringCloudGateway实践过程》本文介绍了如何使用SpringCloudAlibaba2023.0.0.0版本构建一个微服务网关,包括统一路由、限... 目录引子为什么需要微服务网关实践1.统一路由2.限流防刷3.登录鉴权小结引子当前微服务架构已成为中大型系统的标

Spring Cloud LoadBalancer 负载均衡详解

《SpringCloudLoadBalancer负载均衡详解》本文介绍了如何在SpringCloud中使用SpringCloudLoadBalancer实现客户端负载均衡,并详细讲解了轮询策略和... 目录1. 在 idea 上运行多个服务2. 问题引入3. 负载均衡4. Spring Cloud Load

Java深度学习库DJL实现Python的NumPy方式

《Java深度学习库DJL实现Python的NumPy方式》本文介绍了DJL库的背景和基本功能,包括NDArray的创建、数学运算、数据获取和设置等,同时,还展示了如何使用NDArray进行数据预处理... 目录1 NDArray 的背景介绍1.1 架构2 JavaDJL使用2.1 安装DJL2.2 基本操

Go语言利用泛型封装常见的Map操作

《Go语言利用泛型封装常见的Map操作》Go语言在1.18版本中引入了泛型,这是Go语言发展的一个重要里程碑,它极大地增强了语言的表达能力和灵活性,本文将通过泛型实现封装常见的Map操作,感... 目录什么是泛型泛型解决了什么问题Go泛型基于泛型的常见Map操作代码合集总结什么是泛型泛型是一种编程范式,允

Sentinel 断路器在Spring Cloud使用详解

《Sentinel断路器在SpringCloud使用详解》Sentinel是阿里巴巴开源的一款微服务流量控制组件,主要以流量为切入点,从流量路由、流量控制、流量整形、熔断降级、系统自适应过载保护、... 目录Sentinel 介绍同类对比Hystrix:Sentinel:微服务雪崩问题问题原因问题解决方案请

JSON字符串转成java的Map对象详细步骤

《JSON字符串转成java的Map对象详细步骤》:本文主要介绍如何将JSON字符串转换为Java对象的步骤,包括定义Element类、使用Jackson库解析JSON和添加依赖,文中通过代码介绍... 目录步骤 1: 定义 Element 类步骤 2: 使用 Jackson 库解析 jsON步骤 3: 添

Java中List转Map的几种具体实现方式和特点

《Java中List转Map的几种具体实现方式和特点》:本文主要介绍几种常用的List转Map的方式,包括使用for循环遍历、Java8StreamAPI、ApacheCommonsCollect... 目录前言1、使用for循环遍历:2、Java8 Stream API:3、Apache Commons