ros2 Navigation 学习笔记 第五章（the construct 网站）

本章节包括

2D costmap的解释

全局成本图

本地成本图

Nav2如何使用costmap避开障碍

最后将所有导航部分集成到一个启动文件中

成本图

costmap是机器人再网格地图上感知障碍物的二维表示。

每个网格单元包含传感器检测到的障碍物信息

每个单元的代价可能是未知的、为0的、占用的或者膨胀的

不同的颜色表示碰撞障碍物的可能性

然后，controllers planners 还有 recoveries使这些信息安全高效地计算其任务

全局、本地成本图

有两种类型的成本图

根据静态地图中的障碍物生成全局成本图，是planner用来生成长期路径的

根据机器人在周围检测到的新障碍物创建，使用它来生成短期路径并避免动态障碍。

不同点

全局成本图帮助避免已知障碍物，局部成本图帮助避免动态障碍物

全局覆盖整个地图，局部覆盖机器人周围一小块区域

全局成本图是静态的，局部成本图是动态的

在导航系统中添加全局成本图

在planner_server.yaml中加入以下配置

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      robot_radius: 0.15
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      always_send_full_costmap: True

练习5.1

将上述设置列表添加到planner_server.yaml中的文件底部，需要考虑缩进

编译并且source

启动所有的路径规划系统

打开RVIZ，加载路径规划的config，并正确定位机器人

在display中加入costmap并订阅/global_costmap/costmap，选择color scheme，保存config

应该会得到如下图

 在仿真中添加一个额外袁旭，通过以下操作添加额外构造的圆柱体

source /home/simulations/ros2_sims_ws/install/setup.bash
ros2 run gazebo_ros spawn_entity.py -entity construction_cone -database construction_cone

在rviz中你就会看到雷达对

你会观察到

1. 地图上的障碍物以不同的灰色色调展开，以指示机器人无法移动的空间。浅灰色区域是可以被用于规划的

2. 激光会检测到额外的圆锥体，但是不会加入costmap中，意味着物体在路径上不会被考虑，机器人可能撞到它

全局成本地图障碍层

全局成本图是作为不同障碍层的叠加生成的。

每一层都会根据该层如何计算障碍物，在全局成本图中添加一组障碍物

可用成本图层：

static layer-将静态地图中的存在的任何黑点添加为全局成本地图的障碍

inflation layer-将通货膨胀添加到全局成本图中的任何障碍，作为保持的安全距离

obastacle layer-将2D传感器检测到的任何物体添加到全局成本图中

voxel layer-从点云数据添加到全局成本图的3D障碍

你可以指定要应用于“全局成本图”的一个或所有先前层。然而，包含它们，您需要将它添加到插件列表中，然后为每个插件包含配置参数。

    plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

练习5.2

在先前的练习中，没用使用障碍层向全局成本图加入障碍，这就是为什么没用加入那个圆柱体的原因

在本练习中，你需要

1. 将障碍层作为插件加入到全局成本图中

2. 在planner_server.yaml中加入以下内容，与其他插件同一级别，考虑缩进

    obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0

在infaltion_layer之前包含obstracle_layer，否则infation_layer中不会放大新检测到的障碍

现在再启动就可以看到新的障碍物变灰色。

全局成本图的参数

1. 通用参数

global_frame string map 参考框架

height int 5m 成本图高度

width int 5m 成本图宽度

plugin vector  插件名称列表

publish_frequency double 1.0Hz 发布频率 

resolution double 0.1 像素分辨率，单位为米 

robot_base_frame string "base_link" 机器人底座

robot_radius double 0.1m 如果未提供足迹坐标，则机器人使用半径 

update_frequency double 5Hz 成本图更新频率

2. static layer

map_subscribe_transient_local bool true 映射map主题的QoS设置

enabled bool true 是否启用

subscribe_to_updates bool true 收到静态地图更新后订阅它们

transform_tolerance double 0.1m 公差

3. infaltion layer

enabled bool true 是否已经启用

infaltion_radius double 0.55 在致命障碍物周围膨胀的costmap半径

cost_scaling_factor double 1.0 膨胀半径上的指数衰减因子

always_send_full_costmap bool true 是否每次更新都发送完整的costmap，而不是部分更新

4. obstacle layer

enabled bool true 使能 

observation_sources vector {""} 数据源的名字空间

footprint_clearing_enabled bool true 清楚机器人足迹下的所有占用单元格

max_obstacle_height double 2.0 返回添加到占用网格的最大高度

combination_method int 1 枚举用于向主costmap添加数据的方法，默认为最大值

topic string "" 数据主题

obstacle_range double 2.5 costmap中标记障碍物的最大范围

raytrace_range double 3.0 光线追踪从costmap清除障碍物的最大范围

