機器人操作系統(tǒng)（ROS2）入門與實踐 課件 第12章 基于ROS2的綜合應(yīng)用

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

第12章基于ROS2的綜合應(yīng)用12.2采集環(huán)境數(shù)據(jù)12.1方案設(shè)計12.4運行服務(wù)機器人應(yīng)用12.5本章小結(jié)

經(jīng)過前面章節(jié)的學習，已經(jīng)實現(xiàn)了ROS2的環(huán)境建圖、NAV2導航、物品檢測和物品抓取。在這個章節(jié)中，將會把前面的功能組合起來，實現(xiàn)一個綜合應(yīng)用——家庭服務(wù)機器人的飲料的遞送功能。

大致的任務(wù)腳本：機器人從客廳自主導航到廚房，抓取一瓶飲料。然后再自主導航到餐廳，將飲料送給客人。

在這一節(jié)實驗里，將會使用已經(jīng)封裝好導航和物品抓取功能的獨立節(jié)點來輔助完成最終任務(wù)。在使用過程中，可以體驗ROS2的“節(jié)點網(wǎng)絡(luò)”這種模塊化分布式系統(tǒng)設(shè)計，在復雜任務(wù)的實現(xiàn)過程中所具備的優(yōu)越性。第12章基于ROS2的綜合應(yīng)用12.1方案設(shè)計

程序的主體實現(xiàn)流程：1）給機器人設(shè)置好初始位置，向wp_map_tools軟件包的wp_navi_server節(jié)點發(fā)送廚房飲料位置的航點名稱，激活wp_navi_server節(jié)點的航點導航功能。等待wp_navi_server節(jié)點完成第一階段從客廳到廚房的導航任務(wù)。2）接收到wp_navi_server節(jié)點發(fā)來的導航完成的信號之后，向wpr_simulation2軟件包的grab_object_sim節(jié)點發(fā)送指令“startgrab”，激活grab_object_sim節(jié)點的物品抓取功能。等待grab_object_sim節(jié)點完成飲料抓取的任務(wù)。3）接收到grab_object_sim節(jié)點發(fā)來的物品抓取完成的信號之后，再次向wp_map_tools軟件包的wp_navi_server節(jié)點發(fā)送餐廳客人位置的航點名稱，激活wp_navi_server節(jié)點的航點導航功能。等待wp_navi_server節(jié)點完成第二階段從廚房到餐廳的導航任務(wù)。4）接收到wp_navi_server節(jié)點發(fā)來的導航完成的信號，說明機器人成功將飲料帶到餐廳客人處，飲料遞送任務(wù)完成。12.1方案設(shè)計12.1方案設(shè)計狀態(tài)宏定義狀態(tài)行為STEP_WAIT機器人初始狀態(tài)，節(jié)點程序在這個狀態(tài)發(fā)送廚房飲料位置的航點名稱給wp_navi_server節(jié)點，以激活航點導航功能。然后狀態(tài)機跳轉(zhuǎn)到STEP_GOTO_KITCHEN狀態(tài)，等待第一階段導航完成。STEP_GOTO_KITCHEN等待機器人去往廚房的狀態(tài)。在這個狀態(tài)下接收到導航完成的信號，會發(fā)送“startgrab”指令給grab_object_sim節(jié)點，以激活物品抓取功能。然后狀態(tài)機跳轉(zhuǎn)到STEP_GRAB_DRINK狀態(tài)，等待抓取完成。STEP_GRAB_DRINK等待機器人抓取飲料的狀態(tài)。在這個狀態(tài)下接收到抓取完成的信號，會發(fā)送餐廳客人位置的航點名稱給wp_navi_server節(jié)點，再次激活航點導航功能。然后狀態(tài)機跳轉(zhuǎn)到STEP_GOTO_GUEST狀態(tài)，等待第二階段導航完成。STEP_GOTO_GUEST等待機器人去往餐廳客人處的狀態(tài)。在這個狀態(tài)下接收到導航完成的信號，狀態(tài)機會跳轉(zhuǎn)到STEP_DONE狀態(tài)，提示飲料已經(jīng)送到，遞送任務(wù)完成。STEP_DONE任務(wù)已經(jīng)完成的狀態(tài)。12.2采集環(huán)境數(shù)據(jù)12.2.1SLAM環(huán)境建圖source~/ros2_ws/install/setup.bashros2launchwpr_simulation2slam.launch.py打開一個終端：source~/ros2_ws/install/setup.bashros2runwpr_simulation2keyboard_vel_cmd打開第2個子窗口：12.2采集環(huán)境數(shù)據(jù)用鍵盤控制機器人在場景里巡游一遍之后，可以看到建好的地圖。12.2采集環(huán)境數(shù)據(jù)

