机器人操作系统（ROS2）入门与实践 课件 第11章 ROS2的机械臂应用

机器人操作系统（ROS2）入门与实践机器人操作系统（ROS2）入门与实践第1章LinuxUbuntu入门基础第2章ROS2安装与系统架构第3章ROS2编程基础第4章ROS2机器人运动控制第5章激光雷达在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM环境建图第8章ROS2中的NAV2自主导航第9章ROS2中的图像视觉应用第10章ROS2的三维视觉应用第11章ROS2的机械臂应用第12章基于ROS2的综合应用第11章

第11章ROS2的机械臂应用11.2机械臂控制11.1ROS2里的机械臂消息包格式11.3使用机械臂进行物品抓取11.4本章小结11.1ROS2里的机械臂消息包格式

在ROS2中，用于描述机械臂状态的消息格式为sensor_msgs::JointState。如图所示，在官方Wiki页面，对它的格式定义为：

上述的后四个成员都是数组，通常每个数组都包含相同个数的成员，这样可以保持一一对应的关系。比如name[0]的关节，它对应角度是position[0]，对应速度是velocity[0]，以此类推。如果某个项目没有数值，比如effort，可以让这个数组的成员个数为零。11.2机械臂控制

机器人的手臂，只需要两个关节控制量。一个是机械臂基座的升降高度，另一个就是手爪的指间宽度。如表所示，对应sensor_msgs::JointState消息包成员：namepositionlift机械臂基座的升降高度（单位：米）gripper手爪的两指间距（单位：米）

详细操作步骤见教材P359-P370页1、编写节点代码在VSCode中找到mani_pkg软件包，在

“src”文件夹新建文件，命名为“mani_ctrl.cpp”。打开一个新的终端窗口，在工作空间中创建一个新的软件包。cd~/ros2_ws/srcros2pkgcreatemani_pkg11.2机械臂控制

11.2.1编写机械臂控制程序#include"rclcpp/rclcpp.hpp"#include"sensor_msgs/msg/joint_state.hpp"

std::shared_ptr<rclcpp::Node>node;

intmain(intargc,char*argv[]){rclcpp::init(argc,argv);

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

automani_pub=node->create_publisher<sensor_msgs::msg::JointState>("/wpb_home/mani_ctrl",10);

sensor_msgs::msg::JointStatemani_msg;mani_.resize(2);mani_[0]="lift";mani_[1]="gripper";mani_msg.position.resize(2);mani_msg.position[0]=0.0;mani_msg.position[1]=0.0;11.2机械臂控制

rclcpp::Rateloop_rate(0.1);

while(rclcpp::ok()){RCLCPP_WARN(node->get_logger(),"Pose1");mani_msg.position[0]=0.0;mani_msg.position[1]=0.01;mani_pub->publish(mani_msg);loop_rate.sleep();

RCLCPP_WARN(node->get_logger(),"Pose2");mani_msg.position[0]=1.0;mani_msg.position[1]=0.1;mani_pub->publish(mani_msg);loop_rate.sleep();}

rclcpp::shutdown();

return0;}11.2机械臂控制

2、设置编译规则find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)add_executable(mani_ctrlsrc/mani_ctrl.cpp)ament_target_dependencies(mani_ctrl"rclcpp""sensor_msgs")install(TARGETSmani_ctrlDESTINATIONlib/${PROJECT_NAME})3、修改软件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend>11.2机械臂控制

4、编译软件包cd~/ros2_wscolconbuildsourceinstall/setup.bashros2launchwpr_simulation2wpb_mani.launch.py11.2机械臂控制

11.2.2仿真运行机械臂控制程序sourceinstall/setup.bashros2runmani_pkgmani_ctrl打开第2个子窗口。11.2机械臂控制

节点运行起来之后，切换到仿真窗口，就能看到机器人的机械臂开始运动了。先是从折叠状态升上来，机械臂向前展开。然后再降下去，机械臂恢复折叠状态。以此往复，手臂在折叠和展开状态来回切换。11.3使用机械臂进行物品抓取

