(02)Cartographer源码无死角解析-(79) ROS服务→子图压缩与服务发送

news/2024/2/27 22:42:41

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示WX官方认证
 

一、前言

通过前几篇博客,了解到 ROS 是如何发布 Cartographer 计算出来的 3D点云地图、子图位姿、Landmark、2D点云数据、tf、机器人tracking frame轨迹发布等。但是却没有讲解2D栅格地图是如何发发布到 Rviz 进行展示的。

这是因为 2D栅格地图 并不是通过话题的方式进行发布的,而是有一个自己的服务。在 node.cc 构造函数中,可以看到其有创建很多的服务:

	// Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中service_servers_.push_back(node_handle_.advertiseService(kSubmapQueryServiceName, &Node::HandleSubmapQuery, this))
......service_servers_.push_back(node_handle_.advertiseService(kReadMetricsServiceName, &Node::HandleReadMetrics, this));

这些服务实际上就是执行对应的回调函数,从命名可以知道,关于子图的服务对应的回调函数就是 Node::HandleSubmapQuery(),其他的服务这里就暂时不进行讲解了。

该函数主要完成子图的发送,但是很明显,这里都是子图,而不是全局地图,那么全局地图又在哪里获取呢?如本人运行的 src/cartographer_ros/cartographer_ros/launch/demo_backpack_2d.launch 文件,其包含了 backpack_2d.launch,而 backpack_2d.launch 存在如下内容:

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

可知,其运行了一个 cartographer_occupancy_grid_node 的可执行文件,且与栅格占用地图有关,其对应的源码位于 src/cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc 文件中,后面会重点对该文件进行分析。当然,第一步是先分析 Node::HandleSubmapQuery()。

二、Node::HandleSubmapQuery()

/*** @brief 获取对应id轨迹的 索引为submap_index 的submap** @param[in] request 获取submap的请求* @param[out] response 服务的回应* @return true: ROS的service只能返回true, 返回false程序会中断*/
bool Node::HandleSubmapQuery(::cartographer_ros_msgs::SubmapQuery::Request& request,::cartographer_ros_msgs::SubmapQuery::Response& response) {absl::MutexLock lock(&mutex_);map_builder_bridge_.HandleSubmapQuery(request, response);return true;
}

该函数就不做过多解释了,其就是上锁然后调用 MapBuilderBridge::HandleSubmapQuery() 这个函数,其就是一个子图查询服务。

三、HandleSubmapQuery()-整体注释

代码不是很复杂,这里先给出整体注释:

/*** @brief 获取对应id轨迹的 索引为 submap_index 的地图的栅格值及其他信息* * @param[in] request 轨迹id与submap的index* @param[in] response 是否成功*/
void MapBuilderBridge::HandleSubmapQuery(cartographer_ros_msgs::SubmapQuery::Request& request,cartographer_ros_msgs::SubmapQuery::Response& response) {cartographer::mapping::proto::SubmapQuery::Response response_proto;cartographer::mapping::SubmapId submap_id{request.trajectory_id,request.submap_index};// 获取压缩后的地图数据const std::string error =map_builder_->SubmapToProto(submap_id, &response_proto);if (!error.empty()) {LOG(ERROR) << error;response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;response.status.message = error;return;}response.submap_version = response_proto.submap_version();// 将response_proto中的地图栅格值存入到response中for (const auto& texture_proto : response_proto.textures()) {response.textures.emplace_back();// 获取response中存储地图变量的引用auto& texture = response.textures.back();// 对引用的变量进行赋值texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),texture_proto.cells().end());texture.width = texture_proto.width();texture.height = texture_proto.height();texture.resolution = texture_proto.resolution();texture.slice_pose = ToGeometryMsgPose(cartographer::transform::ToRigid3(texture_proto.slice_pose()));}response.status.message = "Success.";response.status.code = cartographer_ros_msgs::StatusCode::OK;
}

四、MapBuilder::SubmapToProto()

该函数具体实现如下:

// 返回压缩后的地图数据
std::string MapBuilder::SubmapToProto(const SubmapId& submap_id, proto::SubmapQuery::Response* const response) {// 进行id的检查if (submap_id.trajectory_id < 0 ||submap_id.trajectory_id >= num_trajectory_builders()) {return "Requested submap from trajectory " +std::to_string(submap_id.trajectory_id) + " but there are only " +std::to_string(num_trajectory_builders()) + " trajectories.";}// 获取地图数据const auto submap_data = pose_graph_->GetSubmapData(submap_id);if (submap_data.submap == nullptr) {return "Requested submap " + std::to_string(submap_id.submap_index) +" from trajectory " + std::to_string(submap_id.trajectory_id) +" but it does not exist: maybe it has been trimmed.";}// 将压缩后的地图数据放入responsesubmap_data.submap->ToResponseProto(submap_data.pose, response);return "";
}

首先检测输入的子图id是否正常,不正常则报错,其告知只能选择那些轨迹的子图。接着就是调用 pose_graph_->GetSubmapData() 从后端获取数据,子图数据存储于后端优化 PoseGraph2D::data::submap_data 这个变量之中。然后进行数据压缩,也就是调用 submap_data.submap->ToResponseProto() 这个函数,其会把压缩之后的数据存放在 response 之中。

五、ProbabilityGrid::DrawToSubmapTexture()-子图压缩

进入到 Submap2D::ToResponseProto 函数之后,可以看到其调用了函数代码 grid()->DrawToSubmapTexture(),该函数实现于 src/cartographer/cartographer/mapping/2d/probability_grid.cc 文件中,来看看该函数的实现。

首先子图在构建的时候会扩张,扩展的地图可能存在很多未知区域,也就是栅格值为 0.5,其是没有太大意义的,所以只需要根据 ProbabilityGrid::known_cells_box_ 把探索过的区域剪裁下来就可以了,然后再进行图像数据的压缩。先看一下该函数的的整体注释,然后再进行细节分析:

// 获取压缩后的地图栅格数据
bool ProbabilityGrid::DrawToSubmapTexture(proto::SubmapQuery::Response::SubmapTexture* const texture,transform::Rigid3d local_pose) const {Eigen::Array2i offset;CellLimits cell_limits;// 根据bounding_box对栅格地图进行裁剪ComputeCroppedLimits(&offset, &cell_limits);std::string cells;// 遍历地图, 将栅格数据存入cellsfor (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {if (!IsKnown(xy_index + offset)) {cells.push_back(0 /* unknown log odds value */);cells.push_back(0 /* alpha */);continue;}// We would like to add 'delta' but this is not possible using a value and// alpha. We use premultiplied alpha, so when 'delta' is positive we can// add it by setting 'alpha' to zero. If it is negative, we set 'value' to// zero, and use 'alpha' to subtract. This is only correct when the pixel// is currently white, so walls will look too gray. This should be hard to// detect visually for the user, though.// 我们想添加 'delta',但使用值和 alpha 是不可能的// 我们使用预乘 alpha,因此当 'delta' 为正时,我们可以通过将 'alpha' 设置为零来添加它。 // 如果它是负数,我们将 'value' 设置为零,并使用 'alpha' 进行减法。 这仅在像素当前为白色时才正确,因此墙壁看起来太灰。 // 但是,这对于用户来说应该很难在视觉上检测到。// delta处于[-127, 127]const int delta =128 - ProbabilityToLogOddsInteger(GetProbability(xy_index + offset));const uint8 alpha = delta > 0 ? 0 : -delta;const uint8 value = delta > 0 ? delta : 0;// 存数据时存了2个值, 一个是栅格值value, 另一个是alpha透明度cells.push_back(value);cells.push_back((value || alpha) ? alpha : 1);}// 保存地图栅格数据时进行压缩common::FastGzipString(cells, texture->mutable_cells());// 填充地图描述信息texture->set_width(cell_limits.num_x_cells);texture->set_height(cell_limits.num_y_cells);const double resolution = limits().resolution();texture->set_resolution(resolution);const double max_x = limits().max().x() - resolution * offset.y();const double max_y = limits().max().y() - resolution * offset.x();*texture->mutable_slice_pose() = transform::ToProto(local_pose.inverse() *transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));return true;
}

