通过里程计可以获取机器人的位置信息，激光雷达可以获取环境的位置和距离信息

先启动上一章的仿真机器人。配置，运行

ros2 launch bot_description gazebo_sim.launch.py

再打开一个终端，打开rviz

固定节点改为里程计

点击“Add”，选择“By topic”里的odeometry

保留数据的长度改为1000

再点击“Add”，添加“Laserscan”（点的大小size可自行调整）

将保留数据的时间改为1000

再打开一个终端，运行键盘控制节点，控制机器人移动

 ros2 run teleop_twist_keyboard teleop_twist_keyboard 

奇了怪了，我在rviz中就能正常显示运动，是gazebo没加载好吗（因为它停止维护了）

真的就在正常移动啊，显示的是墙的点云

从演示效果可以看出，激光雷达能够初步获取周围环境的轮廓信息，但在机器人运动过程中，障碍物位置信息存在明显偏差，主要受数据不同步与传感器噪声影响。

同步定位与地图构建（SLAM）技术，融合里程计与激光雷达数据，能够有效抑制噪声、实现运动补偿，最终完成实时地图构建与精准定位

全局路径规划：确认一条从起点到终点的路线

局部路径规划：依据实时传感器信息，动态调整局部轨迹，实现避障与平稳运动

使用slam_toolbox建图

安装slam_toolbox

sudo apt install ros-humble-slam-toolbox

创建一个新的工作空间，在里面创建src文件夹，将之前项目src下的bot_description复制到该src目录下

mkdir map_ws
cd map_ws/
mkdir src
cp -r ../ros2/src/bot_description src/

可以看到已经成功移动过来了

构建配置运行

ros2 launch bot_description gazebo_sim.launch.py

再打开一个终端进行建图

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True

再打开一个新的终端，输入rviz2，在rviz中查看建立的地图，固定坐标改为map

点击“Add”，选择“By topic”里的Map（由slam_toolbox发布）

等一下就加载成功了

再打开一个终端，运行键盘控制节点，控制小车移动（尽可能的慢）

ros2 run teleop_twist_keyboard teleop_twist_keyboard 

把整个房间逛一遍，就能得到完整的地图了（我这个很漂呀，好像是要修改什么参数的，但我不想细究了）

应该是太漂了吧，有好几个房间似的（好像是有参数可以校准的，但我暂时不去研究了）

将地图保存为文件

安装nav2-map-server（可能之前已经安装好了）

sudo apt install ros-humble-nav2-map-server

在src下新建一个功能包

在其中新建一个maps文件夹

之前的终端不要关闭啊（键盘走完了可以关闭），关闭了重新打开，然后右键在终端中打开maps目录

运行

ros2 run nav2_map_server map_saver_cli -f room

可以看到生成了2个文件（pgm是图片，yaml是描述文件）

机器人导航框架Navigation2

安装Navigation2

sudo apt install ros-humble-navigation2

还可以安装启动示例功能包

sudo apt install ros-humble-nav2-bringup

配置navigation2参数

在bot_navigation2下新建一个config文件夹

将navigation2提供的默认参数配置复制到该文件夹中

在终端中打开该文件夹，运行

cp /opt/ros/humble/share/nav2_bringup/params/nav2_params.yaml  .

编写launch启动导航

有了参数我们就可以编写launch文件传递参数并启动导航了

新建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():
    bot_navigation2_dir = get_package_share_directory('bot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration('map', default=os.path.join(bot_navigation2_dir, 'maps', 'room.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration('params_file', default=os.path.join(bot_navigation2_dir, 'config', 'nav2_params.yaml'))

    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        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 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 参数替换原有参数
            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'),
    ])

在CMakeLists.txt文件中添加拷贝到install目录的代码

然后在工作空间中colcon build，配置，启动仿真

ros2 launch bot_description gazebo_sim.launch.py

再打开一个终端，配置，启动nav2.launch

ros2 launch bot_navigation2 navigation2.launch.py

