提炼总结—ROS2机器人开发（第7章）（中）-CSDN博客

写在最前面的话

为什么做该博客？该博客的特点是什么？
随着DeepSeek、ChatGPT等AI技术的崛起，促使机器人技术发展到了新的高度，诞生了宇树科技、特斯拉为代表的人形机器人，四足机器人等等，越来越多的科技巨头涌入机器人赛道，行业对于相关人才的需求也随之达到了顶峰。本博客的内容是替你阅读所有关于机器人的经典书籍，采用书籍瘦身计划，帮你提炼出核心内容，采用最通俗易懂的语言来解释原理，将书读薄。大大缩短学习时间，助你快速成为机器人时代的佼佼者。

文章目录

写在最前面的话
一、书籍介绍
二、第7章自主导航—让机器人自己动起来（提炼总结）

一、书籍介绍

《ROS2机器人开发从入门到实践》是一本从零基础到实战落地的机器人开发指南，通过"基础理论+工具详解+项目实战"的三阶学习路径，带你掌握机器人操作系统ROS2核心架构。书中不仅包含通信机制、URDF建模等必备知识，更有SLAM导航、机械臂控制等12个工业级案例，配合Git代码库和仿真环境配置指南，帮助开发者快速实现从算法仿真到真机部署的完整闭环。无论是学生、工程师还是科研人员，都能通过本书构建系统的ROS2开发能力，抢占机器人技术前沿阵地。
在这里插入图片描述

二、第7章自主导航—让机器人自己动起来（提炼总结）

7.3节机器人导航框架Navigation2（提炼总结）

Navigation2是一个开源机器人导航框架，目标是让机器人安全地从A点移动到B点，是让机器人自主地进行移动，路径规划、避障和自主脱困等能力便是其基本能力

7.3.1节 Navigation2介绍与安装（提炼总结）

行为树：Behavior Tree，BT，起源于游戏，用于控制游戏角色的行为，例如植物大战僵尸，当僵尸出现，豌豆射手就进行射击
Navigation就是使用行为树进行机器人行为调度的，架构如图所示:

输入有：TF变换、map数据、雷达相机等传感器数据、行为树配置、目标位置
输出有：控制话题/cmd_vel，最终会发布话题控制机器人移动
内部结构：
1、BT导航服务器，根据XML格式的行为树描述文件，调用下面三个服务器中对应的模块完成对机器人的行为控制。
2、右边是规划器服务器，它的任务负责全局路径规划，可以根据配置加载不同的规划器完成规划任务。
3、中间的模块是控制器服务器，负责根据全局路径、结合实时障碍物和局部代价地图完成机器人的控制，可以根据配置加载不同的控制器完成任务。
4、左边的模块是恢复器服务器，可以加载不同的恢复行为完成机器人脱困
流程：
BT导航服务器收到目标点后，由规划器服务器计算到目标点的路径，然后由控制器服务器进行路径跟随，如果遇到卡住的情况，则调用规划器服务器完成脱困
安装Navigation2命令如下：

sudo apt install ros-$ROS_DISTRO-navigation2

安装Navigation2启动示例功能包nav2_bringup，命令如下：

sudo apt install ros-$ROS_DISTRO-nav2-bringup

7.3.2节配置Navigation2参数（提炼总结）

把Navigation2当作一个模型，只要给它输入正确的数据，就可以正常工作。
1、nav2_bringup提供了一个默认参数，只需要在它的基础上修改即可，在功能包fishbot_navigation2下创建config目录，将nav2_bringup提供的默认参数复制到config目录下，在chapt7_ws目录下打开终端

cp /opt/ros/$ROS_DISTRO/share/nav2_bringup/params/nav2_params.yaml src/fishbot_navigation2/config

