Upgraded to ROS Noetic and Python 3

README.md

@@ -1,12 +1,12 @@
 # FAST-LIO-LOCALIZATION
 
-A simple localization framework that can re-localize in built maps based on [FAST-LIO](https://github.com/hku-mars/FAST_LIO). 
+A simple localization framework that can re-localize in built maps based on [FAST-LIO] and works with ROS Noetic(https://github.com/hku-mars/FAST_LIO). 
 
 ## News
 
-- **2021-08-11:** Add **Open3D 0.7** support.
+~~- **2021-08-11:** Add **Open3D 0.7** support.
 
-- **2021-08-09:** Migrate to **Open3D** for better performance.
+~~- **2021-08-09:** Migrate to **Open3D** for better performance.
 
 ## 1. Features
 - Realtime 3D global localization in a pre-built point cloud map. 
@@ -37,32 +37,28 @@ This part of dependency is consistent with FAST-LIO, please refer to the documen
 
 ### 2.2 Dependencies for localization module
 
-- python 2.7
+- ~~python 2.7~~ python 3.8
 
 - [ros_numpy](https://github.com/eric-wieser/ros_numpy)
 
 ```shell
 sudo apt install ros-$ROS_DISTRO-ros-numpy
+pip install numpy==1.21
 ```
 
 - [Open3D](http://www.open3d.org/docs/0.9.0/getting_started.html)
 
-```shell
-pip install open3d==0.9
-```
+~~pip install open3d==0.9~~
+pip install open3d==0.13
 
-Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**: 
-```shell
-ERROR: Package 'pyrsistent' requires a different Python: 2.7.18 not in '>=3.5'
-```
-you may firstly install **pyrsistent**:
-```shell
-pip install pyrsistent==0.15
-```
-Then
-```shell
-pip install open3d==0.9
-```
+~~Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**:~~
+
+~~you may firstly install **pyrsistent**:~~
+
+~~Then~~
+
+~~pip install open3d==0.9~~
+Just pip install open3d==0.13 (or later version)
 
 
 ## 3. Build
@@ -94,7 +90,7 @@ The map can be built using LIO-SAM or FAST-LIO-SLAM.
 
 ### 4.2 Run
 
-1. First, please make sure you're using the **Python 2.7** environment;
+1. First, please make sure you're using the **Python ~~2.7~~ 3.8** environment;
 
 
 2. Run localization, here we take Livox AVIA as an example:
@@ -146,4 +142,4 @@ Thanks for the authors of [FAST-LIO](https://github.com/hku-mars/FAST_LIO) and [
 1. Go over the timestamp issue of the published odometry and tf;
 2. Using integrated points for global localization;
 3. Fuse global localization with the state estimation of FAST-LIO, and smooth the localization trajectory; 
-4. Updating...
+4. Updating...

launch/localization_avia.launch

@@ -16,7 +16,7 @@
     <node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
 
 
-	<arg name="map" default="" />
+	<arg name="map" default="$(find fast_lio_localization)/PCD/map.pcd" />
 	<!-- loalization-->
     <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
 
@@ -25,7 +25,7 @@
 
 	<!-- glbal map-->
     <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
-		  args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
+		  args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />
 
 
 	<group if="$(arg rviz)">

launch/localization_horizon.launch

@@ -24,11 +24,11 @@
 
 	<!-- glbal map-->
 	<node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
-		  args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
+		  args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />
 
 
 	<group if="$(arg rviz)">
 		<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
 	</group>
 
-</launch>
+</launch>

launch/localization_ouster64.launch

@@ -24,7 +24,7 @@
 
     <!-- glbal map-->
     <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
-          args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
+          args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />
 
 
     <group if="$(arg rviz)">

launch/localization_velodyne.launch

@@ -1,6 +1,7 @@
 <launch>
   <!-- Launch file for velodyne16 VLP-16 LiDAR -->
 
+    <include file="$(find velodyne_pointcloud)/launch/VLS128_points.launch"/>
     <arg name="rviz" default="true" />
 
     <rosparam command="load" file="$(find fast_lio_localization)/config/velodyne.yaml" />
@@ -15,7 +16,7 @@
     <param name="pcd_save_enable" type="bool" value="0" />
     <node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
 
-    <arg name="map" default="" />
+    <arg name="map" default="$(find fast_lio_localization)/PCD/scans.pcd" />
     <!-- loalization-->
     <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
 
@@ -24,11 +25,11 @@
 
     <!-- glbal map-->
     <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
-          args="$(arg map) 5 _frame_id:=/map cloud_pcd:=/map" />
+          args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />
 
 
     <group if="$(arg rviz)">
       <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
     </group>
 
-</launch>
+</launch>

scripts/global_localization.py

@@ -1,9 +1,9 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # coding=utf8
 from __future__ import print_function, division, absolute_import
 
 import copy
-import thread
+import _thread
 import time
 
 import open3d as o3d
@@ -40,11 +40,11 @@ def msg_to_array(pc_msg):
 
 
 def registration_at_scale(pc_scan, pc_map, initial, scale):
-    result_icp = o3d.registration.registration_icp(
+    result_icp = o3d.pipelines.registration.registration_icp(
         voxel_down_sample(pc_scan, SCAN_VOXEL_SIZE * scale), voxel_down_sample(pc_map, MAP_VOXEL_SIZE * scale),
         1.0 * scale, initial,
-        o3d.registration.TransformationEstimationPointToPoint(),
-        o3d.registration.ICPConvergenceCriteria(max_iteration=20)
+        o3d.pipelines.registration.TransformationEstimationPointToPoint(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20)
     )
 
     return result_icp.transformation, result_icp.fitness
@@ -216,13 +216,13 @@ def thread_localization():
 
     # The threshold of global localization,
     # only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
-    LOCALIZATION_TH = 0.95
+    LOCALIZATION_TH = 0.8
 
     # FOV(rad), modify this according to your LiDAR type
-    FOV = 1.6
+    FOV = 6.28319
 
     # The farthest distance(meters) within FOV
-    FOV_FAR = 150
+    FOV_FAR = 300
 
     rospy.init_node('fast_lio_localization')
     rospy.loginfo('Localization Node Inited...')
@@ -237,7 +237,7 @@ def thread_localization():
 
     # 初始化全局地图
     rospy.logwarn('Waiting for global map......')
-    initialize_global_map(rospy.wait_for_message('/map', PointCloud2))
+    initialize_global_map(rospy.wait_for_message('map', PointCloud2))
 
     # 初始化
     while not initialized:
@@ -255,6 +255,6 @@ def thread_localization():
     rospy.loginfo('Initialize successfully!!!!!!')
     rospy.loginfo('')
     # 开始定期全局定位
-    thread.start_new_thread(thread_localization, ())
+    _thread.start_new_thread(thread_localization, ())
 
     rospy.spin()

scripts/publish_initial_pose.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # coding=utf8
 from __future__ import print_function, division, absolute_import
 

scripts/transform_fusion.py

@@ -1,9 +1,9 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python3
 # coding=utf8
 from __future__ import print_function, division, absolute_import
 
 import copy
-import thread
+import _thread
 import time
 
 import numpy as np
@@ -83,6 +83,6 @@ def cb_save_map_to_odom(odom_msg):
     pub_localization = rospy.Publisher('/localization', Odometry, queue_size=1)
 
     # 发布定位消息
-    thread.start_new_thread(transform_fusion, ())
+    _thread.start_new_thread(transform_fusion, ())
 
     rospy.spin()