Skip to content

Commit

Permalink
Merge pull request #14 from IOES-Lab/dvl
Browse files Browse the repository at this point in the history
[GSOC-95] Custom ros_gz bridge for DVL plugin
  • Loading branch information
rakeshv24 authored Aug 19, 2024
2 parents 588eba5 + f3b7176 commit 98d4849
Show file tree
Hide file tree
Showing 45 changed files with 3,241 additions and 192 deletions.
3 changes: 3 additions & 0 deletions dave_interfaces/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/UsblCommand.msg"
"msg/UsblResponse.msg"
"msg/Location.msg"
"msg/DVL.msg"
"msg/DVLBeam.msg"
"msg/DVLTarget.msg"
"srv/SetOriginSphericalCoord.srv"
"srv/GetOriginSphericalCoord.srv"
"srv/TransformToSphericalCoord.srv"
Expand Down
5 changes: 5 additions & 0 deletions dave_interfaces/msg/DVL.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
std_msgs/Header header
string type
DVLTarget target
geometry_msgs/TwistWithCovariance velocity
DVLBeam[] beams
5 changes: 5 additions & 0 deletions dave_interfaces/msg/DVLBeam.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
int64 id
string reference
float64 range
bool locked
geometry_msgs/TwistWithCovariance velocity
2 changes: 2 additions & 0 deletions dave_interfaces/msg/DVLTarget.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
string type
float64 range
4 changes: 2 additions & 2 deletions examples/dave_demos/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ To launch a Dave model directly from a Fuel URI, follow these steps:
2. Launch the model using the specified launch file:

```bash
ros2 launch dave_demos object_in_empty_world.launch.py object_name:='mossy_cinder_block'
ros2 launch dave_demos dave_object.launch.py namespace:='mossy_cinder_block' paused:=false
```

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.
Expand Down Expand Up @@ -47,7 +47,7 @@ If you prefer to use model files downloaded from Fuel, proceed as follows:
4. Launch the model using the provided launch file:

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

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.
Expand Down
165 changes: 165 additions & 0 deletions examples/dave_demos/launch/dave_object.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,165 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from launch_ros.substitutions import FindPackageShare


def launch_setup(context, *args, **kwargs):
paused = LaunchConfiguration("paused").perform(context)
gui = LaunchConfiguration("gui").perform(context)
use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
headless = LaunchConfiguration("headless").perform(context)
verbose = LaunchConfiguration("verbose").perform(context)
namespace = LaunchConfiguration("namespace").perform(context)
world_name = LaunchConfiguration("world_name").perform(context)
x = LaunchConfiguration("x").perform(context)
y = LaunchConfiguration("y").perform(context)
z = LaunchConfiguration("z").perform(context)
roll = LaunchConfiguration("roll").perform(context)
pitch = LaunchConfiguration("pitch").perform(context)
yaw = LaunchConfiguration("yaw").perform(context)
use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context)

if world_name != "empty.sdf":
world_filename = f"{world_name}.world"
world_filepath = PathJoinSubstitution(
[FindPackageShare("dave_worlds"), "worlds", world_filename]
).perform(context)
gz_args = [world_filepath]
else:
gz_args = [world_name]

if headless == "true":
gz_args.append("-s")
if paused == "false":
gz_args.append("-r")
if verbose == "true":
gz_args.append("--verbose")

gz_args_str = " ".join(gz_args)

gz_sim_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("ros_gz_sim"),
"launch",
"gz_sim.launch.py",
]
)
]
),
launch_arguments=[
("gz_args", TextSubstitution(text=gz_args_str)),
],
condition=IfCondition(gui),
)

object_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("dave_object_models"),
"launch",
"upload_object.launch.py",
]
)
]
),
launch_arguments={
"gui": gui,
"use_sim_time": use_sim_time,
"namespace": namespace,
"x": x,
"y": y,
"z": z,
"roll": roll,
"pitch": pitch,
"yaw": yaw,
"use_ned_frame": use_ned_frame,
}.items(),
)

return [gz_sim_launch, object_launch]


def generate_launch_description():

