ROS2 Navigation 进阶教程学习笔记 第一章

第一章 关于Nav2的新功能

Nav2提供了新的拱你和工具，使创建机器人应用程序变得更容易

在本单元中，将学习

1. 通过simple Commander API进行基本Nav2操作

2. 通过followwaypoints使用waypoint follower和task executor插件

3. 禁区和限速区简介

然后您将基于Nav2创建一个基本的自主机器人demo。您将经常在一个仿真仓库中执行这些操作。

仓库代码GitHub - aws-robotics/aws-robomaker-small-warehouse-world at ros2

机器人代码Neobotix: Mobile Robot MP-400 (neobotix-robots.com)

1. 开始

测试此仓库中的导航

cd ~/ros2_ws/src
git clone https://bitbucket.org/theconstructcore/nav2_pkgs.git

cd ~/ros2_ws/
colcon build
source install/setup.bash

ros2 launch path_planner_server navigation.launch.py

RVIZ中点击，或者手动发布初始位置

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 3.45, y: 2.15, z: 0.0}, orientation: {z: 1.0, w: 0.0}}}}"

再在RVIZ中给导航目标，即刻开始导航。

2. 简单命令器API

Nav2 simple commander提供了一组通过python3代码和Nav2系统交互的方法。您可以将其视为python API，它允许您轻松与导航堆栈交互。

在本单元中，首先回顾导航机器人创建应用程序的重要方法，包括gotopose(),gothroughposes()以及followwaypoints()

安装简单命令器：

sudo apt update
sudo apt install ros-galactic-nav2-simple-commander

2.1 Navigate to pose

Navigatetopose动作最适用于点对点导航或其他可以在行为树中表示的任务，比如动态对象跟踪。

在下面找到navigatetopose动作的结构

NavigateToPose.action

#goal definition
geometry_msgs/PoseStamped pose
string behavior_tree
---
#result definition
std_msgs/Empty result
---
# feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining

如您所见，动作的输入是您希望机器人导航到的pose和要使用的behavior_tree（可选）。

如果没指定会使用BT behavior中默认的行为树。在动作执行的过程中，会得到一些重要的反馈，比如机器人的姿势、经过的时间、估计剩余时间、剩余距离和导航到目标时执行的恢复次数。这些信息可用于做出良好的自主决策或跟踪进展。

2.1.1 Demo

在下面的演示中，使用Navigattopose动作将机器人从暂存点驱动到货架，供人类将物品放置到机器上。然后将开车到托盘搬运车上，用下一辆卡车运出货舱。

首先创建一个nav2_new_features的新包，将在此单元中创建所有的内容放置在其中。

ros2 pkg create --build-type ament_python nav2_new_features --dependencies rclpy geometry_msgs

在这个包中添加scripts的文件夹并添加名为navigate_to_pose.py的脚本

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import time
from copy import deepcopy

from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration
import rclpy

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

# Shelf positions for picking
shelf_positions = {
    "shelf_A": [-3.829, -7.604],
    "shelf_B": [-3.791, -3.287],
    "shelf_C": [-3.791, 1.254],
    "shelf_D": [-3.24, 5.861]}

# Shipping destination for picked products
shipping_destinations = {
    "recycling": [-0.205, 7.403],
    "pallet_jack7": [-0.073, -8.497],
    "conveyer_432": [6.217, 2.153],
    "frieght_bay_3": [-6.349, 9.147]}

'''
Basic item picking demo. In this demonstration, the expectation
is that a person is waiting at the item shelf to put the item on the robot
and at the pallet jack to remove it
(probably with a button for 'got item, robot go do next task').
'''


