機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐 課件 第1-7章 Linux Ubuntu入門基礎(chǔ) - ROS2中的SLAM環(huá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)動(dòng)控制第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)用第1章1.3Ubuntu22.04使用入門1.1Ubuntu簡(jiǎn)介1.4本章小結(jié)第1章LinuxUbuntu入門基礎(chǔ)1.2安裝Ubuntu22.04

機(jī)器人操作系統(tǒng)（RobotOperatingSystem，ROS）是一個(gè)機(jī)器人軟件平臺(tái)，誕生于2007年，它包含了一系列的軟件庫(kù)和工具用于構(gòu)建機(jī)器人應(yīng)用，目前已成為機(jī)器人領(lǐng)域的普遍標(biāo)準(zhǔn)。ROS雖然被稱為操作系統(tǒng)，但是真正底層的任務(wù)調(diào)度、編譯、尋址等任務(wù)還是由Linux操作系統(tǒng)完成，也就是ROS是一個(gè)運(yùn)行在Linux上的次級(jí)操作系統(tǒng)，Linux有不同的發(fā)行版本，2022年4月22日，Canonical公司宣布推出Ubuntu的最新LTS（長(zhǎng)期支持）版本

22.04，是繼14.04、16.04、18.04、20.04之后的又一個(gè)長(zhǎng)期支持版本，提供5年的技術(shù)支持，將提供免費(fèi)安全和維護(hù)更新至2027年4月，Ubuntu-ROS組合已成為機(jī)器人編程的一個(gè)理想組合。Ubuntu是一個(gè)基于Debian架構(gòu)，以桌面應(yīng)用為主的Linux操作系統(tǒng)。由于LinuxUbuntu是開放源代碼的自由軟件，用戶可以登錄LinuxUbuntu的官方網(wǎng)址免費(fèi)下載該軟件的安裝包。1.1Ubuntu簡(jiǎn)介

對(duì)Ubuntu的安裝有兩種方式，一種是雙系統(tǒng)安裝，另外一種是虛擬機(jī)安裝。因在虛擬機(jī)里運(yùn)行Ubuntu容易出現(xiàn)卡頓現(xiàn)象，本節(jié)只介紹雙系統(tǒng)安裝，即在現(xiàn)有的Windows10系統(tǒng)下如何正確安裝Ubuntu20.04系統(tǒng)。

詳細(xì)安裝步驟見教材P1-P12頁(yè)1.2安裝Ubuntu22.04

教材中用到的網(wǎng)址及代碼如下:

1.2安裝Ubuntu22.041.2.1準(zhǔn)備工作3）下載Ubuntu22.04系統(tǒng)安裝鏡像；下載網(wǎng)址為：/desktop，選擇Ubuntu22.04.3LTS進(jìn)行下載。4）win32diskimager軟件；下載網(wǎng)址為：/projects/win32diskimager/。1.3.8時(shí)間同步問題sudoaptupdatesudoaptinstallntpdatesudontpdatesudo

hwclock--localtime--systohc1.3.9修改默認(rèn)引導(dǎo)系統(tǒng)sudogedit/etc/default/grubsudoupdate-grubUbuntu系統(tǒng)初始界面如下：1.3Ubuntu22.04使用入門屏幕左側(cè)為收藏夾，默認(rèn)圖標(biāo)按鈕共有7個(gè)，從上至下分別是：[Firefox網(wǎng)絡(luò)瀏覽器]、[Thunderbird郵件/新聞]、[文件]、[Rhythmbox]、[LibreOfficeWriter]、[UbuntuSoftware]、[幫助]。

屏幕中間區(qū)域?yàn)楣ぷ鲄^(qū)域，默認(rèn)有主文件夾和回收站；屏幕正上方顯示當(dāng)前日期和時(shí)間，屏幕右上角分別是：輸入法、系統(tǒng)聲音、注銷/關(guān)機(jī)。1.3Ubuntu22.04使用入門Ubuntu中的[文件]圖標(biāo)按鈕和工作區(qū)域中的主文件夾功能相同，類似于Windows中的C:\用戶文件夾，左側(cè)是導(dǎo)航窗口，如圖所示。1.3Ubuntu22.04使用入門

單擊上圖左側(cè)導(dǎo)航窗口的[其他位置]，在右側(cè)出現(xiàn)的界面中選擇[計(jì)算機(jī)]，可以瀏覽Ubuntu的文件系統(tǒng)，如下圖所示。1.3Ubuntu22.04使用入門命令行使用入門

在Ubuntu系統(tǒng)中，有兩種工作模式，一種是圖形化界面模式。另一種模式就是命令行模式。在ROS2中，所有程序的編譯和運(yùn)行都是通過命令行實(shí)現(xiàn)的，Ubuntu中的終端程序就是用于運(yùn)行命令行指令的程序。按下鍵盤組合鍵Ctrl+Alt+T即可調(diào)出終端程序1.3Ubuntu22.04使用入門命令行使用入門在學(xué)習(xí)ROS2的過程中，有一些指令是會(huì)經(jīng)常用到的：1、ls：列出當(dāng)前目錄中的文件和文件夾。2、mkdir：這條指令后面接一個(gè)空格，然后再接一個(gè)文件夾名字，可以

創(chuàng)建一個(gè)新的空文件夾。3、cd：此命令后面接一個(gè)空格，然后再接一個(gè)路徑地址，可以切換終端

程序的當(dāng)前路徑。4、路徑“~”：在Ubuntu中，波浪線“~”是個(gè)很特殊的符號(hào)，它表示當(dāng)前用

戶的主文件夾路徑。1.4本章小結(jié)

