From 073b5b502a07db399979e198b17d6fc38e2b543d Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:06:40 -0500 Subject: [PATCH 01/12] Update localization_avia.launch --- launch/localization_avia.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/localization_avia.launch b/launch/localization_avia.launch index e8ac67e..8b79d1e 100644 --- a/launch/localization_avia.launch +++ b/launch/localization_avia.launch @@ -16,7 +16,7 @@ - + @@ -25,7 +25,7 @@ + args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" /> From 4378f09502cc41c82b920b0d65959a52c71a7ca8 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:07:16 -0500 Subject: [PATCH 02/12] Update localization_horizon.launch --- launch/localization_horizon.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/localization_horizon.launch b/launch/localization_horizon.launch index c03fc06..afc6d85 100644 --- a/launch/localization_horizon.launch +++ b/launch/localization_horizon.launch @@ -24,11 +24,11 @@ + args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" /> - \ No newline at end of file + From 21f74452076031586bd8679289bef6f300838c35 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:07:37 -0500 Subject: [PATCH 03/12] Update localization_ouster64.launch --- launch/localization_ouster64.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/localization_ouster64.launch b/launch/localization_ouster64.launch index 30527be..4c34776 100644 --- a/launch/localization_ouster64.launch +++ b/launch/localization_ouster64.launch @@ -24,7 +24,7 @@ + args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" /> From 65554b57ba98eeee4cc635eba84d723f5d5a9d15 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:08:02 -0500 Subject: [PATCH 04/12] Update localization_velodyne.launch --- launch/localization_velodyne.launch | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/launch/localization_velodyne.launch b/launch/localization_velodyne.launch index a36548d..52aa592 100644 --- a/launch/localization_velodyne.launch +++ b/launch/localization_velodyne.launch @@ -1,6 +1,7 @@ + @@ -15,7 +16,7 @@ - + @@ -24,11 +25,11 @@ + args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" /> - \ No newline at end of file + From 6353331b6db0bb38cb3924f328b26d020f900101 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:08:50 -0500 Subject: [PATCH 05/12] Update global_localization.py --- scripts/global_localization.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/scripts/global_localization.py b/scripts/global_localization.py index 79d9a4c..769548c 100755 --- a/scripts/global_localization.py +++ b/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() From 8667e2c2f8b4c0f8aff624b04b3a19c157c72e6f Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:09:12 -0500 Subject: [PATCH 06/12] Update publish_initial_pose.py --- scripts/publish_initial_pose.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/publish_initial_pose.py b/scripts/publish_initial_pose.py index 0745d69..ff3031e 100755 --- a/scripts/publish_initial_pose.py +++ b/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 From 23ad7d8a4f53853862ae2af005e371901a7c4d09 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:09:32 -0500 Subject: [PATCH 07/12] Update transform_fusion.py --- scripts/transform_fusion.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/scripts/transform_fusion.py b/scripts/transform_fusion.py index 46c8b9c..f04ca56 100755 --- a/scripts/transform_fusion.py +++ b/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() From cfb09b269461a04e326c80a124b28d71bd4fc7ff Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:16:02 -0500 Subject: [PATCH 08/12] Update README.md --- README.md | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 2790e09..26f8a87 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ 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) @@ -51,17 +51,18 @@ sudo apt install ros-$ROS_DISTRO-ros-numpy pip install open3d==0.9 ``` -Notice that, there may be issue when installing **Open3D** directly using pip in **Python2.7**: +~~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' +~~ERROR: Package 'pyrsistent' requires a different Python: 2.7.18 not in '>=3.5'~~ ``` -you may firstly install **pyrsistent**: +~~you may firstly install **pyrsistent**:~~ ```shell -pip install pyrsistent==0.15 +~~pip install pyrsistent==0.15~~ ``` -Then +~~Then~~ ```shell -pip install open3d==0.9 +~~pip install open3d==0.9~~ +Just pip install open3d==0.13 (or later version) ``` @@ -94,7 +95,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 +147,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... \ No newline at end of file +4. Updating... From 9ee3b88abb9c4c089ff626f69741f993f4400b0d Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:16:52 -0500 Subject: [PATCH 09/12] Update README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 26f8a87..016f0d4 100644 --- a/README.md +++ b/README.md @@ -48,7 +48,8 @@ sudo apt install ros-$ROS_DISTRO-ros-numpy - [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**:~~ From 7df5a5878bc1c19195af8517448d12beb8c50efc Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:18:59 -0500 Subject: [PATCH 10/12] Update README.md --- README.md | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 016f0d4..517b275 100644 --- a/README.md +++ b/README.md @@ -47,24 +47,17 @@ sudo apt install ros-$ROS_DISTRO-ros-numpy - [Open3D](http://www.open3d.org/docs/0.9.0/getting_started.html) -```shell ~~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~~ Just pip install open3d==0.13 (or later version) -``` ## 3. Build From 954fa30b51c7c718600e8f6815db2562dc7788b3 Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Mon, 19 Dec 2022 19:20:04 -0500 Subject: [PATCH 11/12] Update README.md --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 517b275..8646694 100644 --- a/README.md +++ b/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. From 7fae5d1f9c0d9f6a64c6ac936f2239b9ee9cb18d Mon Sep 17 00:00:00 2001 From: David Akhihiero Date: Sat, 20 May 2023 15:10:46 -0400 Subject: [PATCH 12/12] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 8646694..52277b2 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,7 @@ This part of dependency is consistent with FAST-LIO, please refer to the documen ```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)