轉載自https://www.jianshu.com/p/38221263c4ee

一、概述

該包是從autoware中提取出來，能夠實現基於激光雷達點雲定位功能的最小包。

注意：使用該包完成定位功能前，需要先通過激光SLAM算法獲取環境pcd點雲地圖。

該包的定位算法種類：

1.純激光雷達點雲定位
2.融合GNSS的點雲定位
3.融合IMU的點雲定位
4.融合GNSS、IMU的點雲定位

二、功能包文件組織結構

該定位功能必須包含包：

autoware_build_flags
messages

gnss_localizer的CMake依賴:

gnss

lidar_localizer的CMake依賴:

pcl_omp_registration
autoware_health_checker -> ros_observer
points_downsampler -> velodyne_pointcloud(apt安裝)
ndt_cpu -> ndt_tku
jsk_rviz_plugins(apt安裝)

map_file的CMake依賴:

vector_map

編譯使用catkin build，不能用catkin_make，會因編譯包的先後順序導致報錯。

功能包文件組織結構

三、安裝

3.1 安裝依賴

sudo apt-get install python-catkin-tools ros-melodic-jsk-rviz-plugins ros-melodic-velodyne-pointcloud ros-melodic-nmea-msgs

3.2 編譯

mkdir ~/catkin_localization/src
cd ~/catkin_localization/src
git clone xx
cd ..
catkin build

四、使用

4.1 快速開始

4.1.1 加載點雲地圖

roslaunch autoware_quickstart_examples small_robot_map.launch

4.1.2 開啓NDT定位

roslaunch autoware_quickstart_examples small_robot_localization.launch

4.1.3 打開rviz，手動給初始位姿

注意初始位姿在(0, 0, 0)，如果給到了其它位姿，則會出現定位錯誤。

rviz -d   default .rviz

4.1.4 播放數據集
因爲是播放數據集，需要設置仿真時間

rosparam set use_sim_time   true

播放數據集，默認暫停，按空格開始播放，

rosbag play --pause -k grass.bag /velodyne_points:=/points_raw

4.1.5 帶GPS定位的節點關係圖

如果有gps話題，則不需要在rviz中手動給初始位姿，記得在ndt的launch文件中啓動gps定位選項。

其中fix爲fix2tfpose節點訂閱的話題名稱，消息類型爲sensor_msgs/NavSatFix

/points_raw爲voxel_grid_filter節點訂閱的話題名稱，消息類型爲sensor_msgs/PointCloud2

4.2 實車測試

4.2.1 通過SLAM算法建立環境點雲地圖

若定位要融合GPS，建圖時需要知道建圖原點處的GPS信息。

4.2.2 實時點雲的frame_id與話題名稱匹配

激光雷達點雲的tf座標系綁定在了/velodyne上，話題名稱爲/points_raw。

4.2.3 加載點雲地圖

roslaunch autoware_quickstart_examples small_robot_map.launch

注意不同的建圖方法會導致pcd.width參數計算的不同，詳情見5.1節中有序點雲方式加載和無序點雲方式加載的區別。

4.2.4 開啓NDT定位

roslaunch autoware_quickstart_examples small_robot_localization.launch

4.2.5 打開rviz，手動給初始位姿
如果沒有GPS，則該步驟是必須的！

4.2.6 實車節點關係圖

從左往右看。

其中/rslidar_sdk_node是速騰16線程激光雷達的官方驅動節點，/rslidar_points是該驅動發佈的話題，/points_raw是將速騰激光雷達點雲的話題轉換爲velodyne激光雷達點雲的話題，兩者數據格式略有區別。未嘗試不轉換，可能不轉換也可以。

/points_raw是經過voxel_grid_filter下采樣後，發佈/filtered_points，/ndt_maching節點訂閱該話題，發佈/ndt_pose，/ndt_pose是pub_odom節點訂閱的話題，該話題給move_base提供里程計信息。

五、解析

5.1 map_file功能包

