rm::nav::Costmap类 参考

二维分层代价地图 更多...

#include <rmvl/nav/map.hpp>

rm::nav::Costmap 的协作图:

Public 成员函数
	Costmap ()
	构造空代价地图
	Costmap (const GridMap &static_map, CostmapOptions options={})
	使用静态占据栅格构造代价地图
MapStatus	reset (const GridMap &static_map, CostmapOptions options={})
	重建几何信息、静态层和空白局部障碍层
MapStatus	setStaticMap (const GridMap &static_map)
	使用几何信息一致的新地图更新静态层
bool	valid () const noexcept
const CostmapOptions &	options () const noexcept
uint32_t	width () const noexcept
uint32_t	height () const noexcept
std::string_view	frameId () const noexcept
std::optional< Cell >	worldToMap (double world_x, double world_y) const noexcept
	将世界坐标转换为栅格坐标
std::optional< msg::Point >	mapToWorld (uint32_t x, uint32_t y) const noexcept
	获取栅格中心的世界坐标
MapStatus	markObstacle (Cell cell) noexcept
	在局部障碍层标记一个栅格障碍
MapStatus	markObstacle (double world_x, double world_y) noexcept
	在局部障碍层标记一个世界坐标障碍
MapStatus	clearObstacles () noexcept
MapStatus	clearRay (double start_x, double start_y, double end_x, double end_y)
	清除局部障碍层中的一条世界坐标射线
void	updateCosts ()
	合并静态层和局部障碍层，并根据配置重新计算膨胀代价
std::optional< uint8_t >	at (uint32_t x, uint32_t y) const noexcept
	获取主代价地图栅格值
bool	collides (const std::vector< msg::Point > &footprint, const msg::Pose &pose) const
	检测机器人 footprint 是否发生碰撞
msg::OccupancyGrid	message () const
	将当前主代价地图导出为 OccupancyGrid

详细描述

二维分层代价地图

内部维护静态层和局部障碍层，updateCosts() 将两层合并后执行障碍膨胀。 局部障碍支持单点标记和射线清除，适合批量接收传感器观测后统一更新主代价地图。

注解

该类不提供内部同步。跨线程访问时由调用方加锁。

构造及析构函数说明

Costmap() [1/2]

rm::nav::Costmap::Costmap

(

)

构造空代价地图

Costmap() [2/2]

rm::nav::Costmap::Costmap ( const GridMap & static_map, CostmapOptions options = {} )

explicit

使用静态占据栅格构造代价地图

参数

[in]	static_map	静态占据栅格
[in]	options	代价地图配置

备注

输入无效时构造为空代价地图，可使用 valid() 检查。

函数调用图:

成员函数说明

at()

std::optional< uint8_t > rm::nav::Costmap::at ( uint32_t x, uint32_t y ) const

noexcept

获取主代价地图栅格值

参数

[in]	x	栅格横向索引
[in]	y	栅格纵向索引

返回

栅格有效时返回代价值，否则返回 std::nullopt

函数调用图:

clearObstacles()

MapStatus rm::nav::Costmap::clearObstacles ( )

noexcept

返回

清空局部障碍层的操作状态

函数调用图:

clearRay()

MapStatus rm::nav::Costmap::clearRay	(	double	start_x,
		double	start_y,
		double	end_x,
		double	end_y )

清除局部障碍层中的一条世界坐标射线

射线会裁剪到地图边界，且不修改静态层。

参数

[in]	start_x	射线起点的世界坐标 X，单位为米
[in]	start_y	射线起点的世界坐标 Y，单位为米
[in]	end_x	射线终点的世界坐标 X，单位为米
[in]	end_y	射线终点的世界坐标 Y，单位为米

返回

地图操作状态

函数调用图:

collides()

bool rm::nav::Costmap::collides	(	const std::vector< msg::Point > &	footprint,
		const msg::Pose &	pose ) const

检测机器人 footprint 是否发生碰撞

参数

[in]	footprint	机器人局部坐标系中的闭合多边形顶点，无需重复首顶点
[in]	pose	机器人在地图坐标系中的平面位姿

返回

footprint 无效、超出地图，或覆盖致命/内切/按配置判定的未知栅格时返回 true

函数调用图:

frameId()

std::string_view rm::nav::Costmap::frameId ( ) const

noexcept

返回

代价地图所属坐标系，无效地图返回空字符串

函数调用图:

height()

uint32_t rm::nav::Costmap::height ( ) const

noexcept

返回

地图高度，单位为格

函数调用图:

mapToWorld()

std::optional< msg::Point > rm::nav::Costmap::mapToWorld ( uint32_t x, uint32_t y ) const

noexcept

获取栅格中心的世界坐标

参数

[in]	x	栅格横向索引
[in]	y	栅格纵向索引

返回

栅格有效时返回世界坐标，否则返回 std::nullopt

函数调用图:

markObstacle() [1/2]

MapStatus rm::nav::Costmap::markObstacle ( Cell cell )

noexcept

在局部障碍层标记一个栅格障碍

参数

[in]

cell

障碍所在的离散栅格

返回

地图操作状态

函数调用图:

markObstacle() [2/2]

MapStatus rm::nav::Costmap::markObstacle ( double world_x, double world_y )

noexcept

在局部障碍层标记一个世界坐标障碍

参数

[in]	world_x	障碍的世界坐标 X，单位为米
[in]	world_y	障碍的世界坐标 Y，单位为米

返回

地图操作状态

函数调用图:

message()

msg::OccupancyGrid rm::nav::Costmap::message

(

)

const

将当前主代价地图导出为 OccupancyGrid

返回

有效时返回转换后的全量占据栅格，否则返回空消息

注解

中间代价值会缩放至 [1, 99]。

函数调用图:

options()

const CostmapOptions & rm::nav::Costmap::options ( ) const

noexcept

返回

当前代价地图配置的常引用

函数调用图:

reset()

MapStatus rm::nav::Costmap::reset	(	const GridMap &	static_map,
		CostmapOptions	options = {} )

重建几何信息、静态层和空白局部障碍层

参数

[in]	static_map	静态占据栅格
[in]	options	代价地图配置

返回

地图操作状态

备注

操作失败时保持原代价地图不变。

函数调用图:

setStaticMap()

MapStatus rm::nav::Costmap::setStaticMap

(

const GridMap &

static_map

)

使用几何信息一致的新地图更新静态层

参数

[in]

static_map

新的静态占据栅格

返回

地图操作状态

注解

新地图的坐标系、尺寸、分辨率和原点必须与当前地图一致。

updateCosts()

void rm::nav::Costmap::updateCosts

(

)

合并静态层和局部障碍层，并根据配置重新计算膨胀代价

注解

markObstacle()、clearObstacles() 和 clearRay() 只修改局部障碍层。调用本函数前， at()、collides() 和 message() 仍读取上一次合成的主代价地图。

函数调用图:

valid()

bool rm::nav::Costmap::valid ( ) const

noexcept

返回

代价地图是否有效

width()

uint32_t rm::nav::Costmap::width ( ) const

noexcept

返回

地图宽度，单位为格

函数调用图:

worldToMap()

std::optional< Cell > rm::nav::Costmap::worldToMap ( double world_x, double world_y ) const

noexcept

将世界坐标转换为栅格坐标

参数

[in]	world_x	世界坐标 X，单位为米
[in]	world_y	世界坐标 Y，单位为米

返回

坐标位于地图内时返回离散栅格，否则返回 std::nullopt

函数调用图: