began work on other plugins

src/subjugator/simulation/subjugator_bringup/config/subjugator_bridge.yaml

@@ -4,4 +4,9 @@
 #   ros_type: "std_msgs/String"
 #   gz_type: "gz.msgs.StringMsg"
 
-[]
+# IMU configuration.
+- topic_name: "/imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  lazy: true
+  direction: GZ_TO_ROS

src/subjugator/simulation/subjugator_description/urdf/sub9.urdf.xacro

@@ -4,6 +4,9 @@
   <!-- XACRO INCLUDES -->
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/fixed_link.xacro"/>
   <xacro:include filename="$(find subjugator_description)/urdf/xacro/camera.xacro"/>
+  <xacro:include filename="$(find subjugator_description)/urdf/xacro/imu_magnetometer.xacro"/>
+  <xacro:include filename="$(find subjugator_description)/urdf/xacro/dvl.xacro"/>
+
 
 
   <!-- KDL Parser does not like inertial base links, so use a 'fake' massless root link -->
@@ -66,4 +69,22 @@
   <!-- <xacro:mil_camera name="front_right_cam" parent="front_stereo" namespace="/camera/front/right" xyz="0 -0.0445 0" rpy="0 0 0"/>
   <xacro:mil_camera name="down_left_cam" parent="base_link" namespace="/camera/down" xyz="0.054 0.105 -0.256" rpy="0 1.57 1.57"/> -->
 
+
+  <xacro:mil_imu_magnetometer
+    name="imu"
+    parent="base_link"
+    imu_topic='/imu/data_raw'
+    xyz="0.21236 0.00254 -0.10233"
+    rpy="0 -1.571 0"
+    rate="210"
+    ax="0.015707963"
+    ay="0.015707963"
+    az="0.015707963"
+    lx="0.08825985"
+    ly="0.08825985"
+    lz="0.08825985"
+  />
+
+  <xacro:mil_dvl name="dvl" parent="base_link" xyz="0.0908 0 -0.2459" rpy="0 0 0.785" gazebo_offset="0 0 -1" rate="6.5"/>
+
 </robot>

src/subjugator/simulation/subjugator_description/urdf/xacro/camera.xacro

@@ -21,7 +21,7 @@
         <update_rate>${fps}</update_rate>
         <visualize>true</visualize>
         <always_on>1</always_on>
-        <topic>${namespace}/image_raw</topic>
+        <topic>${namespace}</topic>
         <camera>
           <horizontal_fov>${fov}</horizontal_fov>
           <image>

src/subjugator/simulation/subjugator_description/urdf/xacro/dvl.xacro

@@ -1,36 +1,68 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro">
+<robot xmlns:xacro="http://ros.org/wiki/xacro" xmlns:experimental="http://sdformat.org/schemas/experimental" xmlns:gz="http://gazebosim.org/schema">
   <!-- Macro for inserting a dvl -->
-  <xacro:macro name="mil_dvl" params="name:=dvl parent:='base_link' xyz:='0 0 0' rpy='0 0 0' gazebo_offset=:='0 0 0' rate:=1">
+  <xacro:macro name="mil_dvl" params="name='dvl' parent='base_link' xyz='0 0 0' rpy='0 0 0' gazebo_offset='0 0 0' rate='1'">
     <link name="${name}"/>
     <joint name="${name}_joint" type="fixed">
       <origin xyz="${xyz}" rpy="${rpy}"/>
       <parent link="${parent}"/>
       <child link="${name}"/>
     </joint>
     <gazebo reference="${name}">
-      <sensor type="ray" name="${name}_sensor">
-        <always_on>true</always_on>
-        <update_rate>${rate}</update_rate>
-        <ray>
-          <scan>
-            <horizontal>
-              <samples>1</samples>
-              <resolution>3</resolution>
-              <min_angle>-0.3</min_angle>
-              <max_angle>0.3</max_angle>
-            </horizontal>
-          </scan>
-          <range>
-            <min>0.05</min>
-            <max>200</max>
-          </range>
-        </ray>
-        <plugin name="${name}_plugin" filename="libmil_dvl_gazebo.so">
-          <frame_id>${name}</frame_id>
-          <offset>${gazebo_offset}</offset>
-        </plugin>
-      </sensor>
+      <experimental:params>
+        <sensor
+            name="teledyne_pathfinder_dvl"
+            type="custom" gz:type="dvl">
+          <pose>0 0 0 0 0 0</pose>
+          <always_on>1</always_on>
+          <update_rate>${rate}</update_rate>
+          <topic>/model/${parent}/dvl/velocity</topic>
+          <gz:dvl>
+            <type>phased_array</type>
+            <arrangement degrees="true">
+              <beam id="1">
+                <aperture>2</aperture>
+                <rotation>45</rotation>
+                <tilt>30</tilt>
+              </beam>
+              <beam>
+                <aperture>2</aperture>
+                <rotation>135</rotation>
+                <tilt>30</tilt>
+              </beam>
+              <beam>
+                <aperture>2</aperture>
+                <rotation>-45</rotation>
+                <tilt>30</tilt>
+              </beam>
+              <beam>
+                <aperture>2</aperture>
+                <rotation>-135</rotation>
+                <tilt>30</tilt>
+              </beam>
+            </arrangement>
+            <tracking>
+              <bottom_mode>
+                <when>best</when>
+                <noise type="gaussian">
+                  <!-- +/- 0.4 cm/s precision at 10 m/s within 2 stddevs -->
+                  <stddev>0.002</stddev>
+                </noise>
+                <visualize>false</visualize>
+              </bottom_mode>
+            </tracking>
+            <!-- Roughly 1 m resolution at a 100m -->
+            <resolution>0.01</resolution>
+            <maximum_range>100.</maximum_range>
+            <minimum_range>0.1</minimum_range>
+            <!-- ENU to SFM -->
+            <reference_frame>0 0 0 0 0 -1.570796</reference_frame>
+          </gz:dvl>
+          <plugin
+            filename="gz-sim-dvl-system"
+            name="gz::sim::systems::DopplerVelocityLogSystem"/>
+        </sensor>
+      </experimental:params>
     </gazebo>
   </xacro:macro>
