10 基于ROS2的综合应用

基于ROS2的综合应用

实验介绍

​ 在本次实验中，我们通过ROS2框架综合应用了导航、机械臂控制和物品抓取等技术，实现了一个服务机器人从厨房取饮料并送到客厅客人的完整流程。我们首先使用SLAM Toolbox构建了环境地图，并设置了任务航点。接着，编写了一个ROS2节点，通过导航系统控制机器人前往厨房，利用机械臂抓取饮料，再导航到客厅将饮料递给客人。整个实验涵盖了环境建图、航点设置、导航控制、机械臂操作等多个环节，全面展示了ROS2在复杂机器人任务中的应用。

实验过程

采集环境数据

​ 在这个应用中，导航功能是通过 wp_map_tools 软件包实现的，所以需要事先对环境信息进行构建。主要用到的环境信息包括两个部分:环境地图和任务的地点坐标。

SLAM 环境建图

​ 场景地图使用 SLAM Toolbox进行创建。确认已经下载了wpr_simulation2 仿真项目，然后按组合键[Ctrl+Alt+T]启动 Terminator 终端，执行如下指令，加载工作空间的环境设置。

source ~/ros2_ws/install/setup.bash

​ 然后执行如下指令，启动仿真场景和建图功能

ros2 launch wpr_simulation2 slam.launch.py

​ 执行上述指令后，会弹出仿真窗口和RViz2窗口。建图过程中需要通过键盘控制机器人移动，回到终端，按组合键[Ctrl+Shift+O]，这时会分出第二个命令行窗口。在第二个窗口中执行如下指令，加载工作空间的环境设置

source ~/ros2_ws/install/setup.bash

​ 然后执行如下指令启动键盘控制节点。

ros2 run wpr_simulation2 keyboard_vel_cmd

​ 这时保持Terminator 终端窗口位于所有窗口的前边，且第二个窗口的标题栏为红色，这样才能让键盘控制节点始终能够接收到按下按键的信号。通过键盘控制机器人在场景里巡游一遍之后，可以看到建好的地图。

​ 保持 Terminator 终端的第二个窗口标题栏为红色。按[X]键，退出键盘控制程序，然后执行如下指令，保存地图到文件。

ros2 run nav2_map_server map_saver_cli -f map

​ 这样会在终端窗口的当前路径下创建两个地图文件:map.pgm和map. yaml。将这两个文件复制到 wpr_simulation2的 maps 文件夹下，之后从这个文件夹加载地图文件。

​ 完成后，关闭终端和仿真环境，准备进人下一阶段的操作

设置航点坐标

​ 创建完地图，接下来在地图上标注出机器人执行任务的航点位置，按组合键[Ctrl+Alt+T]启动Terminator 终端，执行如下指令，加载工作空间的环境设置。

source ~/ros2_ws/install/setup.bash

​ 执行如下指令，启动带有航点设置插件的 RViz2 窗口。

ros2 launch wp_map_tools add_waypoint_sim.launch.py

​ 执行后会启动 RViz2窗口，在窗口中可以看到之前创建的地图

​ 在 RViz2 工具栏中单击[Add Waypoint]按钮，就可以在地图上添加航点。第一个航点在厨房放饮料的桌子前，距离桌子1m(对应地面上的一个格子的距离)。第二个航点在客厅的客人面前，距离客人1m(对应地面上的一个格子的距离)。

​ 航点设置完成后，需要将这些信息保存成文件，保持RViz2 界面别关闭，在 Terminator 终端中，按组合键[Ctrl+Shift+O]，将终端分为上、下两个子窗口。先在新的终端窗口中执行如下指令，加载工作空间的环境设置。

source ~/ros2_ws/install/setup.bash

​ 然后执行如下指令保存航点。

ros2 run wp_map_tools wp_saver

​ 执行完毕后，在主文件夹下会生成一个名为“waypoints.yaml”的文件。

​ 这个文件中保存的就是刚才设置的航点信息。双击打开这个文件，对它的内容进行编辑，修改其中的航点名称:
​ 1)把 Waypoint_1的 Name 从“1”修改为“kitchen”，这是抓取饮料的位置。

​ 2)把 Waypoint_2 的Name 从“2”修改为“guest”，这是最终递送饮料给客人的位置。

编写程序代码

Launch 文件的编写

1.创建软件包

​ 首先在工作空间中创建一个名为“home_pkg”的软件包，打开一个新的终端窗口，输入如下指令，进入工作空间。

cd ~/ros2_ws/src

​ 然后用如下指令创建软件包。

ros2 pkg create home_pkg

​ 创建好软件包后，接下来在这个软件包中创建Launch文件和主任务的程序节点。

2.编写 Launch 文件

​ 先在 home_pkg 软件包中创建一个 Launch 文件，在 VSCode 中找到[home_pkg]软件包，用鼠标右键单击软件包名称，在弹出的快捷菜单中选择[新建文件夹]。此时会提示输入文件夹名称，输入“launch”，按[Enter]键确认。然后用鼠标右键单击[launch]文件夹，在弹出的快捷菜单中选择[新建文件]。此时会提示输入文件名，输入“home.launch.py”，然后按[Enter]键，创建文件。