( 1 ) \color{blue}(1) (1) 首先调用 ComputeCroppedLimits() 求得已知区域的 offset 平移与大小 cell_limits。

( 2 ) \color{blue}(2) (2) 创建一个 std::string cells 实例,用于存储剪切之后的地图,源码中对 XYIndexRangeIterator(cell_limits) 进行迭代,可以理解为 xy_index 就剪切之后(新子图)中像素坐标,xy_index + offset 就是未剪切之前子图(旧子图)中像素坐标。

( 3 ) \color{blue}(3) (3) 判断一下新子图像素对应与旧子图中的位置,是否被探索过,如果没有,则 log odds(栅格值) 与透明度都设置为 0,添加到新地图 cells中。

( 4 ) \color{blue}(4) (4) 如果被探索过,先把其被占用的概率赋值给缩放到 [-127, 127],赋值给 delta,该数值越大,说明被占用的几率越大。

( 5 ) \color{blue}(5) (5) 对于一个像素的描述,使用两个 uint8来描述,也就是 16 个字节。
第一个为像素值 value,第二个为透明度 alpha。总的来说,最后的效果如下:

1.当 delta 大于 0 时,表示需要添加一个正数值。此时,value 被设置为 delta,而 alpha 被设置为 0。也就是说,value 表示要添加的正数值,而 alpha 表示透明度为 0,即完全不透明。

2.当 delta 小于等于 0 时,表示需要减去一个负数值或者不进行任何操作。此时,value 被设置为 0,而 alpha 被设置为 -delta。也就是说,value 为 0 表示不进行任何操作,而 alpha 表示透明度为 -delta,即根据需要减去的负数值的大小确定透明度。

根据这些设定,value 和 alpha 的取值情况如下:

当 delta 大于 0 时,value 大于 0,alpha 为 0。
当 delta 小于等于 0 时,value 为 0,alpha 大于等于 0。

对于占用率比较高的,不透明。对于占用率低,约低则约透明。

( 6 ) \color{blue}(6) (6) 设置号像素值与透明度之后调用 common::FastGzipString() 进行压缩,其内部压缩核心操作为 boost::iostreams::gzip_compressor。

( 7 ) \color{blue}(7) (7) 对剪切之后的地图重新进行描述,如高宽的设置(像素为单位),分辨率,以及世界坐标系下x,y 轴的最大值。且为其设置了位姿,也就是下面这句代码:

  const double max_x = limits().max().x() - resolution * offset.y();const double max_y = limits().max().y() - resolution * offset.x();*texture->mutable_slice_pose() = transform::ToProto(local_pose.inverse() *transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));

这个地方需要注意,首先子图的初始位姿时基于local系的,其与 max_x, max_y 都是世界物理单位。另外,local 系 +x 轴为机器人起始位置正前方,+y 轴为机器人起始位置正左方,本人绘制图像如下:
在这里插入图片描述
红色坐标系为local系,蓝色坐标系为submap系,①表示子图在local系下的位姿,也就是源码中的 local_pose, ②表示切片在local系下的位姿,切片的原点应该是对应于代码中的[resolutionoffset.y(), resolutionoffset.y()],本人也没有理解 *texture->mutable_slice_pose() 最终的结果是什么,从命名来看可能是切片相对于子图的位姿,但是什么求解过程涉及到 limits().max() 与 max_x、max_y。感觉最终获得的结果,不知道是个啥玩意。这里先记录一下,为 疑问 1 \color{red}疑问1 疑问1

六、结语

ProbabilityGrid::DrawToSubmapTexture() 函数会返回到 Submap2D::ToResponseProto() 再返回到 MapBuilder::SubmapToProto() 再到 MapBuilderBridge::HandleSubmapQuery()。回到该函数,可以知道这样就获得了地图压缩之后的结果 response_proto,对于 response_proto 来说其可以存储多个 texture 对象,不过这里只存储了一个。把 response_proto 根式的数据转换成 response 之久,该函数结束,然后 Node::HandleSubmapQuery() 且执行结束。