def main():
    # Recieved virtual request for picking item at Shelf A and bringing to
    # worker at the pallet jack 7 for shipping. This request would
    # contain the shelf ID ("shelf_A") and shipping destination ("pallet_jack7")
    ####################
    request_item_location = 'shelf_C'
    request_destination = 'pallet_jack7'
    ####################

    rclpy.init()

    navigator = BasicNavigator()

    # Set your demo's initial pose
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 3.45
    initial_pose.pose.position.y = 2.15
    initial_pose.pose.orientation.z = 1.0
    initial_pose.pose.orientation.w = 0.0
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to activate fully
    navigator.waitUntilNav2Active()

    shelf_item_pose = PoseStamped()
    shelf_item_pose.header.frame_id = 'map'
    shelf_item_pose.header.stamp = navigator.get_clock().now().to_msg()
    shelf_item_pose.pose.position.x = shelf_positions[request_item_location][0]
    shelf_item_pose.pose.position.y = shelf_positions[request_item_location][1]
    shelf_item_pose.pose.orientation.z = 1.0
    shelf_item_pose.pose.orientation.w = 0.0
    print('Received request for item picking at ' + request_item_location + '.')
    navigator.goToPose(shelf_item_pose)

    # Do something during your route
    # (e.x. queue up future tasks or detect person for fine-tuned positioning)
    # Print information for workers on the robot's ETA for the demonstration
    i = 0
    while not navigator.isTaskComplete():
        i = i + 1
        feedback = navigator.getFeedback()
        if feedback and i % 5 == 0:
            print('Estimated time of arrival at ' + request_item_location +
                  ' for worker: ' + '{0:.0f}'.format(
                      Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
                  + ' seconds.')

    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Got product from ' + request_item_location +
              '! Bringing product to shipping destination (' + request_destination + ')...')
        shipping_destination = PoseStamped()
        shipping_destination.header.frame_id = 'map'
        shipping_destination.header.stamp = navigator.get_clock().now().to_msg()
        shipping_destination.pose.position.x = shipping_destinations[request_destination][0]
        shipping_destination.pose.position.y = shipping_destinations[request_destination][1]
        shipping_destination.pose.orientation.z = 1.0
        shipping_destination.pose.orientation.w = 0.0
        navigator.goToPose(shipping_destination)

    elif result == TaskResult.CANCELED:
        print('Task at ' + request_item_location +
              ' was canceled. Returning to staging point...')
        initial_pose.header.stamp = navigator.get_clock().now().to_msg()
        navigator.goToPose(initial_pose)

    elif result == TaskResult.FAILED:
        print('Task at ' + request_item_location + ' failed!')
        exit(-1)

    while not navigator.isTaskComplete():
        pass

    exit(0)


if __name__ == '__main__':
    main()

备注：从官方文档提取的代码navigation2/nav2_simple_commander/nav2_simple_commander at main · ros-planning/navigation2 · GitHub

查看代码并确定你使用API的位置以及预期的机器人行为。如果不理解整个代码不要担心，在本单元稍后的部分会进行复盘。

完成审查后，编译整个包

cd ros2_ws
colcon build
source install/setup.bash

测试新代码，首先启动并运行导航系统

ros2 launch path_planner_server navigation.launch.py

接着运行新的脚本

python3 ros2_ws/src/nav2_new_features/scripts/navigate_to_pose.py

2.1.2 代码复盘

现在已经看到了API的实际应用，更深入地分析您所执行的python脚本中的内容，请特别注意使用API的部分。现在从头开始

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

这行代码是最重要的之一。导入BasicNavigator类，该类允许您访问API。因此每当您使用API时候都要导入这个类

navigator = BasicNavigator()

实例化应用它

navigator.setInitialPose(initial_pose)

再次，使用setInitialpose方法，再次请阔狭要在initial_pose变量中指定初始位置。（需要是posestamped类型）

navigator.waitUntilNav2Active()

这个方法会组织程序的运行，指导Nav2完全联机并处于生命周期节点的活动状态。

navigator.goToPose(shelf_item_pose)

这里，您使用的是goToPose（）方法，该方法请求机器人导航到指定的姿势。在这种情况下，您要在shelf_item_pose变量中指定姿势（它必须是PoseStamped类型）。

while not navigator.isTaskComplete():

只有在机器人达到目标后，isNavComplete（）方法才会返回True。在处理过程中，它将返回False。

feedback = navigator.getFeedback()

getFeedback（）方法返回NavigateToPose动作服务器的反馈。

result = navigator.getResult()

getResult（）方法从NavigateToPose动作服务器返回结果。

2.2 Navigate Through Poses

该动作最适合用于位置受限的导航请求或者具有一组位置的行为树中表示另外的任务。这不会在每个航点停止，二十作为位置约束驶过他们。

#goal definition
geometry_msgs/PoseStamped[] poses
string behavior_tree
---
#result definition
std_msgs/Empty result
---
#feedback definition
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries
float32 distance_remaining
int16 number_of_poses_remaining