2、编辑nav2_params.yaml，可以看到有很多参数，参数名称带有topic的基本都是关于话题的配置，带有frame的基本都是关于坐标系名称的配置，例如odom_frame_id表示里程计坐标系名称，robot_base_frame表示机器人基础坐标系名称。默认里程计话题是odom，默认雷达数据话题是scan，默认里程计坐标系是odom，默认机器人的基坐标系是base_link。除了修改话题和坐标系名称以保证正确获取以外，还需要考虑机器人的大小这一参数，需要在全局代价地图global_costmap和局部代价地图lccal_csotmap参数进行配置，分别修改robot_radius参数为建模的半径

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: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.12

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.12

7.3.3节编写launch并启动导航（提炼总结）

1、在fishbot_navigation2功能包下新建launch目录，然后在该目录下新建navigation2.launch.py，输入如下代码：

import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource

def generate_launch_description():
    fishbot_navigation2_dir = get_package_share_directory('fishbot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')

    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration('map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration('params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value= use_sim_time, description='Use simulation (Gazebo) clock if true'),
        launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path, description='Full path to map yaml file to load'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path, description='Full path to param file to load'),
        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource([nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'
        ),
    ])

让launch对外提供三个可配置的参数，是否使用仿真时间use_sim_time，地图文件路径map_yaml_path和导航参数路径nav2_param_path，其余默认值已经设计好了
2、修改CMakeLists，添加复制launch、config和maps三个目录到install目录下install命令

install(DIRECTORY launch config maps DESTINATION share/${PROJECT_NAME})

3、在chapt7_ws目录下运行如下代码，构建功能包

colcon build

4、在chapt7_ws目录下，添加环境变量

source install/setup.bash

5、在chapt7_ws目录下，运行节点，先启动仿真

ros2 launch fishbot_description gazebo_sim.launch.py

6、打开新的终端，启动launch文件

ros2 launch fishbot_navigation2 navigation2.launch.py

启动时会报TF相关错误，是因为没有设定机器人的初始位置，在运行launch之后打开的rviz的如下位置找到导航相关的按钮
在这里插入图片描述
2D Pose Estimate时用于初始化位置的工具
Nav2 Goal时设置导航目标点的工具
7、选中2D Pose Estimate，然后单击地图中机器人目前所在的大概位置，不要松开左键，拖动鼠标调整机器人朝向，设置完成后终端就不报错了
在这里插入图片描述
初始化可以看到地图障碍物边界都变大了，这就是代价地图的膨胀图层，膨胀图层是Nacigation2为了防止机器人和障碍物发生碰撞，在原有地图的基础上，将图中障碍物的周围按照一定的半径进行膨胀而形成的
8、在rviz左侧显示部分，修改Global Planner配置，取消全局代价地图Global Costmap的显示，就可以看到局部代价地图及其膨胀层了
在这里插入图片描述

7.3.4节进行单点与路点导航（提炼总结）

1、使用Nav2 Goal按钮给定目标点，单击该按钮，选择一个目标位置和朝向，就可以看到规划出来的全局路径，机器人开始移动了
在这里插入图片描述
一条很短的蓝色线，就是局部规划的路线
2、接下来测试指定路点的导航，路点就是指路过的点，使用路点导航，Navigation2就会在规划路径时按照你指定的顺序进行导航，点击最下面的Waypoint/Nav Through Poses Mode来设置多个目标点的导航，之后使用Nav2 Goal依次设置多个路点，之后点击Start Waypoint Following就可以启动路点导航了
在这里插入图片描述
就可以看到机器人依次走向每一个路点

7.3.5节导航过程中进行动态避障（提炼总结）

在机器人行走的过程中，雷达会不断的检测周围的距离信息，一旦发现新的障碍物就会添加到代价地图中，接着就会再次规划路径

SLAM机器人自主导航

7.3.6节优化导航速度和膨胀半径（提炼总结）