min_obstacle_height double 0.0 要添加以返回占用网格的最小高度。

clearing bool true 是否应在Costmap中清除源光线跟踪。

marking bool true 源是否应在Costmap中标记。

data_type string "laserscan" 输入的数据类型，LaserScan或PointCloud2。

inf_is_valid bool false 激光扫描仪的无限回报对光线投射是否有效

5. voxel layer

详情可见官方文档

添加局部成本图

参数添加到controller的yaml中

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: True
      rolling_window: true
      width: 1
      height: 1
      resolution: 0.05
      robot_radius: 0.15
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.35
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
        

练习5.3

在controller.yaml中添加以上内容

编译

启动路径规划系统

打开rviz的config

在display中再添加一个costmap，选择color scheme。保存设置

配置以显示可视化的local_costmap

彩色的部分是本地成本图，正如你看到的它只覆盖地图的一小部分。

添加圆锥体，将机器人移动到圆锥体一侧

可以看到机器人路过圆锥体时避开圆锥体

机器人形状在costmap中的作用

机器人形状表明costmap中的障碍物会膨胀到疏密程度。在计算给定区域中的路径是否可用时将考虑该形状。可以在局部和全局的costmap参数中指定机器人的形状。

通常使用圆形或接近圆形的机器人，在这些情况下使用radius参数

你会发现之前的参数为0.3

对于更复杂的机器人，比如矩形、大正方体等奇怪的形状，必须使用足迹参数，指示机器人每个点的坐标。

足迹意味着机器人地面上的投影，比如65cm的正方形机器人，可以用以下参数

footprint: '[[0.325, 0.325], [0.325, -0.325], [-0.325, -0.325], [-0.325, 0.325]]'

重要：不要同时指定footprint和robot_radius

controller配置的其他更改

有一个附加更改，在config文件的critics section中。如果用radius参数，就需要指定BaseObstacle

如果用footprint参数，就需要指定ObstacleFootprint

练习5.4

在本练习中，将Cotsmaps配置从使用robot_radius更改为TurtleBot3机器人的足迹。

步骤： 确定机器人的尺寸

为机器人的每个角创建点列表

修改两个Costmap参数

检查导航是否仍正常工作

footprint: '[ [0.089, 0.069], [0.089, -0.069], [-0.089, -0.069], [-0.089, 0.069] ]'

建立一个单独的navigation启动文件

目前你已经完成了所有导航内容，需要有一个管理所有导航节点的节点lifecycle

但是现在有两个，一个是map_server，一个管理的是剩下的。

不过现在要合二为一

练习5.5

在pathplanner_server中，创建一个navigation.launch.py的新启动文件，包含所有导航节点的启动

需要包括

1. map_erver,localization,pathplanner,controller,bt_navigator以及recoveries_server的启动文件

2. 记住用一个nav2_lifecycle_manager管理所有节点

3. include所有节点的config files

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():

    controller_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'controller.yaml')
    bt_navigator_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'bt_navigator.yaml')
    planner_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'planner_server.yaml')
    recovery_yaml = os.path.join(get_package_share_directory('path_planner_server'), 'config', 'recovery.yaml')
    nav2_yaml = os.path.join(get_package_share_directory('localization_server'), 'config', 'amcl_config.yaml')
    map_file = os.path.join(get_package_share_directory('map_server'), 'config', 'turtlebot_area.yaml')
    
    return LaunchDescription([     
        Node(
            package='nav2_map_server',
            executable='map_server',
            name='map_server',
            output='screen',
            parameters=[{'use_sim_time': True}, 
                        {'yaml_filename':map_file}]
        ),
            
        Node(
            package='nav2_amcl',
            executable='amcl',
            name='amcl',
            output='screen',
            parameters=[nav2_yaml]
        ),
        Node(
            package='nav2_controller',
            executable='controller_server',
            name='controller_server',
            output='screen',
            parameters=[controller_yaml]),

        Node(
            package='nav2_planner',
            executable='planner_server',
            name='planner_server',
            output='screen',
            parameters=[planner_yaml]),
            
        Node(
            package='nav2_recoveries',
            executable='recoveries_server',
            name='recoveries_server',
            parameters=[recovery_yaml],
            output='screen'),

        Node(
            package='nav2_bt_navigator',
            executable='bt_navigator',
            name='bt_navigator',
            output='screen',
            parameters=[bt_navigator_yaml]),

        Node(
            package='nav2_lifecycle_manager',
            executable='lifecycle_manager',
            name='lifecycle_manager',
            output='screen',
            parameters=[{'autostart': True},
                        {'node_names': ['map_server',
                                        'amcl',
                                        'controller_server',
                                        'planner_server',
                                        'recoveries_server',
                                        'bt_navigator']}])
    ])