[ROS2] map_server加载地图文件的三种模式_map_server::loadmapfromfile-程序员宅基地

技术标签: ROS  ROS2  map  

map的数据类型

map话题的类型是nav_msgs::msg::OccupancyGrid。使用下面的命令可以查询该类型的数据结构。

ros2 interface show nav_msgs/msg/OccupancyGrid

nav_msgs::msg::OccupancyGrid的数据结构:

# This represents a 2-D grid map
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# MetaData for the map
MapMetaData info
        builtin_interfaces/Time map_load_time
                int32 sec
                uint32 nanosec
        float32 resolution
        uint32 width
        uint32 height
        geometry_msgs/Pose origin
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1

# The map data, in row-major order, starting with (0,0).
# Cell (1, 0) will be listed second, representing the next cell in the x direction.
# Cell (0, 1) will be at the index equal to info.width, followed by (1, 1).
# The values inside are application dependent, but frequently,
# 0 represents unoccupied, 1 represents definitely occupied, and
# -1 represents unknown.
int8[] data

其中data数据成员用于存储地图中的每个栅格值。nav_msgs::msg::OccupancyGrid存储的栅格值范围在[0~100]。0表示栅格未被占用,100表示栅格被占用了,而0到100之间表示被占用的程度。-1表示未知区域。

info成员变量中主要存储地图文件的一些参数。比如:地图大小,分辨率,原点等信息。

加载map的三种模式

map_server功能包支持加载三种类型的图片文件:PGM/PNG/BMP。图片中每个像素的颜色亮度值将被转化成nav_msgs::msg::OccupancyGrid类型中的栅格值,存储在data成员变量中。

加载地图有下面三种方式:

  • trinary
  • scale
  • raw

其中trinary为默认的加载方式。

地图的加载方式通常会被配置在地图文件对应的配置文件中。该配置文件的内容如下:

image: map.pgm
resolution: 0.050000
origin: [-10.000000, -10.000000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

代码中对应的数据结构体:

struct LoadParameters
{
  std::string image_file_name;
  double resolution{0};
  std::vector<double> origin{0, 0, 0};
  double free_thresh;
  double occupied_thresh;
  MapMode mode;
  bool negate;
};

其中mode如果不在yaml文件中写明的话就会使用默认的trinaryfree_threshoccupied_thresh是判断栅格是否被占用的阈值。

地图图片中每个像素可能有多个颜色通道。比如:RGB。像素的明暗程度值是通过求取各个颜色通道的明暗程度值得到的。像素的明暗程度值的范围在[0~1.0]。下面是代码的实现:

      auto pixel = img.pixelColor(x, y);

      std::vector<Magick::Quantum> channels = {pixel.redQuantum(), pixel.greenQuantum(),
        pixel.blueQuantum()};
      if (load_parameters.mode == MapMode::Trinary && img.matte()) {
        // To preserve existing behavior, average in alpha with color channels in Trinary mode.
        // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque
        channels.push_back(MaxRGB - pixel.alphaQuantum());
      }
      double sum = 0;
      for (auto c : channels) {
        sum += c;
      }
      /// on a scale from 0.0 to 1.0 how bright is the pixel?
      double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size());

      // If negate is true, we consider blacker pixels free, and whiter
      // pixels occupied. Otherwise, it's vice versa.
      /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)?
      double occ = (load_parameters.negate ? shade : 1.0 - shade);

默认情况下,我们认为颜色越暗,栅格被占用的概率越大。颜色越亮,栅格被占用的概率越小。

但是当negate置为1时,这个逻辑就反过来了。颜色越暗,栅格被占用的概率越小。颜色越亮,栅格被占用的概率越大。

trinary模式下的栅格赋值方法

trinary模式下的判断比较简单。主要是像素的明暗程度值小于free_thresh就表示栅格没有被占用,给栅格赋0。若大于occupied_thresh就认为被占用了,给栅格赋100。在free_threshoccupied_thresh之间就认为是状态不明,给栅格赋-1。下面是具体的代码实现:

