diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml
deleted file mode 100644
index 9ced34edf957cb6029186e305deb6639cf9e6b17..0000000000000000000000000000000000000000
--- a/.github/workflows/ci.yaml
+++ /dev/null
@@ -1,86 +0,0 @@
----
-name: CI
-
-on:  # NOLINT
-  pull_request:
-  push:
-  schedule:
-    - cron: '0 0 */2 * *'  # Every 2 days at midnight
-
-jobs:
-  test_docker:
-    runs-on: ubuntu-latest
-    strategy:
-      fail-fast: false
-      matrix:
-        gazebo-version: [9, 11]
-        # Define the Docker image(s) associated with each ROS distribution.
-        # The include syntax allows additional variables to be defined, like
-        # docker_image in this case. See documentation:
-        # https://help.github.com/en/actions/reference/workflow-syntax-for-github-actions#example-including-configurations-in-a-matrix-build
-
-        # Platforms are defined in REP 3, and REP 2000:
-        # https://ros.org/reps/rep-0003.html
-        # https://ros.org/reps/rep-2000.html
-        include:
-          # Foxy Fitzroy (May 2020 - May 2023)
-          - docker_image: ros:foxy
-            ros_distribution: foxy
-
-    container: ${{ matrix.docker_image }}
-
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          path: src/plankton
-
-      - name: Install Tools
-        run: |
-          sudo apt update
-          sudo apt install -y build-essential clang cmake git wget libasio-dev libtinyxml2-dev lcov psmisc
-          rosdep update
-          sudo apt install -y python3-colcon-coveragepy-result
-          sudo apt install -y python3-colcon-lcov-result
-
-      - name: Install Gazebo
-        run: |
-          echo Installing Gazebo ${{ matrix.gazebo-version }}
-          sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
-          wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
-          sudo apt update
-          sudo apt install -y gazebo${{ matrix.gazebo-version }}
-          sudo apt install -y libgazebo${{ matrix.gazebo-version }}-dev
-          sudo apt install -y ros-${{ matrix.ros_distribution }}-gazebo-ros-pkgs
-
-      - name: Install Dependencies
-        run: |
-          rosdep update
-          rosdep install --from-paths src --ignore-src --rosdistro ${{ matrix.ros_distribution }} -y
-
-      - name: Build Packages
-        run: |
-          . /opt/ros/${{ matrix.ros_distribution }}/setup.sh
-          colcon build --packages-up-to plankton --merge-install
-
-      - name: Run Unit Tests
-        id: action_ros_ci_step
-        run: |
-          . /opt/ros/${{ matrix.ros_distribution }}/setup.sh
-          colcon test --merge-install --packages-up-to plankton --event-handlers console_cohesion+
-
-      - name: Unit Test Results
-        run: |
-          echo Results for Gazebo ${{ matrix.gazebo-version }}
-          . /opt/ros/${{ matrix.ros_distribution }}/setup.sh
-          colcon test-result --all --verbose
-
-      - name: Upload Logs
-        uses: actions/upload-artifact@v1
-        with:
-          name: colcon-logs-linux
-          path: log/
-        if: always()
diff --git a/.vscode/uuv.vscode.code-snippets b/.vscode/uuv.vscode.code-snippets
deleted file mode 100644
index 12eb5b68011da9192166ffb24671a6c5605b0bd1..0000000000000000000000000000000000000000
--- a/.vscode/uuv.vscode.code-snippets
+++ /dev/null
@@ -1,461 +0,0 @@
-{
-	"py_class_md_docstring": {
-		"prefix": "uuv.py.doc.class",
-		"scope": "python",
-		"description": "Python Class Docstring (Markdown)",
-		"body": [
-			"\"\"\"Class description",
-			"",
-			"> *Attributes*",
-			"",
-			"* `param` (*type:* `data_type`, *value:* `data`): Parameter description",
-			"",
-			"> *Input arguments*",
-			"",
-			"* `param` (*type:* `data_type`, *default:* `data`): Parameter description",
-			"",
-			"\"\"\""
-		]
-	},
-
-	"py_func_md_docstring": {
-		"prefix": "uuv.py.doc.func",
-		"scope": "python",
-		"description": "Python Function Docstring (Markdown)",
-		"body": [
-			"\"\"\"Function description",
-			"",
-			"> *Input arguments*",
-			"",
-			"* `param` (*type:* `data_type`, *default:* `data`): Parameter description",
-			"",
-			"> *Returns*",
-			"",
-			"Description of return values",
-			"\"\"\""
-		]
-	},
-
-	"py_module_md_docstring": {
-		"prefix": "uuv.py.doc.module",
-		"scope": "python",
-		"description": "Python Module Docstring (Markdown)",
-		"body": [
-			"\"\"\"Module description",
-			"\"\"\""
-		]
-	},
-
-	"py_param_md_docstring_snippet": {
-		"prefix": "uuv.py.doc.param",
-		"scope": "python",
-		"description": "Python Parameter Docstring Snippet (Markdown)",
-		"body": [
-			"* `param` (*type:* `data_type`, *default:* `data`): Parameter description"
-		]
-	},
-
-	"py_ros_publisher_md_docstring_snippet": {
-		"prefix": "uuv.py.doc.publisher",
-		"scope": "python",
-		"description": "Python ROS Publisher Docstring Snippet (Markdown)",
-		"body": [
-			"* `topic_name` (*message:* `data_type`): Topic description"
-		]
-	},
-
-	"py_property_md_docstring": {
-		"prefix": "uuv.py.doc.property",
-		"scope": "python",
-		"description": "Python Property Docstring (Markdown)",
-		"body": [
-			"\"\"\"`data_type`: Property description\"\"\""
-		]
-	},
-
-	"py_script_md_docstring": {
-		"prefix": "uuv.py.doc.script",
-		"scope": "python",
-		"description": "Python Script Docstring (Markdown)",
-		"body": [
-			"\"\"\"**Description**",
-			"",
-			"Script description, with some formulas",
-			"",
-			"$$",
-			"\t\\dot{v_c}(t) + \\mu_c v_c(t) = w_c",
-			"$$",
-			"",
-			"**Input ROS parameters**",
-			"",
-			"* `param` (*type:* `data_type`, *default:* `value`): Parameter description",
-			"",
-			"**Input parameters**",
-			"",
-			"* `param` (*type:* `data_type`, *default:* `value`): Parameter description",
-			"",
-			"**Usage**",
-			"",
-			"Examples of usage",
-			"",
-			"```bash",
-			"rosrun pkg_name node_name --param value",
-			"```",
-			"",
-			"**Launch file snippet**",
-			"",
-			"```xml",
-			"<node name=\"name\" pkg=\"pkg_name\" type=\"name\" output=\"screen\">",
-			"\t<rosparam subst_value=\"true\">",
-			"\t\tparam: value",
-			"\t</rosparam>",
-			"</node>",
-			"```",
-			"**ROS services**",
-			"",
-			"> **`service_name`**",
-			"",
-			"*Service description file*",
-			"",
-			"[`service.file`](link_to_service_file)",
-			"",
-			"*Service call*",
-			"",
-			"```bash",
-			"rosservice call service_name",
-			"```",
-			"",
-			"Description of input parameters and return values",
-			"",
-			"\"\"\""
-		]
-	},
-
-	"py_uuv_ros_test": {
-		"scope": "python",
-		"prefix": "uuv.py.ros.test",
-		"description": "ROS Unit Test Script (Python)",
-		"body": [
-			"#!/usr/bin/env python",
-			"# Copyright (c) 2016-$CURRENT_YEAR 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.",
-			"import roslib",
-			"import unittest",
-			"# Other imports",
-			"",
-			"PKG = 'package_name'",
-			"roslib.load_manifest(PKG)",
-			"",
-			"class TestModule(unittest.TestCase):",
-			"\tpass",
-			"",
-			"if __name__ == '__main__':",
-			"\timport rosunit",
-			"\trosunit.unitrun(PKG, 'test_module', TestModule)"
-		]
-	},
-
-	"py_uuv_license_header": {
-		"scope": "python",
-		"prefix": "uuv.py.license",
-		"description": "UUV Simulator License Header (Python)",
-		"body": [
-			"# Copyright (c) 2016-$CURRENT_YEAR 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."
-		]
-	},
-
-	"xml_uuv_license_header": {
-		"scope": "xml",
-		"prefix": "uuv.xml.license",
-		"description": "UUV Simulator License Header (XML)",
-		"body": [
-			"<!-- Copyright (c) 2016-$CURRENT_YEAR The UUV Simulator Authors.",
-			"\tAll rights reserved.",
-			"",
-			"\tLicensed under the Apache License, Version 2.0 (the \"License\");",
-			"\tyou may not use this file except in compliance with the License.",
-			"\tYou may obtain a copy of the License at",
-			"",
-			"\thttp://www.apache.org/licenses/LICENSE-2.0",
-			"",
-			"\tUnless required by applicable law or agreed to in writing, software",
-			"\tdistributed under the License is distributed on an \"AS IS\" BASIS,",
-			"\tWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.",
-			"\tSee the License for the specific language governing permissions and",
-			"\tlimitations under the License.",
-			"-->"
-		]
-	},
-
-	"xml_uuv_xacro_file": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.file",
-		"description": "Xacro file (XML)",
-		"body": [
-			"<?xml version=\"1.0\"?>",
-			"<robot xmlns:xacro=\"http://www.ros.org/wiki/xacro\">",
-			"",
-			"</robot>"
-		]
-	},
-
-	"xml_uuv_xacro_include": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.include",
-		"description": "Xacro include (XML)",
-		"body": [
-			"<xacro:include filename=\"$(find pkg_name)/path/file.urdf.xacro\"/>"
-		]
-	},
-
-	"xml_uuv_xacro_property": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.property",
-		"description": "Xacro property (XML)",
-		"body": [
-			"<xacro:property name=\"param_name\" value=\"param_value\"/>"
-		]
-	},
-
-	"xml_uuv_xacro_macro": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.macro",
-		"description": "Xacro macro (XML)",
-		"body": [
-			"<xacro:macro name=\"macro_name\" params=\"param_1 param_2\">",
-			"",
-			"</xacro:macro>"
-		]
-	},
-
-	"xml_uuv_xacro_insert_block": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.insert_block",
-		"description": "Xacro insert block (XML)",
-		"body": [
-			"<xacro:insert_block name=\"block_name\"/>"
-		]
-	},
-
-	"xml_uuv_xacro_hydrodynamic_model_link": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.hydro.link",
-		"description": "Base for the hydrodynamic model description for one link",
-		"body": [
-			"<link name=\"link_name\">",
-			"\t<neutrally_buoyant>0</neutrally_buoyant>",
-			"",
-			"\t<volume>0</volume>",
-			"",
-			"\t<box>",
-			"\t\t<width>0</width>",
-			"\t\t<length>0</length>",
-			"\t\t<height>0</height>",
-			"\t</box>",
-			"",
-			"<center_of_buoyancy>0 0 0</center_of_buoyancy>",
-			"",
-			"<hydrodynamic_model>",
-			"",
-			"\t<!-- Add the desired hydrodynamic model description here -->",
-			"",
-			"</hydrodynamic_model>",
-			"</link>"
-		]
-	},
-
-	"xml_uuv_xacro_hydrodynamic_model_fossen": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.hydro.fossen",
-		"description": "Inputs for Fossen's hydrodynamic model (XML)",
-		"body": [
-			"<hydrodynamic_model>",
-			"\t<type>fossen</type>",
-			"",
-			"\t<added_mass>",
-			"\t\t0 0 0 0 0 0",
-			"\t\t0 0 0 0 0 0",
-			"\t\t0 0 0 0 0 0",
-			"\t\t0 0 0 0 0 0",
-			"\t\t0 0 0 0 0 0",
-			"\t\t0 0 0 0 0 0",
-			"\t</added_mass>",
-			"",
-			"\t<linear_damping>",
-			"\t\t0 0 0 0 0 0",
-			"\t</linear_damping>",
-			"",
-			"\t<linear_damping_forward_speed>",
-			"\t\t0 0 0 0 0 0",
-			"\t</linear_damping_forward_speed>",
-			"",
-			"\t<quadratic_damping>",
-			"\t\t0 0 0 0 0 0",
-			"\t</quadratic_damping>",
-			"</hydrodynamic_model>"
-		]
-	},
-
-	"xml_uuv_xacro_hydrodynamic_model_sphere": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.hydro.sphere",
-		"description": "Inputs for an approximated Fossen's hydrodynamic model for a sphere (XML)",
-		"body": [
-			"<hydrodynamic_model>",
-          	"\t<type>sphere</type>",
-          	"\t<radius>0</radius>",
-        	"</hydrodynamic_model>"
-		]
-	},
-
-	"xml_uuv_xacro_underwater_object_plugin_sphere": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.hydro.sphere.underwater_object_plugin",
-		"description": "Snippet for the uuv_underwater_object plugin using Fossen's model for a sphere (XML)",
-		"body": [
-			"<gazebo>",
-      		"\t<plugin name=\"uuv_plugin\" filename=\"libuuv_underwater_object_ros_plugin.so\">",
-        	"\t\t<fluid_density>1025.0</fluid_density>",
-        	"\t\t<flow_velocity_topic>hydrodynamics/current_velocity</flow_velocity_topic>",
-			"\t\t<debug>0</debug>",
-			"",
-			"\t\t<link name=\"link_name\">",
-			"\t\t\t<neutrally_buoyant>0</neutrally_buoyant>",
-			"",
-			"\t\t\t<volume>0</volume>",
-			"",
-			"\t\t\t<box>",
-			"\t\t\t\t<width>0</width>",
-			"\t\t\t\t<length>0</length>",
-			"\t\t\t\t<height>0</height>",
-			"\t\t\t</box>",
-			"",
-			"\t\t<center_of_buoyancy>0 0 0</center_of_buoyancy>",
-			"",
-			"\t\t<hydrodynamic_model>",
-			"\t\t\t<type>sphere</type>",
-          	"\t\t\t<radius>0</radius>",
-			"\t\t</hydrodynamic_model>",
-			"</link>",
-			"\t</plugin>",
-    		"</gazebo>"
-		]
-	},
-
-	"xml_uuv_xacro_default_joint_state_publisher": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.joint_state_publisher",
-		"description": "Xacro snippet for the joint state publisher (XML)",
-		"body": [
-			"<xacro:macro name=\"default_joint_state_publisher\" params=\"namespace update_rate\">",
-    		"\t<gazebo>",
-      		"\t\t<plugin name=\"uuv_joint_state_publisher\" filename=\"libuuv_joint_state_publisher.so\">",
-        	"\t\t\t<robotNamespace>${namespace}</robotNamespace>",
-        	"\t\t\t<updateRate>${update_rate}</updateRate>",
-			"\t\t</plugin>",
-    		"\t</gazebo>",
-  			"</xacro:macro>"
-		]
-	},
-
-	"xml_uuv_xacro_underwater_object_plugin_fossen": {
-		"scope": "xml",
-		"prefix": "uuv.xml.xacro.hydro.fossen.underwater_object_plugin",
-		"description": "Snippet for the uuv_underwater_object plugin using Fossen's model (XML)",
-		"body": [
-			"<gazebo>",
-      		"\t<plugin name=\"uuv_plugin\" filename=\"libuuv_underwater_object_ros_plugin.so\">",
-        	"\t\t<fluid_density>1025.0</fluid_density>",
-        	"\t\t<flow_velocity_topic>hydrodynamics/current_velocity</flow_velocity_topic>",
-			"\t\t<debug>0</debug>",
-			"",
-			"\t\t<link name=\"link_name\">",
-			"\t\t\t<neutrally_buoyant>0</neutrally_buoyant>",
-			"",
-			"\t\t\t<volume>0</volume>",
-			"",
-			"\t\t\t<box>",
-			"\t\t\t\t<width>0</width>",
-			"\t\t\t\t<length>0</length>",
-			"\t\t\t\t<height>0</height>",
-			"\t\t\t</box>",
-			"",
-			"\t\t<center_of_buoyancy>0 0 0</center_of_buoyancy>",
-			"",
-			"\t\t<hydrodynamic_model>",
-			"\t\t\t<type>fossen</type>",
-			"",
-			"\t\t\t<added_mass>",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t</added_mass>",
-			"",
-			"\t\t\t<linear_damping>",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t</linear_damping>",
-			"",
-			"\t\t\t<linear_damping_forward_speed>",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t</linear_damping_forward_speed>",
-			"",
-			"\t\t\t<quadratic_damping>",
-			"\t\t\t\t0 0 0 0 0 0",
-			"\t\t\t</quadratic_damping>",
-			"\t\t</hydrodynamic_model>",
-			"</link>",
-			"\t</plugin>",
-    		"</gazebo>"
-		]
-	},
-	
-	"cpp_uuv_license_header": {
-		"scope": "cpp",
-		"prefix": "uuv.cpp.license",
-		"description": "UUV Simulator License Header (C++)",
-		"body": [
-			"// Copyright (c) 2016-$CURRENT_YEAR 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."
-		]	
-	}
-}
\ No newline at end of file
diff --git a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/include/uuv_gazebo_ros_plugins/ThrusterROSPlugin.h b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/include/uuv_gazebo_ros_plugins/ThrusterROSPlugin.h
index 8aa1ba41e72487cb3f88ad85e9ff97c0bdf5b024..f97c9b1cf7a73a8b25ac12e873f3a87a0b62b998 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/include/uuv_gazebo_ros_plugins/ThrusterROSPlugin.h
+++ b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/include/uuv_gazebo_ros_plugins/ThrusterROSPlugin.h
@@ -63,6 +63,8 @@ namespace uuv_simulator_ros
     /// \brief Set new set point (desired thrust [N]) for thruster.
     public: void SetThrustReference(
         const uuv_gazebo_ros_plugins_msgs::msg::FloatStamped::SharedPtr _msg);