保持Terminator終端的第二個窗口標題為紅色。按下鍵盤的X鍵，會退出鍵盤控制程序。然后執(zhí)行如下指令保存地圖到文件：ros2runnav2_map_servermap_saver_cli-fmap這樣會在終端窗口的當前路徑下創(chuàng)建兩個地圖文件：map.pgm和map.yaml。將這兩個文件拷貝到wpr_simulation2的maps文件夾下，一會從這個文件夾加載地圖文件。12.2采集環(huán)境數(shù)據(jù)source~/ros2_ws/install/setup.bashros2launchwp_map_toolsadd_waypoint_sim.launch.py打開一個終端：12.2.2設(shè)置航點坐標12.2采集環(huán)境數(shù)據(jù)在RViz2工具欄的右邊，單擊[AddWaypoint]按鈕，就可以在地圖上添加航點。

第一個航點在廚房的飲料桌子前，距離桌子1米（對應(yīng)地面上一個格子的距離）；第二個航點，在廚房的客人面前，距離客人1米（對應(yīng)地面上的一個格子的距離）。12.2采集環(huán)境數(shù)據(jù)source~/ros2_ws/install/setup.bashros2runwp_map_toolswp_saver打開第2個子窗口：執(zhí)行完畢后，在主文件夾下會生成一個名為“waypoints.yaml”的文件。雙擊打開這個文件對它的內(nèi)容進行編輯，修改其中的航點名稱：把Waypoint_1的Name，從“1”修改為“kitchen”，這是抓取飲料的位置。把Waypoint_2的Name，從“2”修改為“guest”，這是最終遞送飲料給客人的位置。12.3編寫程序代碼12.3.1Launch文件編寫啟動終端Terminator:1、創(chuàng)建軟件包cd~/ros2_ws/srcros2pkgcreatehome_pkg在軟件包中創(chuàng)建一個Launch文件，命名為“l(fā)aunch”。在[launch]文件夾新建文件，命名為“home.launch.py”。2、編寫Launch文件12.3編寫程序代碼importosfromlaunchimportLaunchDescriptionfromlaunch_ros.actionsimportNodefromament_index_python.packagesimportget_package_share_directoryfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSource

defgenerate_launch_description():launch_file_dir=os.path.join(get_package_share_directory('wpr_simulation2'),'launch')home_mani_cmd=IncludeLaunchDescription(PythonLaunchDescriptionSource(os.path.join(launch_file_dir,'robocup_home_mani.launch.py')))12.3編寫程序代碼

map_file=os.path.join(get_package_share_directory('wpr_simulation2'),'maps','map.yaml')

nav_param_file=os.path.join(get_package_share_directory('wpr_simulation2'),'config','nav2_params.yaml')

nav2_launch_dir=os.path.join(get_package_share_directory('nav2_bringup'),'launch')12.3編寫程序代碼navigation_cmd=IncludeLaunchDescription(PythonLaunchDescriptionSource([nav2_launch_dir,'/bringup_launch.py']),launch_arguments={'map':map_file,'use_sim_time':"True",'params_file':nav_param_file}.items(),)

wp_edit_cmd=Node(package='wp_map_tools',executable='wp_edit_node',name='wp_edit_node')

wp_navi_server_cmd=Node(package='wp_map_tools',executable='wp_navi_server',name='wp_navi_server')12.3編寫程序代碼objects_publisher_cmd=Node(package='wpr_simulation2',executable='objects_publisher',name='objects_publisher',parameters=[{"auto_start":False}])

grab_object_cmd=Node(package='wpr_simulation2',executable='grab_object_sim',name='grab_object_sim')12.3編寫程序代碼rviz_file=os.path.join(get_package_share_directory('wpr_simulation2'),'rviz','fetch.rviz')rviz_cmd=Node(package='rviz2',executable='rviz2',name='rviz2',arguments=['-d',rviz_file])