-</robot>
+</robot>

src/subjugator/simulation/subjugator_description/urdf/xacro/imu_magnetometer.xacro

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro
+    name="mil_imu_magnetometer"
+    params="name parent imu_topic xyz rpy rate ax ay az lx ly lz"
+  >
+    <link name="${name}"/>
+    <joint name="${name}_joint" type="fixed">
+      <origin xyz="${xyz}" rpy="${rpy}"/>
+      <parent link="${parent}"/>
+      <child link="${name}"/>
+    </joint>
+    <gazebo reference="${name}">
+      <sensor name="${name}_imu_sensor" type="imu">
+        <always_on>true</always_on>
+        <update_rate>${rate}</update_rate>
+        <topic>${imu_topic}/imu</topic>
+        <imu>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <stddev>${lx}</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>${ly}</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <stddev>${lz}</stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <stddev>${ax}</stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <stddev>${ay}</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <stddev>${az}</stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+        </imu>
+        <plugin
+            filename="gz-sim-imu-system"
+            name="gz::sim::systems::Imu">
+        </plugin>
+
+      </sensor>
+      <sensor name="${name}_mag_sensor" type="magnetometer">
+        <always_on>true</always_on>
+        <update_rate>${rate}</update_rate>
+        <topic>${imu_topic}/imu_mag</topic>
+        <plugin
+            filename="gz-sim-magnetometer-system"
+            name="gz::sim::systems::Magnetometer">
+        </plugin>
+      </sensor>
+    </gazebo>
+    </xacro:macro>
+</robot>

src/subjugator/simulation/subjugator_gazebo/include/dave_gz_sensor_plugins/UnderwaterCamera.hh → src/subjugator/simulation/subjugator_gazebo/include/UnderwaterCamera.hh

@@ -63,6 +63,9 @@ private:
   struct PrivateData;
   std::unique_ptr<PrivateData> dataPtr;
 };
+
+  std::string sanitizeNodeName(const std::string& name);
+
 }  // namespace dave_gz_sensor_plugins
 
 #endif  // DAVE_GZ_SENSOR_PLUGINS__UNDERWATERCAMERA_HH_

src/subjugator/simulation/subjugator_gazebo/src/UnderwaterCamera.cc

@@ -14,7 +14,7 @@
  * limitations under the License.
  */
 
-#include "dave_gz_sensor_plugins/UnderwaterCamera.hh"
+#include "UnderwaterCamera.hh"
 #include <gz/common/Console.hh>
 #include <gz/sim/Model.hh>
 #include <gz/sim/Util.hh>
@@ -109,7 +109,6 @@ void UnderwaterCamera::Configure(
     rclcpp::init(0, nullptr);
   }
 
-  this->ros_node_ = std::make_shared<rclcpp::Node>("underwater_camera_node");
 
   auto rgbdCamera = _ecm.Component<gz::sim::components::RgbdCamera>(_entity);
   if (!rgbdCamera)
@@ -143,6 +142,9 @@ void UnderwaterCamera::Configure(
   this->dataPtr->depth_image_topic = this->dataPtr->topic + "/depth_image";
   this->dataPtr->simulated_image_topic = this->dataPtr->topic + "/simulated_image";
 
+  std::string validName = sanitizeNodeName(this->dataPtr->topic + "_node");
+  this->ros_node_ = std::make_shared<rclcpp::Node>(validName);
+
   sdf::Camera * cameraSdf = sensorSdf.CameraSensor();
 
   // get camera intrinsics
@@ -450,4 +452,22 @@ void UnderwaterCamera::PostUpdate(
   }
 }
 
+// Function to sanitize a ROS node name
+std::string sanitizeNodeName(const std::string& name) {
+    std::string sanitized;
+    for (char c : name) {
+        // Replace invalid characters with '_'
+        if (std::isalnum(c) || c == '_') {
+            sanitized += c;
+        } else {
+            sanitized += '_';
+        }
+    }
+    // Ensure the name doesn't start with a digit
+    if (!sanitized.empty() && std::isdigit(sanitized[0])) {
+        sanitized = "_" + sanitized;
+    }
+    return sanitized;
+}
+
 }  // namespace dave_gz_sensor_plugins

src/subjugator/simulation/subjugator_gazebo/worlds/robosub_2024.world

@@ -18,7 +18,7 @@
             <!-- Physics Settings -->
             <physics name='default_physics' default='0' type='ode'>
                 <real_time_update_rate>100</real_time_update_rate>
-                <max_step_size>0.001</max_step_size>
+                <max_step_size>0.01</max_step_size>
                 <real_time_factor>1.0</real_time_factor>
             </physics>
 
@@ -42,10 +42,6 @@
                 name="gz::sim::systems::Sensors">
                 <render_engine>ogre2</render_engine>
             </plugin>
-            <plugin
-                filename="gz-sim-dvl-system"
-                name="gz::sim::systems::DopplerVelocityLogSystem">
-            </plugin>
             <!-- Buoyancy Plugin -->
             <!--
                 To alter a objects's buoyancy do one or both of the following: