機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐 課件 第11章 ROS2的機(jī)械臂應(yīng)用

機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐第1章LinuxUbuntu入門基礎(chǔ)第2章ROS2安裝與系統(tǒng)架構(gòu)第3章ROS2編程基礎(chǔ)第4章ROS2機(jī)器人運(yùn)動(dòng)控制第5章激光雷達(dá)在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第8章ROS2中的NAV2自主導(dǎo)航第9章ROS2中的圖像視覺(jué)應(yīng)用第10章ROS2的三維視覺(jué)應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第11章

第11章ROS2的機(jī)械臂應(yīng)用11.2機(jī)械臂控制11.1ROS2里的機(jī)械臂消息包格式11.3使用機(jī)械臂進(jìn)行物品抓取11.4本章小結(jié)11.1ROS2里的機(jī)械臂消息包格式

在ROS2中，用于描述機(jī)械臂狀態(tài)的消息格式為sensor_msgs::JointState。如圖所示，在官方Wiki頁(yè)面，對(duì)它的格式定義為：

上述的后四個(gè)成員都是數(shù)組，通常每個(gè)數(shù)組都包含相同個(gè)數(shù)的成員，這樣可以保持一一對(duì)應(yīng)的關(guān)系。比如name[0]的關(guān)節(jié)，它對(duì)應(yīng)角度是position[0]，對(duì)應(yīng)速度是velocity[0]，以此類推。如果某個(gè)項(xiàng)目沒(méi)有數(shù)值，比如effort，可以讓這個(gè)數(shù)組的成員個(gè)數(shù)為零。11.2機(jī)械臂控制

機(jī)器人的手臂，只需要兩個(gè)關(guān)節(jié)控制量。一個(gè)是機(jī)械臂基座的升降高度，另一個(gè)就是手爪的指間寬度。如表所示，對(duì)應(yīng)sensor_msgs::JointState消息包成員：namepositionlift機(jī)械臂基座的升降高度（單位：米）gripper手爪的兩指間距（單位：米）

詳細(xì)操作步驟見(jiàn)教材P359-P370頁(yè)1、編寫節(jié)點(diǎn)代碼在VSCode中找到mani_pkg軟件包，在

“src”文件夾新建文件，命名為“mani_ctrl.cpp”。打開(kāi)一個(gè)新的終端窗口，在工作空間中創(chuàng)建一個(gè)新的軟件包。cd~/ros2_ws/srcros2pkgcreatemani_pkg11.2機(jī)械臂控制

11.2.1編寫機(jī)械臂控制程序#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機(jī)械臂控制

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機(jī)械臂控制

2、設(shè)置編譯規(guī)則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機(jī)械臂控制

4、編譯軟件包c(diǎn)d~/ros2_wscolconbuildsourceinstall/setup.bashros2launchwpr_simulation2wpb_mani.launch.py11.2機(jī)械臂控制

11.2.2仿真運(yùn)行機(jī)械臂控制程序sourceinstall/setup.bashros2runmani_pkgmani_ctrl打開(kāi)第2個(gè)子窗口。11.2機(jī)械臂控制

節(jié)點(diǎn)運(yùn)行起來(lái)之后，切換到仿真窗口，就能看到機(jī)器人的機(jī)械臂開(kāi)始運(yùn)動(dòng)了。先是從折疊狀態(tài)升上來(lái)，機(jī)械臂向前展開(kāi)。然后再降下去，機(jī)械臂恢復(fù)折疊狀態(tài)。以此往復(fù)，手臂在折疊和展開(kāi)狀態(tài)來(lái)回切換。11.3使用機(jī)械臂進(jìn)行物品抓取

在11.2節(jié)實(shí)驗(yàn)里，實(shí)現(xiàn)了機(jī)器人的機(jī)械臂控制。本節(jié)實(shí)驗(yàn)更進(jìn)一步，結(jié)合之前通過(guò)立體視覺(jué)進(jìn)行桌面物品檢測(cè)的功能，實(shí)現(xiàn)對(duì)物品的抓取。在wpr_simulation2軟件包中，有一個(gè)節(jié)點(diǎn)名為“objects_publisher”，已經(jīng)把物品檢測(cè)的算法封裝好了。只需要通過(guò)節(jié)點(diǎn)間的話題通訊就能獲取物品檢測(cè)的結(jié)果。11.3使用機(jī)械臂進(jìn)行物品抓取

