Merge pull request #17 from IOES-Lab/camera

[GSOC-98] Underwater Camera Plugin
IOES-Lab · Sep 4, 2024 · 997c402 · 997c402
2 parents 98d4849 + 0def1a9
commit 997c402
Show file tree

Hide file tree

Showing 29 changed files with 1,325 additions and 267 deletions.
diff --git a/examples/dave_demos/README.md b/examples/dave_demos/README.md
@@ -1,6 +1,6 @@
-## Examples
+# Examples
 
-### 1. Launching a Dave Object Model using Fuel URI
+## Launching a Dave Object Model using Fuel URI
 
 To launch a Dave model directly from a Fuel URI, follow these steps:
 
@@ -18,7 +18,7 @@ To launch a Dave model directly from a Fuel URI, follow these steps:
 
 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.
 
-### 2. Launching a Dave Sensor Model using Downloaded Model Files
+## Launching a Dave Sensor Model using Downloaded Model Files
 
 If you prefer to use model files downloaded from Fuel, proceed as follows:
 
@@ -52,7 +52,39 @@ If you prefer to use model files downloaded from Fuel, proceed as follows:
 
 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.
 
-### 3. Launching a World File
+## Launching a Dave Robot Model
+
+Before launching, ensure to build and source the workspace:
+
+```bash
+colcon build && source install/setup.bash
+```
+
+1. Launching REXROV in an empty world:
+
+```bash
+ros2 launch dave_demos dave_robot.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false
+```
+
+2. Launching REXROV in dave_ocean_waves.world:
+
+```bash
+ros2 launch dave_demos dave_robot.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
+```
+
+3. Launching Slocum Glider in an empty world:
+
+```bash
+ros2 launch dave_demos dave_robot.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false
+```
+
+4. Launching Slocum Glider in dave_ocean_waves.world:
+
+```bash
+ros2 launch dave_demos dave_robot.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false
+```
+
+## Launching a World File
 
 To launch a specific world file, you can specify the world name without the `.world` extension. Follow these steps:
 
@@ -62,14 +94,14 @@ To launch a specific world file, you can specify the world name without the `.wo
 colcon build && source install/setup.bash
 ```
 
-1. Launch the world using the specified launch file
+2. Launch the world using the specified launch file
 
 ```bash
 ros2 launch dave_demos dave_world.launch.py world_name:='dave_ocean_waves'
 ```
 
 To check which worlds are available to launch, refer to `models/dave_worlds/worlds` directory.
 
-The worlds files are linked to use models at https://app.gazebosim.org/ which means you need an internet connection to download the models and it takes some time to download at first launch. The files are saved in temporary directories and are reused in subsequent launches.
+The world files are linked to use models at https://app.gazebosim.org/ which means you need an internet connection to download the models and it takes some time to download at first launch. The files are saved in temporary directories and are reused in subsequent launches.
 
 In this setup, you can dynamically specify different world files by changing the `world_name` argument in the launch command.
diff --git a/examples/dave_demos/launch/dave_object.launch.py b/examples/dave_demos/launch/dave_object.launch.py
@@ -1,44 +1,45 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 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)
+    paused = LaunchConfiguration("paused")
+    gui = LaunchConfiguration("gui")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    debug = LaunchConfiguration("debug")
+    headless = LaunchConfiguration("headless")
+    verbose = LaunchConfiguration("verbose")
+    namespace = LaunchConfiguration("namespace")
+    world_name = LaunchConfiguration("world_name")
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    roll = LaunchConfiguration("roll")
+    pitch = LaunchConfiguration("pitch")
+    yaw = LaunchConfiguration("yaw")
+    use_ned_frame = LaunchConfiguration("use_ned_frame")
 
-    if world_name != "empty.sdf":
+    if world_name.perform(context) != "empty.sdf":
+        world_name = LaunchConfiguration("world_name").perform(context)
         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)
+    if headless.perform(context) == "true":
+        gz_args.append(" -s")
+    if paused.perform(context) == "false":
+        gz_args.append(" -r")
+    if debug.perform(context) == "true":
+        gz_args.append(" -v ")
+        gz_args.append(verbose.perform(context))
 
     gz_sim_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -53,7 +54,7 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments=[
-            ("gz_args", TextSubstitution(text=gz_args_str)),
+            ("gz_args", gz_args),
         ],
         condition=IfCondition(gui),
     )
@@ -105,15 +106,20 @@ def generate_launch_description():
             default_value="true",
             description="Flag to indicate whether to use simulation time",
         ),
+        DeclareLaunchArgument(
+            "debug",
+            default_value="false",
+            description="Flag to enable the gazebo debug flag",
+        ),
         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",