+    public: void SetThrustReferenceFloat64(
+        const std_msgs::msg::Float64::SharedPtr _msg);
 
     /// \brief Return the ROS publish period.
     public: gazebo::common::Time  GetRosPublishPeriod();
@@ -127,6 +129,7 @@ namespace uuv_simulator_ros
 
     /// \brief Subscriber reacting to new reference thrust set points.
     private: rclcpp::Subscription<uuv_gazebo_ros_plugins_msgs::msg::FloatStamped>::SharedPtr mySubThrustReference;
+    private: rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr mySubThrustReferenceFloat64;
 
     /// \brief Publisher for current actual thrust.
     private: rclcpp::Publisher<uuv_gazebo_ros_plugins_msgs::msg::FloatStamped>::SharedPtr myPubThrust;
diff --git a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/ThrusterROSPlugin.cpp b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/ThrusterROSPlugin.cpp
index 271b055964898ef320237b08b40a1187fc3b275d..6141d31fa071329d3968ba18038a2a6f71d05a6d 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/ThrusterROSPlugin.cpp
+++ b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/src/ThrusterROSPlugin.cpp
@@ -66,6 +66,18 @@ void ThrusterROSPlugin::SetThrustReference(
   this->inputCommand = _msg->data;
 }
 
+void ThrusterROSPlugin::SetThrustReferenceFloat64(
+    const std_msgs::msg::Float64::SharedPtr _msg)
+{
+  if (std::isnan(_msg->data))
+  {
+    RCLCPP_WARN(myRosNode->get_logger(), "ThrusterROSPlugin: Ignoring nan command");
+    return;
+  }
+
+  this->inputCommand = _msg->data;
+}
+
 //=============================================================================
 ///////////////////////////////////////////////////////////////////////////////
 gazebo::common::Time ThrusterROSPlugin::GetRosPublishPeriod()
