-
Notifications
You must be signed in to change notification settings - Fork 11
/
Copy pathgraco_lidar.launch.py
153 lines (133 loc) · 6.65 KB
/
graco_lidar.launch.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import TimerAction, OpaqueFunction, PushLaunchConfigurations, PopLaunchConfigurations, DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import launch_testing
import launch_testing.actions
from launch.substitutions import LaunchConfiguration
def launch_setup(context, *args, **kwargs):
config_path = os.path.join(
get_package_share_directory("cslam_experiments"), "config/")
config_file = LaunchConfiguration('config_file').perform(context)
# Params
max_nb_robots = int(LaunchConfiguration('max_nb_robots').perform(context))
dataset = "Graco_" + LaunchConfiguration('sequence').perform(context)
robot_delay_s = LaunchConfiguration('robot_delay_s').perform(context)
launch_delay_s = LaunchConfiguration('launch_delay_s').perform(context)
rate = float(LaunchConfiguration('rate').perform(context))
# Ajust value according to rate
launch_delay_s = float(launch_delay_s) / rate
cslam_processes = []
odom_processes = []
for i in range(max_nb_robots):
proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory("cslam_experiments"),
"launch", "cslam", "cslam_lidar.launch.py")),
launch_arguments={
"config_path": config_path,
"config_file": config_file,
"robot_id": str(i),
"namespace": "/r" + str(i),
"max_nb_robots": str(max_nb_robots),
"enable_simulated_rendezvous": LaunchConfiguration('enable_simulated_rendezvous'),
"rendezvous_schedule_file": os.path.join(get_package_share_directory("cslam_experiments"),
"config", "rendezvous", LaunchConfiguration('rendezvous_config').perform(context)),
}.items(),
)
cslam_processes.append(proc)
odom_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('cslam_experiments'), 'launch',
'odometry', 'rtabmap_s3e_lidar_odometry.launch.py')),
launch_arguments={
'log_level': "info",
"namespace": "/r" + str(i),
"robot_id": str(i),
}.items(),
)
odom_processes.append(odom_proc)
# Launch schedule
schedule = []
for i in range(max_nb_robots):
schedule.append(PushLaunchConfigurations())
schedule.append(
TimerAction(period=float(robot_delay_s) * i,
actions=[cslam_processes[i]]))
schedule.append(PopLaunchConfigurations())
schedule.append(PushLaunchConfigurations())
schedule.append(
TimerAction(period=float(robot_delay_s) * i,
actions=[odom_processes[i]]))
schedule.append(PopLaunchConfigurations())
bag_processes = []
for i in range(max_nb_robots):
bag_file = os.path.join(
get_package_share_directory("cslam_experiments"), "data",
dataset, "Graco-" + str(i))
bag_proc = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory("cslam_experiments"),
"launch",
"sensors",
"bag_graco.launch.py",
)),
launch_arguments={
"namespace": "/r" + str(i),
"bag_file": bag_file,
"rate": str(rate)
}.items(),
)
bag_processes.append(bag_proc)
for i in range(max_nb_robots):
schedule.append(PushLaunchConfigurations())
schedule.append(
TimerAction(period=float(robot_delay_s) * i + float(launch_delay_s),
actions=[bag_processes[i]]))
schedule.append(PopLaunchConfigurations())
tf_process = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments="0 0 0 0 0 0 velodyne base_link".split(" "),
parameters=[])
tf_process_imu = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments="-0.01192 -0.0197 0.1226 0 0 0 velodyne gnss".split(" "),
parameters=[])
tf_process_cam0 = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments="0 0 0 0 0 0 camera_21239776 base_link".split(" "),
parameters=[])
tf_process_cam1 = Node(package="tf2_ros",
executable="static_transform_publisher",
arguments="-0.23223 0 0 0 0 0 camera_21387977 camera_21239776".split(" "),
parameters=[])
schedule.append(PushLaunchConfigurations())
schedule.append(tf_process)
schedule.append(PopLaunchConfigurations())
schedule.append(PushLaunchConfigurations())
schedule.append(tf_process_imu)
schedule.append(PopLaunchConfigurations())
# schedule.append(PushLaunchConfigurations())
# schedule.append(tf_process_cam0)
# schedule.append(PopLaunchConfigurations())
# schedule.append(PushLaunchConfigurations())
# schedule.append(tf_process_cam1)
# schedule.append(PopLaunchConfigurations())
return schedule
def generate_launch_description():
return LaunchDescription([
DeclareLaunchArgument('max_nb_robots', default_value='3'),
DeclareLaunchArgument('sequence', default_value='Ground'),
DeclareLaunchArgument('robot_delay_s', default_value='400', description="Delay between launching each robot. Ajust depending on the computing power of your machine."),
DeclareLaunchArgument('launch_delay_s', default_value='10', description="Delay between launching the bag and the robot. In order to let the robot initialize properly and not loose the first bag data frames."),
DeclareLaunchArgument('config_file',
default_value='graco_lidar.yaml',
description=''),
DeclareLaunchArgument('rate', default_value='1.0'),
DeclareLaunchArgument('enable_simulated_rendezvous', default_value='true'),
DeclareLaunchArgument('rendezvous_config', default_value='graco_ground.config'),
OpaqueFunction(function=launch_setup)
])