要做回航充電躏啰,又不想加其他的傳感器,只能靠lidar來識別充電樁的形狀定位耙册。
顯然给僵,充電樁表面是平整的直線,那么就要從那么多點中提取直線详拙,然后再識別哪一條直線是充電樁帝际。
提取直線就成為了最初的一步蔓同。
在github上找了半天找到一個像樣的
https://github.com/kam3k/laser_line_extraction
試了一把,提取效果還不錯胡本。
記錄一下并把配置說明一下:
topics
訂閱的topic顯然是 scan牌柄,發(fā)布了兩個topic,我們要用的是/line_segments侧甫,/line_markers是用來在rviz上顯示的珊佣,可以在launch中關(guān)閉。
配置
配置列表如下披粟,基本上都好理解咒锻,被我分成了三大類,便于理解守屉。
frame_id (default: "laser") # The frame in which the line segments are published.
scan_topic (default: "scan") # The LaserScan topic.
publish_markers (default: false) # Whether or not markers are published.
-----------------------------------------------------------------------------------------------------
bearing_std_dev (default: 0.001) # The standard deviation of bearing uncertainty in the laser scans (rad).
least_sq_angle_thresh (default: 0.0001) # Change in angle (rad) threshold to stop iterating least squares (least_sq_radius_thresh must also be met).
least_sq_radius_thresh (default: 0.0001) # Change in radius (m) threshold to stop iterating least squares (least_sq_angle_thresh must also be met).
range_std_dev (default: 0.02) # The standard deviation of range uncertainty in the laser scans (m).
---------------------------------------------------------------------------------------------------------
max_line_gap (default: 0.4) # The maximum distance between two points in the same line (m).
min_line_length (default: 0.5) # Lines shorter than this are not published (m).
min_line_points (default: 9) # Lines with fewer points than this are not published.
min_range (default: 0.4) # Points closer than this are ignored (m).
min_split_dist (default: 0.05) # When performing "split" step of split and merge, a split between two points results when the two points are at least this far apart (m).
# 線段split的閾值惑艇,過大時很多線段被合并成一條,過小時拇泛,出現(xiàn)很多碎短的線段
outlier_dist (default: 0.05) # Points who are at least this distance from all their neighbours are considered outliers (m).
消息
最重要的是我們拿到 /line_segments的消息之后如何分析
消息類型是LineSegmentsList滨巴,如下,這不是主要的俺叭。
每個segment的類型LineSegment才是最主要的:
這其中的start, end都好理解:
start 和end分別是這個線段的起點坐標(biāo)和終點坐標(biāo)
radius和angle就不好理解了恭取,于是我們echo了它的消息,如下
radius: 0.752869129181
angle: -0.797926008701
covariance: [1.8016297644862789e-06, 1.2701141258730786e-06, 1.2701141258730786e-06, 9.006992058857577e-07]
start: [-0.6120783090591431, -1.648557186126709]
end: [-0.35934978723526, -1.402082920074463]
-
radius: 1.1387783289
angle: -0.808640360832
covariance: [2.9355262540775584e-06, 2.777204372250708e-06, 2.777204372250708e-06, 2.649457655934384e-06]
start: [-0.08191638439893723, -1.6525081396102905]
end: [0.14225095510482788, -1.4385261535644531]
-
radius: 1.12730681896
angle: -0.787253201008
covariance: [1.4751535672985483e-06, -2.180588808187167e-06, -2.180588808187167e-06, 3.510540864226641e-06]
start: [1.011875033378601, -0.5831754803657532]
end: [1.459415078163147, -0.1372928023338318]
-
radius: 3.53375911713
angle: 0.791161239147
covariance: [3.9494702832598705e-06, -7.934396307973657e-06, -7.934396307973657e-06, 2.223481897090096e-05]
start: [2.499594211578369, 2.497988224029541]
end: [1.9661375284194946, 3.025331497192383]
我的地圖總共擬合出4條直線熄守,相應(yīng)的結(jié)果如下蜈垮,中間的frame是laser的frame,紅色為x軸+裕照,
按照ros中的坐標(biāo)系來說攒发,機器人正前方為0度,逆時針為正晋南,激光數(shù)據(jù)的排列是從-135~135度(我的激光掃描范圍是270度)惠猿,那么上面4個線段從左往右依次是1,2,3,4。
這樣的話我們就能理解 radius和angle的意思了负间。官方給的解釋是極坐標(biāo)下參數(shù)偶妖,我們能夠理解到的是
angle: 原點到直線的垂線的角度,角度以機器人為坐標(biāo)系計算唉擂,范圍是-PI~PI
radius: 原點到直線的距離
根據(jù)這兩個參數(shù),完全不能確定什么檀葛,這兩個參數(shù)完全能夠根據(jù)start和end點計算出來玩祟。不理解這兩個參數(shù)存在的意義。
- covariance是2X2的矩陣屿聋,是radius和angle的協(xié)方差