From 7e0f0898a9e45a6f7629a2b0ae107de43d2a0ea8 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Mon, 19 Aug 2024 07:45:56 -0700
Subject: [PATCH 01/11] Initial commit for underwater camera

---
 gazebo/dave_gz_sensor_plugins/CMakeLists.txt  |  16 +-
 .../UnderwaterCamera.hh                       |  65 ++++
 gazebo/dave_gz_sensor_plugins/package.xml     |   5 +-
 .../src/UnderwaterCamera.cc                   | 330 ++++++++++++++++++
 .../description/underwater_camera/model.sdf   |  51 +++
 .../dave_worlds/worlds/camera_tutorial.world  | 200 +++++++++++
 6 files changed, 665 insertions(+), 2 deletions(-)
 create mode 100644 gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
 create mode 100644 gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
 create mode 100644 models/dave_sensor_models/description/underwater_camera/model.sdf
 create mode 100644 models/dave_worlds/worlds/camera_tutorial.world

diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
index 3fbb5cbc..230c00dd 100644
--- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
+++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
@@ -14,6 +14,7 @@ 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})
@@ -21,15 +22,19 @@ 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,10 @@ 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}
+)
+
 # Specify dependencies for FullSystem using ament_target_dependencies
 ament_target_dependencies(UsblTransceiver
   dave_interfaces
@@ -65,8 +74,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}
 )
 
diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
new file mode 100644
index 00000000..25455577
--- /dev/null
+++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2024 Rakesh Vivekanandan
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#ifndef DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_
+#define DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_
+
+#include <gz/msgs/image.pb.h>
+#include <gz/msgs/camera_info.pb.h>
+#include <gz/sim/System.hh>
+#include <gz/math/Angle.hh>
+#include <memory>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <opencv2/opencv.hpp>
+
+namespace dave_gz_sensor_plugins
+
+{
+class UnderwaterCamera : public gz::sim::System,
+                  public gz::sim::ISystemConfigure,
+                  public gz::sim::ISystemPostUpdate
+{
+public:
+  UnderwaterCamera();
+  ~UnderwaterCamera();
+
+  void Configure(
+    const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
+    gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override;
+
+  void PostUpdate(
+    const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override;
+
+  void CameraCallback(const gz::msgs::Image & image);
+
+  void DepthImageCallback(const gz::msgs::Image & image);
+
+  void SimulateUnderwater(
+    const cv::Mat& _inputImage, const cv::Mat& _inputDepth,
+    cv::Mat& _outputImage);
+
+private:
+  std::shared_ptr<rclcpp::Node> ros_node_;
+
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_gz_sensor_plugins
+
+#endif  // DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_
diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml
index f8e6da97..4ec46fde 100644
--- a/gazebo/dave_gz_sensor_plugins/package.xml
+++ b/gazebo/dave_gz_sensor_plugins/package.xml
@@ -5,6 +5,7 @@
   <description>DAVE sensor plugins</description>
   <maintainer email="gaurav.og.9920@gmail.com">Gaurav kumar</maintainer>
   <maintainer email="helenamoyen@gmail.com">Helena Moyen</maintainer>
+  <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
   <license>Apache 2.0</license>
   <depend>geometry_msgs</depend>
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -13,7 +14,9 @@
   <build_depend>protobuf</build_depend>
   <exec_depend>protobuf</exec_depend>
   <exec_depend>dave_interfaces</exec_depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
new file mode 100644
index 00000000..658dadb8
--- /dev/null
+++ b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2024 Rakesh Vivekanandan
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <gz/common/Console.hh>
+#include <gz/transport/Node.hh>
+#include "gz/plugin/Register.hh"
+#include "gz/sim/EntityComponentManager.hh"
+#include "gz/sim/components/Camera.hh"
+#include "gz/sim/components/DepthCamera.hh"
+#include <sdf/Camera.hh>
+#include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
+
+GZ_ADD_PLUGIN(
+  dave_gz_sensor_plugins::UnderwaterCamera, gz::sim::System,
+  dave_gz_sensor_plugins::UnderwaterCamera::ISystemConfigure,
+  dave_gz_sensor_plugins::UnderwaterCamera::ISystemPostUpdate)
+
+namespace dave_gz_sensor_plugins
+{
+
+struct UnderwaterCamera::PrivateData
+{
+  // Add any private data members here.
+  std::mutex mutex_;
+  gz::transport::Node gz_node;
+  std::string image_topic;
+  std::string depth_image_topic;
+  std::string camera_info_topic;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub;
+
+  /// \brief Width of the image.
+  unsigned int width;
+
+  /// \brief Height of the image.
+  unsigned int height;
+
+  /// \brief Camera intrinsics.
+  double fx;
+  double fy;
+  double cx;
+  double cy;
+
+  /// \brief Temporarily store pointer to previous depth image.
+  const float * lastDepth;
+
+  /// \brief Latest simulated image.
+  unsigned char * lastImage;
+
+  /// \brief Depth to range lookup table (LUT)
+  float* depth2rangeLUT;
+
+  /// \brief Attenuation constants per channel (RGB)
+  float attenuation[3];
+
+  /// \brief Background constants per channel (RGB)
+  unsigned char background[3];
+};
+
+UnderwaterCamera::UnderwaterCamera() : dataPtr(std::make_unique<PrivateData>()) {}
+
+UnderwaterCamera::~UnderwaterCamera() {
+  if (this->dataPtr->lastImage)
+  {
+    delete[] this->dataPtr->lastImage;
+  }
+
+  if (this->dataPtr->depth2rangeLUT)
+  {
+    delete[] this->dataPtr->depth2rangeLUT;
+  }
+}
+
+void UnderwaterCamera::Configure(
+  const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+  gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager)
+{
+  gzdbg << "dave_gz_sensor_plugins::UnderwaterCamera::Configure on entity: " << _entity << std::endl;
+
+  if (!rclcpp::ok())
+  {
+    rclcpp::init(0, nullptr);
+  }
+
+  this->ros_node_ = std::make_shared<rclcpp::Node>("underwater_camera_node");
+
+  auto sensorComp = _ecm.Component<gz::sim::components::DepthCamera>(_entity);
+  if (!sensorComp)
+  {
+    gzerr << "UnderwaterCamera plugin requires a DepthCamera component" << std::endl;
+    return;
+  }
+
+  sdf::Sensor sensorSdf = sensorComp->Data();
+  if(sensorSdf.Type() != sdf::SensorType::DEPTH_CAMERA)
+  {
+    gzerr << "Sensor type is not depth camera" << std::endl;
+    return;
+  }
+
+  if(sensorSdf.CameraSensor() == nullptr)
+  {
+    gzerr << "Camera sensor is null" << std::endl;
+    return;
+  }
+  
+  sdf::Camera *cameraSdf = sensorSdf.CameraSensor();
+  this->dataPtr->width = cameraSdf->ImageWidth();
+  this->dataPtr->height = cameraSdf->ImageHeight();
+  this->dataPtr->fx = cameraSdf->LensIntrinsicsFx();
+  this->dataPtr->fy = cameraSdf->LensIntrinsicsFy();
+  this->dataPtr->cx = cameraSdf->LensIntrinsicsCx();
+  this->dataPtr->cy = cameraSdf->LensIntrinsicsCy();
+
+  // Grab camera topic from SDF
+  if (!_sdf->HasElement("camera_topic"))
+  {
+    this->dataPtr->image_topic = "";
+    gzmsg << "Camera topic set to default:  " << this->dataPtr->image_topic << std::endl;
+  }
+  else
+  {
+    this->dataPtr->image_topic = _sdf->Get<std::string>("camera_topic");
+    gzmsg << "Camera topic: " << this->dataPtr->image_topic << std::endl;
+  }
+
+  // Grab depth image topic from SDF
+  if (!_sdf->HasElement("depth_topic"))
+  {
+    this->dataPtr->depth_image_topic = "";
+    gzmsg << "Depth topic set to default:  " << this->dataPtr->depth_image_topic << std::endl;
+  }
+  else
+  {
+    this->dataPtr->image_topic = _sdf->Get<std::string>("depth_topic");
+    gzmsg << "Depth topic: " << this->dataPtr->image_topic << std::endl;
+  }
+
+  if(!_sdf->HasElement("attenuationR"))
+  {
+    this->dataPtr->attenuation[0] = 1.f / 30.f;
+  }
+  else
+  {
+    this->dataPtr->attenuation[0] = _sdf->Get<float>("attenuationR");
+  }
+
+  if(!_sdf->HasElement("attenuationG"))
+  {
+    this->dataPtr->attenuation[1] = 1.f / 30.f;
+  }
+  else
+  {
+    this->dataPtr->attenuation[1] = _sdf->Get<float>("attenuationG");
+  }
+
+  if(!_sdf->HasElement("attenuationB"))
+  {
+    this->dataPtr->attenuation[2] = 1.f / 30.f;
+  }
+  else
+  {
+    this->dataPtr->attenuation[2] = _sdf->Get<float>("attenuationB");
+  }
+
+  if(!_sdf->HasElement("backgroundR"))
+  {
+    this->dataPtr->background[0] = (unsigned char)0;
+  }
+  else
+  {
+    this->dataPtr->background[0] = (unsigned char)_sdf->Get<int>("backgroundR");
+  }
+
+  if(!_sdf->HasElement("backgroundG"))
+  {
+    this->dataPtr->background[1] = (unsigned char)0;
+  }
+  else
+  {
+    this->dataPtr->background[1] = (unsigned char)_sdf->Get<int>("backgroundG");
+  }
+
+  if(!_sdf->HasElement("backgroundB"))
+  {
+    this->dataPtr->background[2] = (unsigned char)0;
+  }
+  else
+  {
+    this->dataPtr->background[2] = (unsigned char)_sdf->Get<int>("backgroundB");
+  }
+
+
+  // Create and fill depth2range LUT
+  this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height];
+  float * lutPtr = this->dataPtr->depth2rangeLUT;
+  for (int v = 0; v < this->dataPtr->height; v++)
+  {
+      double y_z = (v - this->dataPtr->cy)/this->dataPtr->fy;
+      for (int u = 0; u < this->dataPtr->width; u++)
+      {
+          double x_z = (u - this->dataPtr->cx)/this->dataPtr->fx;
+          // Precompute the per-pixel factor in the following formula:
+          // range = || (x, y, z) ||_2
+          // range = || z * (x/z, y/z, 1.0) ||_2
+          // range = z * || (x/z, y/z, 1.0) ||_2
+          *(lutPtr++) = sqrt(1.0 + x_z*x_z + y_z*y_z);
+      }
+  }
+
+  // Gazebo camera subscriber
+  std::function<void(const gz::msgs::Image &)> camera_callback =
+    std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
+
+  this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback);
+
+  // Gazebo depth image subscriber
+  std::function<void(const gz::msgs::Image &)> depth_callback =
+    std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1);
+
+  this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback);
+
+  // ROS2 publisher
+  this->dataPtr->image_pub =
+    this->ros_node_->create_publisher<sensor_msgs::msg::Image>(this->dataPtr->image_topic, 1);
+}
+
+void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
+
+  gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraCallback" << std::endl;
+
+  // Convert Gazebo image to OpenCV image
+  const cv::Mat image(
+    msg.height(), msg.width(), CV_8UC3, const_cast<unsigned char *>(reinterpret_cast<const unsigned char *>(msg.data().c_str())));
+
+  // Convert depth image to OpenCV image
+  const cv::Mat depth(
+    msg.height(), msg.width(), CV_32FC1, const_cast<float *>(this->dataPtr->lastDepth));
+
+  // Create output image
+  cv::Mat output(msg.height(), msg.width(), CV_8UC3, this->dataPtr->lastImage);
+
+  // Simulate underwater
+  this->SimulateUnderwater(image, depth, output);
+
+  // Publish simulated image
+  sensor_msgs::msg::Image ros_image;
+  ros_image.header.stamp = this->ros_node_->now();
+  ros_image.height = msg.height();
+  ros_image.width = msg.width();
+  ros_image.encoding = "bgr8";
+  ros_image.is_bigendian = false;
+  ros_image.step = msg.width() * 3;
+  ros_image.data = std::vector<unsigned char>(output.data, output.data + output.total() * output.elemSize());
+
+  this->dataPtr->image_pub->publish(ros_image);
+
+}
+
+void UnderwaterCamera::DepthImageCallback(const gz::msgs::Image & msg)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
+
+  gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::DepthImageCallback" << std::endl;
+
+  this->dataPtr->lastDepth = reinterpret_cast<const float *>(msg.data().c_str());
+}
+
+void UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
+  const cv::Mat& _inputDepth, cv::Mat& _outputImage)
+{
+  const float * lutPtr = this->dataPtr->depth2rangeLUT;
+  for (unsigned int row = 0; row < this->dataPtr->height; row++)
+  {
+    const cv::Vec3b* inrow = _inputImage.ptr<cv::Vec3b>(row);
+    const float* depthrow = _inputDepth.ptr<float>(row);
+    cv::Vec3b* outrow = _outputImage.ptr<cv::Vec3b>(row);
+
+    for (int col = 0; col < this->dataPtr->width; col++)
+    {
+      // Convert depth to range using the depth2range LUT
+      float r = *(lutPtr++)*depthrow[col];
+      const cv::Vec3b& in = inrow[col];
+      cv::Vec3b& out = outrow[col];
+
+      if (r < 1e-3)
+        r = 1e10;
+
+      for (int c = 0; c < 3; c++)
+      {
+        // Simplifying assumption: intensity ~ irradiance.
+        // This is not really the case but a good enough approximation
+        // for now (it would be better to use a proper Radiometric
+        // Response Function).
+        float e = std::exp(-r*this->dataPtr->attenuation[c]);
+        out[c] = e*in[c] + (1.0f-e)*this->dataPtr->background[c];
+      }
+    }
+  }
+}
+
+void UnderwaterCamera::PostUpdate(
+  const gz::sim::UpdateInfo & _info, const gz::sim::EntityComponentManager & _ecm)
+{
+  if (!_info.paused)
+  {
+    rclcpp::spin_some(this->ros_node_);
+
+    if (_info.iterations % 1000 == 0)
+    {
+      gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::PostUpdate" << std::endl;
+    }
+  }
+}
+
+}  // namespace dave_gz_sensor_plugins
diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf
new file mode 100644
index 00000000..e96ecd4a
--- /dev/null
+++ b/models/dave_sensor_models/description/underwater_camera/model.sdf
@@ -0,0 +1,51 @@
+
+<?xml version="1.0"?>
+<sdf version="1.4">
+  <model name="underwater_camera">
+    <pose>0 0 0 0 0 0</pose>
+    <link name="link">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <mass>0.1</mass>
+        <inertia>
+          <ixx>0.000166667</ixx>
+          <iyy>0.000166667</iyy>
+          <izz>0.000166667</izz>
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.1 0.1 0.1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.1 0.1 0.1</size>
+          </box>
+        </geometry>
+      </visual>
+      <sensor name="underwater_camera" type="depth_camera">
+        <update_rate>10</update_rate>
+        <!-- <camera_topic>camera</camera_topic>
+        <depth_topic>depth_camera</depth_topic> -->
+        <topic>depth_camera</topic>
+        <camera>
+          <horizontal_fov>1.05</horizontal_fov>
+          <image>
+            <width>256</width>
+            <height>256</height>
+            <format>R_FLOAT32</format>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>10.0</far>
+          </clip>
+        </camera>
+        </sensor>
+    </link>
+    <static>true</static>
+  </model>
+</sdf>
diff --git a/models/dave_worlds/worlds/camera_tutorial.world b/models/dave_worlds/worlds/camera_tutorial.world
new file mode 100644
index 00000000..95a74011
--- /dev/null
+++ b/models/dave_worlds/worlds/camera_tutorial.world
@@ -0,0 +1,200 @@
+<?xml version="1.0" ?>
+<!--
+  Demo world with a widget displaying images coming from a depth camera sensor.
+-->
+<sdf version="1.6">
+  <world name="underwater_camera_sensor">
+    <physics name="1ms" type="ignored">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="MinimalScene" name="3D View">
+        <gz-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </gz-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
+      </plugin>
+
+      <!-- Plugins that add functionality to the scene -->
+      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+        <gz-gui>
+          <property key="state" type="string">floating</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="GzSceneManager" name="Scene Manager">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="InteractiveViewControl" name="Interactive view control">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <plugin filename="CameraTracking" name="Camera Tracking">
+        <gz-gui>
+          <property key="resizable" type="bool">false</property>
+          <property key="width" type="double">5</property>
+          <property key="height" type="double">5</property>
+          <property key="state" type="string">floating</property>
+          <property key="showTitleBar" type="bool">false</property>
+        </gz-gui>
+      </plugin>
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <gz-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </gz-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+        <use_event>true</use_event>
+      </plugin>
+
+      <plugin filename="ImageDisplay" name="Image Display">
+        <gz-gui>
+          <property key="state" type="string">docked</property>
+        </gz-gui>
+        <topic>depth_camera</topic>
+      </plugin>
+    </gui>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <!--plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane-->
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <!--plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane-->
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="box">
+      <pose>0 -1 0.5 0 0 0</pose>
+      <link name="box_link">
+        <inertial>
+          <inertia>
+            <ixx>1</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>1</iyy>
+            <iyz>0</iyz>
+            <izz>1</izz>
+          </inertia>
+          <mass>1.0</mass>
+        </inertial>
+        <collision name="box_collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <visual name="box_visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <pose>0 1 3 0.0 0.0 1.57</pose>
+      <uri>
+      https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Barrel
+      </uri>
+    </include>
+
+  </world>
+</sdf>

