Lenotary 66a9194b59 first commit | před 2 roky | |
---|---|---|
.vscode | před 2 roky | |
configs | před 2 roky | |
include | před 2 roky | |
launch | před 2 roky | |
map | před 2 roky | |
msgs | před 2 roky | |
src | před 2 roky | |
urdf | před 2 roky | |
CMakeLists.txt | před 2 roky | |
README.md | před 2 roky | |
package.xml | před 2 roky |
This repo modified from Autoware lidar_localizer module. Unlike the module in Autoware with haveily dependency on a lot of packages(you need compile all the packages in Autoware project), this repo is clean, simple and with no dependencies. All you need is ROS, and a pcd file(the point cloud map).
Let's start our lidar-based localization learning with this simple repo!
A demo video on MulRan dataset:
clone this repo in your ros workspace/src/
, and then catkin_make
(or catkin build
):
cd catkin_ws/src/
git clone https://github.com/AbangLZU/ndt_localizer.git
cd ..
catkin_make
You need a point cloud map (pcd format) for localization. You can get a HD point cloud map from any HD map supplier, or you can make one yourself (of course, the accuracy will not be as high as the HD map).
make a point cloud by yourself, you can ref my blog: https://blog.csdn.net/AdamShan/article/details/106589633 (Chinese) and https://blog.csdn.net/AdamShan/article/details/106319382 (Chinese).
The code to produce a pcd map: https://github.com/AbangLZU/SC-LeGO-LOAM
Move your map pcd file (.pcd) to the map folder inside this project (ndt_localizer/map
), change the pcd_path in map_loader.launch
to you pcd path, for example:
<arg name="pcd_path" default="$(find ndt_localizer)/map/kaist02.pcd"/>
Config your Lidar point cloud topic in launch/points_downsample.launch
:
<arg name="points_topic" default="/os1_points" />
If your Lidar data is sparse (like VLP-16), you need to config smaller leaf_size
in launch/points_downsample.launch
like 2.0
. If your lidar point cloud is dense (VLP-32, Hesai Pander40P, HDL-64 ect.), keep leaf_size
as 3.0
。
There are two static transform in this project: base_link_to_localizer
and world_to_map
,replace the ouster
with your lidar frame id if you are using a different lidar:
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_localizer" args="0 0 0 0 0 0 base_link ouster"/>
You can config NDT params in ndt_localizer.launch
. Tha main params of NDT algorithm is:
<arg name="trans_epsilon" default="0.05" doc="The maximum difference between two consecutive transformations in order to consider convergence" />
<arg name="step_size" default="0.1" doc="The newton line search maximum step length" />
<arg name="resolution" default="2.0" doc="The ND voxel grid resolution" />
<arg name="max_iterations" default="30.0" doc="The number of iterations required to calculate alignment" />
<arg name="converged_param_transform_probability" default="3.0" doc="" />
These default params work nice with 64 and 32 lidar.
Once you get your pcd map and configuration ready, run the localizer with:
cd catkin_ws
source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch
wait a few seconds for loading map, then you can see your pcd map in rviz like this:
give a init pose of current vehicle with 2D Pose Estimate in the rviz:
This operation will send a init pose to topic /initialpose
.Then you will see the localization result:
The final localization msg will send to /ndt_pose
topic:
---
header:
seq: 1867
stamp:
secs: 1566536121
nsecs: 251423898
frame_id: "map"
pose:
position:
x: -94.8022766113
y: 544.097351074
z: 42.5747337341
orientation:
x: 0.0243843578881
y: 0.0533175268768
z: -0.702325920272
w: 0.709437048124
---
The localizer also publish a tf of base_link
to map
:
---
transforms:
-
header:
seq: 0
stamp:
secs: 1566536121
nsecs: 251423898
frame_id: "map"
child_frame_id: "base_link"
transform:
translation:
x: -94.8022766113
y: 544.097351074
z: 42.5747337341
rotation:
x: 0.0243843578881
y: 0.0533175268768
z: -0.702325920272
w: 0.709437048124
You can follow my blog series in CSDN (Chinese): https://blog.csdn.net/adamshan