本章內(nèi)容是學(xué)習(xí)ROS2之前的必備先修知識(shí)，首先對(duì)Ubuntu22.04的安裝方式進(jìn)行了詳細(xì)示例；接著對(duì)Ubuntu桌面、連接無(wú)線網(wǎng)絡(luò)、軟件源更換、Ubuntu文件系統(tǒng)、終端程序和常用命令、管理員權(quán)限、時(shí)間變量和source、時(shí)間同步、修改默認(rèn)引導(dǎo)系統(tǒng)等入門知識(shí)進(jìn)行了介紹，為后面章節(jié)的學(xué)習(xí)打下良好的基礎(chǔ)。

機(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中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第2章2.3ROS2安裝與配置

第2章ROS2安裝與系統(tǒng)架構(gòu)2.2ROS2系統(tǒng)架構(gòu)2.1ROS2簡(jiǎn)介2.4從APT軟件源安裝應(yīng)用程序2.6從Github下載安裝應(yīng)用程序2.5ROS2的工作空間2.7本章小結(jié)2.8Terminator終端工具2.7VisualStudioCode編輯器ROS2是在ROS的基礎(chǔ)上設(shè)計(jì)開發(fā)的第二代機(jī)器人操作系統(tǒng)，可以幫助我們簡(jiǎn)化機(jī)器人開發(fā)任務(wù)，加速機(jī)器人落地的軟件庫(kù)和工具集。ROS2與ROS1的區(qū)別如下：1）系統(tǒng)架構(gòu)進(jìn)行了顛覆性的變化。

2）軟件API進(jìn)行了重新設(shè)計(jì)。

3）編譯系統(tǒng)進(jìn)行了升級(jí)。4）大量采用新技術(shù)、新的設(shè)計(jì)理念。2.1ROS2簡(jiǎn)介2.2ROS2系統(tǒng)架構(gòu)ROS2系統(tǒng)的架構(gòu)主要由通信（Plumbing）、工具（Tools）、功能（Capabilities）與社區(qū)（Community）四大部分組成，2.2ROS2系統(tǒng)架構(gòu)ROS2文件系統(tǒng)可以劃分為三層，如圖所示：1）操作系統(tǒng)層（OSLayer）2）中間層（MiddlewareLayer）。3）應(yīng)用層（ApplicationLayer）。一般來(lái)說(shuō)，開發(fā)者的工作內(nèi)容主要集中在應(yīng)用層，主要通過編寫一些節(jié)點(diǎn)程序來(lái)實(shí)現(xiàn)機(jī)器人的控制和自主功能。操作系統(tǒng)層和中間層都是實(shí)現(xiàn)好的，可以通過配置參數(shù)來(lái)適應(yīng)上層應(yīng)用的需求。2.2ROS2系統(tǒng)架構(gòu)ROS2的通信模型加入了很多DDS的通信機(jī)制，如下圖所示：

詳細(xì)安裝步驟見教材P29-P35頁(yè)2.3ROS2安裝與配置本書采用的是基于Ubuntu22.04的Humble版本。官方安裝步驟網(wǎng)頁(yè)：/en/humble/Installation/Ubuntu-Install-Debians.html

教材中用到的網(wǎng)址及代碼如下:

2.3ROS2安裝與配置2.3ROS2安裝與配置localesudoaptupdate&&sudoaptinstalllocalessudolocale-genen_USen_US.UTF-8sudoupdate-localeLC_ALL=en_US.UTF-8LANG=en_US.UTF-8exportLANG=en_US.UTF-8sudoaptupdatesudoaptinstallsoftware-properties-common-ysudoadd-apt-repositoryuniversesudoaptinstallcurl-ysudocurl-sSL/ros/rosdistro/master/ros.key-o/usr/share/keyrings/ros-archive-keyring.gpgwget/ros.keysudocpros.key/usr/share/keyrings/ros-archive-keyring.gpg2.3ROS2安裝與配置echo"deb[arch=$(dpkg--print-architecture)signed-by=/usr/share/keyrings/ros-archive-keyring.gpg]/ros2/ubuntu$(./etc/os-release&&echo$UBUNTU_CODENAME)main"|sudotee/etc/apt/sources.list.d/ros2.list>/dev/nullsudoaptupdatesudoaptupgradesudoaptinstallros-humble-desktopecho"source/opt/ros/humble/setup.bash">>~/.bashrcsudoaptinstallros-dev-toolsros2

教材中用到的網(wǎng)址及代碼如下:

2.3ROS2安裝與配置

安裝步驟見教材P36-P39頁(yè)2.4從APT軟件源安裝應(yīng)用程序

安裝好ROS2之后，還需要安裝應(yīng)用程序才能實(shí)現(xiàn)具體的功能，其中最常用的應(yīng)用程序安裝方式是從ROS2的APT軟件源在線安裝。本書通過安裝一個(gè)運(yùn)動(dòng)控制程序?yàn)槔?，學(xué)習(xí)這種安裝方式。sudoaptupdatesudoaptinstallros-humble-rqt-robot-steeringros2runrqt_robot_steering

rqt_robot_steeringros2runturtlesim

turtlesim_node2.5ROS2的工作空間

使用APT源安裝的ROS2應(yīng)用程序都是一些二進(jìn)制的可執(zhí)行文件，如果需要從源代碼開始構(gòu)建ROS2應(yīng)用程序，就需要建立一個(gè)工作空間。在這個(gè)工作空間里構(gòu)建ROS2的項(xiàng)目工程，編寫代碼，然后編譯成可執(zhí)行程序。這個(gè)工作空間其實(shí)是一個(gè)文件夾，其內(nèi)部的子文件夾按照一定的命名和位置規(guī)則組織好即可。mkdir-p~/ros2_ws/srccd~/ros2_wscolconbuild2.6從Github下載安裝應(yīng)用程序

