Skip to content

Commit 98d4849

Browse files
rakeshv24rakeshv24
authored
Merge pull request #14 from IOES-Lab/dvl
[GSOC-95] Custom ros_gz bridge for DVL plugin
2 parents 588eba5 + f3b7176 commit 98d4849

File tree

45 files changed

+3241
-192
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

45 files changed

+3241
-192
lines changed

‎dave_interfaces/CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
1616
"msg/UsblCommand.msg"
1717
"msg/UsblResponse.msg"
1818
"msg/Location.msg"
19+
"msg/DVL.msg"
20+
"msg/DVLBeam.msg"
21+
"msg/DVLTarget.msg"
1922
"srv/SetOriginSphericalCoord.srv"
2023
"srv/GetOriginSphericalCoord.srv"
2124
"srv/TransformToSphericalCoord.srv"

‎dave_interfaces/msg/DVL.msg

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
std_msgs/Header header
2+
string type
3+
DVLTarget target
4+
geometry_msgs/TwistWithCovariance velocity
5+
DVLBeam[] beams

‎dave_interfaces/msg/DVLBeam.msg

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
int64 id
2+
string reference
3+
float64 range
4+
bool locked
5+
geometry_msgs/TwistWithCovariance velocity

‎dave_interfaces/msg/DVLTarget.msg

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
string type
2+
float64 range

‎examples/dave_demos/README.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ To launch a Dave model directly from a Fuel URI, follow these steps:
1313
2. Launch the model using the specified launch file:
1414

1515
```bash
16-
ros2 launch dave_demos object_in_empty_world.launch.py object_name:='mossy_cinder_block'
16+
ros2 launch dave_demos dave_object.launch.py namespace:='mossy_cinder_block' paused:=false
1717
```
1818

1919
This method simplifies the process by pulling the model directly from Fuel, ensuring you always have the latest version without needing to manage local files.
@@ -47,7 +47,7 @@ If you prefer to use model files downloaded from Fuel, proceed as follows:
4747
4. Launch the model using the provided launch file:
4848

4949
```bash
50-
ros2 launch dave_demos sensor_in_empty_world.launch.py sensor_name:='nortek_dvl500_300'
50+
ros2 launch dave_demos dave_sensor.launch.py namespace:='nortek_dvl500_300' world_name:=dvl_world paused:=false z:=-30
5151
```
5252

5353
This approach gives you more control over the models you use, allowing for offline use and customization. It's especially useful when working in environments with limited internet connectivity or when specific model versions are required.

