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

機器人操作系統(tǒng)（ROS2）入門與實踐機器人操作系統(tǒng)（ROS2）入門與實踐第1章LinuxUbuntu入門基礎第2章ROS2安裝與系統(tǒng)架構第3章ROS2編程基礎第4章ROS2機器人運動控制第5章激光雷達在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第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中，用于描述機械臂狀態(tài)的消息格式為sensor_msgs::JointState。如圖所示，在官方Wiki頁面，對它的格式定義為：

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

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

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

“src”文件夾新建文件，命名為“mani_ctrl.cpp”。打開一個新的終端窗口，在工作空間中創(chuàng)建一個新的軟件包。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、設置編譯規(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機械臂控制

4、編譯軟件包cd~/ros2_wscolconbuildsourceinstall/setup.bashros2launchwpr_simulation2wpb_mani.launch.py11.2機械臂控制

11.2.2仿真運行機械臂控制程序sourceinstall/setup.bashros2runmani_pkgmani_ctrl打開第2個子窗口。11.2機械臂控制

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

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

確定了物品檢測的實現(xiàn)方案后，接下來設計這個實驗程序的整體流程：1）使用wpr_simulation2軟件包的objects_publisher節(jié)點，計算出桌面物品的質心坐標。2）根據(jù)物品的質心坐標，驅動機器人底盤移動，讓機器人正面對準物品。和桌子拉開

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

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