​ 下面编写这个 Launch 文件，其内容如下。

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

def generate_launch_description():
    launch_file_dir = os.path.join(get_package_share_directory('wpr_simulation2'), 'launch')
    home_mani_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(launch_file_dir, 'robocup_home_mani.launch.py')
        )
    )

    map_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'maps',
        'map.yaml'
    )
    
    nav_param_file = os.path.join(
        get_package_share_directory('wpr_simulation2'),
        'config',
        'nav2_params.yaml'
    )

    nav2_launch_dir = os.path.join(
        get_package_share_directory('nav2_bringup'), 
        'launch'
    )

    navigation_cmd = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([nav2_launch_dir, '/bringup_launch.py']),
        launch_arguments={
            'map': map_file,
            'use_sim_time': "True",
            'params_file': nav_param_file}.items(),
    )

    wp_edit_cmd = Node(
            package='wp_map_tools',
            executable='wp_edit_node',
            name='wp_edit_node'
        )
    
    wp_navi_server_cmd = Node(
            package='wp_map_tools',
            executable='wp_navi_server',
            name='wp_navi_server'
        )

    objects_publisher_cmd = Node(
            package='wpr_simulation2',
            executable='objects_publisher',
            name='objects_publisher',
            parameters=[
                {"auto_start": False}
            ]
        )

    grab_object_cmd = Node(
            package='wpr_simulation2',
            executable='grab_object_sim',
            name='grab_object_sim'
        )

    rviz_file = os.path.join(get_package_share_directory('wpr_simulation2'), 'rviz', 'fetch.rviz')
    rviz_cmd = Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_file]
        )

    ld = LaunchDescription()
    ld.add_action(home_mani_cmd)
    ld.add_action(navigation_cmd)
    ld.add_action(wp_edit_cmd)
    ld.add_action(wp_navi_server_cmd)
    ld.add_action(objects_publisher_cmd)
    ld.add_action(grab_object_cmd)
    ld.add_action(rviz_cmd)

    return ld

​ 上述内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与wpr_simulation2\demo_launch\12_home.launch.py 文件中的代码进行比对。

​ 文件编写完毕后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。

3.设置安装规则

​ Launch文件编写完成后，还需要为其设置安装规则，才能将其安装到最终执行的目录中去。安装规则写在 home_pkg的 CMakeLists.txt 文件中，在 VSCode 中打开这个文件，添加如下安装规则。

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

​ 这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\12_home.txt 文件中的代码进行比对。

​ 上述规则添加完毕后，一定要保存文件，否则规则无法生效。

服务机器人程序实现

1.编写节点源代码文件

​ 在上一小节中创建了名为“home_pkg”的软件包，可以直接在这个软件包中创建服务机器人程序的源码文件，在 VSCode 中找到[home_pkg]软件包，用鼠标右键单击它的[src]子目录，在弹出的快捷菜单中选择[新建文件]。此时会提示输入文件名，输人“fetch.cpp”，然后按[Enter]键，创建文件。

​ 下面编写这个源代码文件，其内容如下

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

#define STEP_WAIT                    0
#define STEP_GOTO_KITCHEN          1
#define STEP_GRAB_DRINK             2
#define STEP_GOTO_GUEST              3
#define STEP_DONE                     4
static int fetch_step = STEP_WAIT;

std::shared_ptr<rclcpp::Node> node;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr navi_pub;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr behavior_pub;

void NaviResultCallback(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(), "[NaviResultCallback] %s",msg->data.c_str());
    if(fetch_step == STEP_GOTO_KITCHEN && msg->data == "navi done")
    {
        std_msgs::msg::String msg;
        msg.data = "start grab";
        behavior_pub->publish(msg);
        fetch_step = STEP_GRAB_DRINK;
        RCLCPP_INFO(node->get_logger(), "[STEP_GOTO_KITCHEN] -> [STEP_GRAB_DRINK]");
    }

    if(fetch_step == STEP_GOTO_GUEST && msg->data == "navi done")
    {
        fetch_step = STEP_DONE;
        RCLCPP_INFO(node->get_logger(), "[STEP_GOTO_GUEST] -> [STEP_DONE]");
    }
}