@@ -183,7 +195,11 @@ void ThrusterROSPlugin::Load(gazebo::physics::ModelPtr _parent,
       uuv_gazebo_ros_plugins_msgs::msg::FloatStamped
       >(this->commandSubscriber->GetTopic(), 10,
         std::bind(&ThrusterROSPlugin::SetThrustReference, this, _1));
-
+    
+    mySubThrustReferenceFloat64 = myRosNode->create_subscription<
+      std_msgs::msg::Float64
+      >(this->commandSubscriber->GetTopic() + "/rpm", 10,
+        std::bind(&ThrusterROSPlugin::SetThrustReferenceFloat64, this, _1));
   
     myPubThrust = myRosNode->create_publisher<
       uuv_gazebo_ros_plugins_msgs::msg::FloatStamped
diff --git a/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/camera_snippets.xacro b/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/camera_snippets.xacro
index 54058e9890cc4ca079685da5fc2235ad71cd006a..a02b99677ca486bd640f1597ba3ab3fa4c3f0e34 100644
--- a/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/camera_snippets.xacro
+++ b/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/camera_snippets.xacro
@@ -139,7 +139,7 @@
         </camera>
         <plugin name="camera${suffix}_controller" filename="libgazebo_ros_camera.so">
           <!-- <updateRate>${update_rate}</updateRate> -->
-          <camera_name>${namespace}/camera${suffix}</camera_name>
+          <camera_name>camera${suffix}</camera_name>
           <!-- <cameraName>${namespace}/camera${suffix}</cameraName> -->
           <frame_name>camera${suffix}_link_optical</frame_name>
           <!-- <frameName>camera${suffix}_link_optical</frameName> -->
@@ -153,6 +153,7 @@
           <backgroundB>0</backgroundB>
 
           <ros>
+            <namespace>${namespace}</namespace>
             <!-- Use either <argument> with - -ros-args -r or remapping -->
             <remapping>image_raw:=${topic}_image</remapping>
           </ros>
@@ -243,6 +244,10 @@
         </camera>
         <!-- TODO Check parameters for ROS 2 syntax-->
         <plugin name="camera${suffix}_controller" filename="libuuv_gazebo_ros_camera_plugin.so">
+          <ros>
+            <namespace>${namespace}</namespace>
+          </ros>
+          
           <updateRate>${update_rate}</updateRate>
           <cameraName>${namespace}/camera${suffix}</cameraName>
           <frameName>${namespace}/camera${suffix}_link</frameName>
diff --git a/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/dvl_snippets.xacro b/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/dvl_snippets.xacro
index 8a4c8e749c5093c0003976a2f36641f970334f9e..b880b7125822396123b6084b9e4c6de34a58ba37 100644
--- a/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/dvl_snippets.xacro
+++ b/uuv_sensor_plugins/uuv_sensor_ros_plugins/urdf/dvl_snippets.xacro
@@ -133,6 +133,7 @@
 
         <plugin filename="libgazebo_ros_ray_sensor.so" name="dvl${suffix}_sonar${index}">
           <ros>
+            <namespace>${namespace}</namespace>
             <remapping>~/out:=${topic}_sonar${index}</remapping>
           </ros>
           <!-- <gaussianNoise>0.005</gaussianNoise> -->
diff --git a/uuv_teleop/launch/uuv_teleop.launch b/uuv_teleop/launch/uuv_teleop.launch
index 48288b0ea32f6c5c91cde220efb16f82cf7da86d..19528676e16fa273887953f980d7c239b9cff702 100644
--- a/uuv_teleop/launch/uuv_teleop.launch
+++ b/uuv_teleop/launch/uuv_teleop.launch
@@ -4,20 +4,20 @@
   <arg name="joy_id" default="0"/>
   <!-- Joystick mapping - default: XBox 360 controller -->
   <!-- To activate the vehicle teleop, the deadman button has to remain pressed -->
-  <arg name="deadman_button" default="-1"/>
-  <arg name="exclusion_buttons" default="4,5"/>
-  <arg name="axis_roll" default="-1"/>
-  <arg name="axis_pitch" default="-1"/>
-  <arg name="axis_yaw" default="0"/>
-  <arg name="axis_x" default="4"/>
-  <arg name="axis_y" default="3"/>
-  <arg name="axis_z" default="1"/>
-  <arg name="gain_roll" default="0.0"/>
-  <arg name="gain_pitch" default="0.0"/>
-  <arg name="gain_yaw" default="0.2"/>
-  <arg name="gain_x" default="0.3"/>
-  <arg name="gain_y" default="0.3"/>
-  <arg name="gain_z" default="0.3"/>
+    <arg name="deadman_button" default="-1"/>
+    <arg name="exclusion_buttons" default="4,5"/>
+    <arg name="axis_roll" default="-1"/>
+    <arg name="axis_pitch" default="-1"/>
+    <arg name="axis_yaw" default="2"/>
+    <arg name="axis_x" default="1"/>
+    <arg name="axis_y" default="0"/>
+    <arg name="axis_z" default="3"/>
+    <arg name="gain_roll" default="0.0"/>
+    <arg name="gain_pitch" default="0.0"/>
+    <arg name="gain_yaw" default="0.2"/>
+    <arg name="gain_x" default="0.3"/>
+    <arg name="gain_y" default="0.3"/>
+    <arg name="gain_z" default="0.3"/>
   <arg name="output_topic" default="cmd_vel"/>
   <!-- The type of message can also be geometry_msgs/Accel -->
   <arg name="message_type" default="twist"/>