該功能包可加載3D地圖點雲和矢量地圖，此處僅用點雲地圖。

source devel/setup.bash
roslaunch autoware_quickstart_examples small_robot_map.launch

robot_map.launch代碼如下，通過指定點雲地圖文件路徑加載點雲地圖，發佈點雲地圖話題 /points_map。

<launch>
  <rosparam command="load" file="$(find autoware_quickstart_examples)/config/headless_setup.yaml" />
  <!-- TF -->
  <include file="$(find autoware_quickstart_examples)/launch/small_robot_demo/small_robot_tf.launch"/>
  
  <!-- Point Cloud -->
  <node pkg="map_file" type="points_map_loader" name="points_map_loader" args="noupdate $(env HOME)/autoware_sync_dataset/c_test_place.pcd"/>
</launch>

其中small_robot_tf.launch文件發佈靜態tf變換，headless_setup.yaml是NDT定位中的tf參數配置

small_robot_tf.launch代碼如下，設置靜態發佈的tf樹
主要指定world->map; base_link->velodyne; base_link->mobility;這三個靜態tf變換，具體參數可以根據自己的機器人進行調整。默認world與map重合，map與mobility重合。

<launch>
<node pkg="tf2_ros"  type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map" />

<node pkg="tf2_ros"  type="static_transform_publisher" name="map_to_mobility" args="0 0 0 0 0 0 /map /mobility" />

<node pkg="tf2_ros"  type="static_transform_publisher" name="base_link_to_velodyne" args="0.12 0 -0.3 0 0 0 /base_link /velodyne" />
</launch>

headless_setup.yaml代碼如下，指定NDT matching節點中激光雷達與車體座標系之間的tf變換關係。

#T_baselink_velodyne
tf_x: 0.12 #0.12
tf_y: 0
tf_z: -0.3 #0.35
tf_yaw: 0
tf_pitch: 0
tf_roll: 0

localizer: velodyne
use_sim_time:   false

注意!!!

如果rviz中無法顯示/points_map地圖點雲，可能是通過slam算法建立pcd點雲文件width參數不匹配。

1。以有序點雲方式加載

修改map_file/nodes/points_map_loader/points_map_loader.cpp中342行
pcd.width = int(pcd.data.size()/32);
中的32改爲16。

2。以無序點雲方式加載

用cloudcompare等3D點雲編輯工具打開pcd點雲文件，查看軟件終端顯示的點雲數量，將該值直接賦值，此時需要指定pcd.height=1，例如

pcd.width = 23738455;
pcd.height = 1;

可以修改源碼，並且在啓動節點時候添加output="screen"參數，將pcd.width參數打印出來。

output=  "screen"

5.2 gnss_localizer與gnss功能包

gnss_localizer包主要實現將兩種GPS消息類型（nmea_msgs/Sentence與sensor_msgs/NavSatFix）轉換爲UTM座標，其中GPS座標(WGS84)轉UTM座標算法具體實現是在gnss功能包中。

將nmea_msgs/Sentence轉換爲UTM座標

roslaunch gnss_localizer nmea2tfpose.launch

將sensor_msgs/NavSatFix轉換爲UTM座標

roslaunch gnss_localizer fix2tfpose.launch

其中nmea2tfpose.launch代碼如下，基於第7個點的GPS座標作爲原點進行UTM轉換計算。

<!-- -->
<launch>

  <arg name="plane" default="7"/>

  <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
    <param name="plane" value="$(arg plane)"/>
  </node>

</launch>

若原點離實際位置太遠，轉換爲UTM座標誤差較大。

原點參數:0-19，每個參數都對應一個GPS座標點，在gnss/src/geo_pos_conv.cpp中設置，在此文件中把7設置爲了北京的GPS座標。