在11.2节实验里，实现了机器人的机械臂控制。本节实验更进一步，结合之前通过立体视觉进行桌面物品检测的功能，实现对物品的抓取。在wpr_simulation2软件包中，有一个节点名为“objects_publisher”，已经把物品检测的算法封装好了。只需要通过节点间的话题通讯就能获取物品检测的结果。11.3使用机械臂进行物品抓取

确定了物品检测的实现方案后，接下来设计这个实验程序的整体流程：1）使用wpr_simulation2软件包的objects_publisher节点，计算出桌面物品的质心坐标。2）根据物品的质心坐标，驱动机器人底盘移动，让机器人正面对准物品。和桌子拉开

一点距离，让机械臂有抬升的空间。3）控制机械臂抬起，升高到物品的高度，和物品保持平齐。并张开手爪，准备抓取。4）机器人向前移动，让物品进入手爪的两指中间。闭合手爪，夹住物品。5）在夹住物品的状态下，稍微提升一下机械臂，将物品抬高离开桌面。6）最后退回到开始的位置，物品抓取完成。根据这个流程，可以设计一个有限状态机来进行具体实现。

详细操作步骤见教材P371-P393页11.3使用机械臂进行物品抓取状态宏定义状态行为STEP_WAIT机器人初始状态，节点程序在这个状态发送指令“startobjects”，激活objects_publisher节点的物品检测功能。当接收到objects_publisher节点的物品检测结果时，切换到STEP_ALIGN_OBJ状态。STEP_ALIGN_OBJ机器人左右平移，将自己的正前方对准物品。完成物品对准后，跳转到STEP_HAND_UP状态。STEP_HAND_UP机器人抬升机械臂，张开手爪，准备抓取物品。完成这个动作后，跳转到STEP_FORWARD状态。STEP_FORWARD机器人根据抓取目标物的距离，向前移动，让物品进入机器人的手爪两指中间。完成这个动作后，跳转到STEP_GRAB状态。STEP_GRAB机器人闭合手爪，夹住物品。完成这个动作后，跳转到STEP_OBJ_UP状态。STEP_OBJ_UP机器人手臂夹持着物品稍微向上抬升，让物品离开桌面。完成这个动作后，跳转到STEP_BACKWARD状态。STEP_BACKWARD机器人带着物品向后移动一段距离，让物品离开桌面的上空，避免机器人移动转向时碰到桌面上其他物品。完成这个动作后，跳转到STEP_DONE状态。STEP_DONE机器人完成抓取动作，停止运动，抓取动作执行完毕。1、编写节点代码在VSCode中找到mani_pkg软件包，在

“src”文件夹中新建文件，命名为“grab_object.cpp”。下面编写这个代码文件内容：11.3使用机械臂进行物品抓取11.3.1编写物品抓取程序#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>

#defineSTEP_WAIT0#defineSTEP_ALIGN_OBJ1#defineSTEP_HAND_UP2#defineSTEP_FORWARD3#defineSTEP_GRAB4#defineSTEP_OBJ_UP5#defineSTEP_BACKWARD6#defineSTEP_DONE7staticintgrab_step=STEP_WAIT;11.3使用机械臂进行物品抓取std::shared_ptr<rclcpp::Node>node;rclcpp::Publisher<std_msgs::msg::String>::SharedPtrcmd_pub;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtrvel_pub;rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtrmani_pub;

floatobject_x=0.0;floatobject_y=0.0;floatobject_z=0.0;intcount=0;