工作空間創(chuàng)建好之后，可以下載并編譯一些開源項(xiàng)目，然后實(shí)際運(yùn)行起來(lái)體驗(yàn)一下整個(gè)流程。這里介紹一個(gè)開源的仿真項(xiàng)目“wpr_simulation2”，該項(xiàng)目工程里包含了后面實(shí)驗(yàn)將會(huì)用到的仿真環(huán)境。2.6從Github下載安裝應(yīng)用程序sudoaptinstallgitcd~/ros2_ws/src/gitclone/6-robot/wpr_simulation2.gitgitclone/s-robot/wpr_simulation2.gitcdwpr_simulation2/scripts/./install_for_humble.shcd~/ros2_ws/colconbuildsourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pyros2runrqt_robot_steering

rqt_robot_steering

教材中用到的代碼如下:

2.7VisualStudioCode編輯器在Ubuntu中開發(fā)ROS程序有比較多的集成開發(fā)環(huán)境（IDE，即IntegratedDevelopmentEnvironment）。其中VisualStudioCode（簡(jiǎn)稱

VSCode）是微軟開發(fā)的一款跨平臺(tái)開源編輯器。

安裝步驟見教材P48-P54頁(yè)在瀏覽器中打開VSCode官方下載地址：。

安裝時(shí)，先輸入“sudo

dpkg

-icode_”然后按下鍵盤的[Tab]鍵，讓命令行自動(dòng)補(bǔ)齊后面的文件名。2.8Terminator終端工具

在Ubuntu的中自帶了一個(gè)終端工具。但是那個(gè)終端工具在同時(shí)打開多個(gè)窗口時(shí)，顯示得不夠直觀，且切換也不方便。為了便于進(jìn)行ROS2程序的多窗口運(yùn)行，建議安裝Terminator終端工具。

這個(gè)工具在Ubuntu自帶的軟件源中就有，可以直接通過指令安裝。具體操作步驟如下：sudoaptupdatesudoaptinstallterminator2.9本章小結(jié)

本章首先簡(jiǎn)單介紹了ROS，并對(duì)ROS2和ROS1進(jìn)行了對(duì)比；接著對(duì)ROS2系統(tǒng)架構(gòu)包括文件系統(tǒng)、DDS、常用包管理指令進(jìn)行了詳細(xì)說(shuō)明；然后對(duì)ROS2的安裝方法進(jìn)行了詳細(xì)介紹；最后，介紹了ROS2常用的集成Git、wpr_simulation2、編輯器VSCode和相關(guān)插件以及Terminator的安裝。機(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中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第3章3.3面向?qū)ο蟮墓?jié)點(diǎn)Node實(shí)現(xiàn)

第3章ROS2編程基礎(chǔ)3.2話題Topic和消息Message3.1節(jié)點(diǎn)Node和軟件包Package3.4Launch文件3.5本章小結(jié)

節(jié)點(diǎn)Node和軟件包Package是ROS2的程序的組織形式。其中節(jié)點(diǎn)Node相當(dāng)于一個(gè)ROS2的程序文件。當(dāng)我們運(yùn)行一個(gè)ROS2程序的時(shí)候，實(shí)際上運(yùn)行的就是一個(gè)或者多個(gè)節(jié)點(diǎn)文件。軟件包Package是節(jié)點(diǎn)文件的容器，通常是一個(gè)包含了一系列配置文件的目錄。而節(jié)點(diǎn)源碼文件，就放在軟件包目錄的某個(gè)子文件夾中（一般是src文件夾）。3.1節(jié)點(diǎn)Node和軟件包Package創(chuàng)建軟件包Package和節(jié)點(diǎn)Node的詳細(xì)操作步驟：見教材P56-P66頁(yè)cd~/ros2_ws/srcros2pkgcreatemy_pkg3.1.1創(chuàng)建軟件包Package

命名新建文件為“my_node.cpp”。1、編寫節(jié)點(diǎn)代碼3.1節(jié)點(diǎn)Node和軟件包Package3.1.2編寫節(jié)點(diǎn)Node#include"rclcpp/rclcpp.hpp"

intmain(intargc,char*argv[]){

rclcpp::init(argc,argv);

autonode=std::make_shared<rclcpp::Node>("my_node");

RCLCPP_INFO(node->get_logger(),"Helloworld!");while(rclcpp::ok()){;}

rclcpp::shutdown();

return0;}

3.1節(jié)點(diǎn)Node和軟件包Package2、設(shè)置編譯規(guī)則3.1節(jié)點(diǎn)Node和軟件包Packagefind_package(rclcppREQUIRED)add_executable(my_node

src/my_node.cpp)ament_target_dependencies(my_node"rclcpp")install(TARGETS

my_nodeDESTINATIONlib/${PROJECT_NAME})

3、修改軟件包信息<depend>rclcpp</depend>4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild3.1節(jié)點(diǎn)Node和軟件包Package3.1.3運(yùn)行節(jié)點(diǎn)Noderos2runmy_pkg

my_nodesourceinstall/setup.bash

3.2話題Topic和消息Message

在ROS2中，節(jié)點(diǎn)Node之間的通訊最常用的方式是話題Topic和消息Message。其中話題Topic可以理解為一個(gè)網(wǎng)絡(luò)聊天室。在這個(gè)話題聊天室里發(fā)言的節(jié)點(diǎn)，可以稱為發(fā)布者Publisher。而聊天室中所有看到發(fā)言內(nèi)容的節(jié)點(diǎn)，可以稱為訂閱者Subscriber。

在話題聊天室中傳送的發(fā)言內(nèi)容，就是消息Message。所以上述通訊機(jī)制可以概括為：1）發(fā)布者Publisher將要發(fā)生的數(shù)據(jù)打包成消息Message，然后發(fā)送到話題Topic中。2）訂閱者Subscriber訂閱一個(gè)話題Topic，從話題中獲取消息Message。然后把消息中的數(shù)據(jù)解析出來(lái)，進(jìn)行后續(xù)的計(jì)算和使用。

編寫話題發(fā)布者Publisher、話題訂閱者Subscriber的詳細(xì)操作步驟：見教材P67-P85頁(yè)cd~/ros2_ws/srcros2pkgcreatetopic_pkg1、創(chuàng)建軟件包

