Skip to content

Commit b4c86cc

Browse files
evan-palmerevan-palmer
authored
Integrated support for Gazebo clock and use_sim_time (#49)
* Cleaned up launch files and fixed use_sim_time integration * Resolved PR comments
1 parent 37829c2 commit b4c86cc

File tree

17 files changed

+432
-472
lines changed

17 files changed

+432
-472
lines changed

‎README.md

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,6 @@ The main features of the project include:
1313
- External localization support to enable developing custom SLAM algorithms and interfaces for localization sensors
1414
- ArduSub + Gazebo SITL integration for evaluating the performance of your algorithms in a simulation environment
1515
- Interfaces for adjusting hydrodynamic parameters
16-
- Visualization support using Foxglove Studio
1716

1817
## Installation
1918

‎blue_bringup/launch/base.launch.py

Lines changed: 272 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,272 @@
1+
# Copyright 2023, Evan Palmer
2+
#
3+
# Permission is hereby granted, free of charge, to any person obtaining a copy
4+
# of this software and associated documentation files (the "Software"), to deal
5+
# in the Software without restriction, including without limitation the rights
6+
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
# copies of the Software, and to permit persons to whom the Software is
8+
# furnished to do so, subject to the following conditions:
9+
#
10+
# The above copyright notice and this permission notice shall be included in
11+
# all copies or substantial portions of the Software.
12+
#
13+
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16+
# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19+
# THE SOFTWARE.
20+
21+
from launch import LaunchDescription
22+
from launch.actions import (
23+
DeclareLaunchArgument,
24+
ExecuteProcess,
25+
IncludeLaunchDescription,
26+
)
27+
from launch.conditions import IfCondition
28+
from launch.launch_description_sources import PythonLaunchDescriptionSource
29+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
30+
from launch_ros.actions import Node
31+
from launch_ros.substitutions import FindPackageShare
32+
33+
34+
def generate_launch_description() -> LaunchDescription:
35+
"""Generate a launch description to run the system.
36+
37+
Returns:
38+
The base launch file for BlueROV2 configurations.
39+
"""
40+
args = [
41+
DeclareLaunchArgument(
42+
"description_package",
43+
default_value="blue_description",
44+
description=(
45+
"The description package with the blue configuration files. This is"
46+
" typically not set, but is available in case another description"
47+
" package has been defined."
48+
),
49+
),
50+
DeclareLaunchArgument(
51+
"configuration_type",
52+
default_value="bluerov2_heavy",
53+
description="The BlueROV2 configuration type to load.",
54+
choices=["bluerov2_heavy", "bluerov2"],
55+
),
56+
DeclareLaunchArgument(
57+
"controllers_file",
58+
default_value="controllers.yaml",
59+
description="The BlueROV2 Heavy controller configuration file.",
60+
),
61+
DeclareLaunchArgument(
62+
"localization_file",
63+
default_value="localization.yaml",
64+
description="The BlueROV2 Heavy localization configuration file.",
65+
),
66+
DeclareLaunchArgument(
67+
"manager_file",
68+
default_value="manager.yaml",
69+
description="The BlueROV2 Heavy manager configuration file.",
70+
),
71+
DeclareLaunchArgument(
72+
"mavros_file",
73+
default_value="mavros.yaml",
74+
description="The MAVROS configuration file.",
75+
),
76+
DeclareLaunchArgument(
77+
"ardusub_params_file",
78+
default_value="ardusub.parm",
79+
description=(
80+
"The ArduSub parameters that the BlueROV2 should use if running in"
81+
" simulation."
82+
),
83+
),
84+
DeclareLaunchArgument(
85+
"gazebo_world_file",
86+
default_value="bluerov2_heavy_underwater.world",
87+
description="The world configuration to load if using Gazebo.",
88+
),
89+
DeclareLaunchArgument(
90+
"controller",
91+
default_value="ismc",
92+
description=(
93+
"The controller to use; this should be the same name as the"
94+
" controller's executable."
95+
),
96+
choices=["ismc"],
97+
),
98+
DeclareLaunchArgument(
99+
"localization_source",
100+
default_value="gazebo",
101+
choices=["mocap", "camera", "gazebo"],
102+
description="The localization source to stream from.",
103+
),
104+
DeclareLaunchArgument(
105+
"use_camera",
106+
default_value="false",
107+
description=(
108+
"Launch the BlueROV2 camera stream. This is automatically set to true"
109+
" when using the camera for localization."
110+
),
111+
),
112+
DeclareLaunchArgument(
113+
"use_mocap",
114+
default_value="false",
115+
description=(
116+
"Launch the Qualisys motion capture stream. This is automatically"
117+
" set to true when using the motion capture system for localization."
118+
),
119+
),
120+
DeclareLaunchArgument(
121+
"use_sim",
122+
default_value="false",
123+
description="Launch the Gazebo + ArduSub simulator.",
124+
),
125+
]
126+
127+
description_package = LaunchConfiguration("description_package")
128+
configuration_type = LaunchConfiguration("configuration_type")
129+
use_sim = LaunchConfiguration("use_sim")
130+
131+
ardusub_params_filepath = PathJoinSubstitution(
132+
[
133+
FindPackageShare(description_package),
134+
"config",
135+
configuration_type,
136+
LaunchConfiguration("ardusub_params_file"),
137+
]
138+
)
139+
140+
nodes = [
141+
Node(
142+
package="mavros",
143+
executable="mavros_node",
144+
output="screen",
145+
parameters=[
146+
PathJoinSubstitution(
147+
[
148+
FindPackageShare(description_package),
149+
"config",
150+
LaunchConfiguration("mavros_file"),
151+
]
152+
),
153+
{"use_sim_time": use_sim},
154+
],
155+
),
156+
Node(
157+
package="ros_gz_bridge",
158+
executable="parameter_bridge",
159+
arguments=[
160+
# Clock (IGN -> ROS 2)
161+
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
162+
# Odom (IGN -> ROS 2)
163+
[
164+
"/model/",
165+
LaunchConfiguration("configuration_type"),
166+
"/odometry@nav_msgs/msg/Odometry[gz.msgs.Odometry",
167+
],
168+
],
169+
output="screen",
170+
),
171+
]
172+
173+
processes = [
174+
ExecuteProcess(
175+
cmd=[
176+
"gz",
177+
"sim",
178+
"-v",
179+
"3",
180+
"-r",
181+
PathJoinSubstitution(
182+
[
183+
FindPackageShare(description_package),
184+
"gazebo",
185+
"worlds",
186+
LaunchConfiguration("gazebo_world_file"),
187+
]
188+
),
189+
],
190+
output="screen",
191+
condition=IfCondition(use_sim),
192+
),
193+
ExecuteProcess(
194+
cmd=[
195+
"ardusub",
196+
"-S",
197+
"-w",
198+
"-M",
199+
"JSON",
200+
"--defaults",
201+
ardusub_params_filepath,
202+
"-I0",
203+
"--home",
204+
"44.65870,-124.06556,0.0,270.0", # my not-so-secret surf spot
205+
],
206+
output="screen",
207+
condition=IfCondition(use_sim),
208+
),
209+
]
210+
211+
includes = [
212+
IncludeLaunchDescription(
213+
PythonLaunchDescriptionSource(
214+
PathJoinSubstitution(
215+
[FindPackageShare("blue_manager"), "manager.launch.py"]
216+
)
217+
),
218+
launch_arguments={
219+
"config_filepath": PathJoinSubstitution(
220+
[
221+
FindPackageShare(description_package),
222+
"config",
223+
configuration_type,
224+
LaunchConfiguration("manager_file"),
225+
]
226+
),
227+
"use_sim_time": use_sim,
228+
}.items(),
229+
),
230+
IncludeLaunchDescription(
231+
PythonLaunchDescriptionSource(
232+
PathJoinSubstitution(
233+
[FindPackageShare("blue_control"), "launch", "control.launch.py"]
234+
)
235+
),
236+
launch_arguments={
237+
"config_filepath": PathJoinSubstitution(
238+
[
239+
FindPackageShare(description_package),
240+
"config",
241+
configuration_type,
242+
LaunchConfiguration("controllers_file"),
243+
]
244+
),
245+
"controller": LaunchConfiguration("controller"),
246+
"use_sim_time": use_sim,
247+
}.items(),
248+
),
249+
IncludeLaunchDescription(
250+
PythonLaunchDescriptionSource(
251+
PathJoinSubstitution(
252+
[FindPackageShare("blue_localization"), "localization.launch.py"]
253+
)
254+
),
255+
launch_arguments={
256+
"config_filepath": PathJoinSubstitution(
257+
[
258+
FindPackageShare(description_package),
259+
"config",
260+
configuration_type,
261+
LaunchConfiguration("localization_file"),
262+
]
263+
),
264+
"localization_source": LaunchConfiguration("localization_source"),
265+
"use_mocap": LaunchConfiguration("use_mocap"),
266+
"use_camera": LaunchConfiguration("use_camera"),
267+
"use_sim_time": use_sim,
268+
}.items(),
269+
),
270+
]
271+
272+
return LaunchDescription(args + nodes + processes + includes)

0 commit comments

Comments
 (0)