‎examples/dave_demos/launch/dave_object.launch.py

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
from launch import LaunchDescription
2+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
3+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
4+
from launch.launch_description_sources import PythonLaunchDescriptionSource
5+
from launch.conditions import IfCondition
6+
from launch_ros.substitutions import FindPackageShare
7+
8+
9+
def launch_setup(context, *args, **kwargs):
10+
paused = LaunchConfiguration("paused").perform(context)
11+
gui = LaunchConfiguration("gui").perform(context)
12+
use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
13+
headless = LaunchConfiguration("headless").perform(context)
14+
verbose = LaunchConfiguration("verbose").perform(context)
15+
namespace = LaunchConfiguration("namespace").perform(context)
16+
world_name = LaunchConfiguration("world_name").perform(context)
17+
x = LaunchConfiguration("x").perform(context)
18+
y = LaunchConfiguration("y").perform(context)
19+
z = LaunchConfiguration("z").perform(context)
20+
roll = LaunchConfiguration("roll").perform(context)
21+
pitch = LaunchConfiguration("pitch").perform(context)
22+
yaw = LaunchConfiguration("yaw").perform(context)
23+
use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context)
24+
25+
if world_name != "empty.sdf":
26+
world_filename = f"{world_name}.world"
27+
world_filepath = PathJoinSubstitution(
28+
[FindPackageShare("dave_worlds"), "worlds", world_filename]
29+
).perform(context)
30+
gz_args = [world_filepath]
31+
else:
32+
gz_args = [world_name]
33+
34+
if headless == "true":
35+
gz_args.append("-s")
36+
if paused == "false":
37+
gz_args.append("-r")
38+
if verbose == "true":
39+
gz_args.append("--verbose")
40+
41+
gz_args_str = " ".join(gz_args)
42+
43+
gz_sim_launch = IncludeLaunchDescription(
44+
PythonLaunchDescriptionSource(
45+
[
46+
PathJoinSubstitution(
47+
[
48+
FindPackageShare("ros_gz_sim"),
49+
"launch",
50+
"gz_sim.launch.py",
51+
]
52+
)
53+
]
54+
),
55+
launch_arguments=[
56+
("gz_args", TextSubstitution(text=gz_args_str)),
57+
],
58+
condition=IfCondition(gui),
59+
)
60+
61+
object_launch = IncludeLaunchDescription(
62+
PythonLaunchDescriptionSource(
63+
[
64+
PathJoinSubstitution(
65+
[
66+
FindPackageShare("dave_object_models"),
67+
"launch",
68+
"upload_object.launch.py",
69+
]
70+
)
71+
]
72+
),
73+
launch_arguments={
74+
"gui": gui,
75+
"use_sim_time": use_sim_time,
76+
"namespace": namespace,
77+
"x": x,
78+
"y": y,
79+
"z": z,
80+
"roll": roll,
81+
"pitch": pitch,
82+
"yaw": yaw,
83+
"use_ned_frame": use_ned_frame,
84+
}.items(),
85+
)
86+
87+
return [gz_sim_launch, object_launch]
88+
89+
90+
def generate_launch_description():
91+
92+
args = [
93+
DeclareLaunchArgument(
94+
"paused",
95+
default_value="true",
96+
description="Start the simulation paused",
97+
),
98+
DeclareLaunchArgument(
99+
"gui",
100+
default_value="true",
101+
description="Flag to enable the gazebo gui",
102+
),
103+
DeclareLaunchArgument(
104+
"use_sim_time",
105+
default_value="true",
106+
description="Flag to indicate whether to use simulation time",
107+
),
108+
DeclareLaunchArgument(
109+
"headless",
110+
default_value="false",
111+
description="Flag to enable the gazebo headless mode",
112+
),
113+
DeclareLaunchArgument(
114+
"verbose",
115+
default_value="false",
116+
description="Enable verbose mode for Gazebo simulation",
117+
),
118+
DeclareLaunchArgument(
119+
"world_name",
120+
default_value="empty.sdf",
121+
description="Gazebo world file to launch",
122+
),
123+
DeclareLaunchArgument(
124+
"namespace",
125+
default_value="",
126+
description="Namespace",
127+
),
128+
DeclareLaunchArgument(
129+
"x",
130+
default_value="0.0",
131+
description="Initial x position",
132+
),
133+
DeclareLaunchArgument(
134+
"y",
135+
default_value="0.0",
136+
description="Initial y position",
137+
),
138+
DeclareLaunchArgument(
139+
"z",
140+
default_value="0.0",
141+
description="Initial z position",
142+
),
143+
DeclareLaunchArgument(
144+
"roll",
145+
default_value="0.0",
146+
description="Initial roll angle",
147+
),
148+
DeclareLaunchArgument(
149+
"pitch",
150+
default_value="0.0",
151+
description="Initial pitch angle",
152+
),
153+
DeclareLaunchArgument(
154+
"yaw",
155+
default_value="0.0",
156+
description="Initial yaw angle",
157+
),
158+
DeclareLaunchArgument(
159+
"use_ned_frame",
160+
default_value="false",
161+
description="Flag to indicate whether to use the north-east-down frame",
162+
),
163+
]
164+
165+
return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

‎examples/dave_demos/launch/dave_sensor.launch.py

