機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐 課件 第8-12章 ROS2中的NAV2自主導(dǎo)航 -基于ROS2的綜合應(yīng)用

機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐機(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)動控制第5章激光雷達(dá)在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第8章ROS2中的NAV2自主導(dǎo)航第9章ROS2中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第8章8.3開源導(dǎo)航插件的使用

第8章ROS2中的NAV2自主導(dǎo)航8.2使用NAV2進(jìn)行自主導(dǎo)航8.1NAV2的組成結(jié)構(gòu)8.4本章小結(jié)8.1NAV2的組成結(jié)構(gòu)1、MapServer地圖服務(wù)器2、Planner路徑規(guī)劃器3、Smoother路徑平滑器4、AMCL定位器5、Controller運(yùn)動控制器6、Recovery導(dǎo)航恢復(fù)行為7、VelocitySmoother速度平滑器8、BTNavigator行為樹管理器8.1NAV2的組成結(jié)構(gòu)1、MapServer地圖服務(wù)器:導(dǎo)航需要地圖作為路徑規(guī)劃的依據(jù)，這個地圖數(shù)據(jù)是由MapServer地圖服務(wù)器提供了。它會將上一章保存的地圖文件加載進(jìn)來，如所示，然后發(fā)布到“/map”話題中，供其他節(jié)點(diǎn)訂閱獲取。8.1NAV2的組成結(jié)構(gòu)2、Planner路徑規(guī)劃器:路徑規(guī)劃器Planner負(fù)責(zé)生成全局導(dǎo)航路徑。它先從地圖服務(wù)器的“/map”話題獲取全局地圖，再從機(jī)器人的激光雷達(dá)話題獲取雷達(dá)測距的障礙物點(diǎn)云。將兩者疊加后，生成避障用的全局代價地圖。8.1NAV2的組成結(jié)構(gòu)如圖所示，全局代價地圖里，在障礙物的邊緣會膨脹出一層半透明的漸變區(qū)域，這個代表的就是機(jī)器人可能與障礙物發(fā)生碰撞的隱性“代價”。越靠近障礙物，與障礙物碰撞的風(fēng)險越大，于是顏色越鮮艷，隱性“代價”越大。生成了代價地圖之后，路徑規(guī)劃器Planner使用A*和Dijkstra’s這類路徑規(guī)劃算法生成一條連接起始點(diǎn)和目標(biāo)點(diǎn)的路徑曲線（對應(yīng)圖中的①），這就是規(guī)劃出來的全局導(dǎo)航路徑。8.1NAV2的組成結(jié)構(gòu)3、Smoother路徑平滑器:路徑規(guī)劃器Planner生成的路徑曲線是一條理論路線，有可能包含一些急轉(zhuǎn)彎、銳角轉(zhuǎn)折等特征（對應(yīng)圖中的②）。需要路徑平滑器Smoother對路徑進(jìn)行優(yōu)化，在不碰撞障礙物情況下讓路徑的折線部分盡可能圓潤。