void GrabResultCallback(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(), "[GrabResultCallback] %s",msg->data.c_str());
    if(fetch_step == STEP_GRAB_DRINK && msg->data == "grab done")
    {
        std_msgs::msg::String msg;
        msg.data = "guest";
        navi_pub->publish(msg);
        fetch_step = STEP_GOTO_GUEST;
        RCLCPP_INFO(node->get_logger(), "[STEP_GRAB_DRINK] -> [STEP_GOTO_GUEST]");
    }
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    node = std::make_shared<rclcpp::Node>("fetch_node");

    navi_pub = node->create_publisher<std_msgs::msg::String>(
        "/waterplus/navi_waypoint", 
        10
    );
    behavior_pub = node->create_publisher<std_msgs::msg::String>(
        "/wpb_home/behavior", 
        10
    );
    auto navi_result_sub = node->create_subscription<std_msgs::msg::String>(
        "waterplus/navi_result", 
        10, 
        NaviResultCallback
    );
    auto grab_result_sub = node->create_subscription<std_msgs::msg::String>(
        "/wpb_home/grab_result", 
        10, 
        GrabResultCallback
    );
    
    rclcpp::sleep_for(std::chrono::milliseconds(1000));

    rclcpp::Rate loop_rate(30);

    while(rclcpp::ok())
    {
        if(fetch_step == STEP_WAIT)
        {
            std_msgs::msg::String msg;
            msg.data = "kitchen";
            navi_pub->publish(msg);
            fetch_step = STEP_GOTO_KITCHEN;
            RCLCPP_INFO(node->get_logger(), "[STEP_WAIT] -> [STEP_GOTO_KITCHEN]");
        }
        
        rclcpp::spin_some(node);
        loop_rate.sleep();
    }

    rclcpp::shutdown();

    return 0;
}

​ 上述代码可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cpp\12_fetch.cpp 文件中的代码进行比对。

​ 代码编写完毕后，需要进行保存。保存成功后，编辑界面文件名后面的圆点符号会变成一个叉符号。

2.设置编译规则

​ 节点源码的编译规则写在 home_pkg的CMakeLists.txt 文件中，在VSCode 中打开这个文件，在这个文件中添加节点的编译规则。首先使用如下代码寻找节点源码中用到的依赖项。

find_package(rclcpp REQUIRED) 
find_package(std_msgs REQUIRED) 

​ 然后使用如下代码添加节点源码的编译规则。

add_executable(fetch src/fetch.cpp)
ament_target_dependencies(fetch "rclcpp" "std_msgs")

​ 接下来使用如下代码在安装规则中添加节点安装规则。

install(
TARGETS
  fetch
DESTINATION
  lib/${PROJECT_NAME})

​ 这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_cmakelists\12_fetch.txt 文件中的代码进行比对。

​ 上述规则添加完毕后，一定要保存文件，否则规则无法生效。

3.修改软件包信息

​ 在 VSCode 中打开[home_pkg]下的[package.xml]文件，使用如下代码添加依赖项信息。

<depend>rclcpp</depend>
<depend>std_msgs</depend>

​ 这些内容可以从 wpr_simulation2 的例程文件中找到。如果编译报错，可以与 wpr_simulation2\demo_package\12_fetch.xml 文件中的代码进行比对。

​ 文件修改后，一定要保存文件，否则新的包信息无法生效。

4.编译软件包

​ 修改完上述文件后，打开终端窗口，执行如下指令，进入工作空间。

cd ~/ros2_ws

​ 然后执行如下指令，对工作空间中的所有软件包进行编译

colcon build

仿真运行服务机器人程序

​ 下面将在仿真环境中运行刚编写的 Launch 文件和导航节点。先确认现在终端的当前位置还在工作空间目录ros2_ws，在终端执行如下指令。

source install/setup.bash

​ 接着执行如下指令，启动刚才编写的 Launch 文件。

ros2 launch home_pkg home.launch.py

​ 这时会启动仿真环境和各个功能节点除了仿真窗口，还会弹出一个 RViz2 窗口，里面显示了导航使用的地图以及设置好的航点标记。

​ 目前，在 RViz2窗口中还没有显示机器人模型，需要手动设置一下机器人的初始位置，单击 RViz2 工具栏中的[2D Pose Estimate]按钮，在RViz2的地图中，单击机器人所在的位置，按住鼠标左键不放，设置好初始方向。松开鼠标，就能在RViz2中看到机器人的模型了。

​ 下面运行服务机器人流程节点。在 Terminator 终端，按组合键[Ctrl+Shift+O]，会分出第二个子窗口。先在新的终端窗口中执行如下指令，加载工作空间的环境设置。

source install/setup.bash

​ 然后执行如下指令，运行服务机器人的流程节点。

ros2 run home_pkg fetch

​ 运行节点之后，机器人会自动启动第一阶段的导航，去往航点“kitchen ”

​ 机器人到达“kitchen”航点后，会对桌面上的饮料进行抓取。

​ 抓取完成后，机器人会带着饮料，导航去往航点“guest”

​ 机器人到达航点“guest”，饮料递送任务完成

实验总结

​ 通过本次实验，我们成功实现了一个服务机器人从厨房取饮料并送到客厅客人的完整流程。我们首先使用SLAM Toolbox构建了环境地图，并设置了任务航点。接着，编写了一个ROS2节点，通过导航系统控制机器人前往厨房，利用机械臂抓取饮料，再导航到客厅将饮料递给客人。整个过程中，我们不仅掌握了环境建图、航点设置、导航控制和机械臂操作等关键技术，还提升了系统集成和问题解决的能力，为未来开发更复杂的机器人应用奠定了坚实的基础。