如上所见，该动作几乎与Navigatetopose的输入相同，指示现在采用了poses的向量。反馈也是类似的，只包含了新的number_of_poses_remaining字段来跟踪通过过孔点的进度。

2.2.1 Demo

在这个demo中，使用navigatethroughposes动作，让您的机器人沿着已知的路线从仓库的中转点出发。NavigateThroughPoses动作，如NavigateToPose，可以在存在障碍物的情况下偏离，如您将在本演示中使用托盘千斤顶看到的。一旦它完成了路线，它就会重新开始并继续，直到停止。

新建一个名为navigate_through_poses.py的脚本

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import time
from copy import deepcopy

from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration
import rclpy

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

'''
Basic security route patrol demo. In this demonstration, the expectation
is that there are security cameras mounted on the robots recording or being
watched live by security staff.
'''


def main():
    rclpy.init()

    navigator = BasicNavigator()

    # Security route, probably read in from a file for a real application
    # from either a map or drive and repeat.
    security_route = [
        [1.792, 2.144],
        [1.792, -5.44],
        [1.792, -9.427],
        [-3.665, -9.427],
        [-3.665, -4.303],
        [-3.665, 2.330],
        [-3.665, 9.283]]

    # Set your demo's initial pose
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 3.45
    initial_pose.pose.position.y = 2.15
    initial_pose.pose.orientation.z = 1.0
    initial_pose.pose.orientation.w = 0.0
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to activate fully 
    navigator.waitUntilNav2Active()

    # Do security route until dead
    while rclpy.ok():
        # Send your route
        route_poses = []
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.header.stamp = navigator.get_clock().now().to_msg()
        pose.pose.orientation.w = 1.0
        for pt in security_route:
            pose.pose.position.x = pt[0]
            pose.pose.position.y = pt[1]
            route_poses.append(deepcopy(pose))
        navigator.goThroughPoses(route_poses)

        # Do something during your route (e.x. AI detection on camera images for anomalies)
        # Print ETA for the demonstration
        i = 0
        while not navigator.isTaskComplete():
            i = i + 1
            feedback = navigator.getFeedback()
            if feedback and i % 5 == 0:
                print('Estimated time to complete current route: ' + '{0:.0f}'.format(
                      Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
                      + ' seconds.')

                # Some failure mode, must stop since the robot is clearly stuck
                if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
                    print('Navigation has exceeded timeout of 180s, canceling the request.')
                    navigator.cancelTask()

        # If at the end of the route, reverse the route to restart
        security_route.reverse()

        result = navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            print('Route complete! Restarting...')
        elif result == TaskResult.CANCELED:
            print('Security route was canceled, exiting.')
            exit(1)
        elif result == TaskResult.FAILED:
            print('Security route failed! Restarting from the other side...')

    exit(0)


if __name__ == '__main__':
    main()

接着测试

ros2 launch path_planner_server navigation.launch.py

python3 ros2_ws/src/nav2_new_features/scripts/navigate_through_poses.py

2.2.2 代码分析

这段代码与第一段类似，只有一些改动

security_route = [
        [1.792, 2.144],
        [1.792, -5.44],
        [1.792, -9.427],
        [-3.665, -9.427],
        [-3.665, -4.303],
        [-3.665, 2.330],
        [-3.665, 9.283]]

定义一系列你希望机器人通过的位置

for pt in security_route:
        pose.pose.position.x = pt[0]
        pose.pose.position.y = pt[1]
        route_poses.append(deepcopy(pose))

将这些位置附加到route_poses向量。请注意，每个姿势都必须定义为PoseStamped消息。

navigator.goThroughPoses(route_poses)

然后，使用此向量作为调用goThroughPoses（）方法的输入。该方法要求机器人在一组位置中导航。

if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
                print('Navigation has exceeded timeout of 180s, canceling the request.')
                navigator.cancelTask()

if Duration.from_msg(feedback.navigation_time) > Duration(seconds=180.0):
                print('Navigation has exceeded timeout of 180s, canceling the request.')
                navigator.cancelTask()

2.3 航路点跟踪

FollowWaypoints动作最适合于简单的自主任务，您希望在每个航路点停止并执行一个行为（例如，暂停2秒，拍照，等待某人在其上放置一个方框等）。Nav2航路点跟随器服务器包含TaskExecutor插件，用于在每个航点执行任务。

#goal definition
geometry_msgs/PoseStamped[] poses
---
#result definition
int32[] missed_waypoints
---
#feedback definition
uint32 current_waypoint

如您所见，API很简单。它采用一组姿势，其中最后一个姿势是最终目标。反馈是它正在执行的当前航路点ID，如果任何航路点不可导航，则在结束时返回一个缺失的航路点ID矢量。

2.3.1 启动 followwaypoints action

默认情况下，导航系统不会启动FollowWaypoints操作。因此，如果您想使用它，请将其添加到导航启动文件中。 首先为其创建一个配置文件。在path_planner_server包的config文件夹中，添加一个名为waypoint_follower.yaml的新配置文件。

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

在此示例中，加载默认插件WaitAtWaypoint。这些插件在到达每个航路点后将机器人暂停指定时间。可以使用waypoint_pause_duration参数（以毫秒为单位指定）配置等待时间。 还有其他插件可用，如PhotoAtWaypoint或InputAtWaypoint。如果您想了解更多信息，请查看此处的官方文档。 现在是将FollowWaypoints操作添加到启动文件的时候了。为此，需要进行一些修改。 首先，添加新的配置文件路径：

在navigation.launch.py中加入

waypoint_follower_yaml = os.path.join(get_package_share_directory(
        'path_planner_server'), 'config', 'waypoint_follower.yaml')

接下来，添加一个新的Node元素以启动waypoint_follower服务器：

Node(
    package='nav2_waypoint_follower',
    executable='waypoint_follower',
    name='waypoint_follower',
    output='screen',
    parameters=[waypoint_follower_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',
                                'waypoint_follower']}])

之后编译

cd ros2_ws
colcon build
source install/setup.bash

2.3.2 Demo

在本演示中，使用FollowWaypoints动作将机器人从其暂存点驱动到一组检查点。Nav2航路点跟随器TaskExecutor插件可对货架进行图像和RFID扫描，以便进行库存管理分析。 创建名为follow_waypoints.py的新Python脚本：

#! /usr/bin/env python3
# Copyright 2021 Samsung Research America
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import time
from copy import deepcopy

from geometry_msgs.msg import PoseStamped
from rclpy.duration import Duration
import rclpy

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult


def main():
    rclpy.init()

    navigator = BasicNavigator()

    # Inspection route, probably read in from a file for a real application
    # from either a map or drive and repeat.
    inspection_route = [
        [3.461, -0.450],
        [3.461, -2.200],
        [3.661, -4.121],
        [3.661, -5.850]]

    # Set your demo's initial pose
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 3.45
    initial_pose.pose.position.y = 2.15
    initial_pose.pose.orientation.z = 1.0
    initial_pose.pose.orientation.w = 0.0
    navigator.setInitialPose(initial_pose)

    # Wait for navigation to activate fully 
    navigator.waitUntilNav2Active()

    # Send your route
    inspection_points = []
    inspection_pose = PoseStamped()
    inspection_pose.header.frame_id = 'map'
    inspection_pose.header.stamp = navigator.get_clock().now().to_msg()
    inspection_pose.pose.orientation.z = 1.0
    inspection_pose.pose.orientation.w = 0.0
    for pt in inspection_route:
        inspection_pose.pose.position.x = pt[0]
        inspection_pose.pose.position.y = pt[1]
        inspection_points.append(deepcopy(inspection_pose))
    nav_start = navigator.get_clock().now()
    navigator.followWaypoints(inspection_points)

    # Do something during your route (e.x. AI to analyze stock information or upload to the cloud)
    # Print the current waypoint ID for the demonstration
    i = 0
    while not navigator.isTaskComplete():
        i = i + 1
        feedback = navigator.getFeedback()
        if feedback and i % 5 == 0:
            print('Executing current waypoint: ' +
                  str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))

    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Inspection of shelves complete! Returning to start...')
    elif result == TaskResult.CANCELED:
        print('Inspection of shelving was canceled. Returning to start...')
        exit(1)
    elif result == TaskResult.FAILED:
        print('Inspection of shelving failed! Returning to start...')

    # go back to start
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    navigator.goToPose(initial_pose)
    while not navigator.isTaskComplete():
        pass

    exit(0)