到这里,大致明白了子图压缩压缩过程,首先把子图中已经探索过的区域剪切下来,然后再进行压缩。但是遗留下了一个疑问。

疑问 1 : \color{red}疑问1: 疑问1 src/cartographer/cartographer/mapping/2d/probability_grid.cc 文件中的 ProbabilityGrid::DrawToSubmapTexture 函数的 proto::SubmapQuery::Response::SubmapTexture* 示例对象 texture 中texture->mutable_slice_pose() 的含义具体是什么。


http://www.ppmy.cn/news/777904.html

相关文章

kalibr使用照片生成数据

rosrun kalibr kalibr_bagcreater --folder /home/zf/calib_ws/data/hikvision_chessboard_0707/left --output-bag /home/zf/test.bag 生成照片的目录底下要求有cam0,另外照片的数据格式必须得要求有时间:1639644559400963.png

集合面试题--ArrayList数组

介绍数组 数组&#xff08;Array&#xff09;是一种用连续的内存空间存储相同数据类型数据的线性数据结构。数组&#xff08;Array&#xff09;是一种用连续的内存空间存储相同数据类型数据的线性数据结构。 因为int占4个字节&#xff0c;所以也可以理解为占四块内存 数组如何…

小马哥GitHub

https://github.com/mercyblitz/tech-weekly

小马激活

一直都在用这个激活。可用。 解压密码&#xff1a;123 https://files.cnblogs.com/files/beijinglaolei/XMoem7Yjh.rar 转载于:https://www.cnblogs.com/beijinglaolei/p/8152513.html

奇瑞小蚂蚁·魅一款适合新手女司机的代步小车

相信现在大部份女司机不怎么喜欢其是新手对于大点的车子不好掌握&#xff0c;尤其是在比较拥挤的路段以及在停车的时候会有非常大的压力&#xff0c;一个操作不好就会出现擦碰。所以很多女性消费者都会选择一款微型小车来代步。今天给大家介绍的这款就是微型车市场上炒的比较火…

精选文章:人生第一辆车购车八大流程及提车注意事项之最强攻略

精选文章:人生第一辆车购车八大流程及提车注意事项之最强攻略 目录 人生第一辆车购车八大流程及提车注意事项之最强攻略 买车注意事项

春季购车大促!经典瑞迈尊享购车3重豪礼

春归万物苏&#xff0c;美好皆可期。无论是外出郊游&#xff0c;还是商务需要&#xff0c;一辆宜家宜商的创富车显得尤为重要。为了回馈广大用户对江西五十铃的支持&#xff0c;让更多用户畅享瑞迈创富之旅&#xff0c;江西五十铃开展鸿“瑞”当头&#xff0c;“迈”享豪礼活动…

开源版4S店汽车行业通用小程序源码 含预约试驾+购车贷款计算器功能+搭建教程

分享一个4S店汽车行业通用小程序源码&#xff0c;系统开源可二开&#xff0c;含详细的搭建教程。 系统功能包含汽车4S店售卖等&#xff0c;集合汽车在线展示、在线预约、购车计算器、贷款计算器&#xff0c;门店介绍&#xff0c;新闻资讯&#xff0c;汽车详情介绍等功能为一体的…

【23秋招】pony小马智行

第一题 题目&#xff1a;分发糖果 每个人想要的糖果数目数组为arr[]&#xff0c;问第K个糖果会给到谁&#xff1f; class Solution():def __init__(self) -> None:passdef solve(self,n,k,arr):# 暴力模拟nums [(arr[i],i) for i in range(len(arr))]nums.sort(reverseTr…

51小车(一)

1.准备&#xff1a; 借鉴: 详细介绍如何从零开始制作51单片机控制的智能小车&#xff08;一&#xff09;———让小车动起来_慕羽★的博客-CSDN博客_新手怎么制作单片机智能小车 2.已有模块 (1) 已有电机驱动 (2) 最小系统 (3)舵机 (4)车架&#xff08;单层&#xff09;…

推二手车平台搅局,通用汽车能从Carvana手上抢到蛋糕吗?