+            default_value="0",
+            description="Adjust level of console verbosity",
         ),
         DeclareLaunchArgument(
             "world_name",

diff --git a/...ot_launch/launch/robot_in_world.launch.py → ...es/dave_demos/launch/dave_robot.launch.py b/...ot_launch/launch/robot_in_world.launch.py → ...es/dave_demos/launch/dave_robot.launch.py
diff --git a/examples/dave_demos/launch/dave_sensor.launch.py b/examples/dave_demos/launch/dave_sensor.launch.py
@@ -1,44 +1,45 @@
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 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)
+    paused = LaunchConfiguration("paused")
+    gui = LaunchConfiguration("gui")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    debug = LaunchConfiguration("debug")
+    headless = LaunchConfiguration("headless")
+    verbose = LaunchConfiguration("verbose")
+    namespace = LaunchConfiguration("namespace")
+    world_name = LaunchConfiguration("world_name")
+    x = LaunchConfiguration("x")
+    y = LaunchConfiguration("y")
+    z = LaunchConfiguration("z")
+    roll = LaunchConfiguration("roll")
+    pitch = LaunchConfiguration("pitch")
+    yaw = LaunchConfiguration("yaw")
+    use_ned_frame = LaunchConfiguration("use_ned_frame")
 
-    if world_name != "empty.sdf":
+    if world_name.perform(context) != "empty.sdf":
+        world_name = LaunchConfiguration("world_name").perform(context)
         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)
+    if headless.perform(context) == "true":
+        gz_args.append(" -s")
+    if paused.perform(context) == "false":
+        gz_args.append(" -r")
+    if debug.perform(context) == "true":
+        gz_args.append(" -v ")
+        gz_args.append(verbose.perform(context))
 
     gz_sim_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -53,7 +54,7 @@ def launch_setup(context, *args, **kwargs):
             ]
         ),
         launch_arguments=[
-            ("gz_args", TextSubstitution(text=gz_args_str)),
+            ("gz_args", gz_args),
         ],
         condition=IfCondition(gui),
     )
@@ -105,15 +106,20 @@ def generate_launch_description():
             default_value="true",
             description="Flag to indicate whether to use simulation time",
         ),
+        DeclareLaunchArgument(
+            "debug",
+            default_value="false",
+            description="Flag to enable the gazebo debug flag",
+        ),
         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",
+            default_value="0",
+            description="Adjust level of console verbosity",
         ),
         DeclareLaunchArgument(
             "world_name",

diff --git a/examples/dave_robot_launch/CMakeLists.txt b/examples/dave_robot_launch/CMakeLists.txt
diff --git a/examples/dave_robot_launch/README.md b/examples/dave_robot_launch/README.md
diff --git a/examples/dave_robot_launch/package.xml b/examples/dave_robot_launch/package.xml
diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
@@ -14,22 +14,27 @@ find_package(dave_interfaces REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(Protobuf REQUIRED)
 find_package(gz-msgs10 REQUIRED)
+find_package(OpenCV REQUIRED)
 
 # Set version variables
 set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
 set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
 set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
 
+include_directories(${OpenCV_INCLUDE_DIRS})
+
 message(STATUS "Compiling against Gazebo Harmonic")
 
 add_library(UsblTransceiver SHARED src/UsblTransceiver.cc)
 add_library(UsblTransponder SHARED src/UsblTransponder.cc)
 add_library(sea_pressure_sensor SHARED src/sea_pressure_sensor.cc)
+add_library(UnderwaterCamera SHARED src/UnderwaterCamera.cc)
 
 target_include_directories(UsblTransceiver PRIVATE include)
 target_include_directories(UsblTransponder PRIVATE include)
 target_include_directories(sea_pressure_sensor PRIVATE include)
+target_include_directories(UnderwaterCamera PRIVATE include)
 
 target_link_libraries(UsblTransceiver
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
@@ -43,6 +48,11 @@ target_link_libraries(sea_pressure_sensor
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
 )
 
+target_link_libraries(UnderwaterCamera
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  ${OpenCV_LIBS}
+)
+
 # Specify dependencies for FullSystem using ament_target_dependencies
 ament_target_dependencies(UsblTransceiver
   dave_interfaces
@@ -65,8 +75,13 @@ ament_target_dependencies(sea_pressure_sensor
   geometry_msgs
 )
 
+ament_target_dependencies(UnderwaterCamera
+  rclcpp
+  sensor_msgs
+)
+
 # Install targets
-install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor
+install(TARGETS UsblTransceiver UsblTransponder sea_pressure_sensor UnderwaterCamera
   DESTINATION lib/${PROJECT_NAME}
 )