做機(jī)器人仿真的兄弟應(yīng)該都遇到過(guò)這個(gè)尷尬真實(shí)車上跑得好好的Livox點(diǎn)云算法一搬到Gazebo里就各種不對(duì)。最直觀的差異就在消息格式上——真實(shí)雷達(dá)通過(guò)livox_ros_driver2發(fā)布的是livox_interfaces/msg/CustomMsg里面帶著tag、line、offset_time這些“貼心小紙條”而Gazebo里隨便一個(gè)激光雷達(dá)插件出來(lái)的都是ROS標(biāo)準(zhǔn)的sensor_msgs/PointCloud2兩個(gè)格式在字段上根本不通用。于是你會(huì)發(fā)現(xiàn)SLAM、去畸變、特征提取這些代碼在仿真環(huán)境的數(shù)據(jù)流路徑上直接斷掉了。這篇文章就是來(lái)解決這個(gè)問(wèn)題的。我會(huì)從Livox CustomMsg和PointCloud2的差異講起對(duì)比兩種可行的技術(shù)路線然后手把手寫(xiě)一個(gè)獨(dú)立的轉(zhuǎn)換節(jié)點(diǎn)把Gazebo仿真里的PointCloud2實(shí)時(shí)轉(zhuǎn)成Livox CustomMsg最后把我在實(shí)際調(diào)試中踩過(guò)的坑、總結(jié)的排查技巧一并整理出來(lái)。無(wú)論你是剛接觸ROS的仿真新手還是已經(jīng)在跑Livox實(shí)車算法、想在仿真里驗(yàn)證邏輯的開(kāi)發(fā)者這篇都能給你一條能直接上手的路。1. 為什么仿真里沒(méi)有現(xiàn)成的Livox數(shù)據(jù)格式1.1 兩種消息格式的底細(xì)先看清楚兩邊到底差在哪。sensor_msgs/PointCloud2是ROS標(biāo)準(zhǔn)點(diǎn)云格式本質(zhì)是一塊連續(xù)內(nèi)存里面按fields描述的字段順序把每個(gè)點(diǎn)的坐標(biāo)、強(qiáng)度等信息排列起來(lái)。它只關(guān)心“有哪些點(diǎn)、每個(gè)點(diǎn)有什么屬性”不關(guān)心這些點(diǎn)是怎么掃描出來(lái)的。livox_interfaces/msg/CustomMsg就不一樣了它不只是點(diǎn)云還包含了激光雷達(dá)的掃描組織信息。每個(gè)CustomPoint除了x、y、z、intensity之外還有三個(gè)額外字段tag標(biāo)定點(diǎn)屬性正常點(diǎn)、雜散點(diǎn)等line標(biāo)定這條點(diǎn)屬于第幾條掃描線束offset_time標(biāo)定這個(gè)點(diǎn)距離整幀timebase的精確時(shí)間偏移。消息外層還帶著lidar_id和timebase用來(lái)區(qū)分多雷達(dá)組合和使用時(shí)間戳做運(yùn)動(dòng)補(bǔ)償。字段維度PointCloud2Livox CustomMsg坐標(biāo)x/y/zx/y/z強(qiáng)度intensityintensity線束編號(hào)無(wú)line點(diǎn)時(shí)間戳無(wú)offset_time點(diǎn)屬性標(biāo)記無(wú)tag雷達(dá)編號(hào)無(wú)lidar_id這個(gè)差異不是設(shè)計(jì)風(fēng)格問(wèn)題而是Livox的非重復(fù)掃描架構(gòu)決定的。傳統(tǒng)機(jī)械雷達(dá)每個(gè)點(diǎn)可以明確對(duì)應(yīng)到某個(gè)掃描線、某個(gè)角度點(diǎn)云天然帶有掃描結(jié)構(gòu)Livox采用花瓣式非重復(fù)掃描點(diǎn)與點(diǎn)之間的關(guān)系更復(fù)雜算法靠line和offset_time才能做畸變校正和特征提取。所以想跑通livox_ros_driver2下游的那套算法光有坐標(biāo)和強(qiáng)度遠(yuǎn)遠(yuǎn)不夠。1.2 Gazebo默認(rèn)輸出為什么不夠用Gazebo里最常用來(lái)模擬激光雷達(dá)的是gazebo_ros_ray_sensor這一類傳感器插件以及它對(duì)應(yīng)的點(diǎn)云或者scan輸出。插件建模的思路是“規(guī)則網(wǎng)格采樣”在設(shè)定的水平、垂直角分辨率下均勻發(fā)射射線命中物體后生成點(diǎn)。這種輸出天然是規(guī)則的、等間隔的最后封裝成PointCloud2或者LaserScan發(fā)給ROS。問(wèn)題就出在“規(guī)則”兩個(gè)字上。Livox真實(shí)點(diǎn)云是稀疏的、非重復(fù)掃描的、帶有時(shí)間偏移和線束信息的就連點(diǎn)云分布形態(tài)都和規(guī)則網(wǎng)格完全不同。你把Gazebo的PointCloud2直接喂給一個(gè)為L(zhǎng)ivox CustomMsg設(shè)計(jì)的特征提取模塊它連line都拿不到更不用說(shuō)offset_time了算法自然跑不起來(lái)。所以單靠Gazebo自帶插件你無(wú)法得到算法側(cè)真正需要的“Livox風(fēng)味”數(shù)據(jù)。這也是很多人在仿真里驗(yàn)證Livox相關(guān)SLAM時(shí)總感覺(jué)“仿真能用實(shí)車就崩”的原因之一——數(shù)據(jù)通路就不等價(jià)。1.3 轉(zhuǎn)換之后能獲得什么做完P(guān)ointCloud2到CustomMsg的轉(zhuǎn)換你得到的不只是一個(gè)格式不同的消息而是一條和實(shí)車一致的完整數(shù)據(jù)鏈路。下游的畸變校正、特征提取、激光慣性里程計(jì)都可以直接用真實(shí)驅(qū)動(dòng)下的同一套代碼和參數(shù)不需要為仿真單獨(dú)維護(hù)一份算法分支。這意味著算法驗(yàn)證效率大幅提升。仿真里改改場(chǎng)景重量級(jí)回放算法如果崩了大概率不是數(shù)據(jù)格式問(wèn)題而是參數(shù)或策略問(wèn)題定位范圍一下子縮小很多。后面章節(jié)我展開(kāi)講怎么把這個(gè)轉(zhuǎn)換節(jié)點(diǎn)做得既通用又可靠。2. 方案選型硬轉(zhuǎn)還是模擬2.1 方案A直接從仿真插件生成CustomMsg第一個(gè)思路是在Gazebo里用專門(mén)的Livox仿真插件直接從源頭生成CustomMsg。目前社區(qū)里比較常見(jiàn)的是基于LibLivox仿真庫(kù)的livox_laser_simulation方案它可以在仿真環(huán)境中還原Livox的非重復(fù)掃描特性并直接發(fā)布livox_interfaces/msg/CustomMsg。這個(gè)方案的好處是“一步到位”點(diǎn)云分布形態(tài)、線束信息、時(shí)間戳結(jié)構(gòu)都和真實(shí)Livox比較接近。適合對(duì)點(diǎn)云形態(tài)還原度要求高的場(chǎng)景比如驗(yàn)證去畸變算法、研究非重復(fù)掃描的覆蓋特性。但它的代價(jià)也不小。插件基于特定的雷達(dá)模型開(kāi)發(fā)你要換其他型號(hào)的Livox雷達(dá)或者調(diào)整掃描參數(shù)、線束數(shù)、噪聲底數(shù)就得去改插件源碼重新編譯。另外新版本ROS/Gazebo接口變化快老插件未必能平滑適配所有環(huán)境這可能才是最大的隱性成本。2.2 方案B獨(dú)立的PointCloud2轉(zhuǎn)CustomMsg節(jié)點(diǎn)另一種思路是保持Gazebo里的雷達(dá)插件不變只在Host側(cè)寫(xiě)一個(gè)轉(zhuǎn)換節(jié)點(diǎn)訂閱PointCloud2解析出每個(gè)點(diǎn)的坐標(biāo)、強(qiáng)度再做特征分類和時(shí)間映射實(shí)時(shí)組裝成CustomMsg發(fā)出去。這個(gè)方案的優(yōu)勢(shì)非常明顯完全不侵入Gazebo模型和現(xiàn)有仿真環(huán)境Gazebo側(cè)只需要“有個(gè)點(diǎn)云話題”就行無(wú)論是ray sensor還是別的傳感器輸出的PointCloud2都能用。轉(zhuǎn)換邏輯集中在一個(gè)節(jié)點(diǎn)里想調(diào)參數(shù)、加濾波、改線束映射都不需要重新編譯Gazebo插件。缺點(diǎn)是要自己處理特征分類和時(shí)間映射這恰恰是本節(jié)的重點(diǎn)難點(diǎn)。如果只是簡(jiǎn)單遍歷點(diǎn)云挨個(gè)塞進(jìn)CustomPoint那轉(zhuǎn)出來(lái)的消息信息量仍然不足下游算法體驗(yàn)不會(huì)比直接用PointCloud2好多少。2.3 我的建議以我實(shí)際接觸過(guò)的項(xiàng)目來(lái)看如果你只是為了在仿真里跑通感知、SLAM、導(dǎo)航這類下游任務(wù)方案B性價(jià)比更高。它保證“格式對(duì)”改動(dòng)范圍小且整個(gè)轉(zhuǎn)換邏輯是透明的出現(xiàn)問(wèn)題容易排查。方案A適合做傳感器特性研究比如要精確模擬非重復(fù)掃描覆蓋率、驗(yàn)證Livox掃描算法本身這時(shí)候值得投入精力去調(diào)仿真插件。3. 實(shí)操?gòu)牧銓?xiě)一個(gè)PointCloud2轉(zhuǎn)CustomMsg節(jié)點(diǎn)3.1 環(huán)境準(zhǔn)備與工作空間我以Ubuntu 22.04 ROS2 Humble為例這套組合現(xiàn)在比較主流Gazebo用的是Ignition Fortress或新版GazeboGarden/Harmonic。思路完全兼容ROS1只是消息包和編譯方式稍有差異。先確認(rèn)Livox消息接口。ROS2里需要安裝livox_interfaces最省事的方式是直接編譯livox_ros_driver2倉(cāng)庫(kù)它會(huì)帶上消息定義。如果只想在RViz里簡(jiǎn)單看轉(zhuǎn)換結(jié)果不一定完整運(yùn)行驅(qū)動(dòng)但消息定義必須有。創(chuàng)建工作空間建議結(jié)構(gòu)如下livox_ws/ src/ pointcloud2_to_custommsg/ CMakeLists.txt package.xml src/ converter_node.cpp launch/ converter.launch.py在package.xml里聲明對(duì)rclcpp、sensor_msgs、livox_interfaces的依賴。編譯工具我用colcon遇到消息包找不到的問(wèn)題檢查一下livox_interfaces是否已經(jīng)編譯并source到當(dāng)前環(huán)境中。3.2 消息解析與坐標(biāo)提取進(jìn)入核心邏輯前先想清楚一個(gè)問(wèn)題PointCloud2是一塊連續(xù)字節(jié)流你要從fields里找到x、y、z、intensity字段各自的偏移量再根據(jù)point_step逐個(gè)點(diǎn)去讀。千萬(wàn)別硬編碼成“前三個(gè)float是xyz”因?yàn)椴煌?qū)動(dòng)、不同插件產(chǎn)出的PointCloud2字段排布并不一致。用pcl或者ROS自帶的轉(zhuǎn)換函數(shù)可以省掉這部分手工解析工作比如pcl::fromROSMsg能直接塞進(jìn)pcl::PointCloudpcl::PointXYZI。如果你的仿真環(huán)境能保證PointCloud2的字段就是xyzintensity這個(gè)方式最快。但我建議在正式環(huán)境里還是做一層字段偏移量查表防止字段順序變化導(dǎo)致點(diǎn)云整體讀錯(cuò)。讀出來(lái)的坐標(biāo)還需要做一步坐標(biāo)系確認(rèn)。Gazebo里雷達(dá)的坐標(biāo)系可能是sensor_frame而下游算法期望的是base_link或livox_frame。轉(zhuǎn)換節(jié)點(diǎn)里要訂閱對(duì)應(yīng)的TF把每個(gè)點(diǎn)變換到目標(biāo)坐標(biāo)系。這一步很多人會(huì)漏最后點(diǎn)云看起來(lái)沒(méi)有錯(cuò)但拼接、配準(zhǔn)怎么都不對(duì)原因就在坐標(biāo)系沒(méi)對(duì)齊。3.3 特征點(diǎn)分類平面點(diǎn)、邊緣點(diǎn)、雜散點(diǎn)這是把PointCloud2“升級(jí)”成CustomMsg最關(guān)鍵的一步。真實(shí)Livox點(diǎn)云里點(diǎn)會(huì)被標(biāo)記為不同屬性算法在預(yù)處理時(shí)會(huì)根據(jù)這些標(biāo)記區(qū)分正常點(diǎn)、邊緣點(diǎn)和雜散點(diǎn)。仿真PointCloud2里這些信息全都沒(méi)有需要我們自己算一個(gè)近似的分類。我的做法是計(jì)算每個(gè)點(diǎn)的局部曲率。對(duì)當(dāng)前點(diǎn)取前后相鄰的若干個(gè)點(diǎn)用這些鄰域點(diǎn)擬合一個(gè)局部平面然后計(jì)算當(dāng)前點(diǎn)到這個(gè)平面的距離或者用相鄰點(diǎn)之間的距離變化作為粗糙度指標(biāo)。曲率小的點(diǎn)歸為平面點(diǎn)曲率大的歸為邊緣點(diǎn)。具體閾值和場(chǎng)景尺寸有關(guān)我剛調(diào)試時(shí)常用的是0.1米附近在室內(nèi)小場(chǎng)景和室外中距離場(chǎng)景下都比較穩(wěn)但最好針對(duì)自己的仿真場(chǎng)景標(biāo)定一次。雜散點(diǎn)可以用距離跳變檢測(cè)。計(jì)算當(dāng)前點(diǎn)和鄰域點(diǎn)之間的距離如果出現(xiàn)明顯不連續(xù)跳變且周邊點(diǎn)數(shù)支持度很低就把它標(biāo)記為雜散點(diǎn)。這類點(diǎn)在仿真環(huán)境里主要出現(xiàn)在物體邊緣、遮擋邊界附近直接過(guò)濾掉可以讓下游特征提取干凈很多。3.4 時(shí)間戳、線束編號(hào)和tag的填充分類完成后剩下就是給每個(gè)點(diǎn)補(bǔ)上line、offset_time和tag。line本質(zhì)上是把點(diǎn)映射到雷達(dá)的掃描線束編號(hào)。Livox雷達(dá)的真實(shí)線束排布有具體裝調(diào)參數(shù)仿真里我們拿不到但可以根據(jù)點(diǎn)的俯仰角做一個(gè)近似映射。atan2(z, sqrt(x*x y*y))得到俯仰角再根據(jù)雷達(dá)的垂直視場(chǎng)角范圍劃分成若干線束區(qū)間把點(diǎn)分配到對(duì)應(yīng)的line編號(hào)。這個(gè)近似對(duì)大多數(shù)下游算法足夠用想更精確的話要根據(jù)你的雷達(dá)型號(hào)手冊(cè)去查實(shí)際線束角度。offset_time我建議模仿真實(shí)采樣的時(shí)間累積方式已知仿真雷達(dá)的掃描周期又知道點(diǎn)云點(diǎn)的順序代表著不同的采樣時(shí)刻可以按點(diǎn)索引乘以單位時(shí)間間隔來(lái)填充。如果你的Scene里有Gazebo插件輸出的時(shí)間信息也可以直接用header.stamp差值來(lái)標(biāo)定每個(gè)點(diǎn)的精確時(shí)間。注意單位是納秒還是微秒ROS2的time是納秒Livox消息里offset_time一般按微秒理解別混了。tag字段正常點(diǎn)設(shè)為默認(rèn)值雜散點(diǎn)單獨(dú)標(biāo)記具體取值可以參考livox_ros_driver2里的定義習(xí)慣保證下游算法能識(shí)別就行。這一步不需要做到和真實(shí)驅(qū)動(dòng)完全一致但至少要給出可區(qū)分的分類標(biāo)簽。3.5 完整代碼思路與測(cè)試方法下面我寫(xiě)一個(gè)轉(zhuǎn)換節(jié)點(diǎn)的核心骨架邏輯不復(fù)雜重點(diǎn)在字段解析和特征分類兩段。// 核心偽代碼PointCloud2 轉(zhuǎn) CustomMsg 主流程 void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr pc2_msg) { livox_interfaces::msg::CustomMsg custom_msg; custom_msg.header pc2_msg-header; custom_msg.timebase pc2_msg-header.stamp.nanosec / 1000; // 轉(zhuǎn)微秒 custom_msg.lidar_id 0; auto cloud std::make_sharedpcl::PointCloudpcl::PointXYZI(); pcl::fromROSMsg(*pc2_msg, *cloud); for (size_t i 0; i cloud-points.size(); i) { // 1. 坐標(biāo)系變換此處省略TF變換細(xì)節(jié) // 2. 計(jì)算曲率/粗糙度 float curvature computeCurvature(cloud, i, neighbor_num); // 3. 距離跳變檢測(cè) bool is_outlier isDistanceJumpOutlier(cloud, i, distance_thresh); livox_interfaces::msg::CustomPoint point; point.x cloud-points[i].x; point.y cloud-points[i].y; point.z cloud-points[i].z; point.intensity cloud-points[i].intensity; if (is_outlier) { point.tag 1; // 雜散點(diǎn)標(biāo)記 } else if (curvature edge_thresh) { point.tag 2; // 邊緣點(diǎn)標(biāo)記 } else { point.tag 0; // 正常點(diǎn) } point.line computeLineId(cloud-points[i], vertical_fov); point.offset_time i * time_increment_us; custom_msg.points.push_back(point); } custom_msg.point_num custom_msg.points.size(); publisher_-publish(custom_msg); }編譯之后用下面幾條命令做基礎(chǔ)驗(yàn)證source install/setup.bash ros2 launch pointcloud2_to_custommsg converter.launch.py ros2 topic echo /livox/lidar livox_interfaces/msg/CustomMsg --once第一條能正常輸出CustomMsg、字段和點(diǎn)數(shù)符合預(yù)期說(shuō)明基本鏈路已經(jīng)打通。此時(shí)再在rviz里創(chuàng)建一個(gè)CustomMsg Display加載同一個(gè)話題把點(diǎn)云可視化出來(lái)驗(yàn)證分布形態(tài)。這里有個(gè)小提醒RViz2對(duì)自定義消息的支持不一定順手如果顯示異常先用命令行檢查消息內(nèi)容再用Foxglove Studio這類可視化工具輔助查看。4. 噪聲、多雷達(dá)和時(shí)間同步這些坑很隱蔽4.1 仿真點(diǎn)云的去噪仿真環(huán)境不等于無(wú)噪聲環(huán)境。Gazebo里物體邊緣、傳感器近距遮擋、不同材質(zhì)交界處會(huì)產(chǎn)生一些跳變點(diǎn)這些點(diǎn)和真實(shí)雷達(dá)的雜散噪點(diǎn)表現(xiàn)類似。如果轉(zhuǎn)換節(jié)點(diǎn)不做去噪這些異常點(diǎn)會(huì)被當(dāng)成正常點(diǎn)進(jìn)入下游算法影響特征提取。我的做法是兩級(jí)過(guò)濾。第一級(jí)用距離統(tǒng)計(jì)濾波計(jì)算每個(gè)點(diǎn)到鄰域點(diǎn)的平均距離距離均值偏離整體均值的點(diǎn)直接標(biāo)記或剔除第二級(jí)是曲率過(guò)濾將曲率異常大的點(diǎn)打上tag交給下游決定是否丟棄。兩級(jí)都放到轉(zhuǎn)換節(jié)點(diǎn)里不額外起節(jié)點(diǎn)減少傳輸耗時(shí)。4.2 強(qiáng)度值仿真與材質(zhì)反射率Gazebo里激光雷達(dá)點(diǎn)的強(qiáng)度值本質(zhì)上是基于材質(zhì)反射屬性模擬出來(lái)的結(jié)果。但仿真默認(rèn)材質(zhì)的反射率屬性和真實(shí)Livox對(duì)不同材質(zhì)磚墻、金屬、玻璃、植被的強(qiáng)度響應(yīng)相差很大。如果你下游算法依賴intensity做特征比如反射率地圖構(gòu)建仿真結(jié)果只能做參考不能直接拿來(lái)調(diào)參。改進(jìn)的辦法是修改Gazebo的材質(zhì)屬性或者干脆在轉(zhuǎn)換節(jié)點(diǎn)里對(duì)intensity做一次偽標(biāo)定映射把想要的強(qiáng)度范圍壓縮到一個(gè)可用區(qū)間讓仿真點(diǎn)云看起來(lái)“更像”真實(shí)雷達(dá)的強(qiáng)度分布。這個(gè)方法不嚴(yán)謹(jǐn)?shù)銐蜃屢蕾噺?qiáng)度的算法先跑起來(lái)。4.3 多臺(tái)Livox的時(shí)間同步真實(shí)系統(tǒng)里多臺(tái)Livox雷達(dá)的數(shù)據(jù)往往需要同步到統(tǒng)一時(shí)間源使用GPS時(shí)鐘或者PPS信號(hào)來(lái)對(duì)齊。不同的雷達(dá)lidar_id用來(lái)區(qū)分timebase和offset_time則用于點(diǎn)云的時(shí)域補(bǔ)償。Gazebo仿真里多雷達(dá)模型如果直接各自發(fā)包時(shí)間戳天然就是分散的。你需要讓每臺(tái)雷達(dá)的轉(zhuǎn)換節(jié)點(diǎn)都基于同一個(gè)基準(zhǔn)時(shí)間生成timebase并且為每臺(tái)雷達(dá)分配固定的lidar_id。最簡(jiǎn)單的方式是在啟動(dòng)參數(shù)里傳一個(gè)全局時(shí)間偏移轉(zhuǎn)換時(shí)統(tǒng)一使用rclcpp::Clock(RCL_ROS_TIME)獲取當(dāng)前ROS時(shí)間保證各節(jié)點(diǎn)執(zhí)行時(shí)基準(zhǔn)一致。4.4 外參標(biāo)定問(wèn)題我在項(xiàng)目里見(jiàn)過(guò)最好笑的bug就是點(diǎn)云格式轉(zhuǎn)換正常但SLAM出來(lái)的地圖是歪的查了半天發(fā)現(xiàn)是雷達(dá)安裝在仿真模型里的位姿pose和下流算法默認(rèn)的base_link到livox_frame外參不一致。轉(zhuǎn)換節(jié)點(diǎn)只是管理消息格式不負(fù)責(zé)外參標(biāo)定這個(gè)要區(qū)分清楚。仿真里改外參很方便直接在URDF或SDF里調(diào)整雷達(dá)的坐標(biāo)偏置和旋轉(zhuǎn)量即可。真實(shí)車上的外參標(biāo)定另有專門(mén)工具和流程仿真環(huán)境里至少要做到坐標(biāo)系定義和真實(shí)系統(tǒng)一致否則“仿真能跑、實(shí)車就歪”的問(wèn)題還會(huì)一個(gè)接一個(gè)。5. 常見(jiàn)問(wèn)題與排查技巧實(shí)錄5.1 問(wèn)題速查表這幾類問(wèn)題是我實(shí)際用這個(gè)方案時(shí)高頻遇到的直接做成了速查表現(xiàn)象可能原因解決方案話題收到空點(diǎn)云PointCloud2字段解析失敗檢查fields偏移量避免硬編碼字段布局點(diǎn)云方向顛倒/鏡像坐標(biāo)系或TF不匹配確認(rèn)sensor_frame和目標(biāo)坐標(biāo)系打印中心點(diǎn)坐標(biāo)驗(yàn)證CustomMsg的line全部為0俯仰角映射區(qū)間設(shè)置不合理根據(jù)雷達(dá)垂直視場(chǎng)范圍調(diào)整線束區(qū)間點(diǎn)云出現(xiàn)大量邊緣點(diǎn)曲率閾值過(guò)低增大曲率閾值或用鄰域點(diǎn)數(shù)自適應(yīng)offset_time跳變異常時(shí)間單位混用統(tǒng)一納秒/微秒單位按傳感器周期重新映射多雷達(dá)點(diǎn)云疊不到一起外參錯(cuò)誤或時(shí)間基準(zhǔn)不一致檢查URDF中的雷達(dá)pose統(tǒng)一timebase基準(zhǔn)rviz顯示不了CustomMsg自定義消息的RViz插件缺失改用命令行echo驗(yàn)證數(shù)據(jù)或使用Foxglove可視化5.2 幾個(gè)容易被忽略的細(xì)節(jié)第一QoS要設(shè)置成和上游對(duì)齊。Gazebo雷達(dá)話題往往是best_effort而轉(zhuǎn)換節(jié)點(diǎn)默認(rèn)的reliable可能會(huì)接收不到數(shù)據(jù)或者延遲變大。訂閱時(shí)最好顯式匹配上游QoS或者直接在launch里配置qos_profile BEST_EFFORT。這一點(diǎn)在調(diào)通之前極容易被忽略。第二點(diǎn)云數(shù)據(jù)量大時(shí)轉(zhuǎn)換節(jié)點(diǎn)會(huì)成為瓶頸。PointCloud2里如果有幾十萬(wàn)點(diǎn)每個(gè)點(diǎn)都要算曲率、距離跳變、線束映射CPU開(kāi)銷不小。實(shí)測(cè)中我一般先把點(diǎn)云降采樣到下游算法可接受的最小密度再做轉(zhuǎn)換整體延遲能降低一半以上。第三一定要用bag錄制多次測(cè)試。Gazebo場(chǎng)景每次啟動(dòng)略有隨機(jī)抖動(dòng)也不完全相同。先錄一段包含不同場(chǎng)景、不同雷達(dá)的原始PointCloud2再在離線狀態(tài)下反復(fù)調(diào)轉(zhuǎn)換參數(shù)效率比每次改完重啟仿真高得多。等參數(shù)穩(wěn)定后再上實(shí)時(shí)鏈路問(wèn)題定位非?？臁?. 最后的幾點(diǎn)實(shí)操體會(huì)這套轉(zhuǎn)換節(jié)點(diǎn)做下來(lái)我最深的體會(huì)是“格式只是第一步信息語(yǔ)義才是核心”。很多人一開(kāi)始只看到PointCloud2和CustomMsg字段長(zhǎng)度不一樣以為補(bǔ)齊字段就行真正做下去才發(fā)現(xiàn)line和offset_time背后承載的掃描結(jié)構(gòu)信息才是算法能不能穩(wěn)定運(yùn)行的關(guān)鍵。所以我在寫(xiě)這個(gè)節(jié)點(diǎn)時(shí)寧可在特征分類、線束映射、時(shí)間同步上多花點(diǎn)時(shí)間也不愿意只做一層“透?jìng)魇健钡淖侄伟徇\(yùn)。根據(jù)個(gè)人經(jīng)驗(yàn)最穩(wěn)妥的實(shí)施順序是先用bag錄原始PointCloud2離線把轉(zhuǎn)換參數(shù)調(diào)到一個(gè)肉眼看著點(diǎn)云分布合理的狀態(tài)再接實(shí)時(shí)話題先在單獨(dú)節(jié)點(diǎn)里測(cè)試再掛載到完整仿真系統(tǒng)里先用單一雷達(dá)跑通再擴(kuò)展多雷達(dá)時(shí)間同步。每走一步都先確認(rèn)消息內(nèi)容和可視化效果再往下一個(gè)階段推進(jìn)。這個(gè)轉(zhuǎn)換方案后續(xù)還可以繼續(xù)擴(kuò)展比如在仿真點(diǎn)云里疊加更接近Livox實(shí)物的非重復(fù)掃描分布噪聲或者在轉(zhuǎn)換節(jié)點(diǎn)里接入運(yùn)動(dòng)補(bǔ)償邏輯讓仿真數(shù)據(jù)更接近傳感器底層輸出。但第一步先把鏈路打通根據(jù)這套邏輯把基礎(chǔ)節(jié)點(diǎn)跑穩(wěn)定你手里的Gazebo環(huán)境才算真正具備了“Livox數(shù)據(jù)測(cè)試能力”。