近期&#xff0c;通用汽车宣布计划于春天推出大型在线二手车网站CarBravo&#xff0c;该网站将为购车者提供数十万辆二手车的访问权限&#xff0c;将二手车买家与公司以及经销商联系起来。 由于供应链瓶颈和需求激增&#xff0c;2021年美国的二手车市场可谓炙手可热&#xff0…

智慧车行预约小程序,汽车保养、维修、美容、检测预测小程序,前后端完整代码包括车行动态,养车常识,保养预约,维修预约,洗车美容预约

功能介绍 智慧车行小程序&#xff0c;是一个专门为洗车/4S/车辆维修行业打造的小程序&#xff0c;前后端完整代码包括车行动态&#xff0c;养车常识&#xff0c;保养预约&#xff0c;维修预约&#xff0c;洗车美容预约&#xff0c;汽车检测预约等功能&#xff0c;采用腾讯提供的…

2.7 汽车之家口碑爬虫

2.7 汽车之家口碑爬虫 7 汽车之家口碑爬虫 需求分析前端js反爬虫措施分析 1问题描述2解决方法 爬虫框架 1 获取所有车型的id2本爬虫采用scrapy框架分析所需要的评论信息为3将常用设置写入sttings中 结果展示 1.需求分析 因项目需求&#xff0c;要爬取汽车之家的口碑数据进行下…

买车小记

买车小记 前段时间一个老朋友新买了一辆蒙迪欧致胜&#xff0c;约我们一家出去游玩&#xff0c;儿子一看到这款车就眼发亮&#xff0c;钻进驾驶室转动着方向盘&#xff0c;一副认真开车的样子。也难怪&#xff0c;儿子从小就喜欢汽车&#xff0c;家里面汽车的玩具也堆了不少&am…

如何评价小马智行投后估值60亿美元?无人车的追涨杀跌已经开始-1

半年左右&#xff0c;再次传出巨额融资。 自动驾驶公司PonyAI小马智行&#xff0c;在2月官宣丰田主导的4.62亿美元投资后&#xff0c;刚又被曝出了新一轮3亿美元新融资。 该市场传闻中&#xff0c;投资方亦值得注意&#xff0c;加拿大安大略省养老基金以接近2亿美元出资额领投…

去小马激活首页绑定

#问题现象 打开浏览器,默认弹出页面https://www.hao123.com/?tn=92543205_hao_pg查看浏览器设置,并没有设置首页检查浏览器快捷方式属性,发现“目标(T)”栏设置了跳转C:\Program Files (x86)\Google\Chrome\Application\chrome.exe" http://hao.ttmmt.com/?v=1042删除…

PonyAI小马智行官宣再获2.67亿美元融资,创办4年累计吸金超10亿美元

允中 发自 凹非寺 量子位 报道 | 公众号 QbitAI 新一轮融资&#xff0c;PonyAI小马智行现在官宣了。 2.67亿美元&#xff0c;投后估值53亿美元。 由加拿大安大略省教师退休基金会&#xff08;Ontario Teachers’ Pension Plan&#xff0c;OTPP&#xff09;旗下教师创新平台&…

小马

将代码写入txt 文档中编辑完成后将后缀改为asp上传到FTP中 <meta http-equiv"Content-Type" content"text/html; charsetgb2312"> <title>小马</title><% on error resume Next 忽略页面的错误if request("pass")"lo…

小马智行获2.67亿美元新融资,估值超53亿美元

11月6日&#xff0c;小马智行&#xff08;Pony.ai&#xff09;宣布获得由加拿大安大略省教师退休基金会&#xff08;Ontario Teachers Pension Plan&#xff0c;OTPP&#xff09;旗下教师创新平台&#xff08;Teachers’ Innovation Platform, TIP&#xff09;领投总计2.67亿美…

里程碑事件!小马智行获准在京开启自动驾驶出行商业化试点

11月25日&#xff0c;小马智行获得北京市智能网联汽车政策先行区首批“自动驾驶出行服务商业化试点许可”&#xff0c;获准向公众提供商业化的自动驾驶出行服务&#xff08;Robotaxi&#xff09;。至此&#xff0c;小马智行的乘用车业务正式从技术常态化测试迈入商业化模式验证…
最新文章