🎬渡水无言：个人主页渡水无言
❄专栏传送门：《linux专栏》《嵌入式linux驱动开发》《linux系统移植专栏》
❄专栏传送门：《freertos专栏》 《STM32 HAL库专栏》《linux裸机开发专栏》
❄专栏传送门：《产品测评专栏》 《Ai智能体专栏） 《ROS开发专栏》
❄专栏传送门：《BMS专栏》
⭐️流水不争先，争的是滔滔不绝📚博主简介：第二十届中国研究生电子设计竞赛全国二等奖 |国家奖学金 | 省级三好学生
| 省级优秀毕业生获得者 | csdn新星杯TOP1 | 半导纵横专栏博主 | 211在读研究生
在这里主要分享自己学习的linux嵌入式领域知识；有分享错误或者不足的地方欢迎大佬指导，也欢迎各位大佬互相三连
目录
前言
一、实验原理
二、实验程序的整体流程
三、编写物品抓取节点程序
四、修改CMakeLists.txt
五、修改package.xml
六、编译与仿真运行
6.1、编译功能包
6.2、运行过程步骤
总结
前言
上一期博客我们实现了机械臂基础往复运动控制，掌握了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文件，完整代码如下：
#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中，追加抓取节点的编译配置，全部代码如下所示：
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中，追加新增依赖，全部代码如下所示：
<?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、编译功能包
打开终端，输入以下指令：
cd ~/ros2_zice colcon build --packages-select mani_pkg --parallel-workers 1 colcon build --packages-select wpr_simulation2 --parallel-workers 1 source install/setup.bash6.2、运行过程步骤
启动桌面仿真场景
打开终端，输入以下指令：
cd ~/ros2_zice source install/setup.bash ros2 launch wpr_simulation2 wpb_table.launch.py启动Gazebo仿真环境，加载机器人、桌面与物品模型。
启动物品检测封装节点
打开终端，输入以下指令：
cd ~/ros2_zice source install/setup.bash ros2 run wpr_simulation2 objects_publisher该节点会发布/wpb_home/objects_3d话题，包含桌面物体的三维坐标信息。
启动自动抓取节点
新开终端，输入以下指令：
cd ~/ros2_zice source install/setup.bash ros2 run mani_pkg grab_object总结
本期博客我们结合前面完成的三维视觉点云物品检测功能，在Gazebo仿真环境中实现了桌面物品全自动抓取实验。
