机器人操作系统（ROS2）入门与实践 课件 第12章 基于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的综合应用第12章12.3编写程序代码

第12章基于ROS2的综合应用12.2采集环境数据12.1方案设计12.4运行服务机器人应用12.5本章小结

经过前面章节的学习，已经实现了ROS2的环境建图、NAV2导航、物品检测和物品抓取。在这个章节中，将会把前面的功能组合起来，实现一个综合应用——家庭服务机器人的饮料的递送功能。

大致的任务脚本：机器人从客厅自主导航到厨房，抓取一瓶饮料。然后再自主导航到餐厅，将饮料送给客人。

在这一节实验里，将会使用已经封装好导航和物品抓取功能的独立节点来辅助完成最终任务。在使用过程中，可以体验ROS2的“节点网络”这种模块化分布式系统设计，在复杂任务的实现过程中所具备的优越性。第12章基于ROS2的综合应用12.1方案设计

程序的主体实现流程：1）给机器人设置好初始位置，向wp_map_tools软件包的wp_navi_server节点发送厨房饮料位置的航点名称，激活wp_navi_server节点的航点导航功能。等待wp_navi_server节点完成第一阶段从客厅到厨房的导航任务。2）接收到wp_navi_server节点发来的导航完成的信号之后，向wpr_simulation2软件包的grab_object_sim节点发送指令“startgrab”，激活grab_object_sim节点的物品抓取功能。等待grab_object_sim节点完成饮料抓取的任务。3）接收到grab_object_sim节点发来的物品抓取完成的信号之后，再次向wp_map_tools软件包的wp_navi_server节点发送餐厅客人位置的航点名称，激活wp_navi_server节点的航点导航功能。等待wp_navi_server节点完成第二阶段从厨房到餐厅的导航任务。4）接收到wp_navi_server节点发来的导航完成的信号，说明机器人成功将饮料带到餐厅客人处，饮料递送任务完成。12.1方案设计12.1方案设计状态宏定义状态行为STEP_WAIT机器人初始状态，节点程序在这个状态发送厨房饮料位置的航点名称给wp_navi_server节点，以激活航点导航功能。然后状态机跳转到STEP_GOTO_KITCHEN状态，等待第一阶段导航完成。STEP_GOTO_KITCHEN等待机器人去往厨房的状态。在这个状态下接收到导航完成的信号，会发送“startgrab”指令给grab_object_sim节点，以激活物品抓取功能。然后状态机跳转到STEP_GRAB_DRINK状态，等待抓取完成。STEP_GRAB_DRINK等待机器人抓取饮料的状态。在这个状态下接收到抓取完成的信号，会发送餐厅客人位置的航点名称给wp_navi_server节点，再次激活航点导航功能。然后状态机跳转到STEP_GOTO_GUEST状态，等待第二阶段导航完成。STEP_GOTO_GUEST等待机器人去往餐厅客人处的状态。在这个状态下接收到导航完成的信号，状态机会跳转到STEP_DONE状态，提示饮料已经送到，递送任务完成。STEP_DONE任务已经完成的状态。12.2采集环境数据12.2.1SLAM环境建图source~/ros2_ws/install/setup.bashros2launchwpr_simulation2slam.launch.py打开一个终端：source~/ros2_ws/install/setup.bashros2runwpr_simulation2keyboard_vel_cmd打开第2个子窗口：12.2采集环境数据用键盘控制机器人在场景里巡游一遍之后，可以看到建好的地图。12.2采集环境数据

保持Terminator终端的第二个窗口标题为红色。按下键盘的X键，会退出键盘控制程序。然后执行如下指令保存地图到文件：ros2runnav2_map_servermap_saver_cli-fmap这样会在终端窗口的当前路径下创建两个地图文件：map.pgm和map.yaml。将这两个文件拷贝到wpr_simulation2的maps文件夹下，一会从这个文件夹加载地图文件。12.2采集环境数据source~/ros2_ws/install/setup.bashros2launchwp_map_toolsadd_waypoint_sim.launch.py打开一个终端：12.2.2设置航点坐标12.2采集环境数据在RViz2工具栏的右边，单击[AddWaypoint]按钮，就可以在地图上添加航点。

第一个航点在厨房的饮料桌子前，距离桌子1米（对应地面上一个格子的距离）；第二个航点，在厨房的客人面前，距离客人1米（对应地面上的一个格子的距离）。12.2采集环境数据source~/ros2_ws/install/setup.bashros2runwp_map_toolswp_saver打开第2个子窗口：执行完毕后，在主文件夹下会生成一个名为“waypoints.yaml”的文件。双击打开这个文件对它的内容进行编辑，修改其中的航点名称：把Waypoint_1的Name，从“1”修改为“kitchen”，这是抓取饮料的位置。把Waypoint_2的Name，从“2”修改为“guest”，这是最终递送饮料给客人的位置。12.3编写程序代码12.3.1Launch文件编写启动终端Terminator:1、创建软件包cd~/ros2_ws/srcros2pkgcreatehome_pkg在软件包中创建一个Launch文件，命名为“launch”。在[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文件里，添加如下安装规则：3、设置安装规则install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})4、编译软件包cd~/ros2_wscolconbuild12.3编写程序代码12.3.2服务机器人程序实现1、编写节点代码在VSCode中找到home_pkg软件包，在

“src”文件夹新建文件，命名为“fetch.cpp”。下面编写这个代码文件内容：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()寻找节点代码中用到的依赖项：：2、设置编译规则install(TARGETSfetchDESTINATIONlib/${PROJECT_N