From d057573db7880f3da84567f12e7340ec24b1d560 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Mon, 26 Aug 2024 09:11:30 -0700
Subject: [PATCH 02/11] WIP: fixed CV dep errors and changed base camera to
 RGBD over Depth camera

---
 gazebo/dave_gz_sensor_plugins/CMakeLists.txt  |   1 +
 .../UnderwaterCamera.hh                       |   6 +-
 .../src/UnderwaterCamera.cc                   | 246 ++++++++++------
 .../description/underwater_camera/model.sdf   |  31 +-
 .../dave_worlds/worlds/camera_tutorial.world  | 270 +++++++-----------
 5 files changed, 290 insertions(+), 264 deletions(-)

diff --git a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
index 230c00dd..93ff8c46 100644
--- a/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
+++ b/gazebo/dave_gz_sensor_plugins/CMakeLists.txt
@@ -50,6 +50,7 @@ target_link_libraries(sea_pressure_sensor
 
 target_link_libraries(UnderwaterCamera
   gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+  ${OpenCV_LIBS}
 )
 
 # Specify dependencies for FullSystem using ament_target_dependencies
diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
index 25455577..f458d294 100644
--- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
+++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
@@ -48,9 +48,13 @@ public:
 
   void CameraCallback(const gz::msgs::Image & image);
 
+  void CameraInfoCallback(const gz::msgs::CameraInfo & cameraInfo);
+
   void DepthImageCallback(const gz::msgs::Image & image);
 
-  void SimulateUnderwater(
+  cv::Mat ConvertGazeboToOpenCV(const gz::msgs::Image &gz_image);
+
+  cv::Mat SimulateUnderwater(
     const cv::Mat& _inputImage, const cv::Mat& _inputDepth,
     cv::Mat& _outputImage);
 
diff --git a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
index 658dadb8..f64c21e6 100644
--- a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
+++ b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
@@ -20,6 +20,7 @@
 #include "gz/sim/EntityComponentManager.hh"
 #include "gz/sim/components/Camera.hh"
 #include "gz/sim/components/DepthCamera.hh"
+#include "gz/sim/components/RgbdCamera.hh"
 #include <sdf/Camera.hh>
 #include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
 
@@ -36,8 +37,10 @@ struct UnderwaterCamera::PrivateData
   // Add any private data members here.
   std::mutex mutex_;
   gz::transport::Node gz_node;
+  std::string topic;
   std::string image_topic;
   std::string depth_image_topic;
+  std::string simulated_image_topic;
   std::string camera_info_topic;
   rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub;
 
@@ -54,7 +57,7 @@ struct UnderwaterCamera::PrivateData
   double cy;
 
   /// \brief Temporarily store pointer to previous depth image.
-  const float * lastDepth;
+  gz::msgs::Image lastDepth;
 
   /// \brief Latest simulated image.
   unsigned char * lastImage;
@@ -96,57 +99,22 @@ void UnderwaterCamera::Configure(
 
   this->ros_node_ = std::make_shared<rclcpp::Node>("underwater_camera_node");
 
-  auto sensorComp = _ecm.Component<gz::sim::components::DepthCamera>(_entity);
-  if (!sensorComp)
+  // Grab topic from SDF
+  if (!_sdf->HasElement("topic"))
   {
-    gzerr << "UnderwaterCamera plugin requires a DepthCamera component" << std::endl;
-    return;
-  }
-
-  sdf::Sensor sensorSdf = sensorComp->Data();
-  if(sensorSdf.Type() != sdf::SensorType::DEPTH_CAMERA)
-  {
-    gzerr << "Sensor type is not depth camera" << std::endl;
-    return;
-  }
-
-  if(sensorSdf.CameraSensor() == nullptr)
-  {
-    gzerr << "Camera sensor is null" << std::endl;
-    return;
-  }
-  
-  sdf::Camera *cameraSdf = sensorSdf.CameraSensor();
-  this->dataPtr->width = cameraSdf->ImageWidth();
-  this->dataPtr->height = cameraSdf->ImageHeight();
-  this->dataPtr->fx = cameraSdf->LensIntrinsicsFx();
-  this->dataPtr->fy = cameraSdf->LensIntrinsicsFy();
-  this->dataPtr->cx = cameraSdf->LensIntrinsicsCx();
-  this->dataPtr->cy = cameraSdf->LensIntrinsicsCy();
-
-  // Grab camera topic from SDF
-  if (!_sdf->HasElement("camera_topic"))
-  {
-    this->dataPtr->image_topic = "";
-    gzmsg << "Camera topic set to default:  " << this->dataPtr->image_topic << std::endl;
+    this->dataPtr->topic = "";
+    gzmsg << "Camera topic set to default:  " << this->dataPtr->topic << std::endl;
   }
   else
   {
-    this->dataPtr->image_topic = _sdf->Get<std::string>("camera_topic");
-    gzmsg << "Camera topic: " << this->dataPtr->image_topic << std::endl;
+    this->dataPtr->topic = _sdf->Get<std::string>("topic");
+    gzmsg << "Topic: " << this->dataPtr->image_topic << std::endl;
   }
 
-  // Grab depth image topic from SDF
-  if (!_sdf->HasElement("depth_topic"))
-  {
-    this->dataPtr->depth_image_topic = "";
-    gzmsg << "Depth topic set to default:  " << this->dataPtr->depth_image_topic << std::endl;
-  }
-  else
-  {
-    this->dataPtr->image_topic = _sdf->Get<std::string>("depth_topic");
-    gzmsg << "Depth topic: " << this->dataPtr->image_topic << std::endl;
-  }
+  this->dataPtr->image_topic = this->dataPtr->topic + "/image";
+  this->dataPtr->depth_image_topic = this->dataPtr->topic + "/depth_image";
+  this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image";
+  this->dataPtr->camera_info_topic = this->dataPtr->topic + "/camera_info";
 
   if(!_sdf->HasElement("attenuationR"))
   {
@@ -202,73 +170,177 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[2] = (unsigned char)_sdf->Get<int>("backgroundB");
   }
 
+  // Gazebo camera subscriber
+  std::function<void(const gz::msgs::Image &)> camera_callback =
+    std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
+
+  this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback);
+
+  // Gazebo camera info subscriber
+  std::function<void(const gz::msgs::CameraInfo &)> camera_info_callback =
+    std::bind(&UnderwaterCamera::CameraInfoCallback, this, std::placeholders::_1);
+
+  this->dataPtr->gz_node.Subscribe(this->dataPtr->camera_info_topic, camera_info_callback);
+
+  // Gazebo depth image subscriber
+  std::function<void(const gz::msgs::Image &)> depth_callback =
+    std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1);
+
+  this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback);
+
+  // ROS2 publisher
+  this->dataPtr->image_pub =
+    this->ros_node_->create_publisher<sensor_msgs::msg::Image>(this->dataPtr->simulated_image_topic, 1);
+}
+
+void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
+{
+  std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
+
+  gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraInfoCallback" << std::endl;
+
+  if (msg.width() == 0 || msg.height() == 0)
+  {
+      gzerr << "CameraInfo is empty" << std::endl;
+      return;
+  }
+
+  this->dataPtr->width = msg.width();
+  this->dataPtr->height = msg.height();
+
+  // Check if intrinsics are correctly provided
+  if (msg.intrinsics().k().size() < 9)
+  {
+      gzerr << "Invalid camera intrinsics" << std::endl;
+      return;
+  }
+
+  this->dataPtr->fx = msg.intrinsics().k().data()[0];
+  this->dataPtr->fy = msg.intrinsics().k().data()[4];
+  this->dataPtr->cx = msg.intrinsics().k().data()[2];
+  this->dataPtr->cy = msg.intrinsics().k().data()[5];
+
+  // Check if fx and fy are non-zero to avoid division by zero
+  if (this->dataPtr->fx == 0 || this->dataPtr->fy == 0)
+  {
+      gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl;
+      return;
+  }
+
+  // print camera intrinsics
+  gzmsg << "Camera intrinsics: fx=" << this->dataPtr->fx << ", fy=" << this->dataPtr->fy
+        << ", cx=" << this->dataPtr->cx << ", cy=" << this->dataPtr->cy << std::endl;
 
-  // Create and fill depth2range LUT
+  // print image size
+  gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height << std::endl;
+
+  // Free previous LUT memory if it was already allocated
+  if (this->dataPtr->depth2rangeLUT)
+  {
+      delete[] this->dataPtr->depth2rangeLUT;
+  }
+
+  // Allocate memory for the new LUT
   this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height];
-  float * lutPtr = this->dataPtr->depth2rangeLUT;
+  float* lutPtr = this->dataPtr->depth2rangeLUT;
+
+  // Fill depth2range LUT
   for (int v = 0; v < this->dataPtr->height; v++)
   {
-      double y_z = (v - this->dataPtr->cy)/this->dataPtr->fy;
+      double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy;
       for (int u = 0; u < this->dataPtr->width; u++)
       {
-          double x_z = (u - this->dataPtr->cx)/this->dataPtr->fx;
+          double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx;
           // Precompute the per-pixel factor in the following formula:
           // range = || (x, y, z) ||_2
           // range = || z * (x/z, y/z, 1.0) ||_2
           // range = z * || (x/z, y/z, 1.0) ||_2
-          *(lutPtr++) = sqrt(1.0 + x_z*x_z + y_z*y_z);
+          *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z);
       }
   }
+}
 
-  // Gazebo camera subscriber
-  std::function<void(const gz::msgs::Image &)> camera_callback =
-    std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
-
-  this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback);
+cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image &gz_image)
+{
+    int cv_type;
+    switch (gz_image.pixel_format_type())
+    {
+        case gz::msgs::PixelFormatType::RGB_INT8:
+            cv_type = CV_8UC3;
+            break;
+        case gz::msgs::PixelFormatType::RGBA_INT8:
+            cv_type = CV_8UC4;
+            break;
+        case gz::msgs::PixelFormatType::BGR_INT8:
+            cv_type = CV_8UC3;
+            break;
+        case gz::msgs::PixelFormatType::L_INT8:  // MONO8
+            cv_type = CV_8UC1;
+            break;
+        case gz::msgs::PixelFormatType::R_FLOAT32:  // DEPTH32F
+            cv_type = CV_32FC1;
+            break;
+        default:
+            throw std::runtime_error("Unsupported pixel format");
+    }
 
-  // Gazebo depth image subscriber
-  std::function<void(const gz::msgs::Image &)> depth_callback =
-    std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1);
+    // Create OpenCV Mat header that uses the same memory as the Gazebo image data
+    cv::Mat cv_image(
+        gz_image.height(),
+        gz_image.width(),
+        cv_type,
+        const_cast<void*>(reinterpret_cast<const void*>(gz_image.data().data())));
 
-  this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback);
+    // Optionally convert color space if needed (e.g., RGB to BGR)
+    if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8)
+    {
+        cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR);
+    }
+    else if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8)
+    {
+        cv::cvtColor(cv_image, cv_image, cv::COLOR_RGBA2BGRA);
+    }
 
-  // ROS2 publisher
-  this->dataPtr->image_pub =
-    this->ros_node_->create_publisher<sensor_msgs::msg::Image>(this->dataPtr->image_topic, 1);
+    return cv_image;
 }
 
 void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
 
-  gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraCallback" << std::endl;
+  this->dataPtr->lastImage = const_cast<unsigned char *>(reinterpret_cast<const unsigned char *>(msg.data().c_str()));
 
-  // Convert Gazebo image to OpenCV image
-  const cv::Mat image(
-    msg.height(), msg.width(), CV_8UC3, const_cast<unsigned char *>(reinterpret_cast<const unsigned char *>(msg.data().c_str())));
+  if (!this->dataPtr->depth2rangeLUT){
+    gzerr << "Depth2range LUT not initialized" << std::endl;
+    return;
+  }
+  else{
+    gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraCallback" << std::endl;
 
-  // Convert depth image to OpenCV image
-  const cv::Mat depth(
-    msg.height(), msg.width(), CV_32FC1, const_cast<float *>(this->dataPtr->lastDepth));
+    // Convert Gazebo image to OpenCV image
+    cv::Mat image = this->ConvertGazeboToOpenCV(msg);
 
-  // Create output image
-  cv::Mat output(msg.height(), msg.width(), CV_8UC3, this->dataPtr->lastImage);
+    // Convert depth image to OpenCV image using the ConvertGazeboToOpenCV function
+    cv::Mat depth_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastDepth);
 
-  // Simulate underwater
-  this->SimulateUnderwater(image, depth, output);
+    // Create output image
+    cv::Mat output_image(msg.height(), msg.width(), CV_8UC3, this->dataPtr->lastImage);
 
-  // Publish simulated image
-  sensor_msgs::msg::Image ros_image;
-  ros_image.header.stamp = this->ros_node_->now();
-  ros_image.height = msg.height();
-  ros_image.width = msg.width();
-  ros_image.encoding = "bgr8";
-  ros_image.is_bigendian = false;
-  ros_image.step = msg.width() * 3;
-  ros_image.data = std::vector<unsigned char>(output.data, output.data + output.total() * output.elemSize());
+    // Simulate underwater
+    cv::Mat simulated_image = this->SimulateUnderwater(image, depth_image, output_image);
 
-  this->dataPtr->image_pub->publish(ros_image);
+    // Publish simulated image
+    sensor_msgs::msg::Image ros_image;
+    ros_image.header.stamp = this->ros_node_->now();
+    ros_image.height = msg.height();
+    ros_image.width = msg.width();
+    ros_image.encoding = "bgr8";
+    ros_image.is_bigendian = false;
+    ros_image.step = msg.width() * 3;
+    ros_image.data = std::vector<unsigned char>(simulated_image.data, simulated_image.data + simulated_image.total() * simulated_image.elemSize());
 
+    this->dataPtr->image_pub->publish(ros_image);
+  }
 }
 
 void UnderwaterCamera::DepthImageCallback(const gz::msgs::Image & msg)
@@ -277,10 +349,10 @@ void UnderwaterCamera::DepthImageCallback(const gz::msgs::Image & msg)
 
   gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::DepthImageCallback" << std::endl;
 
-  this->dataPtr->lastDepth = reinterpret_cast<const float *>(msg.data().c_str());
+  this->dataPtr->lastDepth = msg;
 }
 
-void UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
+cv::Mat UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
   const cv::Mat& _inputDepth, cv::Mat& _outputImage)
 {
   const float * lutPtr = this->dataPtr->depth2rangeLUT;
@@ -294,6 +366,7 @@ void UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
     {
       // Convert depth to range using the depth2range LUT
       float r = *(lutPtr++)*depthrow[col];
+
       const cv::Vec3b& in = inrow[col];
       cv::Vec3b& out = outrow[col];
 
@@ -311,6 +384,7 @@ void UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
       }
     }
   }