1、打开nav2_params.yaml文件，找到controller_server，其下的FollowPath模块负责将路径转换成角速度和线速度，将该模块下最大旋转角速度参数max_vel_thetra设为0.8，将角加速度参数acc_lim_thetra设为2，如下所示：

      max_vel_theta: 0.8
      acc_lim_theta: 2

2、重新构建并启动Navigation2（重复上一节3后面的步骤），初始化位置后，设置一个需要掉头的目标点，使用如下命令行工具输出速度数据

ros2 topic echo /cmd_vel --once

在这里插入图片描述
会发现对于一些狭窄的区域，经过膨胀之后就没有空白空间了
3、修改膨胀半径，膨胀半径时代价地图中的一个参数，一般设置为机器人的直径大小，修改nav2_params.yaml中对应节点的参数

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.24

      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.24
      always_send_full_costmap: True

完成后重新构建并启动导航，可以看到膨胀半径已经缩小到合理大小了

7.3.7节优化机器人到点精度（提炼总结）

1、Navigation2默认到达目标点并不是精准停靠，而是有一个范围，只要在相应范围内就表示已经达到了目标点，修改nav2_params.yaml中对应节点的参数，如下所示：
general_goal_checker下的

      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 0.15

FollowPath下的

      xy_goal_tolerance: 0.15

将FollowPath下的xy_goal_tolerance修改为0.15，表示在路径跟踪时到目标点位置允许误差为0.15m，general_goal_checker下修改对应位置和角度精度范围，会实时检测当前机器人的位置和目标位置之间的差距，如果在指定范围内会停止
2、完成后重新构建并启动导航，可以看到机器人到点更加精准了。地图默认的分辨率为0.05m，如果到点范围设置得太小，就会造成机器人在目标点左右徘徊，反而不利于导航

7.4节导航应用开发指南（提炼总结）

任务：设计一个自动巡检机器人，在几个不同目标位置之间循环移动并拍照。肯定不能给机器人分配一个人，使用rviz工具来指定目标，然后观察机器人到点后再打开相机拍照保存
实际机器人项目中，导航往往只作为系统一个子模块，通过接口进行调用和状态监测

7.4.1节使用话题初始化机器人位姿（提炼总结）

导航后第一个操作就是初始化机器人位姿，机器人在地图中的位置是由amcl节点根据地图以及传感器数据进行实时计算的。

1、使用命令行发布初始化位姿

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header:{frame_id:map},pose:{pose:{position:{x:0 y:0 z:0}}}}" --once

通过话题告知amcl机器人当前位置在地图坐标系map的原点附近，并且和地图坐标系朝向相同
2、使用python或c++在代码中发布话题来初始化，在nac2_simple_commander的python库，将常用的导航相关操作封装成相应的类，可以直接调用它来实现对导航的操作

在chapt7_ws/src目录下创建功能包fishbot_application，构建类型选择ament_python

ros2 pkg create fishbot_application --build-type ament_python --dependencies rclpy --license Apache-2.0

在对应的目录fishbot_application下新建init_robot_pose.py

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy
def main():
    rclpy.init()
    navigator = BasicNavigator()
    initial_pose = PoseStamped()
    initial_pose.header.frame_id = 'map'
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()
    initial_pose.pose.position.x = 0.0
    initial_pose.pose.position.y = 0.0
    initial_pose.pose.orientation.w = 1.0
    navigator.setInitialPose(initial_pose)
    navigator.waitUntilNav2Active()
    rclpy.spin(navigator)
    rclpy.shutdown()