if __name__ == '__main__':
    main()

测试新代码

ros2 launch path_planner_server navigation.launch.py

ros2 action list

能看到/follow_waypoints

python3 ros2_ws/src/nav2_new_features/scripts/follow_waypoints.py

2.3.2 代码解释

navigator.followWaypoints(inspection_points)

与之前一样，使用inspection_points向量作为调用followWaypoints（）方法的输入。该方法请求机器人遵循一组航路点（PoseStamped消息列表）。这将在每个姿势执行所选的TaskExecutor插件。

i = 0
while not navigator.isTaskComplete():
    i = i + 1
    feedback = navigator.getFeedback()
    if feedback and i % 5 == 0:
        print('Executing current waypoint: ' +
              str(feedback.current_waypoint + 1) + '/' + str(len(inspection_points)))

在此代码中，您正在打印机器人的当前航路点。然而，请记住，您可以在这里执行其他（更有意义的）任务。

3. 成本图过滤器

Nav2以Costmap过滤器的形式提供了一组令人兴奋的功能。您可以将Costmap过滤器视为添加到常规Costmap的额外层（遮罩）。令人兴奋的是，有了这个额外的面具，您可以根据地图上的区域（区域）使机器人具有特定的行为。 例如，您可以使机器人避免前往地图的特定区域（也称为禁区），或根据地图的区域限制机器人的速度（速度限制区域）。

3.1 禁区

3.1.1 编辑地图

回顾如何使用Keepout Mask过滤器使机器人避开环境的某些区域。 “保持遮罩”是一个类似于贴图的文件，包含要作为“保持区域”应用的遮罩。因此，要应用遮罩，需要环境的贴图文件。因此，首先下载文件map_rotated.pgm。您可以通过右键单击该文件并选择下载选项，从IDE轻松下载该文件。 现在，您将用黑色绘制您希望机器人避开的地图区域。

编辑完地图后，再次上传。将编辑后的地图文件重命名为map_keeout.pgm，并将其上载到map_server包的地图文件夹中。 最后，您还必须创建映射的相关YAML文件：

image: map_keepout.pgm
resolution: 0.050000
origin: [-7.000, -10.500000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3.1.2 创建成本图过滤器nodes

创建编辑后的地图文件后，就可以启动所需的节点了。但在此之前，请更新配置文件。首先，您将更新Costmap配置，该配置可以在planner_server.yaml和controller.yaml文件中找到。

filters: ["keepout_filter"]

keepout_filter:
    plugin: "nav2_costmap_2d::KeepoutFilter"
    enabled: True
    filter_info_topic: "/costmap_filter_info"

将更改应用于全局成本图和局部成本图

...

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      ...
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"
            
...

...

local_costmap:
  local_costmap:
    ros__parameters:
      ...
      plugins: ["voxel_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      ...
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "/costmap_filter_info"

...

再启动两个新节点

costmap filter info server此节点将发布nav2_msgs/CostmapFilterInfo消息。这些消息包含诸如过滤器类型或数据转换系数之类的元数据。

mask map server此节点将发布OccupancyGrid消息。

新建一个叫做filters.yaml的文件

costmap_filter_info_server:
  ros__parameters:
    use_sim_time: true
    filter_info_topic: "/costmap_filter_info"
    type: 0
    mask_topic: "/keepout_filter_mask"
    base: 0.0
    multiplier: 1.0

filter_mask_server:
  ros__parameters:
    use_sim_time: true
    frame_id: "map"
    topic_name: "/keepout_filter_mask"
    yaml_filename: "/home/user/ros2_ws/src/nav2_pkgs/map_server/maps/map_keepout.yaml"

备注

type参数使用的costmap过滤器的类型，值为

0表示禁区/首选车道过滤器 1用于速度过滤器（如果速度限制以最大速度的%指定） 2用于速度过滤器（如果速度限制以绝对值（m/s）指定）

mask_topic参数设置要发布过滤器掩码的主题。因此，指定的主题名称必须与Map Server的topic_name参数相同。

基数和乘数是用于为滤波器应用掩码的系数。它们用于以下公式：filter_space_value=基数+乘数*mask_value

对于“禁止区域”，它们需要分别设置为0.0和1.0，以明确显示您从OccupancyGrid值->到过滤器值空间的一对一转换。

再navigation.launch.py中修改，添加以下内容

filters_yaml = os.path.join(get_package_share_directory(
        'path_planner_server'), 'config', 'filters.yaml')

两个新的node

Node(
    package='nav2_map_server',
    executable='map_server',
    name='filter_mask_server',
    output='screen',
    emulate_tty=True,
    parameters=[filters_yaml]),

Node(
    package='nav2_map_server',
    executable='costmap_filter_info_server',
    name='costmap_filter_info_server',
    output='screen',
    emulate_tty=True,
    parameters=[filters_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',
                                'waypoint_follower',
                                'filter_mask_server',
                                'costmap_filter_info_server']}])