+  return _outputImage;
 }
 
 void UnderwaterCamera::PostUpdate(
diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf
index e96ecd4a..17725150 100644
--- a/models/dave_sensor_models/description/underwater_camera/model.sdf
+++ b/models/dave_sensor_models/description/underwater_camera/model.sdf
@@ -1,10 +1,9 @@
 
 <?xml version="1.0"?>
-<sdf version="1.4">
+<sdf version="1.6">
   <model name="underwater_camera">
     <pose>0 0 0 0 0 0</pose>
     <link name="link">
-      <pose>0 0 0 0 0 0</pose>
       <inertial>
         <mass>0.1</mass>
         <inertia>
@@ -27,25 +26,37 @@
           </box>
         </geometry>
       </visual>
-      <sensor name="underwater_camera" type="depth_camera">
+      <sensor name="underwater_camera" type="rgbd_camera">
         <update_rate>10</update_rate>
-        <!-- <camera_topic>camera</camera_topic>
-        <depth_topic>depth_camera</depth_topic> -->
-        <topic>depth_camera</topic>
+        <visualize>true</visualize>
+        <always_on>1</always_on>
+        <topic>rgbd_camera</topic>
         <camera>
           <horizontal_fov>1.05</horizontal_fov>
           <image>
-            <width>256</width>
-            <height>256</height>
-            <format>R_FLOAT32</format>
+            <width>320</width>
+            <height>240</height>
           </image>
           <clip>
             <near>0.1</near>
             <far>10.0</far>
           </clip>
         </camera>
-        </sensor>
+      </sensor>
     </link>
     <static>true</static>
+
+    <plugin
+      filename="UnderwaterCamera"
+      name="dave_gz_sensor_plugins::UnderwaterCamera">
+      <topic>rgbd_camera</topic>
+      <attenuationR>0.01</attenuationR>
+      <attenuationG>0.01</attenuationG>
+      <attenuationB>0.003</attenuationB>
+      <backgroundR>0</backgroundR>
+      <backgroundG>0</backgroundG>
+      <backgroundB>0</backgroundB>
+    </plugin>
+
   </model>
 </sdf>
diff --git a/models/dave_worlds/worlds/camera_tutorial.world b/models/dave_worlds/worlds/camera_tutorial.world
index 95a74011..01a2182a 100644
--- a/models/dave_worlds/worlds/camera_tutorial.world
+++ b/models/dave_worlds/worlds/camera_tutorial.world
@@ -1,9 +1,32 @@
-<?xml version="1.0" ?>
-<!--
-  Demo world with a widget displaying images coming from a depth camera sensor.
+<?xml version="1.0"?>
+<!-- Copyright (c) 2016 The UUV Simulator Authors.
+     All rights reserved.
+
+     Licensed under the Apache License, Version 2.0 (the "License");
+     you may not use this file except in compliance with the License.
+     You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+     Unless required by applicable law or agreed to in writing, software
+     distributed under the License is distributed on an "AS IS" BASIS,
+     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+     See the License for the specific language governing permissions and
+     limitations under the License.
 -->
-<sdf version="1.6">
-  <world name="underwater_camera_sensor">
+<!-- Modified for different Gazebo GUI camera angle -->
+
+<sdf version="1.4">
+  <world name="camera_world">
+    <scene>
+      <ambient>0.01 0.01 0.01 1.0</ambient>
+      <sky>
+        <clouds>
+          <speed>12</speed>
+        </clouds>
+      </sky>
+      <shadows>1</shadows>
+    </scene>
     <physics name="1ms" type="ignored">
       <max_step_size>0.001</max_step_size>
       <real_time_factor>1.0</real_time_factor>
@@ -13,188 +36,101 @@
       name="gz::sim::systems::Physics">
     </plugin>
     <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
     </plugin>
     <plugin
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+		<plugin
+			filename="gz-sim-sensors-system"
+			name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    	</plugin>
+		<plugin
+			filename="gz-sim-dvl-system"
+			name="gz::sim::systems::DopplerVelocityLogSystem">
+		</plugin>
+    <!-- Origin placed somewhere in the middle of the North Sea  -->
+    <spherical_coordinates>
+      <latitude_deg>56.71897669633431</latitude_deg>
+      <longitude_deg>3.515625</longitude_deg>
+    </spherical_coordinates>
 
-    <gui fullscreen="0">
-
-      <!-- 3D scene -->
-      <plugin filename="MinimalScene" name="3D View">
-        <gz-gui>
-          <title>3D View</title>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="string" key="state">docked</property>
-        </gz-gui>
-
-        <engine>ogre2</engine>
-        <scene>scene</scene>
-        <ambient_light>0.4 0.4 0.4</ambient_light>
-        <background_color>0.8 0.8 0.8</background_color>
-        <camera_pose>-6 0 6 0 0.5 0</camera_pose>
-      </plugin>
-
-      <!-- Plugins that add functionality to the scene -->
-      <plugin filename="EntityContextMenuPlugin" name="Entity context menu">
-        <gz-gui>
-          <property key="state" type="string">floating</property>
-          <property key="width" type="double">5</property>
-          <property key="height" type="double">5</property>
-          <property key="showTitleBar" type="bool">false</property>
-        </gz-gui>
-      </plugin>
-      <plugin filename="GzSceneManager" name="Scene Manager">
-        <gz-gui>
-          <property key="resizable" type="bool">false</property>
-          <property key="width" type="double">5</property>
-          <property key="height" type="double">5</property>
-          <property key="state" type="string">floating</property>
-          <property key="showTitleBar" type="bool">false</property>
-        </gz-gui>
-      </plugin>
-      <plugin filename="InteractiveViewControl" name="Interactive view control">
-        <gz-gui>
-          <property key="resizable" type="bool">false</property>
-          <property key="width" type="double">5</property>
-          <property key="height" type="double">5</property>
-          <property key="state" type="string">floating</property>
-          <property key="showTitleBar" type="bool">false</property>
-        </gz-gui>
-      </plugin>
-      <plugin filename="CameraTracking" name="Camera Tracking">
-        <gz-gui>
-          <property key="resizable" type="bool">false</property>
-          <property key="width" type="double">5</property>
-          <property key="height" type="double">5</property>
-          <property key="state" type="string">floating</property>
-          <property key="showTitleBar" type="bool">false</property>
-        </gz-gui>
-      </plugin>
-      <!-- World control -->
-      <plugin filename="WorldControl" name="World control">
-        <gz-gui>
-          <title>World control</title>
-          <property type="bool" key="showTitleBar">false</property>
-          <property type="bool" key="resizable">false</property>
-          <property type="double" key="height">72</property>
-          <property type="double" key="z">1</property>
-
-          <property type="string" key="state">floating</property>
-          <anchors target="3D View">
-            <line own="left" target="left"/>
-            <line own="bottom" target="bottom"/>
-          </anchors>
-        </gz-gui>
-
-        <play_pause>true</play_pause>
-        <step>true</step>
-        <start_paused>true</start_paused>
-        <use_event>true</use_event>
-      </plugin>
+    <!-- Global light source -->
+    <light type="directional" name="sun1">
+      <pose>50 0 150 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>.1 .1 .1 1</specular>
+      <direction>0.3 0.3 -1</direction>
+      <cast_shadows>false</cast_shadows>
+    </light>
 
-      <plugin filename="ImageDisplay" name="Image Display">
-        <gz-gui>
-          <property key="state" type="string">docked</property>
-        </gz-gui>
-        <topic>depth_camera</topic>
-      </plugin>
-    </gui>
+    <light type="directional" name="sun_diffuse">
+      <pose>-50 0 -150 0 0 0</pose>
+      <diffuse>0.6 0.6 0.6 1</diffuse>
+      <specular>0 0 0 1</specular>
+      <direction>-0.3 -0.3 -1</direction>
+      <cast_shadows>false</cast_shadows>
+    </light>
 
-    <light type="directional" name="sun">
-      <cast_shadows>true</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
+    <light type="directional" name="sun_diffuse_1">
+      <pose>-100 500 -20 0 0 0</pose>
       <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>1 1 1 1</specular>
+      <direction>-1 -1 0</direction>
+      <cast_shadows>false</cast_shadows>
+    </light>
+
+    <light type="directional" name="sun_diffuse_2">
+      <pose>-150 -130 50 0 0 0</pose>
+      <diffuse>0.6 0.6 0.6 1</diffuse>
       <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
+      <direction>0.5 0.5 -1</direction>
+      <cast_shadows>false</cast_shadows>
     </light>
 
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <!--plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane-->
-            <box>
-              <size>20 20 0.1</size>
-            </box>
-          </geometry>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <!--plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane-->
-            <box>
-              <size>20 20 0.1</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
+		<!-- Virtual north, east and down frame -->
+		<include>
+			<uri>
+				https://fuel.gazebosim.org/1.0/hmoyen/models/North East Down frame
+			</uri>
+			<pose>0 0 0 0 0 0</pose>
+		</include>
 
-    <model name="box">
-      <pose>0 -1 0.5 0 0 0</pose>
-      <link name="box_link">
-        <inertial>
-          <inertia>
-            <ixx>1</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>1</iyy>
-            <iyz>0</iyz>
-            <izz>1</izz>
-          </inertia>
-          <mass>1.0</mass>
-        </inertial>
-        <collision name="box_collision">
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-        </collision>
+    <!-- Sea surface -->
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Coast Water
+      </uri>
+    </include>
 
-        <visual name="box_visual">
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>1 0 0 1</ambient>
-            <diffuse>1 0 0 1</diffuse>
-            <specular>1 0 0 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
+    <!-- Heightmap -->
+    <include>
+      <uri>https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap</uri>
+      <pose>0 0 -95 0 0 0</pose>
+    </include>
 
     <include>
-      <pose>0 1 3 0.0 0.0 1.57</pose>
+      <pose>10 0 -95 0 0 0</pose>
       <uri>
       https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Barrel
       </uri>
+      <static>1</static>
     </include>
 
+    <plugin
+      filename="gz-sim-buoyancy-system"
+      name="gz::sim::systems::Buoyancy">
+      <graded_buoyancy>
+        <default_density>1025</default_density>
+        <density_change>
+          <above_depth>0</above_depth>
+          <density>1</density>
+        </density_change>
+      </graded_buoyancy>
+    </plugin>
+
   </world>
 </sdf>

From 1aef35a4dfe8a28a86a6f50cf8acaeaa451c73a8 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Wed, 28 Aug 2024 19:29:08 -0700
Subject: [PATCH 03/11] added more objects to camera_tutorial.world

---
 .../dave_worlds/worlds/camera_tutorial.world  | 22 ++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/models/dave_worlds/worlds/camera_tutorial.world b/models/dave_worlds/worlds/camera_tutorial.world
index 01a2182a..def0a581 100644
--- a/models/dave_worlds/worlds/camera_tutorial.world
+++ b/models/dave_worlds/worlds/camera_tutorial.world
@@ -113,11 +113,23 @@
     </include>
 
     <include>
-      <pose>10 0 -95 0 0 0</pose>
-      <uri>
-      https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Barrel
-      </uri>
-      <static>1</static>
+      <name>sunken_vase</name>
+      <pose>7.5 0 -95 0 0 0</pose>
+      <uri>https://fuel.ignitionrobotics.org/1.0/Cole/models/Sunken Vase</uri>
+    </include>
+
+    <include>
+      <name>sunken_vase_distorted</name>
+      <pose>7.5 1 -95 0 0 0</pose>
+      <uri>https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase Distorted</uri>
+      <static>true</static>
+    </include>
+
+    <include>
+      <name>Coral1</name>
+      <pose>7 -1 -95 0 0 0</pose>
+      <uri>https://fuel.ignitionrobotics.org/1.0/Cole/models/Coral01</uri>
+      <static>true</static>
     </include>
 
     <plugin

From 09e2f017b2ff185997ece7118c709cc53ba9fb1d Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Fri, 30 Aug 2024 16:23:13 -0700
Subject: [PATCH 04/11] fixed range issue with the camera plugin

---
 .../UnderwaterCamera.hh                       |  15 +-
 gazebo/dave_gz_sensor_plugins/package.xml     |   2 +-
 .../src/UnderwaterCamera.cc                   | 274 ++++++++++--------
 .../description/underwater_camera/model.sdf   |  23 +-
 4 files changed, 175 insertions(+), 139 deletions(-)

diff --git a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
index f458d294..91725041 100644
--- a/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
+++ b/gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UnderwaterCamera.hh
@@ -18,22 +18,22 @@
 #ifndef DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_
 #define DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_
 
-#include <gz/msgs/image.pb.h>
 #include <gz/msgs/camera_info.pb.h>
-#include <gz/sim/System.hh>
+#include <gz/msgs/image.pb.h>
 #include <gz/math/Angle.hh>
+#include <gz/sim/System.hh>
 #include <memory>
 #include <mutex>
+#include <opencv2/opencv.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
-#include <opencv2/opencv.hpp>
 
 namespace dave_gz_sensor_plugins
 
 {
 class UnderwaterCamera : public gz::sim::System,
-                  public gz::sim::ISystemConfigure,
-                  public gz::sim::ISystemPostUpdate
+                         public gz::sim::ISystemConfigure,
+                         public gz::sim::ISystemPostUpdate
 {
 public:
   UnderwaterCamera();
@@ -52,11 +52,10 @@ public:
 
   void DepthImageCallback(const gz::msgs::Image & image);
 
-  cv::Mat ConvertGazeboToOpenCV(const gz::msgs::Image &gz_image);
+  cv::Mat ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image);
 
   cv::Mat SimulateUnderwater(
-    const cv::Mat& _inputImage, const cv::Mat& _inputDepth,
-    cv::Mat& _outputImage);
+    const cv::Mat & _inputImage, const cv::Mat & _inputDepth, cv::Mat & _outputImage);
 
 private:
   std::shared_ptr<rclcpp::Node> ros_node_;
diff --git a/gazebo/dave_gz_sensor_plugins/package.xml b/gazebo/dave_gz_sensor_plugins/package.xml
index 4ec46fde..de9335f2 100644
--- a/gazebo/dave_gz_sensor_plugins/package.xml
+++ b/gazebo/dave_gz_sensor_plugins/package.xml
@@ -19,4 +19,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
\ No newline at end of file
diff --git a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
index f64c21e6..5e51aa40 100644
--- a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
+++ b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
@@ -14,15 +14,11 @@
  * limitations under the License.
  */
 
+#include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
 #include <gz/common/Console.hh>
 #include <gz/transport/Node.hh>
 #include "gz/plugin/Register.hh"
 #include "gz/sim/EntityComponentManager.hh"
-#include "gz/sim/components/Camera.hh"
-#include "gz/sim/components/DepthCamera.hh"
-#include "gz/sim/components/RgbdCamera.hh"
-#include <sdf/Camera.hh>
-#include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
 
 GZ_ADD_PLUGIN(
   dave_gz_sensor_plugins::UnderwaterCamera, gz::sim::System,
@@ -60,26 +56,27 @@ struct UnderwaterCamera::PrivateData
   gz::msgs::Image lastDepth;
 
   /// \brief Latest simulated image.
-  unsigned char * lastImage;
+  gz::msgs::Image lastImage;
 
   /// \brief Depth to range lookup table (LUT)
-  float* depth2rangeLUT;
+  float * depth2rangeLUT;
 
   /// \brief Attenuation constants per channel (RGB)
   float attenuation[3];
 
   /// \brief Background constants per channel (RGB)
   unsigned char background[3];
+
+  bool firstImage = true;
+
+  float min_range;
+  float max_range;
 };
 
 UnderwaterCamera::UnderwaterCamera() : dataPtr(std::make_unique<PrivateData>()) {}
 
-UnderwaterCamera::~UnderwaterCamera() {
-  if (this->dataPtr->lastImage)
-  {
-    delete[] this->dataPtr->lastImage;
-  }
-
+UnderwaterCamera::~UnderwaterCamera()
+{
   if (this->dataPtr->depth2rangeLUT)
   {
     delete[] this->dataPtr->depth2rangeLUT;
@@ -90,7 +87,8 @@ void UnderwaterCamera::Configure(
   const gz::sim::Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
   gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager)
 {
-  gzdbg << "dave_gz_sensor_plugins::UnderwaterCamera::Configure on entity: " << _entity << std::endl;
+  gzdbg << "dave_gz_sensor_plugins::UnderwaterCamera::Configure on entity: " << _entity
+        << std::endl;
 
   if (!rclcpp::ok())
   {
@@ -102,8 +100,8 @@ void UnderwaterCamera::Configure(
   // Grab topic from SDF
   if (!_sdf->HasElement("topic"))
   {
-    this->dataPtr->topic = "";
-    gzmsg << "Camera topic set to default:  " << this->dataPtr->topic << std::endl;
+    this->dataPtr->topic = "/underwater_camera";
+    gzmsg << "Camera topic set to default: " << this->dataPtr->topic << std::endl;
   }
   else
   {
@@ -116,7 +114,7 @@ void UnderwaterCamera::Configure(
   this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image";
   this->dataPtr->camera_info_topic = this->dataPtr->topic + "/camera_info";
 
-  if(!_sdf->HasElement("attenuationR"))
+  if (!_sdf->HasElement("attenuationR"))
   {
     this->dataPtr->attenuation[0] = 1.f / 30.f;
   }
@@ -125,7 +123,7 @@ void UnderwaterCamera::Configure(
     this->dataPtr->attenuation[0] = _sdf->Get<float>("attenuationR");
   }
 
-  if(!_sdf->HasElement("attenuationG"))
+  if (!_sdf->HasElement("attenuationG"))
   {
     this->dataPtr->attenuation[1] = 1.f / 30.f;
   }
@@ -134,7 +132,7 @@ void UnderwaterCamera::Configure(
     this->dataPtr->attenuation[1] = _sdf->Get<float>("attenuationG");
   }
 
-  if(!_sdf->HasElement("attenuationB"))
+  if (!_sdf->HasElement("attenuationB"))
   {
     this->dataPtr->attenuation[2] = 1.f / 30.f;
   }
@@ -143,7 +141,7 @@ void UnderwaterCamera::Configure(
     this->dataPtr->attenuation[2] = _sdf->Get<float>("attenuationB");
   }
 
-  if(!_sdf->HasElement("backgroundR"))
+  if (!_sdf->HasElement("backgroundR"))
   {
     this->dataPtr->background[0] = (unsigned char)0;
   }
@@ -152,7 +150,7 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[0] = (unsigned char)_sdf->Get<int>("backgroundR");
   }
 
-  if(!_sdf->HasElement("backgroundG"))
+  if (!_sdf->HasElement("backgroundG"))
   {
     this->dataPtr->background[1] = (unsigned char)0;
   }
@@ -161,7 +159,7 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[1] = (unsigned char)_sdf->Get<int>("backgroundG");
   }
 
-  if(!_sdf->HasElement("backgroundB"))
+  if (!_sdf->HasElement("backgroundB"))
   {
     this->dataPtr->background[2] = (unsigned char)0;
   }
@@ -170,6 +168,24 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[2] = (unsigned char)_sdf->Get<int>("backgroundB");
   }
 
+  if (!_sdf->HasElement("min_range"))
+  {
+    this->dataPtr->min_range = 0.01f;
+  }
+  else
+  {
+    this->dataPtr->min_range = _sdf->Get<float>("min_range");
+  }
+
+  if (!_sdf->HasElement("max_range"))
+  {
+    this->dataPtr->max_range = 10.0f;
+  }
+  else
+  {
+    this->dataPtr->max_range = _sdf->Get<float>("max_range");
+  }
+
   // Gazebo camera subscriber
   std::function<void(const gz::msgs::Image &)> camera_callback =
     std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
@@ -189,8 +205,8 @@ void UnderwaterCamera::Configure(
   this->dataPtr->gz_node.Subscribe(this->dataPtr->depth_image_topic, depth_callback);
 
   // ROS2 publisher
-  this->dataPtr->image_pub =
-    this->ros_node_->create_publisher<sensor_msgs::msg::Image>(this->dataPtr->simulated_image_topic, 1);
+  this->dataPtr->image_pub = this->ros_node_->create_publisher<sensor_msgs::msg::Image>(
+    this->dataPtr->simulated_image_topic, 1);
 }
 
 void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
@@ -201,8 +217,8 @@ void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
 
   if (msg.width() == 0 || msg.height() == 0)
   {
-      gzerr << "CameraInfo is empty" << std::endl;
-      return;
+    gzerr << "CameraInfo is empty" << std::endl;
+    return;
   }
 
   this->dataPtr->width = msg.width();
@@ -211,8 +227,8 @@ void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
   // Check if intrinsics are correctly provided
   if (msg.intrinsics().k().size() < 9)
   {
-      gzerr << "Invalid camera intrinsics" << std::endl;
-      return;
+    gzerr << "Invalid camera intrinsics" << std::endl;
+    return;
   }
 
   this->dataPtr->fx = msg.intrinsics().k().data()[0];
@@ -223,8 +239,8 @@ void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
   // Check if fx and fy are non-zero to avoid division by zero
   if (this->dataPtr->fx == 0 || this->dataPtr->fy == 0)
   {
-      gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl;
-      return;
+    gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl;
+    return;
   }
 
   // print camera intrinsics
@@ -232,114 +248,126 @@ void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
         << ", cx=" << this->dataPtr->cx << ", cy=" << this->dataPtr->cy << std::endl;
 
   // print image size
-  gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height << std::endl;
+  gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height
+        << std::endl;
 
   // Free previous LUT memory if it was already allocated
   if (this->dataPtr->depth2rangeLUT)
   {
-      delete[] this->dataPtr->depth2rangeLUT;
+    delete[] this->dataPtr->depth2rangeLUT;
   }
 
   // Allocate memory for the new LUT
   this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height];
-  float* lutPtr = this->dataPtr->depth2rangeLUT;
+  float * lutPtr = this->dataPtr->depth2rangeLUT;
 
   // Fill depth2range LUT
   for (int v = 0; v < this->dataPtr->height; v++)
   {
-      double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy;
-      for (int u = 0; u < this->dataPtr->width; u++)
-      {
-          double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx;
-          // Precompute the per-pixel factor in the following formula:
-          // range = || (x, y, z) ||_2
-          // range = || z * (x/z, y/z, 1.0) ||_2
-          // range = z * || (x/z, y/z, 1.0) ||_2
-          *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z);
-      }
+    double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy;
+    for (int u = 0; u < this->dataPtr->width; u++)
+    {
+      double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx;
+      // Precompute the per-pixel factor in the following formula:
+      // range = || (x, y, z) ||_2
+      // range = || z * (x/z, y/z, 1.0) ||_2
+      // range = z * || (x/z, y/z, 1.0) ||_2
+      *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z);
+    }
   }
 }
 
-cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image &gz_image)
+cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image)
 {
-    int cv_type;
-    switch (gz_image.pixel_format_type())
-    {
-        case gz::msgs::PixelFormatType::RGB_INT8:
-            cv_type = CV_8UC3;
-            break;
-        case gz::msgs::PixelFormatType::RGBA_INT8:
-            cv_type = CV_8UC4;
-            break;
-        case gz::msgs::PixelFormatType::BGR_INT8:
-            cv_type = CV_8UC3;
-            break;
-        case gz::msgs::PixelFormatType::L_INT8:  // MONO8
-            cv_type = CV_8UC1;
-            break;
-        case gz::msgs::PixelFormatType::R_FLOAT32:  // DEPTH32F
-            cv_type = CV_32FC1;
-            break;
-        default:
-            throw std::runtime_error("Unsupported pixel format");
-    }
+  int cv_type;
+  switch (gz_image.pixel_format_type())
+  {
+    case gz::msgs::PixelFormatType::RGB_INT8:
+      cv_type = CV_8UC3;
+      break;
+    case gz::msgs::PixelFormatType::RGBA_INT8:
+      cv_type = CV_8UC4;
+      break;
+    case gz::msgs::PixelFormatType::BGR_INT8:
+      cv_type = CV_8UC3;
+      break;
+    case gz::msgs::PixelFormatType::L_INT8:  // MONO8
+      cv_type = CV_8UC1;
+      break;
+    case gz::msgs::PixelFormatType::R_FLOAT32:  // DEPTH32F
+      cv_type = CV_32FC1;
+      break;
+    default:
+      throw std::runtime_error("Unsupported pixel format");
+  }
 
-    // Create OpenCV Mat header that uses the same memory as the Gazebo image data
-    cv::Mat cv_image(
-        gz_image.height(),
-        gz_image.width(),
-        cv_type,
-        const_cast<void*>(reinterpret_cast<const void*>(gz_image.data().data())));
+  // Create OpenCV Mat header that uses the same memory as the Gazebo image data
+  cv::Mat cv_image(
+    gz_image.height(), gz_image.width(), cv_type,
+    const_cast<void *>(reinterpret_cast<const void *>(gz_image.data().data())));
 
-    // Optionally convert color space if needed (e.g., RGB to BGR)
-    if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8)
-    {
-        cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR);
-    }
-    else if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8)
-    {
-        cv::cvtColor(cv_image, cv_image, cv::COLOR_RGBA2BGRA);
-    }
+  // Optionally convert color space if needed (e.g., RGB to BGR)
+  if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8)
+  {
+    cv::cvtColor(cv_image, cv_image, cv::COLOR_RGB2BGR);
+  }
+  else if (gz_image.pixel_format_type() == gz::msgs::PixelFormatType::RGBA_INT8)
+  {
+    cv::cvtColor(cv_image, cv_image, cv::COLOR_RGBA2BGRA);
+  }
 