命名新建文件為“publisher_node.cpp”。2、編寫節(jié)點(diǎn)代碼3.2話題Topic和消息Message3.2.1編寫話題發(fā)布者Publisher#include"rclcpp/rclcpp.hpp"#include"std_msgs/msg/string.hpp"

intmain(intargc,char*argv[]){

rclcpp::init(argc,argv);

autonode=std::make_shared<rclcpp::Node>("publisher_node");

autopublisher=node->create_publisher<std_msgs::msg::String>("/my_topic",10);

std_msgs::msg::Stringmessage;

message.data="HelloWorld!";

rclcpp::Rateloop_rate(1);

while(rclcpp::ok()){publisher->publish(message);

loop_rate.sleep();}

rclcpp::shutdown();

return0;}3.2話題Topic和消息Message3、設(shè)置編譯規(guī)則4、修改軟件包信息5、編譯軟件包c(diǎn)d~/ros2_wscolconbuild3.2話題Topic和消息Messagefind_package(rclcppREQUIRED)find_package(std_msgsREQUIRED)add_executable(publisher_node

src/publisher_node.cpp)ament_target_dependencies(publisher_node"rclcpp""std_msgs")install(TARGETS

publisher_nodeDESTINATIONlib/${PROJECT_NAME})6、

運(yùn)行節(jié)點(diǎn)Node3.2話題Topic和消息Messagesourceinstall/setup.bashros2runtopic_pkg

publisher_node

在Terminator終端中，按下三個(gè)鍵盤組合鍵[Ctrl+Shift+O]，會(huì)將終端分為上下兩個(gè)子窗口。其中上面的窗口仍然保持publisher_node運(yùn)行，下面的窗口可以輸入新的指令并運(yùn)行。ros2topiclistros2topicecho/my_topic

命名新建文件為“subscriber_node.cpp”。3.2話題Topic和消息Message3.2.2編寫話題訂閱者Subscriber在上一節(jié)中，創(chuàng)建一個(gè)名為topic_pkg的軟件包，并在這個(gè)軟件包中實(shí)現(xiàn)了一個(gè)發(fā)布者節(jié)點(diǎn)。這一節(jié)中，將會(huì)在這個(gè)topic_pkg軟件包中添加一個(gè)訂閱者節(jié)點(diǎn)。1、添加節(jié)點(diǎn)代碼文件3.2話題Topic和消息Message2、編寫節(jié)點(diǎn)代碼#include"rclcpp/rclcpp.hpp"#include"std_msgs/msg/string.hpp"

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

voidCallback(conststd_msgs::msg::String::SharedPtrmsg){RCLCPP_INFO(node->get_logger(),"Receive:%s",msg->data.c_str());}

intmain(intargc,char*argv[]){

rclcpp::init(argc,argv);

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

autosubscriber=node->create_subscription<std_msgs::msg::String>("/my_topic",10,&Callback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}3、設(shè)置編譯規(guī)則4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild3.2話題Topic和消息Messageadd_executable(subscriber_node

src/subscriber_node.cpp)ament_target_dependencies(subscriber_node"rclcpp""std_msgs")install(TARGETS

publisher_node

subscriber_nodeDESTINATIONlib/${PROJECT_NAME})5、

運(yùn)行節(jié)點(diǎn)sourceinstall/setup.bashros2runtopic_pkg

publisher_noderos2runtopic_pkg

subscriber_node打開第2個(gè)子窗口：3.2話題Topic和消息Message上圖是訂閱者節(jié)點(diǎn)subscriber_node接收到發(fā)布者節(jié)點(diǎn)publisher_node發(fā)送來(lái)的消息內(nèi)容。說(shuō)明訂閱者subscriber_node和發(fā)布者publisher_node這兩個(gè)節(jié)點(diǎn)，通過話題“/my_topic”建立起了數(shù)據(jù)通訊。使用std_msgs::msg::String類型的消息包在兩個(gè)節(jié)點(diǎn)間傳遞數(shù)據(jù)。

3.3面向?qū)ο蟮墓?jié)點(diǎn)Node實(shí)現(xiàn)

在前面的實(shí)驗(yàn)里，使用的都是過程式編程，可以很清晰的展示節(jié)點(diǎn)的運(yùn)行過程。但是在大量的開源項(xiàng)目中，最流行的是面向?qū)ο蟮木幊谭绞?。也就是將?jié)點(diǎn)Node封裝成類Class。

話題發(fā)布者Publisher的類封裝、話題訂閱者Subscriber的類封裝的詳細(xì)操作步驟：見教材P85-P91頁(yè)鑒于該部分內(nèi)容屬于進(jìn)階性質(zhì)的學(xué)習(xí)材料，故推薦采取自主學(xué)習(xí)的方式進(jìn)行掌握。

3.4Launch文件

在ROS2中，可以通過Launch文件一次啟動(dòng)多個(gè)節(jié)點(diǎn)，省去了逐個(gè)節(jié)點(diǎn)輸入指令啟動(dòng)的繁瑣。在ROS2中，支持用三種語(yǔ)音來(lái)編寫Launch文件，分別是XML、YAML和Python。其中XML的語(yǔ)法和ROS1的Launch文件格式類似；YAML格式的Launch文件使用比較少；而在ROS2的開源社區(qū)中，則以Python語(yǔ)言的Launch文件最為流行。

分別使用XML、YAML和Python這三種語(yǔ)言構(gòu)建Launch文件的詳細(xì)操作步驟：見教材P91-P104頁(yè)3.4.1XML格式的Launch文件3.4Launch文件1、在VSCode中找到前面實(shí)驗(yàn)構(gòu)建的topic_pkg軟件包。新建文件夾，命名為“l(fā)aunch”。2、在[launch]文件夾中，新建的文件，命名為“pub_sub.launch.xml”。3.4.1XML格式的Launch文件3.4Launch文件3、為這個(gè)pub_sub.launch.xml編寫內(nèi)容：<launch>

