機(jī)器人操作系統(tǒng)（ROS2）入門與實(shí)踐 課件 第5章 激光雷達(dá)在ROS2中的使用

機(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)部分搭載激光測距探頭，其在旋轉(zhuǎn)的過程中每隔一定角度就會(huì)測量一次距離值。當(dāng)其旋轉(zhuǎn)一圈時(shí)，剛好可以對周圍障礙物進(jìn)行均勻的測距采樣，這么一組測距值就是激光雷達(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頁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窗口]對話框。如圖所示，這時(shí)候的RViz2直接顯示為之前保存配置時(shí)的樣子。

激光雷達(dá)數(shù)據(jù)的獲取是通過訂閱激光雷達(dá)發(fā)布的話題，從話題中獲取激光雷達(dá)發(fā)出的消息包來實(shí)現(xiàn)的。如圖所示，將會(huì)實(shí)現(xiàn)一個(gè)訂閱者節(jié)點(diǎn)，訂閱"/scan"話題。從此話題中接收sensor_msgs::LaserScan類型的消息包，并解析出雷達(dá)的測距數(shù)值。5.3激光雷達(dá)數(shù)據(jù)獲取激光雷達(dá)數(shù)據(jù)獲取的詳細(xì)操作步驟：見教材P129-P139頁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_pkglidar_data打開第2個(gè)子窗口。

下面將激光雷達(dá)數(shù)據(jù)獲取和速度控制進(jìn)行結(jié)合，實(shí)現(xiàn)一個(gè)避障程序。程序訂閱雷達(dá)數(shù)據(jù)的“/scan”話題，從中接收激光雷達(dá)節(jié)點(diǎn)發(fā)來的sensor_msgs::LaserScan類型消息包，并解析出雷達(dá)測距值。根據(jù)雷達(dá)測距值讓機(jī)器人做出反應(yīng)：前方?jīng)]有障礙物就前進(jìn)，前方遇到障礙物就轉(zhuǎn)向。5.4基于激光雷達(dá)的避障實(shí)現(xiàn)詳細(xì)操作步驟：見教材P140-P152頁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>::SharedPtrvel_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_behaviorsrc/lidar_behavior.cpp)ament_target_dependencies(lidar_behavior"rclcpp""sensor_msgs""geometry_msgs")install(TARGETSlidar_datalidar_behaviorDESTINATIONlib/${PROJECT_NAME})sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runlidar_pkglidar_behavior5.4基于激光雷達(dá)的避障實(shí)現(xiàn)5.4.2仿真運(yùn)行雷達(dá)避障程序打開第2個(gè)子窗口。節(jié)點(diǎn)運(yùn)行起來之后，切換到剛才的仿真窗口。如圖所