-    return cv_image;
+  return cv_image;
 }
 
 void UnderwaterCamera::CameraCallback(const gz::msgs::Image & msg)
 {
   std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
 
-  this->dataPtr->lastImage = const_cast<unsigned char *>(reinterpret_cast<const unsigned char *>(msg.data().c_str()));
-
-  if (!this->dataPtr->depth2rangeLUT){
+  if (!this->dataPtr->depth2rangeLUT)
+  {
     gzerr << "Depth2range LUT not initialized" << std::endl;
     return;
   }
-  else{
+  else
+  {
     gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraCallback" << std::endl;
 
-    // Convert Gazebo image to OpenCV image
-    cv::Mat image = this->ConvertGazeboToOpenCV(msg);
-
-    // Convert depth image to OpenCV image using the ConvertGazeboToOpenCV function
-    cv::Mat depth_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastDepth);
-
-    // Create output image
-    cv::Mat output_image(msg.height(), msg.width(), CV_8UC3, this->dataPtr->lastImage);
-
-    // Simulate underwater
-    cv::Mat simulated_image = this->SimulateUnderwater(image, depth_image, output_image);
-
-    // Publish simulated image
-    sensor_msgs::msg::Image ros_image;
-    ros_image.header.stamp = this->ros_node_->now();
-    ros_image.height = msg.height();
-    ros_image.width = msg.width();
-    ros_image.encoding = "bgr8";
-    ros_image.is_bigendian = false;
-    ros_image.step = msg.width() * 3;
-    ros_image.data = std::vector<unsigned char>(simulated_image.data, simulated_image.data + simulated_image.total() * simulated_image.elemSize());
-
-    this->dataPtr->image_pub->publish(ros_image);
+    if (this->dataPtr->firstImage)
+    {
+      this->dataPtr->lastImage = msg;
+      this->dataPtr->firstImage = false;
+    }
+    else
+    {
+      // Convert Gazebo image to OpenCV image
+      cv::Mat image = this->ConvertGazeboToOpenCV(msg);
+
+      // Convert depth image to OpenCV image using the ConvertGazeboToOpenCV function
+      cv::Mat depth_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastDepth);
+
+      // Create output image
+      cv::Mat output_image = this->ConvertGazeboToOpenCV(this->dataPtr->lastImage);
+
+      // Simulate underwater
+      cv::Mat simulated_image = this->SimulateUnderwater(image, depth_image, output_image);
+
+      // Publish simulated image
+      sensor_msgs::msg::Image ros_image;
+      ros_image.header.stamp = this->ros_node_->now();
+      ros_image.height = msg.height();
+      ros_image.width = msg.width();
+      ros_image.encoding = "bgr8";
+      ros_image.is_bigendian = false;
+      ros_image.step = msg.width() * 3;
+      ros_image.data = std::vector<unsigned char>(
+        simulated_image.data,
+        simulated_image.data + simulated_image.total() * simulated_image.elemSize());
+
+      this->dataPtr->image_pub->publish(ros_image);
+
+      // Store the current image for the next iteration
+      this->dataPtr->lastImage = msg;
+    }
   }
 }
 
@@ -352,26 +380,32 @@ void UnderwaterCamera::DepthImageCallback(const gz::msgs::Image & msg)
   this->dataPtr->lastDepth = msg;
 }
 
-cv::Mat UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
-  const cv::Mat& _inputDepth, cv::Mat& _outputImage)
+cv::Mat UnderwaterCamera::SimulateUnderwater(
+  const cv::Mat & _inputImage, const cv::Mat & _inputDepth, cv::Mat & _outputImage)
 {
   const float * lutPtr = this->dataPtr->depth2rangeLUT;
   for (unsigned int row = 0; row < this->dataPtr->height; row++)
   {
-    const cv::Vec3b* inrow = _inputImage.ptr<cv::Vec3b>(row);
-    const float* depthrow = _inputDepth.ptr<float>(row);
-    cv::Vec3b* outrow = _outputImage.ptr<cv::Vec3b>(row);
+    const cv::Vec3b * inrow = _inputImage.ptr<cv::Vec3b>(row);
+    const float * depthrow = _inputDepth.ptr<float>(row);
+    cv::Vec3b * outrow = _outputImage.ptr<cv::Vec3b>(row);
 
     for (int col = 0; col < this->dataPtr->width; col++)
     {
       // Convert depth to range using the depth2range LUT
-      float r = *(lutPtr++)*depthrow[col];
+      float r = *(lutPtr++) * depthrow[col];
 
-      const cv::Vec3b& in = inrow[col];
-      cv::Vec3b& out = outrow[col];
+      const cv::Vec3b & in = inrow[col];
+      cv::Vec3b & out = outrow[col];
 
-      if (r < 1e-3)
-        r = 1e10;
+      if (r < this->dataPtr->min_range)
+      {
+        r = this->dataPtr->min_range;
+      }
+      else if (r > this->dataPtr->max_range)
+      {
+        r = this->dataPtr->max_range;
+      }
 
       for (int c = 0; c < 3; c++)
       {
@@ -379,8 +413,8 @@ cv::Mat UnderwaterCamera::SimulateUnderwater(const cv::Mat& _inputImage,
         // This is not really the case but a good enough approximation
         // for now (it would be better to use a proper Radiometric
         // Response Function).
-        float e = std::exp(-r*this->dataPtr->attenuation[c]);
-        out[c] = e*in[c] + (1.0f-e)*this->dataPtr->background[c];
+        float e = std::exp(-r * this->dataPtr->attenuation[c]);
+        out[c] = e * in[c] + (1.0f - e) * this->dataPtr->background[c];
       }
     }
   }
diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf
index 17725150..7a4de195 100644
--- a/models/dave_sensor_models/description/underwater_camera/model.sdf
+++ b/models/dave_sensor_models/description/underwater_camera/model.sdf
@@ -2,9 +2,9 @@
 <?xml version="1.0"?>
 <sdf version="1.6">
   <model name="underwater_camera">
-    <pose>0 0 0 0 0 0</pose>
-    <link name="link">
+    <link name="camera_link">
       <inertial>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.000166667</ixx>
@@ -30,7 +30,7 @@
         <update_rate>10</update_rate>
         <visualize>true</visualize>
         <always_on>1</always_on>
-        <topic>rgbd_camera</topic>
+        <topic>underwater_camera</topic>
         <camera>
           <horizontal_fov>1.05</horizontal_fov>
           <image>
@@ -49,13 +49,16 @@
     <plugin
       filename="UnderwaterCamera"
       name="dave_gz_sensor_plugins::UnderwaterCamera">
-      <topic>rgbd_camera</topic>
-      <attenuationR>0.01</attenuationR>
-      <attenuationG>0.01</attenuationG>
-      <attenuationB>0.003</attenuationB>
-      <backgroundR>0</backgroundR>
-      <backgroundG>0</backgroundG>
-      <backgroundB>0</backgroundB>
+      <topic>underwater_camera</topic>
+      <min_range>0.1</min_range>
+      <max_range>10.0</max_range>
+      <attenuationR>0.8</attenuationR>
+      <attenuationG>0.5</attenuationG>
+      <attenuationB>0.2</attenuationB>
+      <!-- Murky Coastal Waters -->
+      <backgroundR>85</backgroundR>
+      <backgroundG>107</backgroundG>
+      <backgroundB>47</backgroundB>
     </plugin>
 
   </model>

From 3c44496841f2662131e2736815ebfe8f86b91e4f Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Sun, 1 Sep 2024 03:06:08 -0700
Subject: [PATCH 05/11] updated configure function and description files

---
 .../src/UnderwaterCamera.cc                   | 215 ++++++++-------
 .../description/underwater_camera/model.sdf   |  26 +-
 .../dave_worlds/worlds/camera_tutorial.world  | 251 ++++++++++++++++++
 3 files changed, 377 insertions(+), 115 deletions(-)

diff --git a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
index 5e51aa40..230bcfa8 100644
--- a/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
+++ b/gazebo/dave_gz_sensor_plugins/src/UnderwaterCamera.cc
@@ -16,9 +16,20 @@
 
 #include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
 #include <gz/common/Console.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/Util.hh>
+#include <gz/sim/components/Camera.hh>
+#include <gz/sim/components/ParentEntity.hh>
+#include <gz/sim/components/RgbdCamera.hh>
+#include <gz/sim/components/Sensor.hh>
 #include <gz/transport/Node.hh>
+#include <sdf/Camera.hh>
 #include "gz/plugin/Register.hh"
 #include "gz/sim/EntityComponentManager.hh"
+#include "gz/sim/components/Link.hh"
+#include "gz/sim/components/Model.hh"
+#include "gz/sim/components/Name.hh"
+#include "gz/sim/components/World.hh"
 
 GZ_ADD_PLUGIN(
   dave_gz_sensor_plugins::UnderwaterCamera, gz::sim::System,
@@ -31,6 +42,9 @@ namespace dave_gz_sensor_plugins
 struct UnderwaterCamera::PrivateData
 {
   // Add any private data members here.
+  gz::sim::Model model;
+  std::string linkName;
+
   std::mutex mutex_;
   gz::transport::Node gz_node;
   std::string topic;
@@ -97,22 +111,115 @@ void UnderwaterCamera::Configure(
 
   this->ros_node_ = std::make_shared<rclcpp::Node>("underwater_camera_node");
 
-  // Grab topic from SDF
-  if (!_sdf->HasElement("topic"))
+  auto rgbdCamera = _ecm.Component<gz::sim::components::RgbdCamera>(_entity);
+  if (!rgbdCamera)
   {
-    this->dataPtr->topic = "/underwater_camera";
-    gzmsg << "Camera topic set to default: " << this->dataPtr->topic << std::endl;
+    gzerr << "UnderwaterCamera plugin should be attached to a rgbd_camera sesnsor. "
+          << "Failed to initialize." << std::endl;
+    return;
   }
-  else
+
+  // get world entity
+  auto worldEntity = _ecm.EntityByComponents(gz::sim::components::World());
+  auto worldName = _ecm.Component<gz::sim::components::Name>(worldEntity)->Data();
+
+  auto sensorSdf = rgbdCamera->Data();
+  this->dataPtr->topic = sensorSdf.Topic();
+
+  if (sensorSdf.CameraSensor() == nullptr)
   {
-    this->dataPtr->topic = _sdf->Get<std::string>("topic");
-    gzmsg << "Topic: " << this->dataPtr->image_topic << std::endl;
+    gzerr << "Attempting to a load an RGBD Camera sensor, but received "
+          << "a null sensor." << std::endl;
+    return;
+  }
+
+  if (this->dataPtr->topic.empty())
+  {
+    auto scoped = gz::sim::scopedName(_entity, _ecm);
+    this->dataPtr->topic = "/world/" + worldName + "/" + scoped;
   }
 
   this->dataPtr->image_topic = this->dataPtr->topic + "/image";
   this->dataPtr->depth_image_topic = this->dataPtr->topic + "/depth_image";
   this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image";
-  this->dataPtr->camera_info_topic = this->dataPtr->topic + "/camera_info";
+
+  sdf::Camera * cameraSdf = sensorSdf.CameraSensor();
+
+  // get camera intrinsics
+  this->dataPtr->width = cameraSdf->ImageWidth();
+  this->dataPtr->height = cameraSdf->ImageHeight();
+
+  if (this->dataPtr->width == 0 || this->dataPtr->height == 0)
+  {
+    gzerr << "Camera image size is zero" << std::endl;
+    return;
+  }
+
+  this->dataPtr->min_range = cameraSdf->NearClip();
+  this->dataPtr->max_range = cameraSdf->FarClip();
+
+  // Check if min_range and max_range are valid
+  if (this->dataPtr->min_range == 0 || this->dataPtr->max_range == 0)
+  {
+    gzerr << "Invalid min_range or max_range" << std::endl;
+    return;
+  }
+
+  this->dataPtr->fx = cameraSdf->LensIntrinsicsFx();
+  this->dataPtr->fy = cameraSdf->LensIntrinsicsFy();
+  this->dataPtr->cx = cameraSdf->LensIntrinsicsCx();
+  this->dataPtr->cy = cameraSdf->LensIntrinsicsCy();
+
+  // Check if fx and fy are non-zero to avoid division by zero
+  if (this->dataPtr->fx == 0 || this->dataPtr->fy == 0)
+  {
+    gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl;
+    return;
+  }
+
+  // Check if cx, cy are zero. If so, set them to the center of the image
+  if (this->dataPtr->cx == 0)
+  {
+    this->dataPtr->cx = this->dataPtr->width / 2;
+  }
+  if (this->dataPtr->cy == 0)
+  {
+    this->dataPtr->cy = this->dataPtr->height / 2;
+  }
+
+  gzmsg << "Camera intrinsics: fx=" << this->dataPtr->fx << ", fy=" << this->dataPtr->fy
+        << ", cx=" << this->dataPtr->cx << ", cy=" << this->dataPtr->cy << std::endl;
+
+  gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height
+        << std::endl;
+
+  gzmsg << "Min range: " << this->dataPtr->min_range << ", Max range: " << this->dataPtr->max_range
+        << std::endl;
+
+  // Free previous LUT memory if it was already allocated
+  if (this->dataPtr->depth2rangeLUT)
+  {
+    delete[] this->dataPtr->depth2rangeLUT;
+  }
+
+  // Allocate memory for the new LUT
+  this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height];
+  float * lutPtr = this->dataPtr->depth2rangeLUT;
+
+  // Fill depth2range LUT
+  for (int v = 0; v < this->dataPtr->height; v++)
+  {
+    double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy;
+    for (int u = 0; u < this->dataPtr->width; u++)
+    {
+      double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx;
+      // Precompute the per-pixel factor in the following formula:
+      // range = || (x, y, z) ||_2
+      // range = || z * (x/z, y/z, 1.0) ||_2
+      // range = z * || (x/z, y/z, 1.0) ||_2
+      *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z);
+    }
+  }
 
   if (!_sdf->HasElement("attenuationR"))
   {
@@ -168,36 +275,12 @@ void UnderwaterCamera::Configure(
     this->dataPtr->background[2] = (unsigned char)_sdf->Get<int>("backgroundB");
   }
 
-  if (!_sdf->HasElement("min_range"))
-  {
-    this->dataPtr->min_range = 0.01f;
-  }
-  else
-  {
-    this->dataPtr->min_range = _sdf->Get<float>("min_range");
-  }
-
-  if (!_sdf->HasElement("max_range"))
-  {
-    this->dataPtr->max_range = 10.0f;
-  }
-  else
-  {
-    this->dataPtr->max_range = _sdf->Get<float>("max_range");
-  }
-
   // Gazebo camera subscriber
   std::function<void(const gz::msgs::Image &)> camera_callback =
     std::bind(&UnderwaterCamera::CameraCallback, this, std::placeholders::_1);
 
   this->dataPtr->gz_node.Subscribe(this->dataPtr->image_topic, camera_callback);
 
-  // Gazebo camera info subscriber
-  std::function<void(const gz::msgs::CameraInfo &)> camera_info_callback =
-    std::bind(&UnderwaterCamera::CameraInfoCallback, this, std::placeholders::_1);
-
-  this->dataPtr->gz_node.Subscribe(this->dataPtr->camera_info_topic, camera_info_callback);
-
   // Gazebo depth image subscriber
   std::function<void(const gz::msgs::Image &)> depth_callback =
     std::bind(&UnderwaterCamera::DepthImageCallback, this, std::placeholders::_1);
@@ -209,74 +292,6 @@ void UnderwaterCamera::Configure(
     this->dataPtr->simulated_image_topic, 1);
 }
 
-void UnderwaterCamera::CameraInfoCallback(const gz::msgs::CameraInfo & msg)
-{
-  std::lock_guard<std::mutex> lock(this->dataPtr->mutex_);
-
-  gzmsg << "dave_gz_sensor_plugins::UnderwaterCamera::CameraInfoCallback" << std::endl;
-
-  if (msg.width() == 0 || msg.height() == 0)
-  {
-    gzerr << "CameraInfo is empty" << std::endl;
-    return;
-  }
-
-  this->dataPtr->width = msg.width();
-  this->dataPtr->height = msg.height();
-
-  // Check if intrinsics are correctly provided
-  if (msg.intrinsics().k().size() < 9)
-  {
-    gzerr << "Invalid camera intrinsics" << std::endl;
-    return;
-  }
-
-  this->dataPtr->fx = msg.intrinsics().k().data()[0];
-  this->dataPtr->fy = msg.intrinsics().k().data()[4];
-  this->dataPtr->cx = msg.intrinsics().k().data()[2];
-  this->dataPtr->cy = msg.intrinsics().k().data()[5];
-
-  // Check if fx and fy are non-zero to avoid division by zero
-  if (this->dataPtr->fx == 0 || this->dataPtr->fy == 0)
-  {
-    gzerr << "Camera intrinsics have zero focal length (fx or fy)" << std::endl;
-    return;
-  }
-
-  // print camera intrinsics
-  gzmsg << "Camera intrinsics: fx=" << this->dataPtr->fx << ", fy=" << this->dataPtr->fy
-        << ", cx=" << this->dataPtr->cx << ", cy=" << this->dataPtr->cy << std::endl;
-
-  // print image size
-  gzmsg << "Image size: width=" << this->dataPtr->width << ", height=" << this->dataPtr->height
-        << std::endl;
-
-  // Free previous LUT memory if it was already allocated
-  if (this->dataPtr->depth2rangeLUT)
-  {
-    delete[] this->dataPtr->depth2rangeLUT;
-  }
-
-  // Allocate memory for the new LUT
-  this->dataPtr->depth2rangeLUT = new float[this->dataPtr->width * this->dataPtr->height];
-  float * lutPtr = this->dataPtr->depth2rangeLUT;
-
-  // Fill depth2range LUT
-  for (int v = 0; v < this->dataPtr->height; v++)
-  {
-    double y_z = (v - this->dataPtr->cy) / this->dataPtr->fy;
-    for (int u = 0; u < this->dataPtr->width; u++)
-    {
-      double x_z = (u - this->dataPtr->cx) / this->dataPtr->fx;
-      // Precompute the per-pixel factor in the following formula:
-      // range = || (x, y, z) ||_2
-      // range = || z * (x/z, y/z, 1.0) ||_2
-      // range = z * || (x/z, y/z, 1.0) ||_2
-      *(lutPtr++) = sqrt(1.0 + x_z * x_z + y_z * y_z);
-    }
-  }
-}
-
 cv::Mat UnderwaterCamera::ConvertGazeboToOpenCV(const gz::msgs::Image & gz_image)
 {
   int cv_type;
diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf
index 7a4de195..7ae99d90 100644
--- a/models/dave_sensor_models/description/underwater_camera/model.sdf
+++ b/models/dave_sensor_models/description/underwater_camera/model.sdf
@@ -42,24 +42,20 @@
             <far>10.0</far>
           </clip>
         </camera>
+        <plugin
+          filename="UnderwaterCamera"
+          name="dave_gz_sensor_plugins::UnderwaterCamera">
+          <attenuationR>0.8</attenuationR>
+          <attenuationG>0.5</attenuationG>
+          <attenuationB>0.2</attenuationB>
+          <!-- Murky Coastal Waters -->
+          <backgroundR>85</backgroundR>
+          <backgroundG>107</backgroundG>
+          <backgroundB>47</backgroundB>
+        </plugin>
       </sensor>
     </link>
     <static>true</static>
 
-    <plugin
-      filename="UnderwaterCamera"
-      name="dave_gz_sensor_plugins::UnderwaterCamera">
-      <topic>underwater_camera</topic>
-      <min_range>0.1</min_range>
-      <max_range>10.0</max_range>
-      <attenuationR>0.8</attenuationR>
-      <attenuationG>0.5</attenuationG>
-      <attenuationB>0.2</attenuationB>
-      <!-- Murky Coastal Waters -->
-      <backgroundR>85</backgroundR>
-      <backgroundG>107</backgroundG>
-      <backgroundB>47</backgroundB>
-    </plugin>
-
   </model>
 </sdf>
diff --git a/models/dave_worlds/worlds/camera_tutorial.world b/models/dave_worlds/worlds/camera_tutorial.world
index def0a581..3b4be54e 100644
--- a/models/dave_worlds/worlds/camera_tutorial.world
+++ b/models/dave_worlds/worlds/camera_tutorial.world
@@ -91,6 +91,257 @@
       <cast_shadows>false</cast_shadows>
     </light>
 
+    <gui fullscreen="0">
+			<!-- 3D scene -->
+			<plugin filename="MinimalScene" name="3D View">
+				<gz-gui>
+					<title>3D View</title>
+					<property type="bool" key="showTitleBar">false</property>
+					<property type="string" key="state">docked</property>
+				</gz-gui>
+
+				<engine>ogre2</engine>
+				<scene>scene</scene>
+				<ambient_light>0.4 0.4 0.4</ambient_light>
+				<background_color>0.8 0.8 0.8</background_color>
+				<camera_pose>7.5 5.0 -90 0 0.40 -1.57</camera_pose>
+				<camera_clip>
+					<near>0.25</near>
+					<far>2500</far>
+				</camera_clip>
+			</plugin>
+
+			<!-- Plugins that add functionality to the scene -->
+			<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
+				<gz-gui>
+					<property key="state" type="string">floating</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="GzSceneManager" name="Scene Manager">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="InteractiveViewControl" name="Interactive view control">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="CameraTracking" name="Camera Tracking">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="MarkerManager" name="Marker manager">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="SelectEntities" name="Select Entities">
+				<gz-gui>
+					<anchors target="Select entities">
+						<line own="right" target="right" />
+						<line own="top" target="top" />
+					</anchors>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+			<plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+
+			<plugin filename="Spawn" name="Spawn Entities">
+				<gz-gui>
+					<anchors target="Select entities">
+						<line own="right" target="right" />
+						<line own="top" target="top" />
+					</anchors>
+					<property key="resizable" type="bool">false</property>
+					<property key="width" type="double">5</property>
+					<property key="height" type="double">5</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- World control -->
+			<plugin filename="WorldControl" name="World control">
+				<gz-gui>
+					<title>World control</title>
+					<property type="bool" key="showTitleBar">false</property>
+					<property type="bool" key="resizable">false</property>
+					<property type="double" key="height">72</property>
+					<property type="double" key="width">121</property>
+					<property type="double" key="z">1</property>
+
+					<property type="string" key="state">floating</property>
+					<anchors target="3D View">
+						<line own="left" target="left" />
+						<line own="bottom" target="bottom" />
+					</anchors>
+				</gz-gui>
+
+				<play_pause>true</play_pause>
+				<step>true</step>
+				<start_paused>true</start_paused>
+				<use_event>true</use_event>
+
+			</plugin>
+
+			<!-- World statistics -->
+			<plugin filename="WorldStats" name="World stats">
+				<gz-gui>
+					<title>World stats</title>
+					<property type="bool" key="showTitleBar">false</property>
+					<property type="bool" key="resizable">false</property>
+					<property type="double" key="height">110</property>
+					<property type="double" key="width">290</property>
+					<property type="double" key="z">1</property>
+
+					<property type="string" key="state">floating</property>
+					<anchors target="3D View">
+						<line own="right" target="right" />
+						<line own="bottom" target="bottom" />
+					</anchors>
+				</gz-gui>
+
+				<sim_time>true</sim_time>
+				<real_time>true</real_time>
+				<real_time_factor>true</real_time_factor>
+				<iterations>true</iterations>
+			</plugin>
+
+			<!-- Insert simple shapes -->
+			<plugin filename="Shapes" name="Shapes">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="x" type="double">0</property>
+					<property key="y" type="double">0</property>
+					<property key="width" type="double">250</property>
+					<property key="height" type="double">50</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+					<property key="cardBackground" type="string">#666666</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- Insert lights -->
+			<plugin filename="Lights" name="Lights">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="x" type="double">250</property>
+					<property key="y" type="double">0</property>
+					<property key="width" type="double">150</property>
+					<property key="height" type="double">50</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+					<property key="cardBackground" type="string">#666666</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- Translate / rotate -->
+			<plugin filename="TransformControl" name="Transform control">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="x" type="double">0</property>
+					<property key="y" type="double">50</property>
+					<property key="width" type="double">250</property>
+					<property key="height" type="double">50</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+					<property key="cardBackground" type="string">#777777</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- Screenshot -->
+			<plugin filename="Screenshot" name="Screenshot">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="x" type="double">250</property>
+					<property key="y" type="double">50</property>
+					<property key="width" type="double">50</property>
+					<property key="height" type="double">50</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+					<property key="cardBackground" type="string">#777777</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- Video recorder -->
+			<plugin filename="VideoRecorder" name="VideoRecorder">
+				<gz-gui>
+					<property key="resizable" type="bool">false</property>
+					<property key="x" type="double">300</property>
+					<property key="y" type="double">50</property>
+					<property key="width" type="double">50</property>
+					<property key="height" type="double">50</property>
+					<property key="state" type="string">floating</property>
+					<property key="showTitleBar" type="bool">false</property>
+					<property key="cardBackground" type="string">#777777</property>
+				</gz-gui>
+
+				<record_video>
+					<use_sim_time>true</use_sim_time>
+					<lockstep>false</lockstep>
+					<bitrate>4000000</bitrate>
+				</record_video>
+			</plugin>
+
+			<!-- Inspector -->
+			<plugin filename="ComponentInspector" name="Component inspector">
+				<gz-gui>
+					<property type="string" key="state">docked_collapsed</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- Entity tree -->
+			<plugin filename="EntityTree" name="Entity tree">
+				<gz-gui>
+					<property type="string" key="state">docked_collapsed</property>
+				</gz-gui>
+			</plugin>
+
+			<!-- View angle -->
+			<plugin filename="ViewAngle" name="View angle">
+				<gz-gui>
+					<property type="string" key="state">docked_collapsed</property>
+				</gz-gui>
+			</plugin>
+
+      <!-- Image Display -->
+      <plugin filename="ImageDisplay" name="Image display">
+      </plugin>
+		</gui>
+
 		<!-- Virtual north, east and down frame -->
 		<include>
 			<uri>

From cc295ad95a39c7b9c28c51480891d1dcc0f2ddb1 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Tue, 27 Aug 2024 14:52:13 -0700
Subject: [PATCH 06/11] added dvl, magnetometer to rexrov and required ros_gz
 bridges to the launch file

---
 .../description/rexrov/model.sdf              | 206 ++++++++-
 .../launch/upload_robot.launch.py             |  13 +-
 .../dave_worlds/worlds/dave_ocean_waves.world |   8 +
 models/dave_worlds/worlds/new_dvl.world       | 413 ------------------
 4 files changed, 225 insertions(+), 415 deletions(-)
 delete mode 100644 models/dave_worlds/worlds/new_dvl.world

diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf
index 4e4e5d61..0502e25d 100644
--- a/models/dave_robot_models/description/rexrov/model.sdf
+++ b/models/dave_robot_models/description/rexrov/model.sdf
@@ -71,8 +71,201 @@
         <topic>imu</topic>
       </sensor>
 
+      <sensor name="magnetometer_sensor" type="magnetometer">
+        <always_on>true</always_on>
+        <update_rate>100.0</update_rate>
+        <topic>magnetometer</topic>
+        <magnetometer>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>0.1</stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>0.1</stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>0.1</stddev>
+            </noise>
+          </z>
+        </magnetometer>
+      </sensor>
+
+    </link>
+
+    <link name="dvl_link">
+      <pose>-1.34 0 -0.75 0 0 -1.57</pose>
+      <inertial>
+        <mass>3.5</mass>
+        <inertia>
+          <ixx>0.0195872</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.0195872</iyy>
+          <iyz>0</iyz>
+          <izz>0.0151357</izz>
+        </inertia>
+      </inertial>
+      <velocity_decay/>
+      <gravity>1</gravity>
+      <velocity_decay/>
+
+      <sensor
+        name="nortek_dvl500_300" type="custom"
+        gz:type="dvl">
+        <pose>0 0 0 0 0 0</pose>
+        <always_on>1</always_on>
+        <update_rate>8</update_rate>
+        <topic>/dvl/velocity</topic>
+        <gz:dvl>
+          <type>phased_array</type>
+          <arrangement degrees="true">
+            <beam id="1">
+              <aperture>3</aperture>
+              <rotation>-135</rotation>
+              <tilt>25</tilt>
+            </beam>
+            <beam>
+              <aperture>3</aperture>
+              <rotation>135</rotation>
+              <tilt>25</tilt>
+            </beam>
+            <beam>
+              <aperture>3</aperture>
+              <rotation>45</rotation>
+              <tilt>25</tilt>
+            </beam>
+            <beam>
+              <aperture>3</aperture>
+              <rotation>-45</rotation>
+              <tilt>25</tilt>
+            </beam>
+          </arrangement>
+          <tracking>
+            <water_mass_mode>
+              <when>best</when>
+              <water_velocity>
+                <x>0.</x>
+                <y>0.</y>
+                <z>0.</z>
+              </water_velocity>
+              <boundaries>
+                <near>10.</near>
+                <far>100.</far>
+              </boundaries>
+              <bins>10</bins>
+              <noise type="gaussian">
+                <stddev>0.015</stddev>
+              </noise>
+              <visualize>true</visualize>
+            </water_mass_mode>
+            <bottom_mode>
+              <when>best</when>
+              <noise type="gaussian">
+                <stddev>0.005</stddev>
+              </noise>
+              <visualize>true</visualize>
+            </bottom_mode>
+          </tracking>
+          <maximum_range>200.</maximum_range>
+          <minimum_range>0.3</minimum_range>
+          <!-- ENU to FSD -->
+          <reference_frame>0 0 0 3.14 0 -1.57</reference_frame>
+        </gz:dvl>
+      </sensor>
+
+      <self_collide>0</self_collide>
+      <enable_wind>0</enable_wind>
+      <kinematic>0</kinematic>
+      <visual name="dvl_link_visual">
+        <pose>0 0 0 0 0 3.14159</pose>
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+            <!-- Mesh is actually for a Nortek DVL1000-4000m for now -->
+            <uri>model://meshes/nortek_dvl500_300/DVL500-300m.dae</uri>
+          </mesh>
+        </geometry>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name="dvl_link_collision">
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose>0 0 0 3.14159 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.093</radius>
+            <length>0.203</length>
+          </cylinder>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
     </link>
 
+    <joint name="dvl_joint" type="fixed">
+      <pose>-1.34 0 -0.75 0 0 -1.57</pose>
+      <parent>base_link</parent>
+      <child>dvl_link</child>
+      <axis>
+        <xyz>1 0 0</xyz>
+        <limit>
+          <effort>-1</effort>
+          <velocity>-1</velocity>
+        </limit>
+      </axis>
+    </joint>
+
     <link name="thruster1">
       <pose>-0.890895 0.334385 0.528822 0 -1.300794 -0.928689</pose>
       <inertial>
@@ -562,6 +755,11 @@
       name="gz::sim::systems::Imu">
     </plugin>
 
+    <plugin
+      filename="gz-sim-magnetometer-system"
+      name="gz::sim::systems::Magnetometer">
+    </plugin>
+
     <plugin
       filename="gz-sim-odometry-publisher-system"
       name="gz::sim::systems::OdometryPublisher">
@@ -597,5 +795,11 @@
       <kPa_per_meter>9.80638</kPa_per_meter>
     </plugin>
 
+    <plugin
+      filename="DVLBridge"
+      name="dave_ros_gz_plugins::DVLBridge">
+      <topic>/dvl/velocity</topic>
+    </plugin>
+
   </model>
-</sdf>
\ No newline at end of file
+</sdf>
diff --git a/models/dave_robot_models/launch/upload_robot.launch.py b/models/dave_robot_models/launch/upload_robot.launch.py
index 5b499add..443acda4 100755
--- a/models/dave_robot_models/launch/upload_robot.launch.py
+++ b/models/dave_robot_models/launch/upload_robot.launch.py
@@ -126,7 +126,18 @@ def generate_launch_description():
         parameters=[{"use_sim_time_time": use_sim_time}],
     )
 
-    nodes = [tf2_spawner, gz_spawner]
+    # Bridge
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "/imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
+            "/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer"
+        ],
+        output="screen",
+    )
+
+    nodes = [tf2_spawner, gz_spawner, bridge]
 
     event_handlers = [
         RegisterEventHandler(
diff --git a/models/dave_worlds/worlds/dave_ocean_waves.world b/models/dave_worlds/worlds/dave_ocean_waves.world
index 920c9f59..e1038384 100644
--- a/models/dave_worlds/worlds/dave_ocean_waves.world
+++ b/models/dave_worlds/worlds/dave_ocean_waves.world
@@ -43,6 +43,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/new_dvl.world b/models/dave_worlds/worlds/new_dvl.world
deleted file mode 100644
index 0d39c147..00000000
--- a/models/dave_worlds/worlds/new_dvl.world
+++ /dev/null
@@ -1,413 +0,0 @@
-<?xml version="1.0"?>
-<!-- Copyright (c) 2016 The UUV Simulator Authors.
-     All rights reserved.
-
-     Licensed under the Apache License, Version 2.0 (the "License");
-     you may not use this file except in compliance with the License.
-     You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-     Unless required by applicable law or agreed to in writing, software
-     distributed under the License is distributed on an "AS IS" BASIS,
-     WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-     See the License for the specific language governing permissions and
-     limitations under the License.
--->
-
-<sdf version="1.4">
-	<world name="dvl_world">
-      <scene>
-      <!-- For turquoise ambient to match particle effect -->
-      <ambient>0.0 1.0 1.0</ambient>
-      <!-- For default gray ambient -->
-      <background>0.0 0.7 0.8</background>
-
-      <grid>false</grid>
-    </scene>
-
-		<physics name="1ms" type="ignored">
-			<max_step_size>0.001</max_step_size>
-			<real_time_factor>1.0</real_time_factor>
-		</physics>
-		<plugin
-			filename="gz-sim-physics-system"
-			name="gz::sim::systems::Physics">
-		</plugin>
-		<plugin
-			filename="gz-sim-user-commands-system"
-			name="gz::sim::systems::UserCommands">
-		</plugin>
-		<plugin
-			filename="gz-sim-scene-broadcaster-system"
-			name="gz::sim::systems::SceneBroadcaster">
-		</plugin>
-		<plugin filename="gz-sim-contact-system"
-				name="gz::sim::systems::Contact">
-		</plugin>
-		<plugin
-			filename="gz-sim-sensors-system"
-			name="gz::sim::systems::Sensors">
-    	</plugin>
-		<plugin
-			filename="gz-sim-dvl-system"
-			name="gz::sim::systems::DopplerVelocityLogSystem">
-		</plugin>
-
-		<!-- Origin placed somewhere in the middle of the North Sea -->
-		<spherical_coordinates>
-			<latitude_deg>56.71897669633431</latitude_deg>
-			<longitude_deg>3.515625</longitude_deg>
-		</spherical_coordinates>
-
-		<plugin filename="SphericalCoords" name="dave_ros_gz_plugins::SphericalCoords"> </plugin>
-
-		<!-- Global Light Source -->
-		<light type="directional" name="sun1">
-			<pose>50 0 150 0 0 0</pose>
-			<diffuse>1 1 1 1</diffuse>
-			<specular>0.1 0.1 0.1 1</specular>
-			<direction>0.3 0.3 -1</direction>
-			<cast_shadows>false</cast_shadows>
-		</light>
-
-		<light type="directional" name="sun_diffuse_4">
-			<pose>20 0 -90 0 0 0</pose>
-			<diffuse>0.9 0.9 0.9 1</diffuse>
-			<specular>0.2 0.2 0.2 1</specular>
-			<direction>-1 0 0</direction>
-			<cast_shadows>false</cast_shadows>
-		</light>
-
-		<!-- Virtual north, east and down frame -->
-		<include>
-			<uri>
-				https://fuel.gazebosim.org/1.0/hmoyen/models/North-East-Down frame
-			</uri>
-			<pose>0 0 0 0 0 0</pose>
-		</include>
-
-		<!-- Sea surface -->
-		<include>
-			<uri>
-				https://fuel.gazebosim.org/1.0/OpenRobotics/models/Waves
-			</uri>
-		</include>
-
-		<include>
-			<uri>https://fuel.gazebosim.org/1.0/hmoyen/models/Sand Heightmap</uri>
-			<pose>0 0 -100 0 0 0</pose>
-		</include>
-
-		<model name="grabbapole">
-			<static>true</static>
-			<pose>21 -1 -98.5 0 0 0</pose>
-			<link name="gabbapole_link">
-				<inertial>
-					<inertia>
-						<ixx>1</ixx>
-						<ixy>0</ixy>
-						<ixz>0</ixz>
-						<iyy>1</iyy>
-						<iyz>0</iyz>
-						<izz>1</izz>
-					</inertia>
-					<mass>1.0</mass>
-				</inertial>
-				<collision name="grabbapole_collision">
-					<geometry>
-						<cylinder>
-							<radius>0.03</radius>
-							<length>3.0</length>
-						</cylinder>
-					</geometry>
-				</collision>
-				<visual name="grabbapole_visual">
-					<geometry>
-						<cylinder>
-							<radius>0.03</radius>
-							<length>3.0</length>
-						</cylinder>
-					</geometry>
-					<material>
-						<script>
-							<name>Gazebo/Grey</name>
-							<uri>file://media/materials/scripts/gazebo.material</uri>
-						</script>
-					</material>
-				</visual>
-			</link>
-		</model>
-
-		<include>
-			<uri>https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase with Inertia</uri>
-			<pose>20 0 -98 0 0 0</pose>
-		</include>
-
-		<include>
-			<uri>https://fuel.gazebosim.org/1.0/hmoyen/models/Sunken Vase Distorted</uri>
-			<pose>22 -0.5 -100 0 0 1.5708</pose>
-		</include>
-
-		<gui fullscreen="0">
-			<!-- 3D scene -->
-			<plugin filename="MinimalScene" name="3D View">
-				<gz-gui>
-					<title>3D View</title>
-					<property type="bool" key="showTitleBar">false</property>
-					<property type="string" key="state">docked</property>
-				</gz-gui>
-
-				<engine>ogre2</engine>
-				<scene>scene</scene>
-				<ambient_light>0.4 0.4 0.4</ambient_light>
-				<background_color>0.8 0.8 0.8</background_color>
-				<camera_pose>18.7 3.77 -97.5 0 0.29 -0.715</camera_pose>
-				<camera_clip>
-					<near>0.25</near>
-					<far>2500</far>
-				</camera_clip>
-			</plugin>
-
-			<!-- Plugins that add functionality to the scene -->
-			<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
-				<gz-gui>
-					<property key="state" type="string">floating</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="GzSceneManager" name="Scene Manager">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="InteractiveViewControl" name="Interactive view control">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="CameraTracking" name="Camera Tracking">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="MarkerManager" name="Marker manager">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="SelectEntities" name="Select Entities">
-				<gz-gui>
-					<anchors target="Select entities">
-						<line own="right" target="right" />
-						<line own="top" target="top" />
-					</anchors>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-			<plugin filename="VisualizationCapabilities" name="Visualization Capabilities">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-
-			<plugin filename="Spawn" name="Spawn Entities">
-				<gz-gui>
-					<anchors target="Select entities">
-						<line own="right" target="right" />
-						<line own="top" target="top" />
-					</anchors>
-					<property key="resizable" type="bool">false</property>
-					<property key="width" type="double">5</property>
-					<property key="height" type="double">5</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- World control -->
-			<plugin filename="WorldControl" name="World control">
-				<gz-gui>
-					<title>World control</title>
-					<property type="bool" key="showTitleBar">false</property>
-					<property type="bool" key="resizable">false</property>
-					<property type="double" key="height">72</property>
-					<property type="double" key="width">121</property>
-					<property type="double" key="z">1</property>
-
-					<property type="string" key="state">floating</property>
-					<anchors target="3D View">
-						<line own="left" target="left" />
-						<line own="bottom" target="bottom" />
-					</anchors>
-				</gz-gui>
-
-				<play_pause>true</play_pause>
-				<step>true</step>
-				<start_paused>true</start_paused>
-				<use_event>true</use_event>
-
-			</plugin>
-
-			<!-- World statistics -->
-			<plugin filename="WorldStats" name="World stats">
-				<gz-gui>
-					<title>World stats</title>
-					<property type="bool" key="showTitleBar">false</property>
-					<property type="bool" key="resizable">false</property>
-					<property type="double" key="height">110</property>
-					<property type="double" key="width">290</property>
-					<property type="double" key="z">1</property>
-
-					<property type="string" key="state">floating</property>
-					<anchors target="3D View">
-						<line own="right" target="right" />
-						<line own="bottom" target="bottom" />
-					</anchors>
-				</gz-gui>
-
-				<sim_time>true</sim_time>
-				<real_time>true</real_time>
-				<real_time_factor>true</real_time_factor>
-				<iterations>true</iterations>
-			</plugin>
-
-			<!-- Insert simple shapes -->
-			<plugin filename="Shapes" name="Shapes">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="x" type="double">0</property>
-					<property key="y" type="double">0</property>
-					<property key="width" type="double">250</property>
-					<property key="height" type="double">50</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-					<property key="cardBackground" type="string">#666666</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- Insert lights -->
-			<plugin filename="Lights" name="Lights">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="x" type="double">250</property>
-					<property key="y" type="double">0</property>
-					<property key="width" type="double">150</property>
-					<property key="height" type="double">50</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-					<property key="cardBackground" type="string">#666666</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- Translate / rotate -->
-			<plugin filename="TransformControl" name="Transform control">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="x" type="double">0</property>
-					<property key="y" type="double">50</property>
-					<property key="width" type="double">250</property>
-					<property key="height" type="double">50</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-					<property key="cardBackground" type="string">#777777</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- Screenshot -->
-			<plugin filename="Screenshot" name="Screenshot">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="x" type="double">250</property>
-					<property key="y" type="double">50</property>
-					<property key="width" type="double">50</property>
-					<property key="height" type="double">50</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-					<property key="cardBackground" type="string">#777777</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- Video recorder -->
-			<plugin filename="VideoRecorder" name="VideoRecorder">
-				<gz-gui>
-					<property key="resizable" type="bool">false</property>
-					<property key="x" type="double">300</property>
-					<property key="y" type="double">50</property>
-					<property key="width" type="double">50</property>
-					<property key="height" type="double">50</property>
-					<property key="state" type="string">floating</property>
-					<property key="showTitleBar" type="bool">false</property>
-					<property key="cardBackground" type="string">#777777</property>
-				</gz-gui>
-
-				<record_video>
-					<use_sim_time>true</use_sim_time>
-					<lockstep>false</lockstep>
-					<bitrate>4000000</bitrate>
-				</record_video>
-			</plugin>
-
-			<!-- Inspector -->
-			<plugin filename="ComponentInspector" name="Component inspector">
-				<gz-gui>
-					<property type="string" key="state">docked_collapsed</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- Entity tree -->
-			<plugin filename="EntityTree" name="Entity tree">
-				<gz-gui>
-					<property type="string" key="state">docked_collapsed</property>
-				</gz-gui>
-			</plugin>
-
-			<!-- View angle -->
-			<plugin filename="ViewAngle" name="View angle">
-				<gz-gui>
-					<property type="string" key="state">docked_collapsed</property>
-				</gz-gui>
-			</plugin>
-
-		</gui>
-
-    <plugin
-      filename="gz-sim-buoyancy-system"
-      name="gz::sim::systems::Buoyancy">
-      <graded_buoyancy>
-        <default_density>1025</default_density>
-        <density_change>
-          <above_depth>0</above_depth>
-          <density>1</density>
-        </density_change>
-      </graded_buoyancy>
-    </plugin>
-
-	</world>
-</sdf>

From 82c6a02951b364a4bd50429b98c2cfb1df65a6dc Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Wed, 28 Aug 2024 22:34:06 -0700
Subject: [PATCH 07/11] added sensor plugins to robot models, created separate
 launch files for each robot's ros_gz bridge, and slightly modified
 sea_pressure sensor plugin

---
 .../src/sea_pressure_sensor.cc                | 20 ++++-
 .../description/glider_slocum/model.sdf       | 69 +++++++++++++++-
 .../description/rexrov/model.sdf              | 79 ++++++++++++-------
 .../launch/glider_slocum.launch.py            | 24 ++++++
 .../dave_robot_models/launch/rexrov.launch.py | 47 +++++++++++
 .../launch/upload_robot.launch.py             | 51 +++++++++---
 6 files changed, 243 insertions(+), 47 deletions(-)
 create mode 100644 models/dave_robot_models/launch/glider_slocum.launch.py
 create mode 100644 models/dave_robot_models/launch/rexrov.launch.py

diff --git a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc
index eccb40d5..df18db0f 100644
--- a/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc
+++ b/gazebo/dave_gz_sensor_plugins/src/sea_pressure_sensor.cc
@@ -43,6 +43,7 @@ struct SubseaPressureSensorPlugin::PrivateData
   rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr ros_depth_estimate_pub;
   std::shared_ptr<rclcpp::Node> rosNode;
   std::string robotNamespace;
+  std::string topic;
   double noiseAmp = 0.0;
   double noiseSigma = 3.0;
   double inferredDepth = 0.0;
@@ -83,6 +84,15 @@ void SubseaPressureSensorPlugin::Configure(
   }
   this->dataPtr->robotNamespace = _sdf->Get<std::string>("namespace");
 
+  if (_sdf->HasElement("topic"))
+  {
+    this->dataPtr->topic = _sdf->Get<std::string>("topic");
+  }
+  else
+  {
+    this->dataPtr->topic = "sea_pressure";
+  }
+
   if (_sdf->HasElement("saturation"))
   {
     this->dataPtr->saturation = _sdf->Get<double>("saturation");
@@ -124,18 +134,20 @@ void SubseaPressureSensorPlugin::Configure(
 
   this->dataPtr->ros_pressure_sensor_pub =
     this->rosNode->create_publisher<sensor_msgs::msg::FluidPressure>(
-      this->dataPtr->robotNamespace + "/" + "Pressure", rclcpp::QoS(10).reliable());
+      "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic,
+      rclcpp::QoS(10).reliable());
 
   if (this->dataPtr->estimateDepth)
   {
     this->dataPtr->ros_depth_estimate_pub =
       this->rosNode->create_publisher<geometry_msgs::msg::PointStamped>(
-        this->dataPtr->robotNamespace + "/" + "Pressure_depth", rclcpp::QoS(10).reliable());
+        "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic + "_depth",
+        rclcpp::QoS(10).reliable());
   }
 
   this->dataPtr->gz_pressure_sensor_pub =
     this->dataPtr->gazeboNode->Advertise<gz::msgs::FluidPressure>(
-      this->dataPtr->robotNamespace + "/" + "Pressure");
+      "model/" + this->dataPtr->robotNamespace + "/" + this->dataPtr->topic);
 }
 //////////////////////////////////////////
 
@@ -242,4 +254,4 @@ void SubseaPressureSensorPlugin::PostUpdate(
   }
 }
 