然后就看到这样一个地图界面（真的就是我前面保存的地图，就是很飘，我也没去管）

点击“2D Pose Estimate”估计机器人大概位置

然后调整机器人前方的朝向

浅紫色是全局的代价地图，超紫的是局部的代价地图（我这个图真的很不准，大家看看就得了）

可以选择“Global Costmap”是否开关

单点与路点导航

点击“Nav2 Goal”选择一个点，然后给一个该点的朝向

确实走了，但很乱啊

还可以设置路点导航（多点导航）

点击左下角的“Waypoint / Nav Through...”

然后点击“Nav2 Goal”就能设置多个点（及朝向）

机器人行走过程中会进行动态避障

使用话题初始化机器人位姿

导航后第一个操作就是初始化机器人位姿，使用amcl话题计算机器人位置

打开一个终端，配置，运行

ros2 launch bot_description gazebo_sim.launch.py

再打开一个终端，配置，运行

ros2 launch bot_navigation2 navigation2.launch.py

再打开一个终端，配置，通过“ros2 node info /amcl”命令查看节点的相关信息

可以看出，这个节点订阅了雷达的scan话题、map、initialpose（用于接收初始化位姿）

向该话题发布数据就可初始化位姿了

在gazebo中可以看到，初始位置一般都是（0，0，0）

发布数据（map后面不给数据默认是0）

ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "{header: {frame_id: map}}" --once

打开rviz可以看到位置已经初始化完成了

navigation2中有个python包，将常用的导航相关操作封装成了相关类，我们可以直接来调用

在src下新建一个功能包

ros2 pkg create bot_application --build-type ament_python --license Apache-2.0

新建一个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()

if __name__ == '__main__':
    main()

在setup.py中添加可执行文件

然后构建、配置、运行之前的两个终端+新的终端运行

ros2 run bot_application init_robot_pose

在rviz中可以看到也是初始化位姿了

使用TF获取机器人实时位置

使用tf监听就能获得机器人在地图中的实时位置

新建一个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()

在setup.py中添加可执行文件

构建配置，运行前两个终端+

ros2 run bot_application get_robot_pose 

开始获取不到坐标，因为没用初始化位置

点击“2D Pose Estimate”，初始化位置

然后就有数据了

给机器人一个Goal，位置也会实时变换

调用接口进行单点导航

navigation2提供了用于导航调用的动作服务

启动前两个终端，并初始化位姿，再打开一个终端，查看当前的动作

ros2 action list -t

其中的/navigate_to_pose就是用于导航到点处理的服务

看一下当前消息接口的内容

ros2 interface show nav2_msgs/action/NavigateToPose

上面是goal的内容，下面是结果、反馈

接下来我们可以发送一个请求，设置目标位置

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

机器人真的在移动了（只是我这个雷达很漂）

可以看一下反馈，最终位置在（-0.05,-0.36,0.091），导航总共27秒……

当然我们也可以通过代码完成相同的功能

新建一个nav_to_pose.py文件

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

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 后到达')
    result = navigator.getResult()
    navigator.get_logger().info(f'导航结果:{result}')

在setup.py中添加可执行文件

再构建配置，运行前两个终端+新建终端（我们使用了waitUntilNav2Active()函数里的(0,0,0)初始化位姿，如果要指定初始化位姿还需运行init_robot_pose.py文件）

ros2 run bot_application nav_to_pose

小车确实移动了

oo，我前面的目标xyz设置的是000，大家可以改一下位置

使用接口完成路点导航

使用follow_waypoints服务进行多点导航

新建一个waypoint_follower.py文件

from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
import rclpy

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 = 2.0
    goal_pose1.pose.position.y = 1.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 = 0.0
    goal_pose2.pose.position.y = 1.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 = 0.0
    goal_pose3.pose.position.y = 0.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()
    navigator.get_logger().info(f'导航结果:{result}')

在setup.py中添加可执行文件

