Merge pull request #14 from IOES-Lab/dvl

[GSOC-95] Custom ros_gz bridge for DVL plugin

dave_interfaces/CMakeLists.txt

@@ -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"

dave_interfaces/msg/DVL.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+string type
+DVLTarget target
+geometry_msgs/TwistWithCovariance velocity
+DVLBeam[] beams

dave_interfaces/msg/DVLBeam.msg

@@ -0,0 +1,5 @@
+int64 id
+string reference
+float64 range
+bool locked
+geometry_msgs/TwistWithCovariance velocity

dave_interfaces/msg/DVLTarget.msg

@@ -0,0 +1,2 @@
+string type
+float64 range

examples/dave_demos/README.md

@@ -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.
@@ -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.

examples/dave_demos/launch/dave_object.launch.py

@@ -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)])

examples/dave_demos/launch/dave_sensor.launch.py

@@ -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)])