case MapMode::Trinary:
    if (load_parameters.occupied_thresh < occ) {
    map_cell = nav2_util::OCC_GRID_OCCUPIED;
    } else if (occ < load_parameters.free_thresh) {
    map_cell = nav2_util::OCC_GRID_FREE;
    } else {
    map_cell = nav2_util::OCC_GRID_UNKNOWN;
    }

scale模式下的栅格赋值方法

当像素的Alpha值不为0,栅格值被设定为状态不明。像素的明暗程度值小于free_thresh就表示栅格没有被占用,给栅格赋0。若大于occupied_thresh就认为被占用了,给栅格赋100。在free_threshoccupied_thresh之间就线性转换到[0~100]之间。

case MapMode::Scale:
    if (pixel.alphaQuantum() != OpaqueOpacity) {
        map_cell = nav2_util::OCC_GRID_UNKNOWN;
    } else if (load_parameters.occupied_thresh < occ) {
        map_cell = nav2_util::OCC_GRID_OCCUPIED;
    } else if (occ < load_parameters.free_thresh) {
        map_cell = nav2_util::OCC_GRID_FREE;
    } else {
        map_cell = std::rint(
            (occ - load_parameters.free_thresh) /
            (load_parameters.occupied_thresh - load_parameters.free_thresh) * 100.0);
    }
    break;

raw模式下的栅格赋值方法

将像素的明暗程度值当成是百分比,通过下面代码描述的方式对栅格值赋值。