-}  // namespace dave_gz_sensor_plugins
\ No newline at end of file
+}  // namespace dave_gz_sensor_plugins
diff --git a/models/dave_robot_models/description/glider_slocum/model.sdf b/models/dave_robot_models/description/glider_slocum/model.sdf
index 223c30a6..ac542216 100644
--- a/models/dave_robot_models/description/glider_slocum/model.sdf
+++ b/models/dave_robot_models/description/glider_slocum/model.sdf
@@ -39,15 +39,34 @@
           </box>
         </geometry>
       </collision>
+    </link>
 
+    <link name="imu_link">
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.015</mass>
+        <inertia>
+          <ixx>0.00001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.00001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.00001</izz>
+        </inertia>
+      </inertial>
       <sensor name="imu_sensor" type="imu">
         <always_on>true</always_on>
-        <update_rate>1000.0</update_rate>
-        <topic>imu</topic>
+        <update_rate>50.0</update_rate>
+        <topic>/model/glider_slocum/imu</topic>
       </sensor>
-
     </link>
 
+    <joint name="imu_joint" type="fixed">
+      <pose>0 0 0 0 0 0</pose>
+      <parent>base_link</parent>
+      <child>imu_link</child>
+    </joint>
+
     <link name='battery_link'>
       <inertial>
         <pose>0 0 0 0 0 0</pose>
