ROS Navigation Stack之costmap_2d源码分析

这周开始又要开始阅读navigation stack的代码了,最近一段时间本人尝试将navigation stack中的代价地图和路径规划模块提取出来,丢弃掉ROS的wrapper,以便在不同平台中使用。期间撸了一下costmap的源码,关于代码地图的加载、生成、更新有了新的认识,故整理一下。

Costmap_2D类图

costmap_2d主要类图
在Ubuntu中找了几个从源码生成uml的工具,要么收费(scitool的Understand),要么不好用(不点名),没办法,只能用宇宙IDE,一键生成类图,完美!这里只显示了几个关键的包,其余的组件包没有显示,可以单独阅读。
如果一开始没有这个类图时,容易看得云里雾里,有了这个图,整个包的结构就非常清晰了。用过的都知道,这里的代码地图是分层的,有静态地图层(StaticLayer),也有用于表示传感器信息的(ObstacleLayer)等。但现实世界中,机器人是有尺寸的,所以还需要将静态地图层和障碍物层进行膨胀,从而在规划路径的时候能够考虑机器人的尺寸,而膨胀就由InflationLayer完成。

静态地图层(StaticLayer)

静态地图层就比较简单了,map_server加载地图后,将地图发布出来,costmap_2d_ros接受到地图数据后,便加载各个层,静态地图层只是简单地将灰度图中的像素值换算成ros代价地图中的代价值,然后便进入下一层的更新了。这里只是将转换的代码贴出了说一说:

void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
   
  // 前面就是一些检查和初始化,忽略之...
  // 这里就是将图片的像素值转换成代价值了,具体转换可以看interpretValue函数
  for (unsigned int i = 0; i < size_y; ++i)
  {
   
    for (unsigned int j = 0; j < size_x; ++j)
    {
   
      unsigned char value = new_map->data[index];
      costmap_[index] = interpretValue(value);
      ++index;
    }
  }
}

// 转换函数
unsigned char StaticLayer::interpretValue(unsigned char value)
{
   
  // 实际上就是高于一个阈值lethal_threshold那么就认为是障碍物
  // 没有信息的就认为是未探测的区域
  // 否则为自由空间
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;
  
  // 对于其他的数值则进行插值运算
  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}
障碍物层(ObstacleLayer)

障碍物层是将传感器检测到的点云投影到地图中,不同的传感器来源会分别存到Observation类中。只要遍历存储Obsrvation的容器,将检测到的点云的每个点分别投影到地图中,将对应的网格的代价值设为LETHAL_OBSTACLE,其实现函数在void ObstacleLayer::updateBounds(...)中:

void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
   
  ...
  // 先打扫一下旧垃圾,清除一下障碍物
  for (unsigned int i = 0; i < clearing_observations.size(); ++i)
  {
   
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
  }

  // 获取每个观察层的点云数据
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
  {
   
    const Observation& obs = *it;
    const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    //将每个点云添加进障碍物层中
    for (unsigned 
`global_costmap_params.yaml` 是ROS Navigation Stack中用于配置全局代价地图的参数文件。全局代价地图是一个二维网格地图,用于表示机器人环境中的障碍物和可行区域。它使用代价值来表示每个单元格的占用程度,通常是从0到255,其中0表示可通过,255表示障碍物。该文件中的参数可以控制全局代价地图的分辨率、大小、更新频率、代价图的大小等等。 下面是一个示例 `global_costmap_params.yaml` 文件: ``` # global costmap parameters global_costmap: global_frame: map robot_base_frame: base_link update_frequency: 5.0 publish_frequency: 0.0 static_map: true width: 40.0 height: 40.0 resolution: 0.05 transform_tolerance: 0.5 inflation: enabled: true cost_scaling_factor: 5.0 inflation_radius: 0.5 plugins: - {name: obstacles_laser, type: "costmap_2d::ObstacleLayer"} - {name: inflation, type: "costmap_2d::InflationLayer"} # obstacle layer parameters obstacles_laser: observation_sources: laser_scan_sensor laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: laser_scan, marking: true, clearing: true} observation_persistence: 0.0 max_obstacle_height: 2.0 raytrace_range: 2.5 origin_z: 0.0 z_resolution: 0.2 z_voxels: 10 unknown_threshold: 15 mark_threshold: 0 combination_method: 1 # inflation layer parameters inflation: inflation_radius: 0.5 cost_scaling_factor: 5.0 ``` 该文件中包含了全局代价地图的参数和两个插件(障碍物层和膨胀层)的参数。 其中 `global_costmap` 部分包含了全局代价地图的基本参数,例如地图框架、机器人基座坐标系、地图大小、分辨率等等。`inflation` 部分包含了膨胀层的参数,用于在避障时扩大障碍物的区域。`obstacles_laser` 部分包含了障碍物层的参数,用于从激光雷达数据中获取障碍物信息。 通过修改该文件中的参数,可以调整全局代价地图的性能和精度,以适应不同的机器人和环境。
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值