ROS开发专栏---家庭服务机器人饮料递送实验---适配Ubuntu 22.04

🎬 渡水无言 ：个人主页渡水无言

❄专栏传送门 ：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》

❄专栏传送门 ：《freertos专栏》《STM32 HAL库专栏》《linux裸机开发专栏》

❄专栏传送门 ：《产品测评专栏》《Ai智能体专栏）《ROS开发专栏》

❄专栏传送门 ：《BMS专栏》

⭐️流水不争先，争的是滔滔不绝

📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生

| 省级优秀毕业生获得者 | csdn新星杯TOP1 | 半导纵横专栏博主 | 211在读研究生

在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连

前言

[2.1、SLAM 环境建图](#2.1、SLAM 环境建图)

2.2、导航航点设置

[三、新建综合应用功能包 home_pkg](#三、新建综合应用功能包 home_pkg)

四、编写Launch文件

五、编写主任务状态机节点代码

六、功能包编译配置

[6.1、编写 CMakeLists.txt](#6.1、编写 CMakeLists.txt)

[6.2、编写 package.xml](#6.2、编写 package.xml)

前言

经过前面博客的学习，我们已经逐一掌握了 ROS2 基础、Linux 操作、激光雷达、IMU、SLAM 建图、NAV2 自主导航、三维视觉检测、机械臂控制与物品抓取等模块化知识点。

本期博客将整合所有前置功能 ，基于 ROS2 分布式节点设计思想，实现一个家庭服务机器人的综合项目，具体实现的效果为：自主导航→厨房抓取饮料→再次导航→送达客人。

一、实验原理流程

1.1、整体流程

本程序通过多节点协同，实现机器人从客厅到厨房取物、再送至餐厅客人处的完整饮料递送任务，核心流程如下：

初始导航：前往厨房

为机器人设定初始位置后，向 wp_map_tools 包的 wp_navi_server 节点发送厨房饮料位置的航点指令，激活航点导航功能，等待节点完成从客厅到厨房的第一阶段导航任务。

物品抓取：获取饮料

收到 wp_navi_server 节点的导航完成信号后，向 wpr_simulation2 包的 grab_object_sim 节点发送 start grab指令，激活物品抓取功能，等待节点完成饮料抓取任务。

二次导航：前往餐厅

收到grab_object_sim节点的抓取完成信号后，再次向 wp_navi_server 节点发送餐厅客人位置的航点指令，激活航点导航功能，等待节点完成从厨房到餐厅的第二阶段导航任务。

任务完成：递送饮料

收到 wp_navi_server 节点的导航完成信号，确认机器人已将饮料送至餐厅客人处，饮料递送全流程结束。

采用状态机拆分任务逻辑，逻辑清晰、易于扩展维护：

状态宏定义	状态说明
STEP_WAIT	初始等待状态，下发厨房航点导航指令，跳转至去往厨房状态
STEP_GOTO_KITCHEN	等待导航到达厨房，完成后启动物品抓取，跳转至抓取状态
STEP_GRAB_DRINK	等待饮料抓取完成，下发客人航点指令，跳转至去往餐厅状态
STEP_GOTO_GUEST	等待导航到达客人位置，完成后进入结束状态
STEP_DONE	饮料递送任务全部完成，机器人保持静止

1.2、系统架构与节点话题通信逻辑

1. 核心功能节点

主任务节点fetch_node：全局状态机调度，下发导航、抓取控制指令；

航点导航节点wp_navi_server：接收航点名称，完成自主导航并反馈导航结果；

物品检测节点objects_publisher：基于三维点云识别桌面饮料位置；

物品抓取节点grab_object_sim：接收指令完成机械臂抓取，反馈抓取完成信号；

航点编辑节点wp_edit_node：用于在 RViz 中手动添加、保存导航航点；

NAV2 导航框架：负责地图加载、路径规划、避障运动控制。

2. 关键通信话题

/waterplus/navi_waypoint：下发航点名称，控制机器人前往指定点位；

/waterplus/navi_result：导航结果反馈，navi done表示到达目标；

/wpb_home/behavior：下发行为控制指令，如start grab启动物品抓取；

/wpb_home/grab_result：抓取结果反馈，grab done表示抓取完成。

二、实验环境数据采集

2.1、SLAM 环境建图

1、启动仿真 SLAM 建图环境

启动仿真 SLAM 建图环境，打开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 launch wpr_simulation2 slam.launch.py

2、新开终端，键盘控制机器人巡游建图

新开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 run wpr_simulation2 keyboard_vel_cmd

用键盘控制机器人在场景里巡游一遍之后，可以看到建好的地图。如下图所示：

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

cpp 复制代码

ros2 run nav2_map_server map_saver_cli -f map

这样会在终端窗口的当前路径下创建两个地图文件：map.pgm和map.yaml。将这两个文件拷贝到wpr_simulation2的maps文件夹下，一会从这个文件夹加载地图文件。

2.2、导航航点设置

1、启动航点编辑环境

注意：我们先要下载wp_map_tools，这个我们之前下载了。

然后我们要先编译他，打开终端，输入以下指令：

cpp 复制代码

colcon build --packages-select wp_map_tools
source install/setup.bash

再运行航点打开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 launch wp_map_tools add_waypoint_sim.launch.py

在RViz2工具栏的右边，单击 $Add Waypoint$ 按钮，就可以在地图上添加航点。

第一个航点在厨房的饮料桌子前，距离桌子1米（对应地面上一个格子的距离）；第二个航点，在厨房的客人面前，距离客人1米（对应地面上的一个格子的距离）。

2、保存航点生成配置文件

新开一个终端，输入以下指令：

cpp 复制代码

source ~/ros2_zice/install/setup.bash
ros2 run wp_map_tools wp_saver

执行完毕后，在主文件夹下会生成一个名为"waypoints.yaml"的文件。双击打开这个文件对它的内容进行编辑，修改其中的航点名称：

把Waypoint_1的Name，从"1"修改为"kitchen"，这是抓取饮料的位置。

把Waypoint_2的Name，从"2"修改为"guest"，这是最终递送饮料给客人的位置。如下图所示：

三、新建综合应用功能包 home_pkg

在工作空间src目录下创建专用综合应用功能包：

cpp 复制代码

cd ~/ros2_zice/src
ros2 pkg create home_pkg

四、编写Launch文件

在home_pkg/launch新建home.launch.py，统一启动仿真、导航、航点服务、视觉抓取、RViz 等所有节点，全部代码如下所示：

cpp 复制代码

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

五、编写主任务状态机节点代码

在home_pkg/src新建fetch.cpp，基于有限状态机实现全流程任务调度，代码如下：

cpp 复制代码

#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(), "导航结果: %s",msg->data.c_str());
    if(fetch_step == STEP_GOTO_KITCHEN && msg->data == "navi done")
    {
        std_msgs::msg::String cmd;
        cmd.data = "start grab";
        behavior_pub->publish(cmd);
        fetch_step = STEP_GRAB_DRINK;
        RCLCPP_INFO(node->get_logger(), "已到达厨房，启动饮料抓取");
    }

    if(fetch_step == STEP_GOTO_GUEST && msg->data == "navi done")
    {
        fetch_step = STEP_DONE;
        RCLCPP_INFO(node->get_logger(), "已送达客人位置，饮料递送任务完成！");
    }
}

// 抓取结果回调
void GrabResultCallback(const std_msgs::msg::String::SharedPtr msg)
{
    RCLCPP_INFO(node->get_logger(), "抓取结果: %s",msg->data.c_str());
    if(fetch_step == STEP_GRAB_DRINK && msg->data == "grab done")
    {
        std_msgs::msg::String cmd;
        cmd.data = "guest";
        navi_pub->publish(cmd);
        fetch_step = STEP_GOTO_GUEST;
        RCLCPP_INFO(node->get_logger(), "饮料抓取完成，前往客人位置");
    }
}

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 cmd;
            cmd.data = "kitchen";
            navi_pub->publish(cmd);
            fetch_step = STEP_GOTO_KITCHEN;
            RCLCPP_INFO(node->get_logger(), "任务启动，前往厨房");
        }        
        rclcpp::spin_some(node);
        loop_rate.sleep();
    }

    rclcpp::shutdown();
    return 0;
}

