機器人操作系統（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的綜合應用第5章5.4基于激光雷達的避障實現5.1ROS2里的激光雷達數據5.5本章小結第5章激光雷達在ROS2中的使用5.2在RViz2中查看激光雷達數據5.3激光雷達數據獲取

激光雷達的旋轉部分搭載激光測距探頭，其在旋轉的過程中每隔一定角度就會測量一次距離值。當其旋轉一圈時，剛好可以對周圍障礙物進行均勻的測距采樣，這么一組測距值就是激光雷達的輸出數值。

5.1ROS2里的激光雷達數據

ROS2中的激光雷達的節點會在指定名稱的話題里發布消息。話題名稱在REP-138中推薦為“/scan”。消息的類型為sensor_msgs::LaserScan，其格式如圖所示：

5.1ROS2里的激光雷達數據

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

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

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

激光雷達數據的獲取是通過訂閱激光雷達發布的話題，從話題中獲取激光雷達發出的消息包來實現的。如圖所示，將會實現一個訂閱者節點，訂閱"/scan"話題。從此話題中接收sensor_msgs::LaserScan類型的消息包，并解析出雷達的測距數值。5.3激光雷達數據獲取激光雷達數據獲取的詳細操作步驟：見教材P129-P139頁5.3激光雷達數據獲取5.3.1編寫雷達數據獲取程序cd~/ros2_ws/srcros2pkgcreatelidar_pkg1、編寫節點代碼新建文件，命名為“lidar_data.cpp”。5.3激光雷達數據獲取#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激光雷達數據獲取2、設置編譯規則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激光雷達數據獲取5.3.2仿真運行雷達數據獲取程序sourceinstall/setup.bashros2launchwpr_simulation2wpb_simple.launch.pysourceinstall/setup.bashros2runlidar_pkglidar_data打開第2個子窗口。

下面將激光雷達數據獲取和速度控制進行結合，實現一個避障程序。程序訂閱雷達數據的“/scan”話題，從中接收激光雷達節點發來的sensor_msgs::LaserScan類型消息包，并解析出雷達測距值。根據雷達測距值讓機器人做出反應：前方沒有障礙物就前進，前方遇到障礙物就轉向。5.4基于激光雷達的避障實現詳細操作步驟：見教材P140-P152頁5.4.1編寫雷達避障程序5.4基于激光雷達的避障實現避障邏輯流程圖5.4.1編寫雷達避障程序1、編寫節點代碼新建文件，命名為“lidar_behavior.cpp”。5.4基于激光雷達的避障實現#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基于激光雷達的避障實現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基于激光雷達的避障實現2、設置編譯規則3、修改軟件包信息4、編譯軟件包<depend>rclcpp</depend><depend>sensor_msgs</depend><depend>geometry_msgs</depend>cd~/ros2_wscolconbuild5.4基于激光雷達的避障實現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基于激光雷達的避障實現5.4.2仿真運行雷達避障程序打開第2個子窗口。節點運行起來之后，切換到剛才的仿真窗口。如圖所