@@ -88,6 +107,33 @@
       <start_on_motion>true</start_on_motion>
     </plugin>
 
+    <link name="navsat_link">
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.001</mass>
+        <inertia>
+          <ixx>0.00001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.00001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.00001</izz>
+        </inertia>
+      </inertial>
+
+      <sensor name="navsat_sensor" type="navsat">
+        <always_on>true</always_on>
+        <update_rate>30.0</update_rate>
+        <topic>/model/glider_slocum/navsat</topic>
+      </sensor>
+    </link>
+
+    <joint name="navsat_joint" type="fixed">
+      <pose>0 0 0 0 0 0</pose>
+      <parent>base_link</parent>
+      <child>navsat_link</child>
+    </joint>
+
     <link name="propeller_link">
       <visual name="propeller_link_visual">
         <pose>0 0 0 0 0 0</pose>
@@ -179,6 +225,11 @@
       name="gz::sim::systems::Imu">
     </plugin>
 
+    <plugin
+      filename="gz-sim-navsat-system"
+      name="gz::sim::systems::NavSat">
+    </plugin>
+
     <plugin
       filename="gz-sim-odometry-publisher-system"
       name="gz::sim::systems::OdometryPublisher">
@@ -193,6 +244,18 @@
       name="gz::sim::systems::JointStatePublisher">
     </plugin>
 
+    <!-- Sea Pressure Sensor Plugin -->
+    <plugin
+      filename="sea_pressure_sensor"
+      name=" dave_gz_sensor_plugins::SubseaPressureSensorPlugin">
+      <namespace>glider_slocum</namespace>
+      <topic>sea_pressure</topic>
+      <update_rate>10</update_rate>
+      <noise_sigma>3.0</noise_sigma>
+      <estimate_depth_on>true</estimate_depth_on>
+      <standard_pressure>101.325</standard_pressure>
+      <kPa_per_meter>9.80638</kPa_per_meter>
+    </plugin>
 
   </model>
 </sdf>
diff --git a/models/dave_robot_models/description/rexrov/model.sdf b/models/dave_robot_models/description/rexrov/model.sdf
index 0502e25d..4d383d2c 100644
--- a/models/dave_robot_models/description/rexrov/model.sdf
+++ b/models/dave_robot_models/description/rexrov/model.sdf
@@ -64,41 +64,81 @@
           </box>
         </geometry>
       </collision>
+    </link>
 
-      <sensor name="imu_sensor" type="imu">
-        <always_on>true</always_on>
-        <update_rate>1000.0</update_rate>
-        <topic>imu</topic>
-      </sensor>
+    <link name="magnetometer_link">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial>
+        <mass>0.015</mass>
+        <inertia>
+          <ixx>0.00001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.00001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.00001</izz>
+        </inertia>
+      </inertial>
 
       <sensor name="magnetometer_sensor" type="magnetometer">
         <always_on>true</always_on>
-        <update_rate>100.0</update_rate>
-        <topic>magnetometer</topic>
+        <update_rate>50.0</update_rate>
+        <topic>/model/rexrov/magnetometer</topic>
         <magnetometer>
           <x>
             <noise type="gaussian">
               <mean>0.0</mean>
-              <stddev>0.1</stddev>
+              <stddev>1.0</stddev>
             </noise>
           </x>
           <y>
             <noise type="gaussian">
               <mean>0.0</mean>
-              <stddev>0.1</stddev>
+              <stddev>1.0</stddev>
             </noise>
           </y>
           <z>
             <noise type="gaussian">
               <mean>0.0</mean>
-              <stddev>0.1</stddev>
+              <stddev>1.4</stddev>
             </noise>
           </z>
         </magnetometer>
       </sensor>
+    </link>
 
+    <joint name="magnetometer_joint" type="fixed">
+      <pose>0 0 0 0 0 0</pose>
+      <parent>base_link</parent>
+      <child>magnetometer_link</child>
+    </joint>
+
+    <link name="imu_link">
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.015</mass>
+        <inertia>
+          <ixx>0.00001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.00001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.00001</izz>
+        </inertia>
+      </inertial>
+      <sensor name="imu_sensor" type="imu">
+        <always_on>true</always_on>
+        <update_rate>50.0</update_rate>
+        <topic>/model/rexrov/imu</topic>
+      </sensor>
     </link>
 
+    <joint name="imu_joint" type="fixed">
+      <pose>0 0 0 0 0 0</pose>
+      <parent>base_link</parent>
+      <child>imu_link</child>
+    </joint>
+
     <link name="dvl_link">
       <pose>-1.34 0 -0.75 0 0 -1.57</pose>
       <inertial>
@@ -122,7 +162,7 @@
         <pose>0 0 0 0 0 0</pose>
         <always_on>1</always_on>
         <update_rate>8</update_rate>
-        <topic>/dvl/velocity</topic>
+        <topic>/model/rexrov/dvl/velocity</topic>
         <gz:dvl>
           <type>phased_array</type>
           <arrangement degrees="true">
@@ -257,13 +297,6 @@
       <pose>-1.34 0 -0.75 0 0 -1.57</pose>
       <parent>base_link</parent>
       <child>dvl_link</child>
-      <axis>
-        <xyz>1 0 0</xyz>
-        <limit>
-          <effort>-1</effort>
-          <velocity>-1</velocity>
-        </limit>
-      </axis>
     </joint>
 
     <link name="thruster1">
@@ -772,14 +805,6 @@
     <plugin
       filename="gz-sim-joint-state-publisher-system"
       name="gz::sim::systems::JointStatePublisher">
-      <joint_name>thruster1_joint</joint_name>
-      <joint_name>thruster2_joint</joint_name>
-      <joint_name>thruster3_joint</joint_name>
-      <joint_name>thruster4_joint</joint_name>
-      <joint_name>thruster5_joint</joint_name>
-      <joint_name>thruster6_joint</joint_name>
-      <joint_name>thruster7_joint</joint_name>
-      <joint_name>thruster8_joint</joint_name>
     </plugin>
 
     <!-- Sea Pressure Sensor Plugin -->
@@ -798,7 +823,7 @@
     <plugin
       filename="DVLBridge"
       name="dave_ros_gz_plugins::DVLBridge">
-      <topic>/dvl/velocity</topic>
+      <topic>/model/rexrov/dvl/velocity</topic>
     </plugin>
 
   </model>