<nodepkg="topic_pkg"exec="publisher_node"name="publisher_node"/>

<nodepkg="topic_pkg"exec="subscriber_node"name="subscriber_node"/></launch>4、在VSCode中打開[topic_pkg]軟件包的[CMakeLists.txt]文件。在文件中添加如下安裝規(guī)則：install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})3.4.1XML格式的Launch文件3.4Launch文件5、編譯軟件包c(diǎn)d~/ros2_wscolconbuild6、

運(yùn)行Launch文件sourceinstall/setup.bashros2launchtopic_pkgpub_sub.launch.xml可以看到，發(fā)布者和接收者節(jié)點(diǎn)通過這個(gè)XML格式的launch文件全都運(yùn)行起來(lái)了。3.4.2YAML格式的Launch文件3.4Launch文件1、在VSCode中找到前面實(shí)驗(yàn)構(gòu)建的topic_pkg軟件包。用鼠標(biāo)右鍵單擊其中的[launch]文件夾，在彈出的[菜單]對(duì)話框中選擇[新建文件…]，命名為“pub_sub.launch.xml”。3.4.2YAML格式的Launch文件3.4Launch文件2、為這個(gè)pub_sub.launch.yaml編寫內(nèi)容：launch:-node:pkg:"topic_pkg"exec:"publisher_node"name:"publisher_node"-node:pkg:"topic_pkg"exec:"subscriber_node"name:"subscriber_node"3、在VSCode中打開[topic_pkg]軟件包的[CMakeLists.txt]文件。添加安裝規(guī)則：install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})3.4.2YAML格式的Launch文件3.4Launch文件4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild5、

運(yùn)行Launch文件sourceinstall/setup.bashros2launchtopic_pkg

pub_sub.launch.yaml可以看到，發(fā)布者和接收者節(jié)點(diǎn)通過這個(gè)YAML格式的launch文件全都運(yùn)行起來(lái)了。3.4.3Python格式的Launch文件3.4Launch文件1、在VSCode中找到前面實(shí)驗(yàn)構(gòu)建的topic_pkg軟件包。用鼠標(biāo)右鍵單擊其中的[launch]文件夾，在彈出的[菜單]對(duì)話框中選擇[新建文件…]，命名為“pub_sub.launch.py”。3.4Launch文件2、為這個(gè)pub_sub.launch.py編寫內(nèi)容：fromlaunch_ros.actionsimportNodefromlaunchimportLaunchDescription

defgenerate_launch_description():

publisher_cmd=Node(package='topic_pkg',executable='publisher_node',name='publisher_node')

subscriber_cmd=Node(package='topic_pkg',executable='subscriber_node',name='subscriber_node')

ld=LaunchDescription()

ld.add_action(publisher_cmd)ld.add_action(subscriber_cmd)

returnld3.4.3Python格式的Launch文件3.4Launch文件3、在VSCode中打開[topic_pkg]軟件包的[CMakeLists.txt]文件。添加安裝規(guī)則：install(DIRECTORYlaunchDESTINATIONshare/${PROJECT_NAME})3.4.3Python格式的Launch文件4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild5、

運(yùn)行Launch文件sourceinstall/setup.bashros2launchtopic_pkgpub_sub.launch.py3.4Launch文件3.4.3Python格式的Launch文件可以看到，發(fā)布者和接收者節(jié)點(diǎn)通過這個(gè)Python格式的launch文件全都運(yùn)行起來(lái)了。3.5本章小結(jié)

本章主要是對(duì)ROS2的基本程序概念進(jìn)行介紹和編程。首先詳細(xì)介紹了ROS2程序的組織形式節(jié)點(diǎn)Node和軟件包Package，以及節(jié)點(diǎn)Node之間最常用的通訊方式話題Topic和消息Message的實(shí)現(xiàn)；

接著，使用面向?qū)ο蟮姆绞?，編程?shí)現(xiàn)節(jié)點(diǎn)Node，其中包括話題發(fā)布者Publisher和話題訂閱者Subscriber的類封裝；最后，利用三種語(yǔ)言構(gòu)建Launch文件，實(shí)現(xiàn)一次同時(shí)啟動(dòng)多個(gè)節(jié)點(diǎ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)動(dòng)控制第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)用第4章

第4章ROS2機(jī)器人運(yùn)動(dòng)控制4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)4.1速度消息包格式4.3本章小結(jié)4.1速度消息包格式

對(duì)機(jī)器人的速度控制是通過向機(jī)器人的核心節(jié)點(diǎn)發(fā)送速度消息來(lái)實(shí)現(xiàn)的，這個(gè)消息的類型在ROS里已經(jīng)有了定義，就是geometry_msgs::Twist。本實(shí)例編程實(shí)現(xiàn)一個(gè)發(fā)布者節(jié)點(diǎn)，如圖所示，向"/cmd_vel"話題發(fā)布geometry_msgs::Twist類型的消息包。以此來(lái)實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)速度的控制。4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)(詳細(xì)操作步驟：見教材P107-P120頁(yè))4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)cd~/ros2_ws/srcros2pkgcreatevel_pkg4.2.1編寫速度控制程序#include<rclcpp/rclcpp.hpp>#include<geometry_msgs/msg/twist.hpp>

intmain(intargc,char**argv){

rclcpp::init(argc,argv);

autonode=std::make_shared<rclcpp::Node>("velocity_command_node");

autovel_pub=node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);

1、編寫節(jié)點(diǎn)代碼在VSCode中找到vel_pkg軟件包，在

“src”文件夾新建文件，命名為“vel_node.cpp”。4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)

geometry_msgs::msg::Twistvel_msg;

vel_msg.linear.x=0.1;

vel_msg.linear.y=0.0;

vel_msg.linear.z=0.0;

vel_msg.angular.x=0.0;

vel_msg.angular.y=0.0;