void geo_pos_conv::set_plane( int num)
{
  int lon_deg, lon_min, lat_deg, lat_min;   // longitude and latitude of origin of each plane in Japan
  if (num == 0)
  {
    lon_deg = 0;
    lon_min = 0;
    lat_deg = 0;
    lat_min = 0;
  }
  else if (num == 1)
  {
    lon_deg = 33;
    lon_min = 0;
    lat_deg = 129;
    lat_min = 30;
  }

  ...

  else if (num == 7)  // beijing
  {
    lon_deg = 40;  //36
    lon_min = 0;
    lat_deg = 116;  //137
    lat_min = 10;
  }
  ...
}

若要重映射話題名稱，可以在launch文件中設置remap參數

<!-- -->
<launch>

  <arg name="plane" default="7"/>

  <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
    <param name="plane" value="$(arg plane)"/>
    <remap from="nmea_sentence" to="your_nmea_sentence_topic_name"/>
      <remap from="gnss_pose" to="your_gnss_pose_topic_name"/>
  </node>

</launch>

其中

nmea_sentence爲nmea2tfpose節點訂閱的話題名稱，消息類型爲nmea_msgs/Sentence
gnss_pose爲nmea2tfpose節點發布的話題名稱，消息類型爲geometry_msgs/PoseStamped。

fix2tfpose.launch代碼如下，基於第7個點的GPS座標作爲原點進行UTM轉換計算，也是調用gnss包進行UTM座標計算。

void geo_pos_conv::set_plane(int num)
{
  int lon_deg, lon_min, lat_deg, lat_min;  // longitude and latitude of origin of each plane in Japan
  if (num == 0)
  {
    lon_deg = 0;
    lon_min = 0;
    lat_deg = 0;
    lat_min = 0;
  }
  else if (num == 1)
  {
    lon_deg = 33;
    lon_min = 0;
    lat_deg = 129;
    lat_min = 30;
  }

  ...

  else if (num == 7) // beijing
  {
    lon_deg = 40; //36
    lon_min = 0;
    lat_deg = 116; //137
    lat_min = 10;
  }
  ...
}

若要重映射話題名稱，可以在launch文件中設置remap參數

<!-- -->
<launch>

  <arg name="plane" default="7"/>

  <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="log">
    <param name="plane" value="$(arg plane)"/>
    <remap from="nmea_sentence" to="your_nmea_sentence_topic_name"/>
      <remap from="gnss_pose" to="your_gnss_pose_topic_name"/>
  </node>

</launch>

其中

fix爲fix2tfpose節點訂閱的話題名稱，消息類型爲sensor_msgs/NavSatFix
gnss_pose爲fix2tfpose節點發布的話題名稱，消息類型爲geometry_msgs/PoseStamped。

5.3 lidar_localizer包

lidar_localizer包主要實現基於NDT的點雲定位算法，其中可以設置launch文件中的參數，選擇是否融合GNSS、IMU、輪速里程計等進行融合定位。

roslaunch autoware_quickstart_examples small_robot_localization.launch

其中small_robot_localization.launch代碼如下，主要是調用points_downsampler的launch文件和lidar_localizer的laucnh文件實現基於純點雲定位功能，可選融合GPS進行定位。

<launch>

  <!-- setting path parameter -->
  <arg name="get_height" value="true" />

  <!-- points downsampler -->
  <include file="$(find points_downsampler)/launch/points_downsample.launch" />

  <!-- fix2tfpose -->
  <!-- <include file="$(find gnss_localizer)/launch/fix2tfpose.launch"/> -->

  <!-- ndt_matching -->
  <include file="$(find lidar_localizer)/launch/ndt_matching.launch">
    <arg name="get_height" value="$(arg get_height)" />
  </include>

</launch>

其中get_height參數是用來指定ndt_matching是否獲取高度信息，若爲true，則獲取高度信息，否則不獲取高度信息。若假設機器人或無人車認爲是在平面上運動，可以設置該參數爲false，或者不設置該參數，在ndt_matching.launch文件中該參數默認爲false。

points_downsample.launch代碼如下，對實時激光雷達點雲數據進行下采樣。