case MapMode::Raw: {
    double occ_percent = std::round(shade * 255);
    if (nav2_util::OCC_GRID_FREE <= occ_percent &&
    	occ_percent <= nav2_util::OCC_GRID_OCCUPIED)
    {
    	map_cell = static_cast<int8_t>(occ_percent);
    } else {
    	map_cell = nav2_util::OCC_GRID_UNKNOWN;
    }
    	break;
    

参考:

https://navigation.ros.org/tutorials/docs/navigation2_with_keepout_filter.html#prepare-filter-mask


觉得有用就点赞吧!

我是首飞,一个帮大家填坑的机器人开发攻城狮。

另外在公众号《首飞》内回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2等机器人行业常用技术资料。

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/shoufei403/article/details/124238359

智能推荐

用installshield打包时在注册表里注册项和键值的动态设定方法_注册表new - > key-程序员宅基地

文章浏览阅读1.1w次。在11.5版本里,注册表的函数变化不多我在这里要记录的是打包的时候注册表的问题,注册表分项和键,打开注册表(开始->运行,输入regedit,确定),可以在左边的树形里看见很多文件夹的图标,这些是项,点击一个项,可以在右边看见他的键值,每个项都有一个“默认”,有的项还有很多键值在打包的时候,我们有时候要记录很多东西,嗯,我们要记录安装文件的类型(服务器端程序还是客户端程序),我们要记录软件_注册表new - > key

全志平台lichee启动时间优化_initial_debug-程序员宅基地

文章浏览阅读490次。1. 前言全志平台默认的SDK系统启动+相机出图的时间较长,普遍是十几秒,使用体验较差; 这里尝试通过一些方法优化整体启动时间2.统计内核耗时模块打开这个宏initial_debug,每个驱动的初始化起始时间和结束时间都打印出来了。有了这个时间,基本就可以确定哪些部分需要优化了。 我的做法是只关注耗时10000us以上的驱动。根据上面统计,可根据使用场景的需求, 尝试可以关闭相应不需要的模块3. 降低打印等级开发过程中往往会把系统的打印等级设置为最高,而实际上发布出的固件_initial_debug

《HelloGitHub》第 67 期-程序员宅基地

文章浏览阅读1.5k次。兴趣是最好的老师,HelloGitHub 让你对编程感兴趣!简介分享 GitHub 上有趣、入门级的开源项目。https://github.com/521xueweihan/HelloGi..._开源 lexer

如何做代码评审(code review)_code review怎么做-程序员宅基地

文章浏览阅读5.7k次。Code Review 即日常所说的代码评审或代码回顾,主要是在软件开发的过程中,对功能源代码进行评审,其目的是找出并修正软件开发过程中出现的错误的过程,提高和改进代码质量的过程。_code review怎么做

SSM9==SSM项目启动过程、xml配置SSM项目及需要的3大配置文件、原生SSM未前后端分离的电商网站项目(角色管理员、购买者)只使用了最基础的注解,Model传参_ssm框架 启动传参-程序员宅基地

文章浏览阅读2.1k次。SSM项目的启动过程:ssm框架中,项目启动过程以及web.xml配置详解_菜鸟不会飞-程序员宅基地_ssm项目启动入口是什么本篇主要在基于SSM的框架,深入讲解web.xml的配置web.xml 每个javaEE项目中都会有,web.xml文件是用来初始化配置信息:比如Welcome页面、servlet、servlet-mapping、filter、listener、启动加载级别等。 web.xml配置文件内容如下:&lt;!DOCTYPE web-app PUBLIC..._ssm框架 启动传参

npm设置代理,解决安装不了或安装失败等问题_npm config set disturl 失败-程序员宅基地

文章浏览阅读1.6k次。登陆后不用重启!npm config set proxy http://工号:密码@代理服务器地址:80npm config set https-proxy http://工号:密码@代理服务器地址:80_npm config set disturl 失败

随便推点

MongoDB 的简介、特点以及详解_mongodb特点-程序员宅基地

文章浏览阅读1.8k次,点赞2次,收藏2次。1、概念_mongodb特点

Main class [org.apache.oozie.action.hadoop.HiveMain], exit code [10]_attempt recovered after rm restartmain class [org.-程序员宅基地

文章浏览阅读3k次。oozie执行hiveql,发生这样的异常!Main class [org.apache.oozie.action.hadoop.HiveMain], exit code [10]_attempt recovered after rm restartmain class [org.apache.oozie.action.hadoop

计算机毕业设计springboot网上排课系统的设计与实现w0d059【附源码+数据库+部署+LW】_排课系统数据库设计-程序员宅基地

文章浏览阅读255次。选题背景:随着互联网的快速发展,各行各业都在积极探索数字化转型的道路。教育领域也不例外,传统的排课方式已经无法满足现代学生和教师的需求。为了提高教学效率、优化资源利用以及提供更好的学习体验,许多学校和机构开始引入网上排课系统。这种系统通过在线平台实现课程的安排和管理,为学生和教师提供了更加便捷和灵活的排课方式。选题意义:网上排课系统的出现对于教育领域具有重要的意义。首先,它能够提高教学效率。传统的排课方式需要大量的人力和时间,而网上排课系统可以自动化地完成排课过程,减少了繁琐的手工操作,提高了排课的_排课系统数据库设计

对IO流关闭的思考_c++ 流不关闭-程序员宅基地

文章浏览阅读7.9k次,点赞3次,收藏13次。流必须要关闭的原因java相对C,C++来说不需要手动释放内存,在对象引用被消除之后,正常情况下内存资源是会被垃圾回收,那么在使用完IO流之后为什么需要手动关闭. 这是为了回收系统资源,比如释放占用的端口,文件句柄,网络操作数据库应用等.对Unix系统来说,所有的资源都可以抽象成文件,所以可以通过lsof来观察。看下面这个例子,我们创建许多的IO流但是不关闭public clas..._c++ 流不关闭

无法启动此程序,因为计算机中丢失api-ms-win-downlevel-shlwapi-l1-1-0.dll文件-程序员宅基地

文章浏览阅读2.5w次,点赞2次,收藏4次。最近遇到如题所示弹框的问题,花了很长时间才解决,解决方法却很简单。故记录下来。解决方法:找到相同系统却没有该问题的情况(比如出问题的是64位的win7系统,那就找一个没有该问题的64位的win7系统)。将正常系统的api-ms-win-downlevel-shlwapi-l1-1-0.dll文件复制到出现该问题的电脑的相应位置。就可以解决了。但是同样有一些问题需要注意详细步骤:有一些dl..._api-ms-win-downlevel-shlwapi-l1-1-0.dll

ResponseEntity, @ResponseBody, @ResponseStatus返回Json对象数据用法区分_responseentity json-程序员宅基地

文章浏览阅读325次。ResponseEntity:code:/*常用*/@GetMapping("/hello")public ResponseEntity<String> hello(){ return new ResponseEntity<>("hello word!",HttpStatus.OK);}/*设置HTTP头*/@GetMapping("/hello")pu..._responseentity json