ROS开发专栏---ROS2 机械臂应用入门（2）---机械臂自动抓取物品实验---适配Ubuntu 22.04

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

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

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

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

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

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

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

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

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

前言

前言

上一期博客我们实现了机械臂基础往复运动控制，掌握了JointState关节消息发布的方法。

本期博客将结合前面完成的三维视觉点云物品检测 功能，在Gazebo仿真环境中实现**桌面物品全自动抓取，**具体实验效果为机器人自动检测桌面物体三维坐标，控制底盘对位、机械臂升降、夹爪开合，实现抓取物品的功能。

一、实验原理

本次实验结合之前通过立体视觉进行桌面物品检测的功能，实现对物品的抓取。

在wpr_simulation2软件包中，有一个节点名为"objects_publisher"，已经把物品检测的算法封装好了。只需要通过节点间的话题通讯就能获取物品检测的结果。整体流程如下所示：

相机驱动节点：发布RGB-D相机三维点云话题 /kinect2/sd/points，包含桌面与物体的空间信息。

物品检测节点（objects_publisher）：wpr_simulation2 包中已封装好的节点，接收点云数据，计算并发布桌面物品的三维质心坐标，无需手动实现点云算法。

物品抓取节点：本实验的核心控制节点，订阅物品坐标后，通过有限状态机控制底盘与机械臂协同动作，完成抓取流程。

机械臂节点：订阅 /wpb_home/mani_ctrl 话题，执行机械臂升降、夹爪开合动作。

底盘节点：订阅 /cmd_vel 话题，控制机器人前后左右移动。

二、实验程序的整体流程

根据实验需求，将抓取任务拆分为 6 个步骤，采用有限状态机（FSM）实现流程控制：

物品检测：使用objects_publisher节点，计算桌面物品的质心三维坐标。

底盘对位：根据物品坐标，驱动底盘移动，让机器人正面对准物品，并与桌子拉开安全距离，为机械臂抬升预留空间。

抬臂张爪：控制机械臂抬起，升高至与物品平齐的高度，同时张开手爪，准备抓取。

前移抓取：机器人向前移动，使物品进入手爪两指之间，随后闭合手爪夹紧物品。

抬升物品：在夹紧状态下，小幅抬升机械臂，使物品离开桌面。

后退复位：机器人退回到初始位置，抓取任务完成。

对应状态机定义如下：

三、编写物品抓取节点程序

在~/ros2_zice/src/mani_pkg/src目录下，新建grab_object.cpp文件，完整代码如下：

cpp 复制代码

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <wpr_simulation2/msg/object.hpp>

// 有限状态机状态定义
#define STEP_WAIT         0
#define STEP_ALIGN_OBJ    1
#define STEP_HAND_UP      2
#define STEP_FORWARD      3
#define STEP_GRAB         4
#define STEP_OBJ_UP       5
#define STEP_BACKWARD     6
#define STEP_DONE         7

static int grab_step = STEP_WAIT;
std::shared_ptr<rclcpp::Node> node;

// 发布器定义
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_pub;
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr mani_pub;

// 物体坐标缓存
float object_x = 0.0, object_y = 0.0, object_z = 0.0;
const float align_x = 1.0;
const float align_y = 0.0;