確定了物品檢測(cè)的實(shí)現(xiàn)方案后，接下來(lái)設(shè)計(jì)這個(gè)實(shí)驗(yàn)程序的整體流程：1）使用wpr_simulation2軟件包的objects_publisher節(jié)點(diǎn)，計(jì)算出桌面物品的質(zhì)心坐標(biāo)。2）根據(jù)物品的質(zhì)心坐標(biāo)，驅(qū)動(dòng)機(jī)器人底盤移動(dòng)，讓機(jī)器人正面對(duì)準(zhǔn)物品。和桌子拉開(kāi)

一點(diǎn)距離，讓機(jī)械臂有抬升的空間。3）控制機(jī)械臂抬起，升高到物品的高度，和物品保持平齊。并張開(kāi)手爪，準(zhǔn)備抓取。4）機(jī)器人向前移動(dòng)，讓物品進(jìn)入手爪的兩指中間。閉合手爪，夾住物品。5）在夾住物品的狀態(tài)下，稍微提升一下機(jī)械臂，將物品抬高離開(kāi)桌面。6）最后退回到開(kāi)始的位置，物品抓取完成。根據(jù)這個(gè)流程，可以設(shè)計(jì)一個(gè)有限狀態(tài)機(jī)來(lái)進(jìn)行具體實(shí)現(xiàn)。

詳細(xì)操作步驟見(jiàn)教材P371-P393頁(yè)11.3使用機(jī)械臂進(jìn)行物品抓取狀態(tài)宏定義狀態(tài)行為STEP_WAIT機(jī)器人初始狀態(tài)，節(jié)點(diǎn)程序在這個(gè)狀態(tài)發(fā)送指令“startobjects”，激活objects_publisher節(jié)點(diǎn)的物品檢測(cè)功能。當(dāng)接收到objects_publisher節(jié)點(diǎn)的物品檢測(cè)結(jié)果時(shí)，切換到STEP_ALIGN_OBJ狀態(tài)。STEP_ALIGN_OBJ機(jī)器人左右平移，將自己的正前方對(duì)準(zhǔn)物品。完成物品對(duì)準(zhǔn)后，跳轉(zhuǎn)到STEP_HAND_UP狀態(tài)。STEP_HAND_UP機(jī)器人抬升機(jī)械臂，張開(kāi)手爪，準(zhǔn)備抓取物品。完成這個(gè)動(dòng)作后，跳轉(zhuǎn)到STEP_FORWARD狀態(tài)。STEP_FORWARD機(jī)器人根據(jù)抓取目標(biāo)物的距離，向前移動(dòng)，讓物品進(jìn)入機(jī)器人的手爪兩指中間。完成這個(gè)動(dòng)作后，跳轉(zhuǎn)到STEP_GRAB狀態(tài)。STEP_GRAB機(jī)器人閉合手爪，夾住物品。完成這個(gè)動(dòng)作后，跳轉(zhuǎn)到STEP_OBJ_UP狀態(tài)。STEP_OBJ_UP機(jī)器人手臂夾持著物品稍微向上抬升，讓物品離開(kāi)桌面。完成這個(gè)動(dòng)作后，跳轉(zhuǎn)到STEP_BACKWARD狀態(tài)。STEP_BACKWARD機(jī)器人帶著物品向后移動(dòng)一段距離，讓物品離開(kāi)桌面的上空，避免機(jī)器人移動(dòng)轉(zhuǎn)向時(shí)碰到桌面上其他物品。完成這個(gè)動(dòng)作后，跳轉(zhuǎn)到STEP_DONE狀態(tài)。STEP_DONE機(jī)器人完成抓取動(dòng)作，停止運(yùn)動(dòng)，抓取動(dòng)作執(zhí)行完畢。1、編寫節(jié)點(diǎn)代碼在VSCode中找到mani_pkg軟件包，在

“src”文件夾中新建文件，命名為“grab_object.cpp”。下面編寫這個(gè)代碼文件內(nèi)容：11.3使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取

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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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使用機(jī)械臂進(jìn)行物品抓取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