vel_msg.angular.z=0.0;

rclcpp::Rateloop_rate(30);while(rclcpp::ok()){

vel_pub->publish(vel_msg);

loop_rate.sleep();}

rclcpp::shutdown();

return0;}

4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)find_package(rclcppREQUIRED)find_package(geometry_msgsREQUIRED)add_executable(vel_node

src/vel_node.cpp)ament_target_dependencies(vel_node"rclcpp""geometry_msgs")install(TARGETSvel_nodeDESTINATIONlib/${PROJECT_NAME})<depend>rclcpp</depend><depend>geometry_msgs</depend>2、設(shè)置編譯規(guī)則3、修改軟件包信息4、編譯軟件包c(diǎn)d~/ros2_wscolconbuild4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)5、節(jié)點(diǎn)自測(cè)sourceinstall/setup.bashros2runvel_pkg

vel_node打開第2個(gè)子窗口ros2topiclistros2topicecho/cmd_vel4.2機(jī)器人運(yùn)動(dòng)控制的實(shí)現(xiàn)4.2.2仿真運(yùn)行速度控制程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runvel_pkg

vel_node打開第2個(gè)子窗口節(jié)點(diǎn)運(yùn)行起來(lái)之后，可以看到機(jī)器人以0.1米/秒的速度向前移動(dòng)，速度控制功能實(shí)現(xiàn)成功。4.3本章小結(jié)

本章主要介紹了發(fā)布者節(jié)點(diǎn)的一個(gè)典型應(yīng)用，通過編寫節(jié)點(diǎn)代碼、設(shè)置編譯規(guī)則、修改軟件包信息、編譯軟件包、節(jié)點(diǎn)自測(cè)、仿真運(yùn)行速度控制程序?qū)崿F(xiàn)了對(duì)機(jī)器人運(yùn)動(dò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中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第5章5.4基于激光雷達(dá)的避障實(shí)現(xiàn)5.1ROS2里的激光雷達(dá)數(shù)據(jù)5.5本章小結(jié)第5章激光雷達(dá)在ROS2中的使用5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)5.3激光雷達(dá)數(shù)據(jù)獲取

激光雷達(dá)的旋轉(zhuǎn)部分搭載激光測(cè)距探頭，其在旋轉(zhuǎn)的過程中每隔一定角度就會(huì)測(cè)量一次距離值。當(dāng)其旋轉(zhuǎn)一圈時(shí)，剛好可以對(duì)周圍障礙物進(jìn)行均勻的測(cè)距采樣，這么一組測(cè)距值就是激光雷達(dá)的輸出數(shù)值。

5.1ROS2里的激光雷達(dá)數(shù)據(jù)

ROS2中的激光雷達(dá)的節(jié)點(diǎn)會(huì)在指定名稱的話題里發(fā)布消息。話題名稱在REP-138中推薦為“/scan”。消息的類型為sensor_msgs::LaserScan，其格式如圖所示：

5.1ROS2里的激光雷達(dá)數(shù)據(jù)

在ROS2中，提供了一個(gè)圖形化工具，可以直觀的查看傳感器的數(shù)值。這個(gè)工具名字叫“RViz2”，其中“RViz”是“ROSVisualizationTool”的縮寫，“2”表示第二代。

5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)在RViz2中查看激光雷達(dá)的數(shù)據(jù)的詳細(xì)操作步驟：見教材P122-P128頁(yè)5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)5.2.1RViz2的使用source~/ros2_ws/install/setup.bashros2launchwpr_simulation2wpb_simple.launch.py這時(shí)會(huì)彈出一個(gè)窗口，如圖所示，顯示一臺(tái)機(jī)器人，面前放置了一個(gè)書柜。機(jī)器人的激光雷達(dá)掃描到了書柜的下半部分。5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)source~/ros2_ws/install/setup.bashrviz2打開第2個(gè)子窗口。5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)source~/ros2_ws/install/setup.bashrviz2打開第2個(gè)子窗口。5.2在RViz2中查看激光雷達(dá)數(shù)據(jù)5.2.3界面配置的加載rviz2-d~/lidar.rviz

指令執(zhí)行后，會(huì)彈出[RViz2窗口]對(duì)話框。如圖所示，這時(shí)候的RViz2直接顯示為之前保存配置時(shí)的樣子。

激光雷達(dá)數(shù)據(jù)的獲取是通過訂閱激光雷達(dá)發(fā)布的話題，從話題中獲取激光雷達(dá)發(fā)出的消息包來(lái)實(shí)現(xiàn)的。如圖所示，將會(huì)實(shí)現(xiàn)一個(gè)訂閱者節(jié)點(diǎn)，訂閱"/scan"話題。從此話題中接收sensor_msgs::LaserScan類型的消息包，并解析出雷達(dá)的測(cè)距數(shù)值。5.3激光雷達(dá)數(shù)據(jù)獲取激光雷達(dá)數(shù)據(jù)獲取的詳細(xì)操作步驟：見教材P129-P139頁(yè)5.3激光雷達(dá)數(shù)據(jù)獲取5.3.1編寫雷達(dá)數(shù)據(jù)獲取程序cd~/ros2_ws/srcros2pkgcreatelidar_pkg1、編寫節(jié)點(diǎn)代碼新建文件，命名為“l(fā)idar_data.cpp”。5.3激光雷達(dá)數(shù)據(jù)獲取#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/laser_scan.hpp>

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

voidLidarCallback(constsensor_msgs::msg::LaserScan::SharedPtrmsg){intnNum=msg->ranges.size();intnMid=nNum/2;floatfMidDist=msg->ranges[nMid];RCLCPP_INFO(node->get_logger(),"ranges[%d]=%fm",nMid,fMidDist);}