// 三维物品检测话题回调
void ObjectCallback(const wpr_simulation2::msg::Object::SharedPtr msg)
{
    if (grab_step == STEP_WAIT) {
        grab_step = STEP_ALIGN_OBJ;
    }
    if (grab_step == STEP_ALIGN_OBJ && !msg->x.empty()) {
        // 取第一个物体的三维坐标
        object_x = msg->x[0];
        object_y = msg->y[0];
        object_z = msg->z[0];
        RCLCPP_INFO(node->get_logger(), "获取物体坐标: (%.2f, %.2f, %.2f)", 
                    object_x, object_y, object_z);
    }
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    node = std::make_shared<rclcpp::Node>("grab_object_node");

    // 1. 初始化发布器
    cmd_pub = node->create_publisher<std_msgs::msg::String>("/wpb_home/behavior", 10);
    vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
    mani_pub = node->create_publisher<sensor_msgs::msg::JointState>("/wpb_home/mani_ctrl", 10);

    // 2. 订阅三维物品检测话题
    auto object_sub = node->create_subscription<wpr_simulation2::msg::Object>(
        "/wpb_home/objects_3d", 10, ObjectCallback);

    rclcpp::Rate loop_rate(30);
    while (rclcpp::ok())
    {
        rclcpp::spin_some(node);

        // ------------------------------
        // 状态1：等待并启动物品检测
        // ------------------------------
        if (grab_step == STEP_WAIT)
        {
            std_msgs::msg::String start_msg;
            start_msg.data = "start objects";
            cmd_pub->publish(start_msg);
            RCLCPP_INFO(node->get_logger(), "[STEP_WAIT] 启动物品检测...");
        }

        // ------------------------------
        // 状态2：底盘对位，对准物体
        // ------------------------------
        if (grab_step == STEP_ALIGN_OBJ)
        {
            float diff_x = object_x - align_x;
            float diff_y = object_y - align_y;
            geometry_msgs::msg::Twist vel_msg;

            if (fabs(diff_x) > 0.02 || fabs(diff_y) > 0.01)
            {
                vel_msg.linear.x = diff_x * 0.8;
                vel_msg.linear.y = diff_y * 0.8;
            }
            else
            {
                vel_msg.linear.x = 0;
                vel_msg.linear.y = 0;
                std_msgs::msg::String stop_msg;
                stop_msg.data = "stop objects";
                cmd_pub->publish(stop_msg);
                grab_step = STEP_HAND_UP;
                RCLCPP_INFO(node->get_logger(), "[STEP_ALIGN_OBJ] 对位完成");
            }
            vel_pub->publish(vel_msg);
        }

        // ------------------------------
        // 状态3：抬升机械臂，张开夹爪
        // ------------------------------
        if (grab_step == STEP_HAND_UP)
        {
            RCLCPP_INFO(node->get_logger(), "[STEP_HAND_UP] 抬升机械臂...");
            sensor_msgs::msg::JointState mani_msg;
            mani_msg.name.resize(2);
            mani_msg.name[0] = "lift";
            mani_msg.name[1] = "gripper";
            mani_msg.position.resize(2);
            mani_msg.position[0] = object_z;
            mani_msg.position[1] = 0.15; // 夹爪张开
            mani_pub->publish(mani_msg);
            rclcpp::sleep_for(std::chrono::milliseconds(3000));
            grab_step = STEP_FORWARD;
        }

        // ------------------------------
        // 状态4：底盘前移，靠近物体
        // ------------------------------
        if (grab_step == STEP_FORWARD)
        {
            RCLCPP_INFO(node->get_logger(), "[STEP_FORWARD] 向前靠近物体...");
            geometry_msgs::msg::Twist vel_msg;
            vel_msg.linear.x = 0.1;
            vel_msg.linear.y = 0;
            for (int i = 0; i < 50; i++)
            {
                vel_pub->publish(vel_msg);
                rclcpp::sleep_for(std::chrono::milliseconds(100));
            }
            grab_step = STEP_GRAB;
        }

        // ------------------------------
        // 状态5：闭合夹爪，抓取物体
        // ------------------------------
        if (grab_step == STEP_GRAB)
        {
            RCLCPP_INFO(node->get_logger(), "[STEP_GRAB] 闭合夹爪...");
            // 停止底盘
            geometry_msgs::msg::Twist vel_msg;
            vel_msg.linear.x = 0;
            vel_msg.linear.y = 0;
            vel_pub->publish(vel_msg);

            // 闭合夹爪
            sensor_msgs::msg::JointState mani_msg;
            mani_msg.name.resize(2);
            mani_msg.name[0] = "lift";
            mani_msg.name[1] = "gripper";
            mani_msg.position.resize(2);
            mani_msg.position[0] = object_z;
            mani_msg.position[1] = 0.07; // 夹爪闭合
            mani_pub->publish(mani_msg);
            rclcpp::sleep_for(std::chrono::milliseconds(2000));
            grab_step = STEP_OBJ_UP;
        }

        // ------------------------------
        // 状态6：抬升物体，离开桌面
        // ------------------------------
        if (grab_step == STEP_OBJ_UP)
        {
            RCLCPP_INFO(node->get_logger(), "[STEP_OBJ_UP] 抬升物体...");
            sensor_msgs::msg::JointState mani_msg;
            mani_msg.name.resize(2);
            mani_msg.name[0] = "lift";
            mani_msg.name[1] = "gripper";
            mani_msg.position.resize(2);
            mani_msg.position[0] = object_z + 0.05; // 抬升5cm
            mani_msg.position[1] = 0.07; // 保持夹紧
            mani_pub->publish(mani_msg);
            rclcpp::sleep_for(std::chrono::milliseconds(2000));
            grab_step = STEP_BACKWARD;
        }

        // ------------------------------
        // 状态7：底盘后退，完成抓取
        // ------------------------------
        if (grab_step == STEP_BACKWARD)
        {
            RCLCPP_INFO(node->get_logger(), "[STEP_BACKWARD] 后退复位...");
            geometry_msgs::msg::Twist vel_msg;
            vel_msg.linear.x = -0.1;
            vel_msg.linear.y = 0;
            for (int i = 0; i < 50; i++)
            {
                vel_pub->publish(vel_msg);
                rclcpp::sleep_for(std::chrono::milliseconds(100));
            }
            grab_step = STEP_DONE;
            RCLCPP_INFO(node->get_logger(), "[STEP_DONE] 物品抓取完成！");
        }

        // 结束状态：保持静止
        if (grab_step == STEP_DONE)
        {
            geometry_msgs::msg::Twist vel_msg;
            vel_msg.linear.x = 0;
            vel_msg.linear.y = 0;
            vel_pub->publish(vel_msg);
        }

        loop_rate.sleep();
    }

    rclcpp::shutdown();
    return 0;
}