args = [
DeclareLaunchArgument(
"paused",
default_value="true",
description="Start the simulation paused",
),
DeclareLaunchArgument(
"gui",
default_value="true",
description="Flag to enable the gazebo gui",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Flag to indicate whether to use simulation time",
),
DeclareLaunchArgument(
"headless",
default_value="false",
description="Flag to enable the gazebo headless mode",
),
DeclareLaunchArgument(
"verbose",
default_value="false",
description="Enable verbose mode for Gazebo simulation",
),
DeclareLaunchArgument(
"world_name",
default_value="empty.sdf",
description="Gazebo world file to launch",
),
DeclareLaunchArgument(
"namespace",
default_value="",
description="Namespace",
),
DeclareLaunchArgument(
"x",
default_value="0.0",
description="Initial x position",
),
DeclareLaunchArgument(
"y",
default_value="0.0",
description="Initial y position",
),
DeclareLaunchArgument(
"z",
default_value="0.0",
description="Initial z position",
),
DeclareLaunchArgument(
"roll",
default_value="0.0",
description="Initial roll angle",
),
DeclareLaunchArgument(
"pitch",
default_value="0.0",
description="Initial pitch angle",
),
DeclareLaunchArgument(
"yaw",
default_value="0.0",
description="Initial yaw angle",
),
DeclareLaunchArgument(
"use_ned_frame",
default_value="false",
description="Flag to indicate whether to use the north-east-down frame",
),
]

return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])
165 changes: 165 additions & 0 deletions examples/dave_demos/launch/dave_sensor.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,165 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.conditions import IfCondition
from launch_ros.substitutions import FindPackageShare


def launch_setup(context, *args, **kwargs):
paused = LaunchConfiguration("paused").perform(context)
gui = LaunchConfiguration("gui").perform(context)
use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
headless = LaunchConfiguration("headless").perform(context)
verbose = LaunchConfiguration("verbose").perform(context)
namespace = LaunchConfiguration("namespace").perform(context)
world_name = LaunchConfiguration("world_name").perform(context)
x = LaunchConfiguration("x").perform(context)
y = LaunchConfiguration("y").perform(context)
z = LaunchConfiguration("z").perform(context)
roll = LaunchConfiguration("roll").perform(context)
pitch = LaunchConfiguration("pitch").perform(context)
yaw = LaunchConfiguration("yaw").perform(context)
use_ned_frame = LaunchConfiguration("use_ned_frame").perform(context)

if world_name != "empty.sdf":
world_filename = f"{world_name}.world"
world_filepath = PathJoinSubstitution(
[FindPackageShare("dave_worlds"), "worlds", world_filename]
).perform(context)
gz_args = [world_filepath]
else:
gz_args = [world_name]

if headless == "true":
gz_args.append("-s")
if paused == "false":
gz_args.append("-r")
if verbose == "true":
gz_args.append("--verbose")

gz_args_str = " ".join(gz_args)

gz_sim_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("ros_gz_sim"),
"launch",
"gz_sim.launch.py",
]
)
]
),
launch_arguments=[
("gz_args", TextSubstitution(text=gz_args_str)),
],
condition=IfCondition(gui),
)

sensor_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
PathJoinSubstitution(
[
FindPackageShare("dave_sensor_models"),
"launch",
"upload_sensor.launch.py",
]
)
]
),
launch_arguments={
"gui": gui,
"use_sim_time": use_sim_time,
"namespace": namespace,
"x": x,
"y": y,
"z": z,
"roll": roll,
"pitch": pitch,
"yaw": yaw,
"use_ned_frame": use_ned_frame,
}.items(),
)

return [gz_sim_launch, sensor_launch]


def generate_launch_description():

args = [
DeclareLaunchArgument(
"paused",
default_value="true",
description="Start the simulation paused",
),
DeclareLaunchArgument(
"gui",
default_value="true",
description="Flag to enable the gazebo gui",
),
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Flag to indicate whether to use simulation time",
),
DeclareLaunchArgument(
"headless",
default_value="false",
description="Flag to enable the gazebo headless mode",
),
DeclareLaunchArgument(
"verbose",
default_value="false",
description="Enable verbose mode for Gazebo simulation",
),
DeclareLaunchArgument(
"world_name",
default_value="empty.sdf",
description="Gazebo world file to launch",
),
DeclareLaunchArgument(
"namespace",
default_value="",
description="Namespace",
),
DeclareLaunchArgument(
"x",
default_value="0.0",
description="Initial x position",
),
DeclareLaunchArgument(
"y",
default_value="0.0",
description="Initial y position",
),
DeclareLaunchArgument(
"z",
default_value="0.0",
description="Initial z position",
),
DeclareLaunchArgument(
"roll",
default_value="0.0",
description="Initial roll angle",
),
DeclareLaunchArgument(
"pitch",
default_value="0.0",
description="Initial pitch angle",
),
DeclareLaunchArgument(
"yaw",
default_value="0.0",
description="Initial yaw angle",
),
DeclareLaunchArgument(
"use_ned_frame",
default_value="false",
description="Flag to indicate whether to use the north-east-down frame",
),
]

return LaunchDescription(args + [OpaqueFunction(function=launch_setup)])
Loading

0 comments on commit 98d4849

Please sign in to comment.