在setup.py中对该节点进行注册，在功能包清单文件中声明对nav2_simple_commander、rclpy和geometry_msgs的依赖
编辑setup.py

    entry_points={
        'console_scripts': [
            'init_robot_pose = fishbot_application.init_robot_pose:main',
        ],

编辑package.xml

 <depend>rclpy</depend>
 <depend>nav2_simple_commander</depend>
 <depend>geometry_msgs</depend>

重新构建功能包，启动导航，之后运行该节点，观察是否完成初始化位姿

ros2 launch fishbot_description gazebo_sim.launch.py

ros2 launch fishbot_navigation2 navigation2.launch.py

ros2 run fishbot_application init_robot_pose

7.4.2节使用TF获取机器人实时位置（提炼总结）

1、在fishbot_application/fishbot_application下新建文件get_robot_pose.py，编写如下代码：

import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener ,Buffer
from tf_transformations import euler_from_quaternion

class TFListener(Node):
    def __init__(self):
        super().__init__('tf2_listener')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.timer = self.create_timer(1, self.get_transform)

    def get_transform(self):
        try:
            tf = self.buffer.lookup_transform('map', 'base_footprint', rclpy.time.Time(seconds=0),rclpy.time.Duration(seconds=1))
            transform = tf.transform
            rotation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w
            ])
            self.get_logger().info(f'平移: {transform.translation},旋转四元数: {transform.rotation},旋转欧拉角: {rotation_euler}')
        except Exception as e:
            self.get_logger().warn(f'不能够获取坐标转换，原因：{str(e)}')

def main():
    rclpy.init()
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()

2、在setup.py中对该节点进行注册

        entry_points={
        'console_scripts': [
            'init_robot_pose = fishbot_application.init_robot_pose:main',
            'get_robot_pose = fishbot_application.get_robot_pose:main'
        ],
    },

3、重构功能包，先运行仿真，之后运行导航，接着初始化位姿，最后启动该节点

ros2 launch fishbot_description gazebo_sim.launch.py

ros2 launch fishbot_navigation2 navigation2.launch.py

ros2 run fishbot_application get_robot_pose

在这里插入图片描述
可以看到机器人在地图中位姿已经被实时输出了

7.4.3节调用接口进行单点导航（提炼总结）

Navigation2对外提供了用于导航调用的动作服务，动作通信是ROS2四大通信机制之一，主要用于控制场景，它的优点在于其反馈机制，当客户端发送目标给服务端后，除了等待服务端处理完成，还可以收到服务端的处理进度
查看动作列表命令

ros2 action list

查看动作的具体信息

ros2 action info /navigate_to_pose -t

1、使用命令行请求机器人到达目标点

ros2 action send_goal /navigation_to_pose nav2_msgs/action/NavigateToPose "{pose:{header:{frame_id:map},pose:{position:{x:2,y:2}}}}" --feedback

代码的命令用于请求机器人移动到地图坐标系中x、y都是2的点

2、使用代码请求机器人到达目标点
在python中nav2_simple_controller库已经将动作客户端代码封装到BasicNavigator节点中，使用该节点的对应函数就可以实现导航操作

在fishbot_application/fishbot_application下新建文件nav_to_pose.py，编写如下代码：

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator , TaskResult
import rclpy
from rclpy.duration import Duration

def main() :
    rclpy.init()
    navigator = BasicNavigator()
    navigator.waitUntilNav2Active()

    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'map'
    goal_pose.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose.pose.position.x = 1.0
    goal_pose.pose.position.y = 1.0
    goal_pose.pose.orientation.w = 1.0

    navigator.goToPose(goal_pose)
    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(
           f'预计:{Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s后到达')
        if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0):
            navigator.cancelTask()
        result = navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            navigator.get_logger().info('成功到达目标')
        elif result == TaskResult.CANCELED:
            navigator.get_logger().info('任务被取消')
        elif result == TaskResult.FAILED:
            navigator.get_logger().info('任务失败')
        else:
            navigator.get_logger().info('未知结果')

在setup.py中对该节点进行注册

        entry_points={
        'console_scripts': [
            'init_robot_pose = fishbot_application.init_robot_pose:main',
            'get_robot_pose = fishbot_application.get_robot_pose:main',
            'nav_to_pose = fishbot_application.nav_to_pose:main'

        ],
    },