四、修改`CMakeLists.txt`

在~/ros2_zice/src/mani_pkg/CMakeLists.txt中，追加抓取节点的编译配置，全部代码如下所示：

cpp 复制代码

cmake_minimum_required(VERSION 3.8)
project(mani_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(sensor_msgs REQUIRED)
# 新增依赖
find_package(std_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(wpr_simulation2 REQUIRED)

# 编译基础控制节点
add_executable(mani_ctrl src/mani_ctrl.cpp)
ament_target_dependencies(mani_ctrl rclcpp sensor_msgs)

# 编译抓取节点
add_executable(grab_object src/grab_object.cpp)
ament_target_dependencies(grab_object
  rclcpp
  std_msgs
  geometry_msgs
  sensor_msgs
  wpr_simulation2
)

# 安装所有节点
install(TARGETS
  mani_ctrl
  grab_object
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

五、修改`package.xml`

在~/ros2_zice/src/mani_pkg/package.xml中，追加新增依赖，全部代码如下所示：

cpp 复制代码

<?xml version="1.0"?>
<package format="3">
  <name>mani_pkg</name>
  <version>0.0.0</version>
  <description>ROS2机械臂控制与物品抓取功能包</description>
  <maintainer email="robot@todo.com">robot</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
  <!-- 新增依赖 -->
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>wpr_simulation2</depend>
</package>

六、编译与仿真运行

6.1、编译功能包

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

cpp 复制代码

cd ~/ros2_zice
colcon build --packages-select mani_pkg --parallel-workers 1
colcon build --packages-select wpr_simulation2 --parallel-workers 1
source install/setup.bash

6.2、运行过程步骤

启动桌面仿真场景

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

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 launch wpr_simulation2 wpb_table.launch.py

启动Gazebo仿真环境，加载机器人、桌面与物品模型。

启动物品检测封装节点

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

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 run wpr_simulation2 objects_publisher

该节点会发布/wpb_home/objects_3d话题，包含桌面物体的三维坐标信息。

启动自动抓取节点

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

cpp 复制代码

cd ~/ros2_zice
source install/setup.bash
ros2 run mani_pkg grab_object

总结

本期博客我们结合前面完成的三维视觉点云物品检测 功能，在Gazebo仿真环境中实现了桌面物品全自动抓取实验。