六、功能包编译配置

6.1、编写 CMakeLists.txt

用于告诉编译器如何编译 C++ 代码、把文件安装到哪里，代码如下所示：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(home_pkg)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
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}
)

# 这一行就是关键！把 launch 目录安装到 share 里
install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

6.2、编写 package.xml

声明包名、作者、依赖库

cpp 复制代码

<?xml version="1.0"?>
<package format="3">
  <name>home_pkg</name>
  <version>0.0.0</version>
  <description>家庭服务机器人综合任务包：导航+视觉+抓取</description>
  <maintainer email="user@todo.com">user</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
</package>

七、编译与运行

7.1、编译

cpp 复制代码

cd ~/ros2_zice
colcon build --packages-select wpr_simulation2 wp_map_tools home_pkg
source install/setup.bash

7.2、运行服务机器人程序

打开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 launch home_pkg home.launch.py

如下图所示：

在RViz2窗口中还没有显示机器人模型，需要手动设置一下机器人的初始位置。如图所示，单击一下RViz2的工具栏里的2D Pos Estimate按钮，如下图所示：

7.3、启动自动递送任务

新开终端，输入以下指令：

cpp 复制代码

source install/setup.bash
ros2 run home_pkg fetch

节点执行之后，机器人会自动启动第一阶段的导航，去往航点"kitchen"；机器人到达"kitchen"航点后，会对桌面上的饮料进行抓取。抓取完成后，机器人会带着饮料，导航去往航点"guest"；机器人到达航点"guest"，饮料递送任务完成。如下图所示：

总结

本期博客整合所有前置功能 ，基于 ROS2 分布式节点设计思想，实现了一个家庭服务机器人的综合项目，具体实现的效果为：自主导航→厨房抓取饮料→再次导航→送达客人。