LOAM_velodyne學習(四)

TransformMaintenance

來到了最后一個模塊,代碼不是很長,我們在看完代碼之后,再詳細說明這個模塊的功能

依然主函數開始

int main(int argc, char** argv)
{ros::init(argc, argv, "transformMaintenance");ros::NodeHandle nh;//訂閱了兩個節點ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry> ("/aft_mapped_to_init", 5, odomAftMappedHandler);//發布一個節點ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);pubLaserOdometry2Pointer = &pubLaserOdometry2;laserOdometry2.header.frame_id = "/camera_init";laserOdometry2.child_frame_id = "/camera";tf::TransformBroadcaster tfBroadcaster2;tfBroadcaster2Pointer = &tfBroadcaster2;laserOdometryTrans2.frame_id_ = "/camera_init";laserOdometryTrans2.child_frame_id_ = "/camera";ros::spin();return 0;
}

主函數非常簡單,可以看出integrated_to_init消息是由發布器pubLaserOdometry2發布的,實際上就是由發布器pubLaserOdometry2Pointer發布的。我們找到pubLaserOdometry2Pointer,發現回調函數laserOdometryHandler就是pubLaserOdometry2Pointer的發布函數,這意味著,發現每次接收到laser_odom_to_init消息并調用回調函數laserOdometryHandler時,就發布一次integrated_to_init消息。

看來重點是這個回調函數laserOdometryHandler,我們來仔細看看

void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{double roll, pitch, yaw;//對收到的消息進行解析,得到transformSumgeometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;transformSum[3] = laserOdometry->pose.pose.position.x;transformSum[4] = laserOdometry->pose.pose.position.y;transformSum[5] = laserOdometry->pose.pose.position.z;//位姿更新transformAssociateToMap();//位姿信息進行存儲,準備發布geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped[2], -transformMapped[0], -transformMapped[1]);laserOdometry2.header.stamp = laserOdometry->header.stamp;laserOdometry2.pose.pose.orientation.x = -geoQuat.y;laserOdometry2.pose.pose.orientation.y = -geoQuat.z;laserOdometry2.pose.pose.orientation.z = geoQuat.x;laserOdometry2.pose.pose.orientation.w = geoQuat.w;laserOdometry2.pose.pose.position.x = transformMapped[3];laserOdometry2.pose.pose.position.y = transformMapped[4];laserOdometry2.pose.pose.position.z = transformMapped[5];pubLaserOdometry2Pointer->publish(laserOdometry2);laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2);
}

但是,這里還是有個小坑的。這個節點接收了兩個消息,分別是laserOdometry節點和laserMapping節點發布的,而這兩個節點發布的頻率不同,那么是怎么處理的呢?

我們仔細看一看剩下的一個回調函數

void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{double roll, pitch, yaw;geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformAftMapped[0] = -pitch;transformAftMapped[1] = -yaw;transformAftMapped[2] = roll;transformAftMapped[3] = odomAftMapped->pose.pose.position.x;transformAftMapped[4] = odomAftMapped->pose.pose.position.y;transformAftMapped[5] = odomAftMapped->pose.pose.position.z;transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}

也是很簡單的解析函數,作用是在接收到了laserMapping的消息后,更新位姿,這里注意,laserMapping發布的是優化過后的位姿!看到這里,就逐漸能明白作者如何完成兩個不同頻率之間的協調了。

當接收到laserMapping的消息后,立馬更新位姿,這樣得到了優化的結果;而這個優化結果會被回調函數laserOdometryHandler???????里的transformAssociateToMap這一個函數一直利用來建圖,一直到下一次接收到laserMapping???????的消息,再一次更新位姿,我們畫圖來說明:

也就是說,最后采用的位姿是TransformMaintenance發布的integrated_to_init消息。而且由上面的分析可知,TransformMaintenance的發布頻率和laserOdometry的發布頻率是一致的。

本文來自互聯網用戶投稿,該文觀點僅代表作者本人,不代表本站立場。本站僅提供信息存儲空間服務,不擁有所有權,不承擔相關法律責任。
如若轉載,請注明出處:http://www.pswp.cn/news/253936.shtml
繁體地址,請注明出處:http://hk.pswp.cn/news/253936.shtml
英文地址,請注明出處:http://en.pswp.cn/news/253936.shtml

如若內容造成侵權/違法違規/事實不符,請聯系多彩編程網進行投訴反饋email:809451989@qq.com,一經查實,立即刪除!

相關文章

PHP數據庫類

<?phpclass Db{//私有靜態屬性存儲實例化對象自身private static $instance;//存儲PDO類的實例化private $pdo;//PDOStatement類private $stmt;//禁止外部實例化對象&#xff0c;鏈接數據庫private function __construct($config,$port,$charset){try{$this->pdo new P…

oracle參數文件、控制文件、數據文件、日志文件的位置及查詢方法

參數文件&#xff1a;所有參數文件一般在 $ORACLE_HOME/dbs 下 sqlplus查詢語句&#xff1a;show parameter spfile; 網絡連接文件&#xff1a; $ORACLE_HOME/dbs/network/admin 目錄中 控制文件&#xff1a;select * from v$controlfile; 數據文件&#xff1a;一般在oracleda…

Bishops Alliance—— 最大上升子序列

原題鏈接&#xff1a;http://codeforces.com/gym/101147/problem/F 題意&#xff1a;n*n的棋盤&#xff0c;給m個主教的坐標及其私有距離p&#xff0c;以及常數C&#xff0c;求位于同一對角線上滿足條件&#xff1a;dist(i, j) > p[i]^2 p[j]^2 C 的主教集合的元素個數最…

LeGO-LOAM學習

前言 在學習了LOAM之后&#xff0c;了解到LeGO-LOAM&#xff08;面向復雜情況的輕量級優化地面的雷達里程計&#xff09;&#xff0c;進行了一個學習整理。 Github&#xff1a;https://github.com/RobustFieldAutonomyLab/LeGO-LOAM 論文&#xff1a;https://github.com/Robu…

char data[0]用法總結

struct MyData { int nLen; char data[0]; }; 開始沒有理解紅色部分的內容&#xff0c;上網搜索下&#xff0c;發現用處很大&#xff0c;記錄下來。 在結構中&#xff0c;data是一個數組名&#xff1b;但該數組沒有元素&#xff1b;該數組…

(一)低功耗設計目的與功耗的類型

一、低功耗設計的目的 1.便攜性設備等需求 電子產品在我們生活中扮演了極其重要的作用&#xff0c;便攜性的電子設備便是其中一種。便攜性設備需要電池供電、需要消耗電池的能量。在同等電能提供下&#xff0c;低功耗設計的產品就能夠工作更長的時間。時間的就是生命&#xff…

(轉)徹底學會使用epoll(一)——ET模式實現分析

注&#xff1a;之前寫過兩篇關于epoll實現的文章&#xff0c;但是感覺懂得了實現原理并不一定會使用&#xff0c;所以又決定寫這一系列文章&#xff0c;希望能夠對epoll有比較清楚的認識。是請大家轉載務必注明出處&#xff0c;算是對我勞動成果的一點點尊重吧。另外&#xff0…

MFC的消息映射有什么作用

絕對以下這三個解釋的比較簡潔&#xff0c;特此做個記錄&#xff01;以感謝回答的這些人&#xff01; MFC的消息映射有什么作用: Windows操作系統主要是有消息來處理的&#xff0c;每個程序都有自己的消息隊列&#xff0c;并且這些消息是有優先級的&#xff0c;也就是誰會先…

線性表的鏈式存儲結構

鏈式存儲結構的定義 1.概念定義&#xff1a; - n個結點離散分配 - 彼此通過指針相連 - 每個結點只有一個前驅結點和一個后繼結點 - 首結點沒有前驅結點&#xff0c;尾結點沒有后繼結點 2.專業術語 -首結點&#xff1a;第一個有有效數據的結點 -尾結點&#xff1a;最后一個有有效…

Apache 設置http跳轉至HTTPS訪問

為什么80%的碼農都做不了架構師&#xff1f;>>> <VirtualHost>...</VirtualHost> 中添加如下配置 <IfModule mod_rewrite.c>RewriteEngine onRewriteCond %{SERVER_PORT} 80RewriteRule ^(.*)$ https://域名/$1 [R301,L] </IfModule> 轉…

JAVA線程概念

一、程序與進程 1、程序&#xff1a;一段靜態的代碼。 2、進程&#xff1a;程序的一次動態執行過程&#xff0c;它對應從代碼加載、執行到執行完畢的一個完整過程。 3、進程也稱任務&#xff0c;支持多個進程同時執行的OS就被稱為多進程OS或多任務OS。 二、進程與線程 在一…

(二)功耗的分析

前面學習了進行低功耗的目的個功耗的構成&#xff0c;今天就來分享一下功耗的分析。由于是面向數字IC前端設計的學習&#xff0c;所以這里的功耗分析是基于DC中的power compiler工具&#xff1b;更精確的功耗分析可以采用PT&#xff0c;關于PT的功耗分析可以查閱其他資料&#…

Hibernate創建hqll時報錯

Hibernate 問題,在執行Query session.createQuery(hql) 報錯誤 出錯截圖&#xff1a; 這條語句在java運行環境下&#xff0c;直接連數據庫不出錯&#xff0c;如果在hiberante,struts環境下就出錯 出錯原因&#xff1a;jar包沖突&#xff0c;struts2和hibernate框架中都有antlr包…

.NET Core TDD 前傳: 編寫易于測試的代碼 -- 全局狀態

第1篇: 講述了如何創造"縫". "縫"(seam)是需要知道的概念. 第2篇, 避免在構建對象時寫出不易測試的代碼. 第3篇, 依賴項和迪米特法則. 本文是第4篇, 將介紹全局狀態引起的問題. 全局狀態 全局狀態, 也可以叫做應用程序狀態, 它是一組變量, 這些變量維護著…

(三)系統與架構級低功耗設計

前面講解了使用EDA工具&#xff08;主要是power compiler&#xff09;進行功耗分析的流程&#xff0c;這里我們將介紹在數字IC中進行低功耗設計的方法&#xff0c;同時也結合EDA工具&#xff08;主要是Design Compiler&#xff09;如何實現。我們的講解的低功耗設計主要是自頂向…

python---統計列表中數字出現的次數

1 import collections 2 3 a [1,2,3,1,2,3,4,1,2,5,4,6,7,7,8,9,6,2,23,4,2,1,5,6,7,8,2] 4 b collections.Counter(a) 5 for c in b&#xff1a; print c,b[c] 轉載于:https://www.cnblogs.com/lxs1314/p/7236321.html

MFC入門(一)——MFC是一個編程框架

MFC (Microsoft Foundation Class Library)中的各種類結合起來構成了一個應用程序框架&#xff0c;它的目的就是讓程序員在此基礎上來建立Windows下的應用程序&#xff0c;這是一種相對SDK來說更為簡單的方法。因為總體上&#xff0c;MFC框架定義了應用程序的輪廓&#xff0c;并…

2.數據結構筆記學習--線性表基本操作

線性表的結構定義&#xff1a; 順序表的結構定義&#xff1a; typedef struct {int data[maxSize]; //存放順序表元素的數組&#xff0c;一般用 int A[maxSize];int length; //存放順序表的長度,一般用 int n; }SeqList; 單鏈表結點定義&#xff1a; typedef struct L…

(四)RTL級低功耗設計

前面介紹了系統級的低功耗設計&#xff0c;換句話說就是在系統級降低功耗可以考慮的方面。系統級的低功耗設計&#xff0c;主要是由系統級設計、具有豐富經驗的人員實現&#xff0c;雖然還輪不到我們設計&#xff0c;我們了解一下還是比較好的。我們前端設計人員的重點不在系統…

Unity3D 游戲前端開發技能樹(思維導圖)

如果做游戲也是一種游戲,那么這個游戲的自由度實在是太高了.(導圖源文件鏈接&#xff1a;http://pan.baidu.com/s/1eSHpH5o 密碼&#xff1a;qzl5) 最近要用思維導圖軟件Xmind把自己的思路好好捋一捋,算是溫故知新吧. 轉載于:https://www.cnblogs.com/qiaogaojian/p/6098962.ht…