重构功能包，先运行仿真，之后运行导航，接着初始化位姿，最后启动该节点

ros2 launch fishbot_description gazebo_sim.launch.py

ros2 launch fishbot_navigation2 navigation2.launch.py

ros2 run fishbot_application nav_to_pose

在这里插入图片描述

7.4.4节使用接口完成路点导航（提炼总结）

路点导航和单点导航一样都是通过动作进行调用的，动作服务的名称是follow_waypoint

1、在fishbot_application/fishbot_application下新建文件waypoint_follower.py，编写如下代码：

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
import rclpy
from rclpy.duration import Duration

def main():
    rclpy.init()
    navigator = BasicNavigator()
    navigator.waitUntilNav2Active()

    goal_poses = []
    goal_pose1 = PoseStamped()
    goal_pose1.header.frame_id = 'map'
    goal_pose1.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose1.pose.position.x = 0.0
    goal_pose1.pose.position.y = 0.0
    goal_pose1.pose.orientation.w = 1.0
    goal_poses.append(goal_pose1)
    goal_pose2 = PoseStamped()
    goal_pose2.header.frame_id = 'map'
    goal_pose2.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose2.pose.position.x = 2.0
    goal_pose2.pose.position.y = 0.0
    goal_pose2.pose.orientation.w = 1.0
    goal_poses.append(goal_pose2)
    goal_pose3 = PoseStamped()
    goal_pose3.header.frame_id = 'map'
    goal_pose3.header.stamp = navigator.get_clock().now().to_msg()
    goal_pose3.pose.position.x = 2.0
    goal_pose3.pose.position.y = 2.0
    goal_pose3.pose.orientation.w = 1.0
    goal_poses.append(goal_pose3)

    navigator.followWaypoints(goal_poses)

    while not navigator.isTaskComplete():
        feedback = navigator.getFeedback()
        navigator.get_logger().info(f'当前目标编号:{feedback.current_waypoint}')   
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        navigator.get_logger().info('导航成功')
    elif result == TaskResult.CANCELED:
        navigator.get_logger().info('导航取消')
    else:
        navigator.get_logger().info('导航失败')

2、在setup.py中对该节点进行注册

        entry_points={
        'console_scripts': [
            'init_robot_pose = fishbot_application.init_robot_pose:main',
            'get_robot_pose = fishbot_application.get_robot_pose:main',
            'nav_to_pose = fishbot_application.nav_to_pose:main',
            'waypoint_follower = fishbot_application.waypoint_follower:main'

        ],

3、重构功能包，先运行仿真，之后运行导航，接着初始化位姿，最后启动该节点

ros2 launch fishbot_description gazebo_sim.launch.py

ros2 launch fishbot_navigation2 navigation2.launch.py

ros2 run fishbot_application waypoint_follower

在这里插入图片描述
观察发现机器人先运行到目标0、之后运行到目标1、最后运行到目标2

提炼总结—ROS2机器人开发（第7章）（中）

写在最前面的话

文章目录

一、书籍介绍

二、第7章 自主导航—让机器人自己动起来（提炼总结）

7.3节 机器人导航框架Navigation2（提炼总结）

7.3.1节 Navigation2介绍与安装（提炼总结）

7.3.2节 配置Navigation2参数（提炼总结）

7.3.3节 编写launch并启动导航（提炼总结）

7.3.4节 进行单点与路点导航（提炼总结）

7.3.5节 导航过程中进行动态避障（提炼总结）

7.3.6节 优化导航速度和膨胀半径（提炼总结）

7.3.7节 优化机器人到点精度（提炼总结）

7.4节 导航应用开发指南（提炼总结）

7.4.1节 使用话题初始化机器人位姿（提炼总结）

7.4.2节 使用TF获取机器人实时位置（提炼总结）

7.4.3节 调用接口进行单点导航（提炼总结）

7.4.4节 使用接口完成路点导航（提炼总结）