Skip to content

Commit 7487d71

Browse files
alexlin2alexlin2
committed
added fastlio2 relocalization
1 parent 21395a0 commit 7487d71

16 files changed

Lines changed: 802 additions & 16 deletions

‎src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@
66
from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
77
from launch_ros.actions import Node
88
from launch.substitutions import LaunchConfiguration
9+
from launch.conditions import IfCondition, UnlessCondition
910

1011
def generate_launch_description():
12+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1113
world_name = LaunchConfiguration('world_name')
1214
sensorOffsetX = LaunchConfiguration('sensorOffsetX')
1315
sensorOffsetY = LaunchConfiguration('sensorOffsetY')
@@ -16,6 +18,11 @@ def generate_launch_description():
1618
vehicleY = LaunchConfiguration('vehicleY')
1719
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
1820

21+
declare_use_fastlio2 = DeclareLaunchArgument(
22+
'use_fastlio2',
23+
default_value='false',
24+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
25+
)
1926
declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
2027
declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
2128
declare_sensorOffsetY = DeclareLaunchArgument('sensorOffsetY', default_value='0.0', description='')
@@ -62,7 +69,15 @@ def generate_launch_description():
6269
start_arise_slam = IncludeLaunchDescription(
6370
PythonLaunchDescriptionSource(os.path.join(
6471
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
65-
)
72+
),
73+
condition=UnlessCondition(use_fastlio2)
74+
)
75+
76+
start_fastlio2 = IncludeLaunchDescription(
77+
PythonLaunchDescriptionSource(os.path.join(
78+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
79+
),
80+
condition=IfCondition(use_fastlio2)
6681
)
6782

6883
start_visualization_tools = IncludeLaunchDescription(
@@ -89,6 +104,7 @@ def generate_launch_description():
89104
ld = LaunchDescription()
90105

91106
# Add the actions
107+
ld.add_action(declare_use_fastlio2)
92108
ld.add_action(declare_world_name)
93109
ld.add_action(declare_sensorOffsetX)
94110
ld.add_action(declare_sensorOffsetY)
@@ -102,6 +118,7 @@ def generate_launch_description():
102118
ld.add_action(start_terrain_analysis_ext)
103119
ld.add_action(start_sensor_scan_generation)
104120
ld.add_action(start_arise_slam)
121+
ld.add_action(start_fastlio2)
105122
ld.add_action(start_visualization_tools)
106123
ld.add_action(start_joy)
107124

‎src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@
66
from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
77
from launch_ros.actions import Node
88
from launch.substitutions import LaunchConfiguration
9+
from launch.conditions import IfCondition, UnlessCondition
910

1011
def generate_launch_description():
12+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1113
exploration_planner_config = LaunchConfiguration('exploration_planner_config')
1214
world_name = LaunchConfiguration('world_name')
1315
sensorOffsetX = LaunchConfiguration('sensorOffsetX')
@@ -17,6 +19,11 @@ def generate_launch_description():
1719
vehicleY = LaunchConfiguration('vehicleY')
1820
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
1921

22+
declare_use_fastlio2 = DeclareLaunchArgument(
23+
'use_fastlio2',
24+
default_value='false',
25+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
26+
)
2027
declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='')
2128
declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
2229
declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
@@ -64,7 +71,15 @@ def generate_launch_description():
6471
start_arise_slam = IncludeLaunchDescription(
6572
PythonLaunchDescriptionSource(os.path.join(
6673
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
67-
)
74+
),
75+
condition=UnlessCondition(use_fastlio2)
76+
)
77+
78+
start_fastlio2 = IncludeLaunchDescription(
79+
PythonLaunchDescriptionSource(os.path.join(
80+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
81+
),
82+
condition=IfCondition(use_fastlio2)
6883
)
6984

7085
start_visualization_tools = IncludeLaunchDescription(
@@ -99,6 +114,7 @@ def generate_launch_description():
99114
ld = LaunchDescription()
100115

101116
# Add the actions
117+
ld.add_action(declare_use_fastlio2)
102118
ld.add_action(declare_exploration_planner_config)
103119
ld.add_action(declare_world_name)
104120
ld.add_action(declare_sensorOffsetX)
@@ -113,6 +129,7 @@ def generate_launch_description():
113129
ld.add_action(start_terrain_analysis_ext)
114130
ld.add_action(start_sensor_scan_generation)
115131
ld.add_action(start_arise_slam)
132+
ld.add_action(start_fastlio2)
116133
ld.add_action(start_visualization_tools)
117134
ld.add_action(start_joy)
118135
ld.add_action(start_tare_planner)

‎src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_route_planner.launch.py‎

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
from launch_ros.substitutions import FindPackageShare
1111

1212
def generate_launch_description():
13+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1314
use_pct_planner = LaunchConfiguration('use_pct_planner')
1415
route_planner_config = LaunchConfiguration('route_planner_config')
1516
world_name = LaunchConfiguration('world_name')
@@ -20,6 +21,11 @@ def generate_launch_description():
2021
vehicleY = LaunchConfiguration('vehicleY')
2122
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
2223

24+
declare_use_fastlio2 = DeclareLaunchArgument(
25+
'use_fastlio2',
26+
default_value='false',
27+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
28+
)
2329
declare_use_pct_planner = DeclareLaunchArgument(
2430
'use_pct_planner',
2531
default_value='true',
@@ -72,7 +78,15 @@ def generate_launch_description():
7278
start_arise_slam = IncludeLaunchDescription(
7379
PythonLaunchDescriptionSource(os.path.join(
7480
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
75-
)
81+
),
82+
condition=UnlessCondition(use_fastlio2)
83+
)
84+
85+
start_fastlio2 = IncludeLaunchDescription(
86+
PythonLaunchDescriptionSource(os.path.join(
87+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
88+
),
89+
condition=IfCondition(use_fastlio2)
7690
)
7791

7892
start_visualization_tools = IncludeLaunchDescription(
@@ -117,6 +131,7 @@ def generate_launch_description():
117131
ld = LaunchDescription()
118132

119133
# Add the actions
134+
ld.add_action(declare_use_fastlio2)
120135
ld.add_action(declare_use_pct_planner)
121136
ld.add_action(declare_route_planner_config)
122137
ld.add_action(declare_world_name)
@@ -132,6 +147,7 @@ def generate_launch_description():
132147
ld.add_action(start_terrain_analysis_ext)
133148
ld.add_action(start_sensor_scan_generation)
134149
ld.add_action(start_arise_slam)
150+
ld.add_action(start_fastlio2)
135151
ld.add_action(start_visualization_tools)
136152
ld.add_action(start_joy)
137153
ld.add_action(start_far_planner)

‎src/base_autonomy/vehicle_simulator/launch/system_real_robot.launch.py‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@
66
from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
77
from launch_ros.actions import Node
88
from launch.substitutions import LaunchConfiguration
9+
from launch.conditions import IfCondition, UnlessCondition
910

1011
def generate_launch_description():
12+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1113
world_name = LaunchConfiguration('world_name')
1214
sensorOffsetX = LaunchConfiguration('sensorOffsetX')
1315
sensorOffsetY = LaunchConfiguration('sensorOffsetY')
@@ -16,6 +18,11 @@ def generate_launch_description():
1618
vehicleY = LaunchConfiguration('vehicleY')
1719
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
1820

21+
declare_use_fastlio2 = DeclareLaunchArgument(
22+
'use_fastlio2',
23+
default_value='true',
24+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
25+
)
1926
declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
2027
declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
2128
declare_sensorOffsetY = DeclareLaunchArgument('sensorOffsetY', default_value='0.0', description='')
@@ -62,7 +69,15 @@ def generate_launch_description():
6269
start_arise_slam = IncludeLaunchDescription(
6370
PythonLaunchDescriptionSource(os.path.join(
6471
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
65-
)
72+
),
73+
condition=UnlessCondition(use_fastlio2)
74+
)
75+
76+
start_fastlio2 = IncludeLaunchDescription(
77+
PythonLaunchDescriptionSource(os.path.join(
78+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
79+
),
80+
condition=IfCondition(use_fastlio2)
6681
)
6782

6883
start_visualization_tools = IncludeLaunchDescription(
@@ -94,6 +109,7 @@ def generate_launch_description():
94109
ld = LaunchDescription()
95110

96111
# Add the actions
112+
ld.add_action(declare_use_fastlio2)
97113
ld.add_action(declare_world_name)
98114
ld.add_action(declare_sensorOffsetX)
99115
ld.add_action(declare_sensorOffsetY)
@@ -107,6 +123,7 @@ def generate_launch_description():
107123
ld.add_action(start_terrain_analysis_ext)
108124
ld.add_action(start_sensor_scan_generation)
109125
ld.add_action(start_arise_slam)
126+
ld.add_action(start_fastlio2)
110127
ld.add_action(start_visualization_tools)
111128
ld.add_action(start_joy)
112129
ld.add_action(start_mid360)

‎src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_exploration_planner.launch.py‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,8 +6,10 @@
66
from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
77
from launch_ros.actions import Node
88
from launch.substitutions import LaunchConfiguration
9+
from launch.conditions import IfCondition, UnlessCondition
910

1011
def generate_launch_description():
12+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1113
exploration_planner_config = LaunchConfiguration('exploration_planner_config')
1214
world_name = LaunchConfiguration('world_name')
1315
sensorOffsetX = LaunchConfiguration('sensorOffsetX')
@@ -17,6 +19,11 @@ def generate_launch_description():
1719
vehicleY = LaunchConfiguration('vehicleY')
1820
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
1921

22+
declare_use_fastlio2 = DeclareLaunchArgument(
23+
'use_fastlio2',
24+
default_value='false',
25+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
26+
)
2027
declare_exploration_planner_config = DeclareLaunchArgument('exploration_planner_config', default_value='indoor_small', description='')
2128
declare_world_name = DeclareLaunchArgument('world_name', default_value='real_world', description='')
2229
declare_sensorOffsetX = DeclareLaunchArgument('sensorOffsetX', default_value='0.05', description='')
@@ -64,7 +71,15 @@ def generate_launch_description():
6471
start_arise_slam = IncludeLaunchDescription(
6572
PythonLaunchDescriptionSource(os.path.join(
6673
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
67-
)
74+
),
75+
condition=UnlessCondition(use_fastlio2)
76+
)
77+
78+
start_fastlio2 = IncludeLaunchDescription(
79+
PythonLaunchDescriptionSource(os.path.join(
80+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
81+
),
82+
condition=IfCondition(use_fastlio2)
6883
)
6984

7085
start_visualization_tools = IncludeLaunchDescription(
@@ -104,6 +119,7 @@ def generate_launch_description():
104119
ld = LaunchDescription()
105120

106121
# Add the actions
122+
ld.add_action(declare_use_fastlio2)
107123
ld.add_action(declare_exploration_planner_config)
108124
ld.add_action(declare_world_name)
109125
ld.add_action(declare_sensorOffsetX)
@@ -118,6 +134,7 @@ def generate_launch_description():
118134
ld.add_action(start_terrain_analysis_ext)
119135
ld.add_action(start_sensor_scan_generation)
120136
ld.add_action(start_arise_slam)
137+
ld.add_action(start_fastlio2)
121138
ld.add_action(start_visualization_tools)
122139
ld.add_action(start_joy)
123140
ld.add_action(start_tare_planner)

‎src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch.py‎

Lines changed: 17 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
from launch_ros.substitutions import FindPackageShare
1111

1212
def generate_launch_description():
13+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
1314
use_pct_planner = LaunchConfiguration('use_pct_planner')
1415
route_planner_config = LaunchConfiguration('route_planner_config')
1516
world_name = LaunchConfiguration('world_name')
@@ -20,6 +21,11 @@ def generate_launch_description():
2021
vehicleY = LaunchConfiguration('vehicleY')
2122
checkTerrainConn = LaunchConfiguration('checkTerrainConn')
2223

24+
declare_use_fastlio2 = DeclareLaunchArgument(
25+
'use_fastlio2',
26+
default_value='false',
27+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
28+
)
2329
declare_use_pct_planner = DeclareLaunchArgument(
2430
'use_pct_planner',
2531
default_value='false',
@@ -72,7 +78,15 @@ def generate_launch_description():
7278
start_arise_slam = IncludeLaunchDescription(
7379
PythonLaunchDescriptionSource(os.path.join(
7480
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
75-
)
81+
),
82+
condition=UnlessCondition(use_fastlio2)
83+
)
84+
85+
start_fastlio2 = IncludeLaunchDescription(
86+
PythonLaunchDescriptionSource(os.path.join(
87+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
88+
),
89+
condition=IfCondition(use_fastlio2)
7690
)
7791

7892
start_visualization_tools = IncludeLaunchDescription(
@@ -122,6 +136,7 @@ def generate_launch_description():
122136
ld = LaunchDescription()
123137

124138
# Add the actions
139+
ld.add_action(declare_use_fastlio2)
125140
ld.add_action(declare_use_pct_planner)
126141
ld.add_action(declare_route_planner_config)
127142
ld.add_action(declare_world_name)
@@ -137,6 +152,7 @@ def generate_launch_description():
137152
ld.add_action(start_terrain_analysis_ext)
138153
ld.add_action(start_sensor_scan_generation)
139154
ld.add_action(start_arise_slam)
155+
ld.add_action(start_fastlio2)
140156
ld.add_action(start_visualization_tools)
141157
ld.add_action(start_joy)
142158
ld.add_action(start_far_planner)

‎src/base_autonomy/vehicle_simulator/launch/system_real_slam_only.launch.py‎

Lines changed: 21 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,32 @@
22

33
from ament_index_python.packages import get_package_share_directory
44
from launch import LaunchDescription
5-
from launch.actions import IncludeLaunchDescription
5+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
6+
from launch.conditions import IfCondition, UnlessCondition
67
from launch.launch_description_sources import PythonLaunchDescriptionSource
8+
from launch.substitutions import LaunchConfiguration
79

810
def generate_launch_description():
11+
use_fastlio2 = LaunchConfiguration('use_fastlio2')
12+
13+
declare_use_fastlio2 = DeclareLaunchArgument(
14+
'use_fastlio2',
15+
default_value='false',
16+
description='Use FASTLIO2 (with PGO/localizer bridge) instead of arise_slam_mid360 (true/false)'
17+
)
918

1019
start_arise_slam = IncludeLaunchDescription(
1120
PythonLaunchDescriptionSource(os.path.join(
1221
get_package_share_directory('arise_slam_mid360'), 'launch', 'arize_slam.launch.py')
13-
)
22+
),
23+
condition=UnlessCondition(use_fastlio2)
24+
)
25+
26+
start_fastlio2 = IncludeLaunchDescription(
27+
PythonLaunchDescriptionSource(os.path.join(
28+
get_package_share_directory('fastlio2_autonomy_bridge'), 'launch', 'fastlio2_autonomy.launch.py')
29+
),
30+
condition=IfCondition(use_fastlio2)
1431
)
1532

1633
start_mid360 = IncludeLaunchDescription(
@@ -21,7 +38,9 @@ def generate_launch_description():
2138
ld = LaunchDescription()
2239

2340
# Add the actions
41+
ld.add_action(declare_use_fastlio2)
2442
ld.add_action(start_arise_slam)
43+
ld.add_action(start_fastlio2)
2544
ld.add_action(start_mid360)
2645

2746
return ld

‎src/slam/FASTLIO2_ROS2/fastlio2/config/lio.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
imu_topic: /livox/imu
22
lidar_topic: /livox/lidar
3-
body_frame: body
4-
world_frame: lidar
3+
body_frame: sensor
4+
world_frame: map
55
print_time_cost: false
6-
print_odom: true
6+
print_odom: false
77
odom_log_interval: 0.5 # seconds between odom logs (2Hz)
88

99
lidar_filter_num: 6

0 commit comments

Comments
 (0)