<launch>
  <arg name="sync" default="false" />
  <arg name="node_name" default="voxel_grid_filter" />
  <arg name="points_topic" default="points_raw" />
  <arg name="output_log" default="false" />
  <arg name="measurement_range" default="200" />

  <node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)">
    <param name="points_topic" value="$(arg points_topic)" />
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
    <param name="output_log" value="$(arg output_log)" />
    <param name="measurement_range" value="$(arg measurement_range)" />
  </node>
</launch>

該節點訂閱話題名稱爲points_raw，消息類型爲sensor_msgs/PointCloud2。

若需要融合GPS數據，可以在small_robot_localization.launch文件中取消註釋fix2tfpose節點。

ndt_matching.launch代碼如下，其中對ndt所需要的參數進行了配置，主要的參數有：1 默認使用cpu；2 默認不使用gnss；3 默認不使用odom；4 默認不使用imu；5 默認不使用IMU方向正負變換等

<launch>

  <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
  <arg name="use_gnss" default="false" />
  <arg name="use_odom" default="false" />
  <arg name="use_imu" default="false" />
  <arg name="imu_upside_down" default="false" />
  <arg name="imu_topic" default="/imu_raw" />
  <arg name="queue_size" default="1" />
  <arg name="offset" default="linear" />
  <arg name="get_height" default="false" />
  <arg name="use_local_transform" default="false" />
  <arg name="sync" default="false" />
  <arg name="output_log_data" default="false" />
  <arg name="gnss_reinit_fitness" default="500.0" />

  <node pkg="lidar_localizer" type="ndt_matching" name="ndt_matching" output="log">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_gnss" value="$(arg use_gnss)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="queue_size" value="$(arg queue_size)" />
    <param name="offset" value="$(arg offset)" />
    <param name="get_height" value="$(arg get_height)" />
    <param name="use_local_transform" value="$(arg use_local_transform)" />
    <param name="output_log_data" value="$(arg output_log_data)" />
    <param name="gnss_reinit_fitness" value="$(arg gnss_reinit_fitness)" />
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
  </node>

</launch>

注意！！！

在基於ndt_matching節點進行定位時，需要給定/initialpose。若融合了GNSS，則由GNSS提供初始位姿，否則需要手動給/initialpose初始位姿，不然ndt_matching節點無法完成初始化。

手動給/initialpose初始位姿的方法：通過rviz的工具欄2D Pose Estimate給定初始位姿。注意該位姿是在world座標系下的，所以手動給定初始位姿時，最好打開官方的rviz配置文件，然後再給，
rviz -d default.rviz。

TODO:

1。確定好用哪個數據集。解決grss還有高度的問題。高度問題不包含z即可。

Autoware定位與建圖模塊(一)——定位 Label: Research

轉載自https://www.jianshu.com/p/38221263c4ee

一、概述

二、功能包文件組織結構

三、安裝

3.1 安裝依賴

3.2 編譯

四、使用

4.1 快速開始

4.2 實車測試

五、解析

5.1 map_file功能包

5.2 gnss_localizer與gnss功能包

5.3 lidar_localizer包

TODO:

【筆記】動手學深度學習-前言

公司新來一個幹練小夥，把 MyBatis 替換成 MyBatis-Plus，上線後哭暈在廁所。。。

支持非IE瀏覽器真的那麼難嗎？

爲啥就那麼痛恨IE？

Brian Sun：回覆“爲啥就那麼痛恨IE？”

體驗下，大廠在使用功能的API網關！

見鬼了！我家的 WiFi 只有下雨天才能正常使用...

短視頻文案提取原來如此簡單

oa系統集成及案例樣式

開發人員爲什麼要支持非IE瀏覽器的四個故事

常用時序收斂方法

FPGA原語

FPGA時序約束實戰

keep_hierarchy約束在三模冗餘中的應用

Libero Soc 如何導入模塊module Label: Research

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結