diff --git a/models/dave_robot_models/launch/glider_slocum.launch.py b/models/dave_robot_models/launch/glider_slocum.launch.py
new file mode 100644
index 00000000..59ba3060
--- /dev/null
+++ b/models/dave_robot_models/launch/glider_slocum.launch.py
@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    glider_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=[
+            "/model/glider_slocum/battery/battery/state@sensor_msgs/msg/BatteryState@gz.msgs.BatteryState",
+            "/model/glider_slocum/joint/propeller_joint/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double",
+            "/model/glider_slocum/joint/propeller_joint/ang_vel@std_msgs/msg/Float64@gz.msgs.Double",
+            "/model/glider_slocum/joint/propeller_joint/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean",
+            "/model/glider_slocum/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry",
+            "/model/glider_slocum/odometry_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
+            "/model/glider_slocum/pose@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V",
+            "/model/glider_slocum/navsat@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat",
+            "/model/glider_slocum/imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([glider_bridge])
diff --git a/models/dave_robot_models/launch/rexrov.launch.py b/models/dave_robot_models/launch/rexrov.launch.py
new file mode 100644
index 00000000..59198c60
--- /dev/null
+++ b/models/dave_robot_models/launch/rexrov.launch.py
@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    thruster_joints = [
+        "/model/rexrov/joint/thruster1_joint",
+        "/model/rexrov/joint/thruster2_joint",
+        "/model/rexrov/joint/thruster3_joint",
+        "/model/rexrov/joint/thruster4_joint",
+        "/model/rexrov/joint/thruster5_joint",
+        "/model/rexrov/joint/thruster6_joint",
+        "/model/rexrov/joint/thruster7_joint",
+        "/model/rexrov/joint/thruster8_joint",
+    ]
+
+    rexrov_arguments = (
+        [
+            f"{joint}/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double"
+            for joint in thruster_joints
+        ]
+        + [
+            f"{joint}/ang_vel@std_msgs/msg/Float64@gz.msgs.Double"
+            for joint in thruster_joints
+        ]
+        + [
+            f"{joint}/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean"
+            for joint in thruster_joints
+        ]
+        + [
+            "/model/rexrov/odometry@nav_msgs/msg/Odometry@gz.msgs.Odometry",
+            "/model/rexrov/odometry_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance",
+            "/model/rexrov/pose@geometry_msgs/msg/PoseArray@gz.msgs.Pose_V",
+            "/model/rexrov/imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
+            "/model/rexrov/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer",
+        ]
+    )
+
+    rexrov_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=rexrov_arguments,
+        output="screen",
+    )
+
+    return LaunchDescription([rexrov_bridge])
diff --git a/models/dave_robot_models/launch/upload_robot.launch.py b/models/dave_robot_models/launch/upload_robot.launch.py
index 443acda4..b22c0543 100755
--- a/models/dave_robot_models/launch/upload_robot.launch.py
+++ b/models/dave_robot_models/launch/upload_robot.launch.py
@@ -1,10 +1,20 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler, LogInfo
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.actions import (
+    DeclareLaunchArgument,
+    RegisterEventHandler,
+    LogInfo,
+    IncludeLaunchDescription,
+)
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    EqualsSubstitution,
+)
 from launch.conditions import IfCondition
 from launch_ros.substitutions import FindPackageShare
 from launch.event_handlers import OnProcessExit
 from launch_ros.actions import Node
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
@@ -126,18 +136,33 @@ def generate_launch_description():
         parameters=[{"use_sim_time_time": use_sim_time}],
     )
 
-    # Bridge
-    bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
-        arguments=[
-            "/imu@sensor_msgs/msg/Imu@gz.msgs.IMU",
-            "/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer"
-        ],
-        output="screen",
+    nodes = [tf2_spawner, gz_spawner]
+
+    # Include the Rexrov launch file if the namespace is "rexrov"
+    rexrov_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [FindPackageShare("dave_robot_models"), "launch", "rexrov.launch.py"]
+            )
+        ),
+        condition=IfCondition(EqualsSubstitution(namespace, "rexrov")),
+    )
+
+    # Include the Glider launch file if the namespace is "glider_slocum"
+    glider_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("dave_robot_models"),
+                    "launch",
+                    "glider_slocum.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(EqualsSubstitution(namespace, "glider_slocum")),
     )
 
-    nodes = [tf2_spawner, gz_spawner, bridge]
+    include = [rexrov_launch, glider_launch]
 
     event_handlers = [
         RegisterEventHandler(
@@ -145,4 +170,4 @@ def generate_launch_description():
         )
     ]
 
-    return LaunchDescription(args + nodes + event_handlers)
+    return LaunchDescription(args + nodes + event_handlers + include)

From c293aa4c8a29db0dee5720407a67e9b24cd8c2f9 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Wed, 28 Aug 2024 22:40:08 -0700
Subject: [PATCH 08/11] added sensor and dvl plugins to all world files

---
 models/dave_robot_models/launch/rexrov.launch.py       | 10 ++--------
 models/dave_worlds/worlds/dave_Santorini.world         |  8 ++++++++
 models/dave_worlds/worlds/dave_bimanual_example.world  |  8 ++++++++
 models/dave_worlds/worlds/dave_electrical_mating.world |  8 ++++++++
 models/dave_worlds/worlds/dave_graded_seabed.world     |  8 ++++++++
 models/dave_worlds/worlds/dave_integrated.world        |  9 ++++++++-
 models/dave_worlds/worlds/dave_ocean_models.world      |  8 ++++++++
 .../worlds/dave_ocean_waves_mossy_ground.world         |  8 ++++++++
 .../worlds/dave_ocean_waves_transient_current.world    |  9 ++++++++-
 models/dave_worlds/worlds/dave_plug_and_socket.world   |  8 ++++++++
 10 files changed, 74 insertions(+), 10 deletions(-)

diff --git a/models/dave_robot_models/launch/rexrov.launch.py b/models/dave_robot_models/launch/rexrov.launch.py
index 59198c60..39e00c0d 100644
--- a/models/dave_robot_models/launch/rexrov.launch.py
+++ b/models/dave_robot_models/launch/rexrov.launch.py
@@ -16,14 +16,8 @@ def generate_launch_description():
     ]
 
     rexrov_arguments = (
-        [
-            f"{joint}/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double"
-            for joint in thruster_joints
-        ]
-        + [
-            f"{joint}/ang_vel@std_msgs/msg/Float64@gz.msgs.Double"
-            for joint in thruster_joints
-        ]
+        [f"{joint}/cmd_thrust@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints]
+        + [f"{joint}/ang_vel@std_msgs/msg/Float64@gz.msgs.Double" for joint in thruster_joints]
         + [
             f"{joint}/enable_deadband@std_msgs/msg/Bool@gz.msgs.Boolean"
             for joint in thruster_joints
diff --git a/models/dave_worlds/worlds/dave_Santorini.world b/models/dave_worlds/worlds/dave_Santorini.world
index 7d172e8a..f6704ceb 100644
--- a/models/dave_worlds/worlds/dave_Santorini.world
+++ b/models/dave_worlds/worlds/dave_Santorini.world
@@ -43,6 +43,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed at the center of the tile -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/dave_bimanual_example.world b/models/dave_worlds/worlds/dave_bimanual_example.world
index 56287925..b6cc0cc2 100644
--- a/models/dave_worlds/worlds/dave_bimanual_example.world
+++ b/models/dave_worlds/worlds/dave_bimanual_example.world
@@ -36,6 +36,14 @@
 		<plugin filename="gz-sim-contact-system"
 				name="gz::sim::systems::Contact">
 		</plugin>
+		<plugin
+			filename="gz-sim-sensors-system"
+			name="gz::sim::systems::Sensors">
+			</plugin>
+		<plugin
+			filename="gz-sim-dvl-system"
+			name="gz::sim::systems::DopplerVelocityLogSystem">
+		</plugin>
 		<!-- Origin placed somewhere in the middle of the North Sea -->
 		<spherical_coordinates>
 			<latitude_deg>56.71897669633431</latitude_deg>
diff --git a/models/dave_worlds/worlds/dave_electrical_mating.world b/models/dave_worlds/worlds/dave_electrical_mating.world
index 18434f24..b724b5a2 100644
--- a/models/dave_worlds/worlds/dave_electrical_mating.world
+++ b/models/dave_worlds/worlds/dave_electrical_mating.world
@@ -34,6 +34,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/dave_graded_seabed.world b/models/dave_worlds/worlds/dave_graded_seabed.world
index 1b4690a8..557573bb 100644
--- a/models/dave_worlds/worlds/dave_graded_seabed.world
+++ b/models/dave_worlds/worlds/dave_graded_seabed.world
@@ -41,6 +41,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/dave_integrated.world b/models/dave_worlds/worlds/dave_integrated.world
index dde9496d..c4fccf98 100644
--- a/models/dave_worlds/worlds/dave_integrated.world
+++ b/models/dave_worlds/worlds/dave_integrated.world
@@ -36,7 +36,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
-
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/dave_ocean_models.world b/models/dave_worlds/worlds/dave_ocean_models.world
index c1c293e0..724914e7 100644
--- a/models/dave_worlds/worlds/dave_ocean_models.world
+++ b/models/dave_worlds/worlds/dave_ocean_models.world
@@ -43,6 +43,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
diff --git a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world
index 88beb352..8bccf334 100644
--- a/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world
+++ b/models/dave_worlds/worlds/dave_ocean_waves_mossy_ground.world
@@ -34,6 +34,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
     <scene>
       <ambient>0.01 0.01 0.01 1.0</ambient>
       <sky>
diff --git a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world
index 1fd5c19f..215c309b 100644
--- a/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world
+++ b/models/dave_worlds/worlds/dave_ocean_waves_transient_current.world
@@ -43,7 +43,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
-
+		<plugin
+			filename="gz-sim-sensors-system"
+			name="gz::sim::systems::Sensors">
+    	</plugin>
+		<plugin
+			filename="gz-sim-dvl-system"
+			name="gz::sim::systems::DopplerVelocityLogSystem">
+		</plugin>
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>
       <latitude_deg>56.71897669633431</latitude_deg>
diff --git a/models/dave_worlds/worlds/dave_plug_and_socket.world b/models/dave_worlds/worlds/dave_plug_and_socket.world
index bdb3b1f5..c380a757 100644
--- a/models/dave_worlds/worlds/dave_plug_and_socket.world
+++ b/models/dave_worlds/worlds/dave_plug_and_socket.world
@@ -34,6 +34,14 @@
       filename="gz-sim-scene-broadcaster-system"
       name="gz::sim::systems::SceneBroadcaster">
     </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      </plugin>
+    <plugin
+      filename="gz-sim-dvl-system"
+      name="gz::sim::systems::DopplerVelocityLogSystem">
+    </plugin>
 
     <!-- Origin placed somewhere in the middle of the North Sea  -->
     <spherical_coordinates>

From 40cd90344923c93aec11e772705643f4bc29fa6e Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Fri, 30 Aug 2024 01:06:45 -0700
Subject: [PATCH 09/11] minor changes to launch arguments and renamed robot
 launch file

---
 .../dave_demos/launch/dave_object.launch.py   | 62 ++++++++++---------
 .../dave_demos/launch/dave_sensor.launch.py   | 62 ++++++++++---------
 examples/dave_robot_launch/README.md          |  8 +--
 ...n_world.launch.py => dave_robot.launch.py} |  0
 4 files changed, 72 insertions(+), 60 deletions(-)
 rename examples/dave_robot_launch/launch/{robot_in_world.launch.py => dave_robot.launch.py} (100%)

diff --git a/examples/dave_demos/launch/dave_object.launch.py b/examples/dave_demos/launch/dave_object.launch.py
index 46395a29..5568d595 100755
--- 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,6 +106,11 @@ 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",
@@ -112,8 +118,8 @@ def generate_launch_description():
         ),
         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_demos/launch/dave_sensor.launch.py b/examples/dave_demos/launch/dave_sensor.launch.py
index a436aae2..e41eaaa2 100644
--- 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,6 +106,11 @@ 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",
@@ -112,8 +118,8 @@ def generate_launch_description():
         ),
         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/README.md b/examples/dave_robot_launch/README.md
index 1669f0ed..76ceb5f1 100644
--- a/examples/dave_robot_launch/README.md
+++ b/examples/dave_robot_launch/README.md
@@ -9,23 +9,23 @@ colcon build && source install/setup.bash
 ### 1. Launching REXROV in an empty world
 
 ```bash
-ros2 launch dave_robot_launch robot_in_world.launch.py z:=2.0 namespace:=rexrov world_name:=empty.sdf paused:=false
+ros2 launch dave_robot_launch 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_robot_launch robot_in_world.launch.py z:=-5 namespace:=rexrov world_name:=dave_ocean_waves paused:=false
+ros2 launch dave_robot_launch 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_robot_launch robot_in_world.launch.py z:=0.2 namespace:=glider_slocum world_name:=empty.sdf paused:=false
+ros2 launch dave_robot_launch 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_robot_launch robot_in_world.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false
+ros2 launch dave_robot_launch dave_robot.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false
 ```
diff --git a/examples/dave_robot_launch/launch/robot_in_world.launch.py b/examples/dave_robot_launch/launch/dave_robot.launch.py
similarity index 100%
rename from examples/dave_robot_launch/launch/robot_in_world.launch.py
rename to examples/dave_robot_launch/launch/dave_robot.launch.py

From 07add59842379f4008138d82359ab5b1db532769 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Fri, 30 Aug 2024 01:42:39 -0700
Subject: [PATCH 10/11] combined dave_robot_launch package with dave_demos
 package for consistency

---
 examples/dave_demos/README.md                 | 44 ++++++++++++++++---
 .../launch/dave_robot.launch.py               |  0
 examples/dave_robot_launch/CMakeLists.txt     | 11 -----
 examples/dave_robot_launch/README.md          | 31 -------------
 examples/dave_robot_launch/package.xml        | 12 -----
 5 files changed, 38 insertions(+), 60 deletions(-)
 rename examples/{dave_robot_launch => dave_demos}/launch/dave_robot.launch.py (100%)
 delete mode 100644 examples/dave_robot_launch/CMakeLists.txt
 delete mode 100644 examples/dave_robot_launch/README.md
 delete mode 100644 examples/dave_robot_launch/package.xml

diff --git a/examples/dave_demos/README.md b/examples/dave_demos/README.md
index e913d4c3..8ebefe92 100644
--- 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,7 +94,7 @@ 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'
@@ -70,6 +102,6 @@ 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_robot_launch/launch/dave_robot.launch.py b/examples/dave_demos/launch/dave_robot.launch.py
similarity index 100%
rename from examples/dave_robot_launch/launch/dave_robot.launch.py
rename to examples/dave_demos/launch/dave_robot.launch.py
diff --git a/examples/dave_robot_launch/CMakeLists.txt b/examples/dave_robot_launch/CMakeLists.txt
deleted file mode 100644
index bee48de8..00000000
--- a/examples/dave_robot_launch/CMakeLists.txt
+++ /dev/null
@@ -1,11 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(dave_robot_launch)
-
-find_package(ament_cmake REQUIRED)
-
-install(
-  DIRECTORY launch
-  DESTINATION share/dave_robot_launch
-)
-
-ament_package()
diff --git a/examples/dave_robot_launch/README.md b/examples/dave_robot_launch/README.md
deleted file mode 100644
index 76ceb5f1..00000000
--- a/examples/dave_robot_launch/README.md
+++ /dev/null
@@ -1,31 +0,0 @@
-## Examples
-
-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_robot_launch 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_robot_launch 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_robot_launch 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_robot_launch dave_robot.launch.py x:=4 z:=-1.5 namespace:=glider_slocum world_name:=dave_ocean_waves paused:=false
-```
diff --git a/examples/dave_robot_launch/package.xml b/examples/dave_robot_launch/package.xml
deleted file mode 100644
index 0854972c..00000000
--- a/examples/dave_robot_launch/package.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
-  <name>dave_robot_launch</name>
-  <version>0.1.0</version>
-  <description>A package that shows a demo to launch robot models.</description>
-  <maintainer email="rakeshvivek97@gmail.com">Rakesh Vivekanandan</maintainer>
-  <license>MIT</license>
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>
\ No newline at end of file

From 0def1a9f991b5e4dabb27c668d989d490b6b34e4 Mon Sep 17 00:00:00 2001
From: Rakesh Vivekanandan <rakeshvivek97@gmail.com>
Date: Sun, 1 Sep 2024 04:22:15 -0700
Subject: [PATCH 11/11] linting error fix

---
 .../dave_sensor_models/description/underwater_camera/model.sdf   | 1 -
 1 file changed, 1 deletion(-)

diff --git a/models/dave_sensor_models/description/underwater_camera/model.sdf b/models/dave_sensor_models/description/underwater_camera/model.sdf
index 7ae99d90..03d594ee 100644
--- a/models/dave_sensor_models/description/underwater_camera/model.sdf
+++ b/models/dave_sensor_models/description/underwater_camera/model.sdf
@@ -1,4 +1,3 @@
-
 <?xml version="1.0"?>
 <sdf version="1.6">
   <model name="underwater_camera">