Lines changed: 165 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,165 @@
1+
from launch import LaunchDescription
2+
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
3+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
4+
from launch.launch_description_sources import PythonLaunchDescriptionSource
5+
from launch.conditions import IfCondition
6+
from launch_ros.substitutions import FindPackageShare
7+
8+
9+
def launch_setup(context, *args, **kwargs):
10+
paused = LaunchConfiguration("paused").perform(context)
11+
gui = LaunchConfiguration("gui").perform(context)
12+
use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
13+
headless = LaunchConfiguration("headless").perform(context)
14+
verbose = LaunchConfiguration("verbose").perform(context)
15+
namespace = LaunchConfiguration("namespace").perform(context)
16+
world_name = LaunchConfiguration("world_name").perform(context)
17+
x = LaunchConfiguration("x").perform(context)
18+
y = LaunchConfiguration("y").perform(context)
19+
z = LaunchConfiguration("z").perform(context)
20+
roll = LaunchConfiguration("roll").perform(context)
21+
pitch = LaunchConfiguration("pitch").perform(context)
22+
yaw = LaunchConfiguration("yaw").perform(context)
23+
use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context)
24+
25+
if world_name != "empty.sdf":
26+
world_filename = f"{world_name}.world"
27+
world_filepath = PathJoinSubstitution(
28+
[FindPackageShare("dave_worlds"), "worlds", world_filename]
29+
).perform(context)
30+
gz_args = [world_filepath]
31+
else:
32+
gz_args = [world_name]
33+
34+
if headless == "true":
35+
gz_args.append("-s")
36+
if paused == "false":
37+
gz_args.append("-r")
38+
if verbose == "true":
39+
gz_args.append("--verbose")
40+
41+
gz_args_str = " ".join(gz_args)
42+
43+
gz_sim_launch = IncludeLaunchDescription(
44+
PythonLaunchDescriptionSource(
45+
[
46+
PathJoinSubstitution(
47+
[
48+
FindPackageShare("ros_gz_sim"),
49+
"launch",
50+
"gz_sim.launch.py",
51+
]
52+
)
53+
]
54+
),
55+
launch_arguments=[
56+
("gz_args", TextSubstitution(text=gz_args_str)),
57+
],
58+
condition=IfCondition(gui),
59+
)
60+
61+
sensor_launch = IncludeLaunchDescription(
62+
PythonLaunchDescriptionSource(
63+
[
64+
PathJoinSubstitution(
65+
[
66+
FindPackageShare("dave_sensor_models"),
67+
"launch",
68+
"upload_sensor.launch.py",
69+
]
70+
)
71+
]
72+
),
73+
launch_arguments={
74+
"gui": gui,
75+
"use_sim_time": use_sim_time,
76+
"namespace": namespace,
77+
"x": x,
78+
"y": y,
79+
"z": z,
80+
"roll": roll,
81+
"pitch": pitch,
82+
"yaw": yaw,
83+
"use_ned_frame": use_ned_frame,
84+
}.items(),
85+
)
86+
87+
return [gz_sim_launch, sensor_launch]
88+
89+
90+
def generate_launch_description():
91+
92+
args = [
93+
DeclareLaunchArgument(
94+
"paused",
95+
default_value="true",
96+
description="Start the simulation paused",
97+
),
98+
DeclareLaunchArgument(
99+
"gui",
100+
default_value="true",
101+
description="Flag to enable the gazebo gui",
102+
),
103+
DeclareLaunchArgument(
104+
"use_sim_time",
105+
default_value="true",
106+
description="Flag to indicate whether to use simulation time",
107+
),
108+
DeclareLaunchArgument(
109+
"headless",
110+
default_value="false",
111+
description="Flag to enable the gazebo headless mode",
112+
),
113+
DeclareLaunchArgument(
114+
"verbose",
115+
default_value="false",
116+
description="Enable verbose mode for Gazebo simulation",
117+
),
118+
DeclareLaunchArgument(
119+
"world_name",
120+
default_value="empty.sdf",
121+
description="Gazebo world file to launch",
122+
),
123+
DeclareLaunchArgument(
124+
"namespace",
125+
default_value="",
126+
description="Namespace",
127+
),
128+
DeclareLaunchArgument(
129+
"x",
130+
default_value="0.0",
131+
description="Initial x position",
132+
),
133+
DeclareLaunchArgument(
134+
"y",
135+
default_value="0.0",
136+
description="Initial y position",
137+
),
138+
DeclareLaunchArgument(
139+
"z",
140+
default_value="0.0",
141+
description="Initial z position",
142+
),
143+
DeclareLaunchArgument(
144+
"roll",
145+
default_value="0.0",
146+
description="Initial roll angle",
147+
),
148+
DeclareLaunchArgument(
149+
"pitch",
150+
default_value="0.0",
151+
description="Initial pitch angle",
152+
),
153+
DeclareLaunchArgument(
154+
"yaw",
155+
default_value="0.0",
156+
description="Initial yaw angle",
157+
),
158+
DeclareLaunchArgument(
159+
"use_ned_frame",
160+
default_value="false",
161+
description="Flag to indicate whether to use the north-east-down frame",
162+
),
163+
]
164+
165+
return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])

0 commit comments

Comments
 (0)