floatalign_x=1.0;floatalign_y=0.0;11.3使用机械臂进行物品抓取voidObjectCallback(constwpr_simulation2::msg::Object::SharedPtrmsg){if(grab_step==STEP_WAIT){grab_step=STEP_ALIGN_OBJ;}if(grab_step==STEP_ALIGN_OBJ){object_x=msg->x[0];object_y=msg->y[0];object_z=msg->z[0];}}11.3使用机械臂进行物品抓取intmain(intargc,char**argv){rclcpp::init(argc,argv);

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

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);11.3使用机械臂进行物品抓取autoobject_sub=node->create_subscription<wpr_simulation2::msg::Object>("/wpb_home/objects_3d",10,ObjectCallback);

rclcpp::Rateloop_rate(30);

while(rclcpp::ok()){if(grab_step==STEP_WAIT){std_msgs::msg::Stringstart_msg;start_msg.data="startobjects";cmd_pub->publish(start_msg);}11.3使用机械臂进行物品抓取

if(grab_step==STEP_ALIGN_OBJ){floatdiff_x=object_x-align_x;floatdiff_y=object_y-align_y;geometry_msgs::msg::Twistvel_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::Stringstart_msg;start_msg.data="stopobjects";cmd_pub->publish(start_msg);grab_step=STEP_HAND_UP;}RCLCPP_INFO(node->get_logger(),"[STEP_ALIGN_OBJ]vel=(%.2f,%.2f)",vel_msg.linear.x,vel_msg.linear.y);vel_pub->publish(vel_msg);}11.3使用机械臂进行物品抓取if(grab_step==STEP_HAND_UP){RCLCPP_INFO(node->get_logger(),"[STEP_HAND_UP]");sensor_msgs::msg::JointStatemani_msg;mani_.resize(2);mani_[0]="lift";mani_[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(8000));grab_step=STEP_FORWARD;}11.3使用机械臂进行物品抓取if(grab_step==STEP_FORWARD){RCLCPP_INFO(node->get_logger(),"[STEP_FORWARD]object_x=%.2f",object_x);geometry_msgs::msg::Twistvel_msg;for(size_ti=0;i<80;i++){vel_msg.linear.x=0.1;vel_msg.linear.y=0;vel_pub->publish(vel_msg);rclcpp::sleep_for(std::chrono::milliseconds(100));}grab_step=STEP_GRAB;}11.3使用机械臂进行物品抓取if(grab_step==STEP_GRAB){RCLCPP_INFO(node->get_logger(),"[STEP_GRAB]");geometry_msgs::msg::Twistvel_msg;vel_msg.linear.x=0;vel_msg.linear.y=0;vel_pub->publish(vel_msg);sensor_msgs::msg::JointStatemani_msg;mani_.resize(2);mani_[0]="lift";mani_[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(5000));grab_step=STEP_OBJ_UP;}11.3使用机械臂进行物品抓取if(grab_step==STEP_OBJ_UP){RCLCPP_INFO(node->get_logger(),"[STEP_OBJ_UP]");sensor_msgs::msg::JointStatemani_msg;mani_.resize(2);mani_[0]="lift";mani_[1]="gripper";mani_msg.position.resize(2);mani_msg.position[0]=object_z+0.05;mani_msg.position[1]=0.07;mani_pub->publish(mani_msg);rclcpp::sleep_for(std::chrono::milliseconds(5000));grab_step=STEP_BACKWARD;}11.3使用机械臂进行物品抓取if(grab_step==STEP_BACKWARD){RCLCPP_INFO(node->get_logger(),"[STEP_BACKWARD]");geometry_msgs::msg::Twistvel_msg;vel_msg.linear.x=-0.1;vel_msg.linear.y=0;vel_pub->publish(vel_msg);rclcpp::sleep_for(std::chrono::milliseconds(10000));grab_step=STEP_DONE;RCLCPP_INFO(node->get_logger(),"[STEP_DONE]");}if(grab_step==STEP_DONE){geometry_msgs::msg::Twistvel_msg;vel_msg.linear.x=0;vel_msg.linear.y=0;vel_pub->publish(vel_msg);}rclcpp::spin_some(node);loop_rate.sleep