构建配置，运行前两个终端+新建终端运行

ros2 run bot_application waypoint_follower

机器人系统架构设计——实战

需求：

• 巡检机器人能够在不同的目标点之间进行循环移动
• 到达每个目标点时播放对应的语音提示
• 到达目标点时，通过摄像头拍摄实时图像并保存到本地

下图来自鱼香ROS机器人B站up主

编写巡检控制节点

在src目录下新建一个功能包

ros2 pkg create autopartol_robot --buil
d-type ament_python --dependencies rclpy nav2_simple_commander --license Apache-2.0

新建一个partol_node.py文件

import rclpy
from geometry_msgs.msg import PoseStamped, Pose
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion, quaternion_from_euler
import math

class PartolNode(BasicNavigator):
    def __init__(self, node_name='partol_node'):
        super().__init__(node_name)
        # 导航相关定义
        self.declare_parameter('initial_point', [0.0, 0.0, 0.0])
        self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57])
        self.initial_point_ = self.get_parameter('initial_point').value
        self.target_points_ = self.get_parameter('target_points').value
       
    def get_pose_by_xyyaw(self, x, y, yaw):
        """
        通过 x,y,yaw 合成 PoseStamped
        """
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        rotation_quat = quaternion_from_euler(0, 0, yaw)
        pose.pose.orientation.x = rotation_quat[0]
        pose.pose.orientation.y = rotation_quat[1]
        pose.pose.orientation.z = rotation_quat[2]
        pose.pose.orientation.w = rotation_quat[3]
        return pose

    def init_robot_pose(self):
        """
        初始化机器人位姿
        """
        # 从参数获取初始化点
        self.initial_point_ = self.get_parameter('initial_point').value
        # 合成位姿并进行初始化
        self.setInitialPose(self.get_pose_by_xyyaw(
            self.initial_point_[0], self.initial_point_[1], self.initial_point_[2]))
        # 等待直到导航激活
        self.waitUntilNav2Active()

    def get_target_points(self):
        """
        通过参数值获取目标点集合        
        """
        points = []
        self.target_points_ = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points_)/3)):
            x = self.target_points_[index*3]
            y = self.target_points_[index*3+1]
            yaw = self.target_points_[index*3+2]
            points.append([x, y, yaw])
            self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})')
        return points

    def nav_to_pose(self, target_pose):
        """
        导航到指定位姿
        """
        result = self.goToPose(target_pose)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f'剩余距离: {feedback.distance_remaining}')
        # 最终结果判断
        result = self.getResult()
        self.get_logger().info(f'导航结果：{result}')

    def get_current_pose(self):
        """
        通过TF获取当前位姿
        """
        while rclpy.ok():
            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}')
                return transform
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
    
def main():
    rclpy.init()
    patrol = PartolNode()
    patrol.init_robot_pose()

    while rclpy.ok():
        for point in patrol.get_target_points():
            x, y, yaw = point[0], point[1], point[2]
            # 导航到目标点
            target_pose = patrol.get_pose_by_xyyaw(x, y, yaw)
            patrol.nav_to_pose(target_pose)
            
    rclpy.shutdown()

在setup.py文件里添加可执行文件

新建一个config文件夹，新建partol_config.yaml文件，用来设置参数

注意确认以下这些导航点的位置（不要在柱子、墙体等不可进入区域）

patrol_node:
  ros__parameters:
    initial_point: [0.0, 0.0, 0.0]
    target_points: [
      0.0,0.0,0.0,
      1.0,2.0,3.14,
      -4.5,1.5,1.57,
      -8.0,-5.0,1.57,
      1.0,-5.0,3.14,
      ]

在setup.py中配置install拷贝

构建配置，运行前两个终端+新建一个终端运行

ros2 run autopartol_robot partol_node --ros-args --params-file ./src/autopartol_robot/config/partol_config.yaml ode --ros-args --params-file ./src/autopartol_robot/config/partol

是在运行了