intmain(intargc,char**argv){

rclcpp::init(argc,argv);

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

autolidar_sub=node->create_subscription<sensor_msgs::msg::LaserScan>("/scan",10, LidarCallback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}5.3激光雷達(dá)數(shù)據(jù)獲取2、設(shè)置編譯規(guī)則3、修改軟件包信息4、編譯軟件包find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)add_executable(lidar_datasrc/lidar_data.cpp)ament_target_dependencies(lidar_data"rclcpp""sensor_msgs")install(TARGETSlidar_dataDESTINATIONlib/${PROJECT_NAME})<depend>rclcpp</depend><depend>sensor_msgs</depend>cd~/ros2_wscolconbuild5.3激光雷達(dá)數(shù)據(jù)獲取5.3.2仿真運(yùn)行雷達(dá)數(shù)據(jù)獲取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runlidar_pkg

lidar_data打開第2個(gè)子窗口。

下面將激光雷達(dá)數(shù)據(jù)獲取和速度控制進(jìn)行結(jié)合，實(shí)現(xiàn)一個(gè)避障程序。程序訂閱雷達(dá)數(shù)據(jù)的“/scan”話題，從中接收激光雷達(dá)節(jié)點(diǎn)發(fā)來(lái)的sensor_msgs::LaserScan類型消息包，并解析出雷達(dá)測(cè)距值。根據(jù)雷達(dá)測(cè)距值讓機(jī)器人做出反應(yīng)：前方?jīng)]有障礙物就前進(jìn)，前方遇到障礙物就轉(zhuǎn)向。5.4基于激光雷達(dá)的避障實(shí)現(xiàn)詳細(xì)操作步驟：見教材P140-P152頁(yè)5.4.1編寫雷達(dá)避障程序5.4基于激光雷達(dá)的避障實(shí)現(xiàn)避障邏輯流程圖5.4.1編寫雷達(dá)避障程序1、編寫節(jié)點(diǎn)代碼新建文件，命名為“l(fā)idar_behavior.cpp”。5.4基于激光雷達(dá)的避障實(shí)現(xiàn)#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/laser_scan.hpp>#include<geometry_msgs/msg/twist.hpp>

std::shared_ptr<rclcpp::Node>node;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr

vel_pub;intnCount=0;

voidLidarCallback(constsensor_msgs::msg::LaserScan::SharedPtrmsg){intnNum=msg->ranges.size();intnMid=nNum/2;floatfMidDist=msg->ranges[nMid];RCLCPP_INFO(node->get_logger(),"ranges[%d]=%fm",nMid,fMidDist);

if(nCount>0){nCount--;return;}

geometry_msgs::msg::Twistvel_msg;if(fMidDist<1.5f){vel_msg.angular.z=0.3;

nCount=100;}else{vel_msg.linear.x=0.1;}

vel_pub->publish(vel_msg);}5.4基于激光雷達(dá)的避障實(shí)現(xiàn)intmain(intargc,char**argv){

rclcpp::init(argc,argv);

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

vel_pub=node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel",10);autolidar_sub=node->create_subscription<sensor_msgs::msg::LaserScan>("/scan",10, LidarCallback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}5.4基于激光雷達(dá)的避障實(shí)現(xiàn)2、設(shè)置編譯規(guī)則3、修改軟件包信息4、編譯軟件包<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>geometry_msgs</depend>cd~/ros2_wscolconbuild5.4基于激光雷達(dá)的避障實(shí)現(xiàn)find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(geometry_msgsREQUIRED)add_executable(lidar_behavior

src/lidar_behavior.cpp)ament_target_dependencies(lidar_behavior"rclcpp""sensor_msgs""geometry_msgs")install(TARGETSlidar_data

lidar_behaviorDESTINATIONlib/${PROJECT_NAME})sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runlidar_pkg

lidar_behavior5.4基于激光雷達(dá)的避障實(shí)現(xiàn)5.4.2仿真運(yùn)行雷達(dá)避障程序打開第2個(gè)子窗口。節(jié)點(diǎn)運(yùn)行起來(lái)之后，切換到剛才的仿真窗口。如圖所示，可以看到機(jī)器人先以0.1米/秒的速度向前移動(dòng)。當(dāng)機(jī)器人距離書柜1.5米時(shí)，機(jī)器人停止前進(jìn)，開始向左逆時(shí)針旋轉(zhuǎn)。旋轉(zhuǎn)一段時(shí)間后，機(jī)器人恢復(fù)直行，順利的避開前方的書柜。5.5本章小結(jié)