ld=LaunchDescription()ld.add_action(home_mani_cmd)ld.add_action(navigation_cmd)ld.add_action(wp_edit_cmd)ld.add_action(wp_navi_server_cmd)ld.add_action(objects_publisher_cmd)ld.add_action(grab_object_cmd)ld.add_action(rviz_cmd)

returnld12.3編寫程序代碼在home_pkg的CMakeLists.txt文件里，添加如下安裝規(guī)則：3、設(shè)置安裝規(guī)則install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})4、編譯軟件包cd~/ros2_wscolconbuild12.3編寫程序代碼12.3.2服務(wù)機器人程序?qū)崿F(xiàn)1、編寫節(jié)點代碼在VSCode中找到home_pkg軟件包，在

“src”文件夾新建文件，命名為“fetch.cpp”。下面編寫這個代碼文件內(nèi)容：12.3編寫程序代碼#include<rclcpp/rclcpp.hpp>#include<std_msgs/msg/string.hpp>

#defineSTEP_WAIT0#defineSTEP_GOTO_KITCHEN1#defineSTEP_GRAB_DRINK2#defineSTEP_GOTO_GUEST3#defineSTEP_DONE4staticintfetch_step=STEP_WAIT;

std::shared_ptr<rclcpp::Node>node;rclcpp::Publisher<std_msgs::msg::String>::SharedPtrnavi_pub;rclcpp::Publisher<std_msgs::msg::String>::SharedPtrbehavior_pub;12.3編寫程序代碼voidNaviResultCallback(conststd_msgs::msg::String::SharedPtrmsg){RCLCPP_INFO(node->get_logger(),"[NaviResultCallback]%s",msg->data.c_str());if(fetch_step==STEP_GOTO_KITCHEN&&msg->data=="navidone"){std_msgs::msg::Stringmsg;msg.data="startgrab";behavior_pub->publish(msg);fetch_step=STEP_GRAB_DRINK;RCLCPP_INFO(node->get_logger(),"[STEP_GOTO_KITCHEN]->[STEP_GRAB_DRINK]");}

if(fetch_step==STEP_GOTO_GUEST&&msg->data=="navidone"){fetch_step=STEP_DONE;RCLCPP_INFO(node->get_logger(),"[STEP_GOTO_GUEST]->[STEP_DONE]");}}12.3編寫程序代碼voidGrabResultCallback(conststd_msgs::msg::String::SharedPtrmsg){RCLCPP_INFO(node->get_logger(),"[GrabResultCallback]%s",msg->data.c_str());if(fetch_step==STEP_GRAB_DRINK&&msg->data=="grabdone"){std_msgs::msg::Stringmsg;msg.data="guest";navi_pub->publish(msg);fetch_step=STEP_GOTO_GUEST;RCLCPP_INFO(node->get_logger(),"[STEP_GRAB_DRINK]->[STEP_GOTO_GUEST]");}}12.3編寫程序代碼intmain(intargc,char**argv){rclcpp::init(argc,argv);

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

navi_pub=node->create_publisher<std_msgs::msg::String>("/waterplus/navi_waypoint",10);behavior_pub=node->create_publisher<std_msgs::msg::String>("/wpb_home/behavior",10);autonavi_result_sub=node->create_subscription<std_msgs::msg::String>("waterplus/navi_result",10,NaviResultCallback);autograb_result_sub=node->create_subscription<std_msgs::msg::String>("/wpb_home/grab_result",10,GrabResultCallback);12.3編寫程序代碼rclcpp::sleep_for(std::chrono::milliseconds(1000));

rclcpp::Rateloop_rate(30);

while(rclcpp::ok()){if(fetch_step==STEP_WAIT){std_msgs::msg::Stringmsg;msg.data="kitchen";navi_pub->publish(msg);fetch_step=STEP_GOTO_KITCHEN;RCLCPP_INFO(node->get_logger(),"[STEP_WAIT]->[STEP_GOTO_KITCHEN]");}rclcpp::spin_some(node);loop_rate.sleep();}

rclcpp::shutdown();

return0;}12.3編寫程序代碼

在home_pkg的CMakeLists.txt文件里，使用find_package()尋找節(jié)點代碼中用到的依賴項：：2、設(shè)置編譯規(guī)則install(TARGETSfetchDESTINATIONlib/${PROJECT_N