同時在一些危險路段讓路徑線路盡可能遠(yuǎn)離障礙物，避免機(jī)器人因?yàn)槎ㄎ徽`差或者控制不善導(dǎo)致與障礙物發(fā)送剮蹭。8.1NAV2的組成結(jié)構(gòu)4、AMCL定位器：有了導(dǎo)航路徑之后，還需要機(jī)器人知道自己在地圖中的位置。在NAV2中，使用的AMCL進(jìn)行機(jī)器人的自我定位，這是一種基于概率統(tǒng)計的粒子濾波算法。8.1NAV2的組成結(jié)構(gòu)8.1NAV2的組成結(jié)構(gòu)5、Controller運(yùn)動控制器：運(yùn)動控制器會從路徑平滑器Smoother獲取優(yōu)化后的最終路徑曲線（對應(yīng)圖中的③），然后根據(jù)AMCL定位器給出的機(jī)器人定位信息，規(guī)劃出機(jī)器人當(dāng)前位置的運(yùn)動策略，盡量貼合這條路徑曲線。運(yùn)動控制器Controller按照策略計算出機(jī)器人的運(yùn)動速度，然后通過話題發(fā)送速度消息包給速度平滑器（對應(yīng)圖中的④）。最終由速度平滑器來控制機(jī)器人進(jìn)行運(yùn)動。8.1NAV2的組成結(jié)構(gòu)6、Recovery導(dǎo)航恢復(fù)行為：導(dǎo)航恢復(fù)行為Recovery的作用是讓機(jī)器人從極端情況下脫離險境。這些恢復(fù)行為包括：ClearingActions、nav2_behaviors/Spin、nav2_behaviors/Wait、nav2_behaviors/BackUp，這些行為也是通過話題發(fā)送速度消息包給速度平滑器（對應(yīng)圖中的⑥），由速度平滑器來驅(qū)動機(jī)器人進(jìn)行移動的。8.1NAV2的組成結(jié)構(gòu)7、VelocitySmoother速度平滑器：速度平滑器的作用是將上游運(yùn)動控制器發(fā)送來的速度進(jìn)行平滑處理，避免出現(xiàn)控制數(shù)值的突變，盡量保護(hù)硬件設(shè)備的安全運(yùn)行。速度平滑器通常會運(yùn)行在一個比較高的頻率，數(shù)倍于上游運(yùn)動控制器的控制頻率。這樣它就能夠?qū)⑸嫌慰刂瓢l(fā)來的速度值，進(jìn)行離散插值，將一個突變的數(shù)值變化過程，分解成一個逐步變化的過程。8.1NAV2的組成結(jié)構(gòu)8、BTNavigator行為樹管理器：上圖描述的只是NAV2的默認(rèn)處理流程，這個流程是通過一種名為BehaviorTree行為樹的形式來組織的。NAV2的默認(rèn)行為樹已經(jīng)設(shè)計得相當(dāng)簡潔完善。簡單調(diào)整一些參數(shù)數(shù)值，就能夠滿足大部分導(dǎo)航任務(wù)的要求，直接使用就行。8.2使用NAV2進(jìn)行自主導(dǎo)航

要在ROS2中使用NAV2，需要先安裝相應(yīng)的軟件包。在終端中執(zhí)行如下指令；sudoaptinstallros-humble-navigation2ROS2官方建議安裝一個NAV2的Bringup軟件包，借助這個軟件包的Launch文件來啟動NAV2。如圖8-9所示，這個軟件包的安裝指令是：

sudoaptinstallros-humble-nav2-bringup使用NAV2進(jìn)行自主導(dǎo)航的詳細(xì)操作步驟：見教材P208-P231頁8.2.1NAV2的安裝8.2使用NAV2進(jìn)行自主導(dǎo)航8.2.2使用NAV2實(shí)現(xiàn)自主導(dǎo)航1、準(zhǔn)備地圖文件source~/ros2_ws/install/setup.bashros2launchwpr_simulation2slam.launch.py啟動終端Terminator:打開第二個命令行窗口:source~/ros2_ws/install/setup.bashros2runwpr_simulation2keyboard_vel_cmd保持Terminator終端窗口位于所有窗口的前邊，且第二個窗口的標(biāo)題欄為紅色，這樣才能讓鍵盤控制節(jié)點(diǎn)始終能夠接收到鍵盤按下的信號

。

機(jī)器人在場景里巡游一遍之后，可以看到建好的地圖了。8.2使用NAV2進(jìn)行自主導(dǎo)航ros2runnav2_map_servermap_saver_cli-fmap在終端窗口的當(dāng)前路徑下創(chuàng)建兩個地圖文件：map.pgm和map.yaml。將這兩個文件拷貝到wpr_simulation2的maps文件夾下，后面將從這個文件夾加載地圖文件。8.2使用NAV2進(jìn)行自主導(dǎo)航啟動終端Terminator:2、編寫導(dǎo)航Launch文件cd~/ros2_ws/srcros2pkgcreatenav_pkg在軟件包中創(chuàng)建一個Launch文件，命名為“l(fā)aunch”。在[launch]文件夾新建文件，命名為“nav.launch.py”。8.2使用NAV2進(jìn)行自主導(dǎo)航importosfromlaunchimportLaunchDescription

fromlaunch_ros.actionsimportNodefromament_index_python.packagesimportget_package_share_directoryfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSource

defgenerate_launch_description():

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')8.2使用NAV2進(jìn)行自主導(dǎo)航

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(),)

rviz_file=os.path.join(get_package_share_directory('wpr_simulation2'),'rviz','navi.rviz')

rviz_cmd=Node(package='rviz2',executable='rviz2',name='rviz2',arguments=['-d',rviz_file])

ld=LaunchDescription()

ld.add_action(navigation_cmd)

ld.add_action(rviz_cmd)

returnld8.2使用NAV2進(jìn)行自主導(dǎo)航在nav_pkg的CMakeLists.txt文件里，添加如下安裝規(guī)則：3、設(shè)置安裝規(guī)則install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild8.2使用NAV2進(jìn)行自主導(dǎo)航8.2.3仿真環(huán)境運(yùn)行自主導(dǎo)航sourceinstall/setup.bashros2launchwpr_simulation2robocup_home.launch.py8.2使用NAV2進(jìn)行自主導(dǎo)航打開第二個命令行窗口:sourceinstall/setup.bashros2launchnav_pkgnav.launch.py8.2使用NAV2進(jìn)行自主導(dǎo)航設(shè)置機(jī)器人初始位置需要使用RViz2的工具欄里的[2DPosEstimate]按鈕。8.2使用NAV2進(jìn)行自主導(dǎo)航設(shè)置好機(jī)器人的初始位置后，使用Rviz2界面上方工具條里的[Nav2Goal]按鈕，為機(jī)器人指定導(dǎo)航的目標(biāo)地點(diǎn)和朝向。全局規(guī)劃器會自動規(guī)劃出一條紫色的路徑。這條路徑從機(jī)器人當(dāng)前點(diǎn)出發(fā)，避開障礙物，一直到導(dǎo)航目標(biāo)點(diǎn)結(jié)束。8.2使用NAV2進(jìn)行自主導(dǎo)航路徑規(guī)劃完成后，機(jī)器人模型會開始沿著這條路徑移動。切換到仿真窗口，可以看到機(jī)器人也開始沿著這條路徑移動。機(jī)器人到終點(diǎn)后，會原地旋轉(zhuǎn)，調(diào)整航向角，如圖8-44所示，最終朝向剛才設(shè)置目標(biāo)點(diǎn)時綠色箭頭的方向。8.3開源導(dǎo)航插件的使用

本節(jié)介紹一款開源的地圖導(dǎo)航可視化插件，可以在地圖上設(shè)置多個目標(biāo)航點(diǎn)。然后通過簡單的消息發(fā)送，就能驅(qū)使機(jī)器人導(dǎo)航前往指定的航點(diǎn)。極大提升ROS2中調(diào)用NAV2導(dǎo)航服務(wù)的開發(fā)體驗(yàn)。詳細(xì)操作步驟見教材P232-P257頁8.3開源導(dǎo)航插件的使用8.3.1安裝導(dǎo)航插件

在使用前，需要下載插件源碼并編譯安裝。cd~/ros2_ws/srcgitclone/6-robot/wp_map_tools.gitgitclone/s-robot/wp_map_tools.git

源碼下載完畢后，進(jìn)入到這個插件源碼目錄的scripts文件夾中，安裝編譯這個項目需要的依賴項：cd~/ros2_ws/src/wp_map_tools/scripts/./install_for_humble.sh

編譯剛才下載的插件源碼包：cd~/ros2_ws/colconbuild8.3開源導(dǎo)航插件的使用

在使用前，首先需要按照8.2.2的步驟建好環(huán)境地圖，并把地圖文件拷貝到wpr_simulation2的maps文件夾里。8.3.2添加航點(diǎn)8.3開源導(dǎo)航插件的使用cd~/ros2_ws/srcsourceinstall/setup.bash打開一個終端：ros2launchwp_map_toolsadd_waypoint_sim.launch.py回車執(zhí)行會啟動RViz2窗口，如圖所示，窗口里可以看到之前創(chuàng)建的地圖。8.3開源導(dǎo)航插件的使用在Rviz2工具欄的右邊，可以看到新增了一個[AddWaypoint]按鈕，單擊[AddWaypoint]按鈕，就可以在地圖上添加航點(diǎn)，在RViz2窗口里的地圖找到要添加航點(diǎn)的位置，單擊鼠標(biāo)左鍵并按住不放，會出現(xiàn)一個綠色箭頭，箭頭的尾部就是所添加航點(diǎn)的坐標(biāo)位置。拖動鼠標(biāo)，綠色箭頭會跟著旋轉(zhuǎn)，箭頭指向就是航點(diǎn)的朝向。8.3開源導(dǎo)航插件的使用使用上述方法，在地圖上設(shè)置更多的航點(diǎn)：8.3開源導(dǎo)航插件的使用執(zhí)行完畢后，在用戶的主文件夾下會生成一個名為“waypoints.yaml”的文件。打開第2個子窗口：sourceinstall/setup.bashros2runwp_map_tools

wp_saver8.3開源導(dǎo)航插件的使用如圖所示，這個文件里保存的就是設(shè)置的航點(diǎn)信息，雙擊打開這個文件對其內(nèi)容進(jìn)行編輯。8.3開源導(dǎo)航插件的使用8.3.3啟動導(dǎo)航服務(wù)

在wp_map_tools軟件包中準(zhǔn)備了兩個節(jié)點(diǎn)可以簡化這個調(diào)用過程：

wp_edit_node節(jié)點(diǎn)。這個節(jié)點(diǎn)會從主文件夾下的“waypoints.yaml”文件中獲取之前保存的航點(diǎn)信息，供其他節(jié)點(diǎn)查詢使用。

wp_navi_server節(jié)點(diǎn)。這個節(jié)點(diǎn)會從話題“/waterplus/navi_waypoint”中獲取導(dǎo)航的目標(biāo)航點(diǎn)名稱，然后從wp_edit_node節(jié)點(diǎn)查詢該航點(diǎn)的坐標(biāo)和朝向，接著調(diào)用NAV2的原生導(dǎo)航接口，完成導(dǎo)航任務(wù)。導(dǎo)航完成后，會向話題“/waterplus/navi_result”發(fā)送信息“navidone”提示導(dǎo)航已經(jīng)完成。

有了這兩個節(jié)點(diǎn)，就可以通過話題通訊完成導(dǎo)航任務(wù)，下面將介紹如何使用這兩個節(jié)點(diǎn)。

編寫導(dǎo)航Launch文件在nav_pkg軟件包中的[launch]文件夾新建文件，命名為“waypoint_nav.launch.py”。8.3開源導(dǎo)航插件的使用8.3開源導(dǎo)航插件的使用importosfromlaunchimportLaunchDescription

fromlaunch_ros.actionsimportNodefromament_index_python.packagesimportget_package_share_directoryfromlaunch.actionsimportIncludeLaunchDescriptionfromlaunch.launch_description_sourcesimportPythonLaunchDescriptionSource

defgenerate_launch_description():

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')8.3開源導(dǎo)航插件的使用

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(),)

rviz_file=os.path.join(get_package_share_directory('wp_map_tools'),'rviz','navi.rviz')

rviz_cmd=Node(package='rviz2',executable='rviz2',name='rviz2',arguments=['-d',rviz_file])8.3開源導(dǎo)航插件的使用

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')

ld=LaunchDescription()

ld.add_action(navigation_cmd)

ld.add_action(rviz_cmd)

ld.add_action(wp_edit_cmd)ld.add_action(wp_navi_server_cmd)

returnld8.3開源導(dǎo)航插件的使用8.3.4構(gòu)建航點(diǎn)導(dǎo)航程序

下面會編寫一個節(jié)點(diǎn)，向話題“/waterplus/navi_waypoint”發(fā)送導(dǎo)航目的地的航點(diǎn)名稱，激活wp_navi_server節(jié)點(diǎn)的導(dǎo)航功能，完成導(dǎo)航任務(wù)。1、編寫節(jié)點(diǎn)代碼在VSCode中找到nav_pkg軟件包，在

“src”文件夾新建文件，命名為“waypoint_navigation.cpp”。8.3開源導(dǎo)航插件的使用#include<rclcpp/rclcpp.hpp>#include<std_msgs/msg/string.hpp>

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

voidResultCallback(conststd_msgs::msg::String::SharedPtrmsg){if(msg->data=="navidone"){RCLCPP_INFO(node->get_logger(),"Arrived!");}}

intmain(intargc,char**argv){

rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("waypoint_navigation_node");8.3開源導(dǎo)航插件的使用autonavigation_pub=node->create_publisher<std_msgs::msg::String>("/waterplus/navi_waypoint",10);autoresult_sub=node->create_subscription<std_msgs::msg::String>("/waterplus/navi_result",10,ResultCallback);

rclcpp::sleep_for(std::chrono::milliseconds(1000));

std_msgs::msg::Stringwaypoint_msg;

waypoint_msg.data="1";

navigation_pub->publish(waypoint_msg);

rclcpp::spin(node);

rclcpp::shutdown();return0;}8.3開源導(dǎo)航插件的使用2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(std_msgsREQUIRED)add_executable(waypoint_navigation

src/waypoint_navigation.cpp)ament_target_dependencies(waypoint_navigation"rclcpp""std_msgs")install(TARGETSwaypoint_navigationDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>std_msgs</depend>4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild8.3開源導(dǎo)航插件的使用8.3.5仿真運(yùn)行航點(diǎn)導(dǎo)航程序sourceinstall/setup.bashros2launchwpr_simulation2robocup_home.launch.py8.3開源導(dǎo)航插件的使用sourceinstall/setup.bashros2launchnav_pkgwaypoint_nav.launch.py打開第2個子窗口。8.3開源導(dǎo)航插件的使用此時窗口中還沒有顯示機(jī)器人模型，需要手動設(shè)置一下機(jī)器人的初始位置。8.3開源導(dǎo)航插件的使用sourceinstall/setup.bashrunnav_pkg

waypoint_navigation打開第3個子窗口。8.3開源導(dǎo)航插件的使用節(jié)點(diǎn)執(zhí)行之后，RViz2中便規(guī)劃出去往航點(diǎn)“1”的路線，機(jī)器人按照路線開始移動。機(jī)器人導(dǎo)航到目標(biāo)航點(diǎn)位置后，會調(diào)整自己的朝向，直到與航點(diǎn)“1”的標(biāo)記箭頭方向一致。8.4本章小結(jié)

本章主要是對ROS2中的NAV2導(dǎo)航系統(tǒng)進(jìn)行介紹和編程。首先詳細(xì)介紹了NAV2的組成結(jié)構(gòu)和各功能模塊；接著，構(gòu)建了一個NAV2自主導(dǎo)航應(yīng)用實(shí)例，包括準(zhǔn)備地圖文件、編寫導(dǎo)航Launch文件、設(shè)置安裝規(guī)則、編譯軟件包、仿真環(huán)境運(yùn)行自主導(dǎo)航、NAV2的參數(shù)設(shè)置；最后，基于一款開源的地圖導(dǎo)航可視化插件，實(shí)現(xiàn)在地圖上設(shè)置多個目標(biāo)航點(diǎn)，通過消息發(fā)送，驅(qū)使機(jī)器人導(dǎo)航前往指定的航點(diǎn)，該插件極大提升了ROS2中調(diào)用NAV2導(dǎo)航服務(wù)的開發(fā)體驗(yàn)。機(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)動控制第5章激光雷達(dá)在ROS2中的使用第6章IMU在ROS2中的使用第7章ROS2中的SLAM環(huán)境建圖第8章ROS2中的NAV2自主導(dǎo)航第9章ROS2中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第9章9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)

第9章ROS2中的圖像視覺應(yīng)用9.2OpenCV顏色特征提取和目標(biāo)定位9.1視覺圖像數(shù)據(jù)的獲取9.4基于圖像視覺的人臉檢測實(shí)現(xiàn)9.5本章小結(jié)9.1視覺圖像數(shù)據(jù)的獲取

詳細(xì)操作步驟見教材P260-P272頁

視覺圖像數(shù)據(jù)的獲取是通過訂閱相機(jī)驅(qū)動節(jié)點(diǎn)發(fā)布的話題，從話題中獲取相機(jī)發(fā)出的消息包來實(shí)現(xiàn)的。機(jī)器人頭部安裝的是KinectV2RGB-D相機(jī)，對應(yīng)的話題名稱為“/kinect2/qhd/image_raw”，話題中的消息包格式為sensor_msgs::Image。9.1視覺圖像數(shù)據(jù)的獲取本實(shí)驗(yàn)將會實(shí)現(xiàn)一個訂閱者節(jié)點(diǎn)，訂閱相機(jī)發(fā)布的話題"/kinect2/qhd/image_raw"。從此話題中接收sensor_msgs::Image類型的消息包，并將其中的圖像數(shù)據(jù)轉(zhuǎn)換成OpenCV的格式。最后使用OpenCV的圖形顯示接口，將圖像顯示在圖形窗口中。9.1.1編寫圖像數(shù)據(jù)獲取程序cd~/ros2_ws/srcros2pkgcreatecv_pkg打開一個新的終端窗口，在工作空間中創(chuàng)建一個名為“cv_pkg”的軟件包。1、編寫節(jié)點(diǎn)代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_image.cpp”。下面編寫這個代碼文件內(nèi)容：9.1視覺圖像數(shù)據(jù)的獲取#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

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

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){

cv_bridge::CvImagePtr

cv_ptr;cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

cv::MatimgOriginal=cv_ptr->image;cv::imshow("RGB",imgOriginal);cv::waitKey(1);}9.1視覺圖像數(shù)據(jù)的獲取intmain(intargc,char**argv){

rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_image_node");

autorgb_sub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

cv::namedWindow("RGB");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();

return0;}9.1視覺圖像數(shù)據(jù)的獲取2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)add_executable(cv_image

src/cv_image.cpp)ament_target_dependencies(cv_image"rclcpp""sensor_msgs""cv_bridge""OpenCV")install(TARGETScv_imageDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend>4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild9.1視覺圖像數(shù)據(jù)的獲取9.1.2仿真運(yùn)行圖像數(shù)據(jù)獲取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.py9.1視覺圖像數(shù)據(jù)的獲取sourceinstall/setup.bashros2runcv_pkg

cv_image打開第2個子窗口。

節(jié)點(diǎn)運(yùn)行起來之后，會彈出一個名為“RGB”窗口程序，顯示機(jī)器人頭部相機(jī)所看到的四個顏色球的圖像。9.1視覺圖像數(shù)據(jù)的獲取sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試這個圖像是不是實(shí)時獲取的，可以借助wpr_simulation2附帶的程序讓中間的桔色球動起來，以便進(jìn)行對比觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機(jī)運(yùn)動。此時再切換到“RGB”窗口，可以看到圖像中的桔色球也跟著運(yùn)動，說明這個采集到的圖像是實(shí)時更新的。9.2OpenCV顏色特征提取和目標(biāo)定位

詳細(xì)操作步驟見教材P272-P291頁

在9.1節(jié)的實(shí)驗(yàn)里，實(shí)現(xiàn)了從機(jī)器人的頭部相機(jī)獲取機(jī)器人的視覺圖像。這一次將繼續(xù)深入，使用OpenCV實(shí)現(xiàn)機(jī)器人視覺中的顏色特征提取和目標(biāo)定位功能。1）對機(jī)器人視覺圖像進(jìn)行顏色空間轉(zhuǎn)換，從RGB空間轉(zhuǎn)換到HSV空間，排除光照影響。2）對轉(zhuǎn)換后的圖像進(jìn)行二值化處理，將目標(biāo)物體分割提取出來。3）對提取到的目標(biāo)像素進(jìn)行計算統(tǒng)計，得出目標(biāo)物的質(zhì)心坐標(biāo)。1、編寫節(jié)點(diǎn)代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_hsv.cpp”。下面編寫這個代碼文件內(nèi)容：9.2OpenCV顏色特征提取和目標(biāo)定位9.2.1編寫特征提取和目標(biāo)定位程序#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

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

usingnamespacecv;usingnamespacestd;staticintiLowH=10;staticintiHighH=40;

staticintiLowS=90;staticintiHighS=255;

staticintiLowV=1;staticintiHighV=255;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){

cv_bridge::CvImagePtr

cv_ptr;

cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

MatimgOriginal=cv_ptr->image;

MatimgHSV;

cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);

9.2OpenCV顏色特征提取和目標(biāo)定位vector<Mat>hsvSplit;split(imgHSV,hsvSplit);

equalizeHist(hsvSplit[2],hsvSplit[2]);merge(hsvSplit,imgHSV);

MatimgThresholded;

inRange(imgHSV,Scalar(iLowH,iLowS,iLowV),Scalar(iHighH,iHighS,iHighV),

imgThresholded);

Matelement=getStructuringElement(MORPH_RECT,Size(5,5));

morphologyEx(imgThresholded,imgThresholded,MORPH_OPEN,element);

morphologyEx(imgThresholded,imgThresholded,MORPH_CLOSE,element);9.2OpenCV顏色特征提取和目標(biāo)定位intnTargetX=0;intnTargetY=0;intnPixCount=0;intnImgWidth=imgThresholded.cols;intnImgHeight=imgThresholded.rows;for(inty=0;y<nImgHeight;y++){for(intx=0;x<nImgWidth;x++){if(imgThresholded.data[y*nImgWidth+x]==255){

nTargetX+=x;

nTargetY+=y;

nPixCount++;}}}9.2OpenCV顏色特征提取和目標(biāo)定位if(nPixCount>0){

nTargetX/=nPixCount;

nTargetY/=nPixCount;

printf("Target(%d,%d)PixelCount=%d\n",nTargetX,nTargetY,nPixCount);Pointline_begin=Point(nTargetX-10,nTargetY);Pointline_end=Point(nTargetX+10,nTargetY);line(imgOriginal,line_begin,line_end,Scalar(255,0,0));

line_begin.x=nTargetX;line_begin.y=nTargetY-10;

line_end.x=nTargetX;line_end.y=nTargetY+10;line(imgOriginal,line_begin,line_end,Scalar(255,0,0));}else{printf("Targetdisappeared...\n");}

imshow("RGB",imgOriginal);

imshow("HSV",imgHSV);

imshow("Result",imgThresholded);cv::waitKey(5);}9.2OpenCV顏色特征提取和目標(biāo)定位intmain(intargc,char**argv){

rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_hsv_node");

autorgb_sub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

namedWindow("Threshold",WINDOW_AUTOSIZE);

createTrackbar("LowH","Threshold",&iLowH,179);

createTrackbar("HighH","Threshold",&iHighH,179);

createTrackbar("LowS","Threshold",&iLowS,255);

createTrackbar("HighS","Threshold",&iHighS,255);

createTrackbar("LowV","Threshold",&iLowV,255);

createTrackbar("HighV","Threshold",&iHighV,255);

namedWindow("RGB");

namedWindow("HSV");

namedWindow("Result");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();return0;}9.2OpenCV顏色特征提取和目標(biāo)定位2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)add_executable(cv_hsv

src/cv_hsv.cpp)ament_target_dependencies(cv_hsv"rclcpp""sensor_msgs""cv_bridge""OpenCV")install(TARGETScv_hsvDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend>4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild9.2OpenCV顏色特征提取和目標(biāo)定位sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.py9.2OpenCV顏色特征提取和目標(biāo)定位9.2.2仿真運(yùn)行特征提取和目標(biāo)定位程序sourceinstall/setup.bashros2runcv_pkg

cv_hsv打開第2個子窗口。

節(jié)點(diǎn)運(yùn)行起來之后，會彈出四個窗口，分別是“RGB”窗口、“HSV”窗口、“Result”窗口、“Threshold”窗口。9.2OpenCV顏色特征提取和目標(biāo)定位9.2OpenCV顏色特征提取和目標(biāo)定位切換到運(yùn)行cv_hsv節(jié)點(diǎn)的終端窗口，可以看到追蹤的目標(biāo)物的中心坐標(biāo)值。sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試目標(biāo)追蹤的效果，可以借助wpr_simulation2附帶的程序讓中間的桔色球動起來，以便進(jìn)行對比觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機(jī)運(yùn)動。此時再切換到“Result”窗口，觀察圖像中的桔色目標(biāo)球移動時，顏色特征提取的效果。9.2OpenCV顏色特征提取和目標(biāo)定位9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)

詳細(xì)操作步驟見教材P292-P310頁

在9.2節(jié)的實(shí)驗(yàn)里，使用OpenCV實(shí)現(xiàn)機(jī)器人視覺中的顏色特征提取和目標(biāo)定位功能。如圖所示，這一次將對目標(biāo)定位功能進(jìn)行擴(kuò)展，根據(jù)目標(biāo)位置計算速度并輸出給機(jī)器人，讓機(jī)器人跟隨球進(jìn)行移動，實(shí)現(xiàn)一個目標(biāo)跟隨的閉環(huán)控制。9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)在編寫例程代碼前，先設(shè)計一下這個程序的實(shí)現(xiàn)思路，如以下四步：1）對機(jī)器人視覺圖像進(jìn)行顏色空間轉(zhuǎn)換，從RGB空間轉(zhuǎn)換到HSV空間，排除光照影響。2）對轉(zhuǎn)換后的圖像進(jìn)行二值化處理，將目標(biāo)物體分割提取出來。3）對提取到的目標(biāo)像素進(jìn)行計算統(tǒng)計，得出目標(biāo)物的質(zhì)心坐標(biāo)。4）根據(jù)目標(biāo)位置計算機(jī)器人運(yùn)動速度，完成目標(biāo)跟隨功能。1、編寫節(jié)點(diǎn)代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_follow.cpp.cpp”。下面編寫這個代碼文件內(nèi)容：#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>#include<geometry_msgs/msg/twist.hpp>

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

usingnamespacecv;usingnamespacestd;9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)9.3.1編寫目標(biāo)追蹤程序staticintiLowH=10;staticintiHighH=40;

staticintiLowS=90;staticintiHighS=255;

staticintiLowV=1;staticintiHighV=255;geometry_msgs::msg::Twistvel_cmd;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr

vel_pub;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){

cv_bridge::CvImagePtr

cv_ptr;

cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

MatimgOriginal=cv_ptr->image;

MatimgHSV;

cvtColor(imgOriginal,imgHSV,COLOR_BGR2HSV);

9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)vector<Mat>hsvSplit;split(imgHSV,hsvSplit);

equalizeHist(hsvSplit[2],hsvSplit[2]);merge(hsvSplit,imgHSV);

MatimgThresholded;

inRange(imgHSV,Scalar(iLowH,iLowS,iLowV),Scalar(iHighH,iHighS,iHighV),

imgThresholded);

Matelement=getStructuringElement(MORPH_RECT,Size(5,5));

morphologyEx(imgThresholded,imgThresholded,MORPH_OPEN,element);

morphologyEx(imgThresholded,imgThresholded,MORPH_CLOSE,element);9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)intnTargetX=0;intnTargetY=0;intnPixCount=0;intnImgWidth=imgThresholded.cols;intnImgHeight=imgThresholded.rows;for(inty=0;y<nImgHeight;y++){for(intx=0;x<nImgWidth;x++){if(imgThresholded.data[y*nImgWidth+x]==255){

nTargetX+=x;

nTargetY+=y;

nPixCount++;}}}9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)if(nPixCount>0){

nTargetX/=nPixCount;

nTargetY/=nPixCount;

printf("Target(%d,%d)PixelCount=%d\n",nTargetX,nTargetY,nPixCount);Pointline_begin=Point(nTargetX-10,nTargetY);Pointline_end=Point(nTargetX+10,nTargetY);line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);

line_begin.x=nTargetX;line_begin.y=nTargetY-10;

line_end.x=nTargetX;line_end.y=nTargetY+10;line(imgOriginal,line_begin,line_end,Scalar(255,0,0),3);

floatfVelFoward=(nImgHeight/2-nTargetY)*0.002;floatfVelTurn=(nImgWidth/2-nTargetX)*0.003;

vel_cmd.linear.x=fVelFoward;

vel_cmd.linear.y=0;

vel_cmd.linear.z=0;

vel_cmd.angular.x=0;

vel_cmd.angular.y=0;

vel_cmd.angular.z=fVelTurn;}9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)else{

printf("Targetdisappeared...\n");

vel_cmd.linear.x=0;

vel_cmd.linear.y=0;

vel_cmd.linear.z=0;

vel_cmd.angular.x=0;

vel_cmd.angular.y=0;

vel_cmd.angular.z=0;}

vel_pub->publish(vel_cmd);

imshow("Result",imgThresholded);

imshow("RGB",imgOriginal);cv::waitKey(5);}9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)intmain(intargc,char**argv){

rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_follow_node");

vel_pub=node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);autosub=node->create_subscription<sensor_msgs::msg::Image>("/kinect2/qhd/image_raw",1,CamRGBCallback);

namedWindow("RGB");

namedWindow("Result");

rclcpp::spin(node);

cv::destroyAllWindows();

rclcpp::shutdown();return0;}9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)2、設(shè)置編譯規(guī)則find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(cv_bridgeREQUIRED)find_package(OpenCVREQUIRED)find_package(geometry_msgsREQUIRED)add_executable(cv_follow

src/cv_follow.cpp)ament_target_dependencies(cv_follow

"rclcpp""sensor_msgs""cv_bridge""OpenCV""geometry_msgs")install(TARGETScv_followDESTINATIONlib/${PROJECT_NAME})3、修改軟件包信息<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>cv_bridge</depend><depend>OpenCV</depend><depend>geometry_msgs</depend>9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)9.3.2仿真運(yùn)行目標(biāo)追蹤程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_balls.launch.pysourceinstall/setup.bash打開第2個子窗口。

節(jié)點(diǎn)運(yùn)行起來之后，會彈出兩個窗口，分別是“RGB”窗口和“Result”窗口。9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)ros2runcv_pkgcv_follow9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)切換到仿真窗口，可以看到里面的機(jī)器人對準(zhǔn)桔色球，輕微向后移動，與桔色球保持固定距離。如圖所示，在運(yùn)行cv_follow節(jié)點(diǎn)的終端窗口，可以看到追蹤目標(biāo)物的中心坐標(biāo)值在不停地刷新。sourceinstall/setup.bashros2runwpr_simulation2ball_random_move

為了測試機(jī)器人追蹤目標(biāo)球的效果，可以借助wpr_simulation2附帶的程序讓桔色球動起來。以便進(jìn)行觀察。打開第3個子窗口。

執(zhí)行之后，可以看到仿真窗口里的桔色球開始隨機(jī)運(yùn)動，而機(jī)器人也追著桔色球在移動。此時再切換到“RGB”窗口，觀察藍(lán)色十字標(biāo)記是對桔色目標(biāo)球的追蹤效果。9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)9.3基于圖像視覺的目標(biāo)追蹤實(shí)現(xiàn)

經(jīng)過前面三個實(shí)驗(yàn)，終于將識別檢測和運(yùn)動行為結(jié)合起來，形成一個典型的視覺閉環(huán)控制系統(tǒng)。機(jī)器人與外部世界的交互，形式雖然多樣，但是本質(zhì)上都是這樣一套“識別→定位→操作”的閉環(huán)控制系統(tǒng)。通過這樣一個簡單的例子，了解和學(xué)習(xí)這種實(shí)現(xiàn)思路，可以為將來構(gòu)建更復(fù)雜的機(jī)器人系統(tǒng)奠定一個基礎(chǔ)。9.4基于圖像視覺的人臉檢測實(shí)現(xiàn)本節(jié)借助現(xiàn)成的人臉識別算法庫來實(shí)現(xiàn)人臉檢測，這個算法庫的調(diào)用在wpr_simulaiton2的face_detector.py節(jié)點(diǎn)中已經(jīng)實(shí)現(xiàn)，直接使用即可。face_detector.py節(jié)點(diǎn)會訂閱話題“/face_detector_input”，作為人臉圖像的輸入。圖像中的人臉被檢測到后，其坐標(biāo)值會發(fā)布到話題“/face_position”中去。所以，這個實(shí)驗(yàn)只需要編寫一個節(jié)點(diǎn)，從相機(jī)的話題中獲取圖片，轉(zhuǎn)發(fā)給face_detector.py節(jié)點(diǎn)進(jìn)行人臉檢測。然后從face_detector.py的“/face_position”話題獲取人臉坐標(biāo)結(jié)果即可。9.4基于圖像視覺的人臉檢測實(shí)現(xiàn)

詳細(xì)操作步驟見教材P311-P326頁在編寫代碼前，需要安裝人臉檢測節(jié)點(diǎn)face_detector.py的依賴項。cd~/ros2_ws/src/wpr_simulation2/scripts/./install_dep_face.sh9.4.1編寫人臉檢測程序1、編寫節(jié)點(diǎn)代碼在cv_pkg軟件包中的[src]文件夾新建文件，命名為“cv_face_detect.cpp”。#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/image.hpp>#include<sensor_msgs/msg/region_of_interest.hpp>#include<cv_bridge/cv_bridge.h>#include<opencv2/imgproc/imgproc.hpp>#include<opencv2/highgui/highgui.hpp>

std::shared_ptr<rclcpp::Node>node;cv::MatimgFace;rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr

frame_pub;

voidCamRGBCallback(constsensor_msgs::msg::Image::SharedPtrmsg){

cv_bridge::CvImagePtr

cv_ptr;

cv_ptr=cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);

imgFace=cv_ptr->image;

frame_pub->publish(*msg);}9.4基于圖像視覺的人臉檢測實(shí)現(xiàn)voidFacePosCallback(constsensor_msgs::msg::RegionOfInterest::SharedPtrmsg){cv::rectangle(imgFace,cv::Point(msg->x_offset,msg->y_offset),cv::Point(msg->x_offset+msg->width,msg->y_offset+msg->height),cv::Scalar(0,0,255),2,cv::LINE_8);cv::imshow("Face",imgFace);cv::waitKey(1);}

9.4基于圖像視覺的人臉檢測實(shí)現(xiàn)intmain(intargc,char**argv){

rclcpp::init(argc,argv);node=std::make_shared<rclcpp::Node>("cv_face_detect");

autorgb_sub=node->create_subscription<sensor_msg