本章主要是對(duì)激光雷達(dá)在ROS2中的使用進(jìn)行介紹和編程。首先介紹了ROS2里的激光雷達(dá)消息包格式以及如何在RViz2中查看激光雷達(dá)數(shù)據(jù)；接著，通過訂閱激光雷達(dá)發(fā)布的話題，從話題中獲取激光雷達(dá)發(fā)出的消息包獲取激光雷達(dá)數(shù)據(jù)；最后，將激光雷達(dá)數(shù)據(jù)獲取和速度控制進(jìn)行結(jié)合，編程實(shí)現(xiàn)了一個(gè)機(jī)器人避障功能。機(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中的圖像視覺應(yīng)用第10章ROS2的三維視覺應(yīng)用第11章ROS2的機(jī)械臂應(yīng)用第12章基于ROS2的綜合應(yīng)用第6章6.3基于IMU的航向鎖定實(shí)現(xiàn)

第6章IMU在ROS2中的使用6.2IMU數(shù)據(jù)獲取6.1IMU的話題名稱6.4本章小結(jié)

與激光雷達(dá)一樣，ROS2中的IMU節(jié)點(diǎn)會(huì)在特定的話題里發(fā)布數(shù)據(jù)消息。話題名稱在REP-145中建議為“/imu/data_raw”、“/imu/data”和“/imu/mag”三個(gè)。6.1ROS2里的IMU數(shù)據(jù)話題內(nèi)容/imu/data_raw加速度計(jì)輸出的矢量加速度和陀螺儀輸出的旋轉(zhuǎn)加速度。/imu/data/imu/data_raw的數(shù)據(jù)再加上融合后的四元數(shù)姿態(tài)描述/imu/mag磁強(qiáng)計(jì)輸出的磁強(qiáng)數(shù)據(jù)

IMU數(shù)據(jù)的獲取是通過訂閱IMU節(jié)點(diǎn)發(fā)布的話題，從話題中獲取IMU節(jié)點(diǎn)發(fā)出的消息包來(lái)實(shí)現(xiàn)的。如圖所示：6.2IMU數(shù)據(jù)獲取詳細(xì)操作步驟見教材P155-P166頁(yè)cd~/ros2_ws/srcros2pkgcreateimu_pkg1、編寫節(jié)點(diǎn)代碼新建文件，命名為“imu_data.cpp”。6.2IMU數(shù)據(jù)獲取6.2.1編寫IMU數(shù)據(jù)獲取程序#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/imu.hpp>#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>

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

voidIMUCallback(constsensor_msgs::msg::Imu::SharedPtrmsg){tf2::Quaterniontf2_quaternion;tf2_quaternion.setX(msg->orientation.x);tf2_quaternion.setY(msg->orientation.y);tf2_quaternion.setZ(msg->orientation.z);tf2_quaternion.setW(msg->orientation.w);

tf2::Matrix3x3matrix(tf2_quaternion);

doubleroll,pitch,yaw;

matrix.getRPY(roll,pitch,yaw);roll=roll*180/M_PI;pitch=pitch*180/M_PI;yaw=yaw*180/M_PI;RCLCPP_INFO(node->get_logger(),"roll=%.0fpitch=%.0fyaw=%.0f",roll,pitch,yaw);}6.2IMU數(shù)據(jù)獲取intmain(intargc,char**argv){

rclcpp::init(argc,argv);

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

autosub=node->create_subscription<sensor_msgs::msg::Imu>("/imu",10,IMUCallback);

rclcpp::spin(node);

rclcpp::shutdown();

return0;}6.2IMU數(shù)據(jù)獲取2、設(shè)置編譯規(guī)則3、修改軟件包信息4、編譯軟件包find_package(rclcppREQUIRED)find_package(sensor_msgsREQUIRED)find_package(tf2REQUIRED)add_executable(imu_data

src/imu_data.cpp)ament_target_dependencies(imu_data"rclcpp""sensor_msgs""tf2")install(TARGETSimu_dataDESTINATIONlib/${PROJECT_NAME})<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>tf2</depend>cd~/ros2_wscolconbuild6.2IMU數(shù)據(jù)獲取6.2.2仿真運(yùn)行雷達(dá)數(shù)據(jù)獲取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runimu_pkg

imu_data6.2IMU數(shù)據(jù)獲取打開第2個(gè)子窗口。6.2IMU數(shù)據(jù)獲取

下面實(shí)現(xiàn)一個(gè)航向鎖定的功能。編寫一個(gè)節(jié)點(diǎn)，先訂閱“/imu/data”話題。從此話題中接收IMU節(jié)點(diǎn)發(fā)來(lái)的sensor_msgs::Imu類型消息包，解析出機(jī)器人的姿態(tài)角度。設(shè)置一個(gè)目標(biāo)航向角度，讓機(jī)器人朝著這個(gè)航向運(yùn)動(dòng)。若運(yùn)行過程中機(jī)器人的姿態(tài)被改變了，導(dǎo)致航向角與目標(biāo)角度不一致。則根據(jù)航向角偏差值，計(jì)算出糾偏速度值，打包成geometry_msgs::Twist類型的消息包，發(fā)布到“/cmd_vel”話題中。機(jī)器人底盤節(jié)點(diǎn)會(huì)從“/cmd_vel”話題獲取速度值并執(zhí)行，以此來(lái)實(shí)現(xiàn)航向鎖定的行為。如圖所示：6.3基于IMU的航向鎖定實(shí)現(xiàn)詳細(xì)操作步驟見教材P167-P179頁(yè)1、編寫節(jié)點(diǎn)代碼新建文件，命名為“imu_behavior.cpp”。6.3基于IMU的航向鎖定實(shí)現(xiàn)6.3.1編寫航向鎖定程序按照上一個(gè)實(shí)驗(yàn)，在工作空間中創(chuàng)建一個(gè)imu_pkg軟件包。如果已經(jīng)創(chuàng)建好了，可以直接使用，不用再重復(fù)創(chuàng)建。具體操作步驟如下：#include<rclcpp/rclcpp.hpp>#include<sensor_msgs/msg/imu.hpp>#include<geometry_msgs/msg/twist.hpp>#include<tf2/LinearMath/Quaternion.h>#include<tf2/LinearMath/Matrix3x3.h>

std::shared_ptr<rclcpp::Node>node;rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr

vel_pub;

voidIMUCallback(constsensor_msgs::msg::Imu::SharedPtrmsg){tf2::Quaterniontf2_quaternion;tf2_quaternion.setX(msg->orientation.x);tf2_quaternion.setY(msg->orientation.y);tf2_quaternion.setZ(msg->orientation.z);tf2_quaternion.setW(msg->orientation.w);

tf2::Matrix3x3matrix(tf2_quaternion);

doubleroll,pitch,yaw;

matrix.getRPY(roll,pitch,yaw);roll=roll*180/M_PI;pitch=pitch*180/M_PI;yaw=yaw*180/M_PI;RCLCPP_INFO(node->get_logger(),"roll=%.0fpitch=%.0fyaw=%.0f",roll,pitch,yaw);6.3基于IMU的航向鎖定實(shí)現(xiàn)doubletarget_yaw=90;

geometry_msgs::msg::Twistvel_msg;doublediff_angle=target_yaw-yaw;

vel_msg.angular.z=diff_angle*0.01;

vel_msg.linear.x=0.1;vel_pub->publish(vel_msg);}

intmain(intargc,char**argv){

rclcpp::init(argc,argv);