编译测试

cd ros2_ws
colcon build
source install/setup.bash

ros2 launch path_planner_server navigation.launch.py

便成功设置了禁区

注释

当尝试在禁区附近导航时，可能会出现以下错误：

[controller_server-3] [ERROR] [1654803915.474550722] [DWBLocalPlanner]: 1.00: ObstacleFootprint/Trajectory Hits Obstacle.

如果出现此错误，可以打开controller.yaml参数，并从批评者列表中删除名为ObstructFootprint的批评者。

#critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
    
critics: ["RotateToGoal", "Oscillation", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]

3.2 限速区

3.2.1 编辑地图

限速过滤器的原理与用于禁区的原理类似。然而，在这种情况下，掩码值将具有不同的含义：与地图上的单元格相对应的区域的编码速度限制。

如您所知，OccupancyGrid值属于[0.100]范围。对于速度过滤器，0值表示地图对应区域中没有速度限制。[1..100]范围内的值通过以下公式线性转换为速度限制值：speed_limit=filter_mask_data*乘数+基数

简化意味着使用的灰色百分比越轻，速度限制就越低，反之亦然。 首先再次下载文件map_rotated.pgm。 现在，用灰色（对不同区域使用不同的色调）绘制地图上您希望机器人有速度限制的区域。 示例如下：

编辑完地图后，再次上传。将编辑后的地图文件重命名为map_speed.pgm，并将其上载到map_server包的maps文件夹中。 最后，创建映射的关联YAML文件：

image: map_speed.pgm
resolution: 0.050000
origin: [-7.000, -10.500000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3.1.2 初始化限速节点

在本单元中，仅将速度过滤器应用于全局成本图。因此，首先，更新过滤器参数：

在planner_server.yaml中

# For Keepout Zones
#filters: ["keepout_filter"]

# For Speed Limits
filters: ["speed_filter"]

以及

# For Keepout Zones
#filters: ["keepout_filter"]

# For Speed Limits
filters: ["speed_filter"]

在controller_server中也要加入speed_limit_topic

...

controller_server:
  ros__parameters:
    ...
    # For Speed Limits
    speed_limit_topic: "/speed_limit"
    ...

现在更新文件filters.yaml以设置速度过滤器所需的参数：

costmap_filter_info_server:
  ros__parameters:
    use_sim_time: true
    filter_info_topic: "/costmap_filter_info"
    # For Keepout Zones
    #type: 0
    #mask_topic: "/keepout_filter_mask"
    #base: 0.0
    #multiplier: 1.0
    # For Speed Limits
    type: 1
    mask_topic: "/speed_filter_mask"
    base: 100.0
    multiplier: -1.0

filter_mask_server:
  ros__parameters:
    use_sim_time: true
    frame_id: "map"
    # For Keepout Zones
    #topic_name: "/keepout_filter_mask"
    #yaml_filename: "/home/user/ros2_ws/src/map_server/maps/map_keepout.yaml"
    # For Speed Limits
    topic_name: "/speed_filter_mask"
    yaml_filename: "/home/user/ros2_ws/src/nav2_pkgs/map_server/maps/map_speeds.yaml"

接着编译测试

cd ros2_ws
colcon build
source install/setup.bash

ros2 launch path_planner_server navigation.launch.py

ros2 launch path_planner_server navigation.launch.py

如果您正确遵循了所有说明，您将得到如下所示的Costmap：

在RVIZ中设置

 

 向机器人发送Nav2目标，并检查速度限制如何根据机器人的导航区域而变化