diff --git a/uuv_assistants/src/message_to_tf.cpp b/uuv_assistants/src/message_to_tf.cpp
index 4dbc52d4..f65a3dd9 100644
--- a/uuv_assistants/src/message_to_tf.cpp
+++ b/uuv_assistants/src/message_to_tf.cpp
@@ -38,7 +38,7 @@
 
 #include <tf2/LinearMath/Transform.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 
 std::string g_odometry_topic;
@@ -72,15 +72,6 @@ void addTransform(std::vector<geometry_msgs::msg::TransformStamped>& transforms,
   transforms.push_back(new_msg);
 }
 
-namespace tf2 
-{
-  //Specialization for Point msg
-  static inline void fromMsg(const geometry_msgs::msg::Point& msgIn, tf2::Vector3& out) 
-  {
-    out = Vector3(msgIn.x, msgIn.y, msgIn.z);
-  }
-}
-
 std::string stripSlash(const std::string & in)
 {
   if (in.size() && in[0] == '/')
diff --git a/uuv_gazebo_plugins/uuv_gazebo_plugins/src/ThrusterPlugin.cpp b/uuv_gazebo_plugins/uuv_gazebo_plugins/src/ThrusterPlugin.cpp
index 38169b93..62efeefa 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_plugins/src/ThrusterPlugin.cpp
+++ b/uuv_gazebo_plugins/uuv_gazebo_plugins/src/ThrusterPlugin.cpp
@@ -20,7 +20,7 @@
 // limitations under the License.
 
 #include <boost/algorithm/string.hpp>
-#include <boost/bind.hpp>
+#include <boost/bind/bind.hpp>
 #include <boost/shared_ptr.hpp>
 
 #include <limits>
diff --git a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/CMakeLists.txt b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/CMakeLists.txt
index c06fcb68..f67ca7f5 100644
--- a/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/CMakeLists.txt
+++ b/uuv_gazebo_plugins/uuv_gazebo_ros_plugins/CMakeLists.txt
@@ -21,6 +21,7 @@ find_package(tf2_ros REQUIRED)
 find_package(uuv_gazebo_plugins REQUIRED)
 find_package(uuv_gazebo_ros_plugins_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
+find_package(gazebo_ros REQUIRED)
 
 set(BASE_LIBS  
   ament_cmake 
@@ -34,6 +35,7 @@ set(BASE_LIBS
   uuv_gazebo_plugins 
   uuv_gazebo_ros_plugins_msgs 
   visualization_msgs 
+  gazebo_ros
 )
 
 include_directories(include)
diff --git a/uuv_gazebo_worlds/launch/auv_underwater_world.launch b/uuv_gazebo_worlds/launch/auv_underwater_world.launch
index de544252..eaba1603 100644
--- a/uuv_gazebo_worlds/launch/auv_underwater_world.launch
+++ b/uuv_gazebo_worlds/launch/auv_underwater_world.launch
@@ -25,8 +25,6 @@ Underwater environment with the ODE physics engine configured for vehicles using
         <arg name="headless" value="false"/>
         <arg name="debug" value="false"/>
         <arg name="verbose" value="true"/>
-        <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-        <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
     </include>
 
     <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/empty_underwater_world.launch b/uuv_gazebo_worlds/launch/empty_underwater_world.launch
index 3b3663d9..ed4c6eb4 100644
--- a/uuv_gazebo_worlds/launch/empty_underwater_world.launch
+++ b/uuv_gazebo_worlds/launch/empty_underwater_world.launch
@@ -24,8 +24,6 @@ clearly show it is not the case-->
         <arg name="headless" value="false"/>
         <arg name="debug" value="false"/>
         <arg name="verbose" value="true"/>
-        <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-        <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
     </include>
 
      <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/herkules_ship_wreck.launch b/uuv_gazebo_worlds/launch/herkules_ship_wreck.launch
index 5bd407b1..c2218c35 100644
--- a/uuv_gazebo_worlds/launch/herkules_ship_wreck.launch
+++ b/uuv_gazebo_worlds/launch/herkules_ship_wreck.launch
@@ -18,8 +18,6 @@
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
     <arg name="verbose" value="true" />
-    <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-    <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
   </include>
 
    <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/lake.launch b/uuv_gazebo_worlds/launch/lake.launch
index 2bb539af..e4c82522 100644
--- a/uuv_gazebo_worlds/launch/lake.launch
+++ b/uuv_gazebo_worlds/launch/lake.launch
@@ -21,8 +21,6 @@
         <arg name="headless" value="false"/>
         <arg name="debug" value="false"/>
         <arg name="verbose" value="true"/>
-        <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-        <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
     </include>
 
      <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/mangalia.launch b/uuv_gazebo_worlds/launch/mangalia.launch
index eaeafa57..740f1b21 100644
--- a/uuv_gazebo_worlds/launch/mangalia.launch
+++ b/uuv_gazebo_worlds/launch/mangalia.launch
@@ -18,8 +18,6 @@
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
     <arg name="verbose" value="true" />
-    <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-    <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
   </include>
 
   <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/mangalia_manipulation.launch b/uuv_gazebo_worlds/launch/mangalia_manipulation.launch
index 57f785bd..5acf21d4 100644
--- a/uuv_gazebo_worlds/launch/mangalia_manipulation.launch
+++ b/uuv_gazebo_worlds/launch/mangalia_manipulation.launch
@@ -18,8 +18,6 @@
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
     <arg name="verbose" value="true" />
-    <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-    <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
   </include>
 
   <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/munkholmen.launch b/uuv_gazebo_worlds/launch/munkholmen.launch
index efb43b2d..a116478a 100644
--- a/uuv_gazebo_worlds/launch/munkholmen.launch
+++ b/uuv_gazebo_worlds/launch/munkholmen.launch
@@ -18,8 +18,6 @@
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
     <arg name="verbose" value="true" />
-    <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-    <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
   </include>
 
   <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/ocean_waves.launch b/uuv_gazebo_worlds/launch/ocean_waves.launch
index 5b903669..d023c91d 100644
--- a/uuv_gazebo_worlds/launch/ocean_waves.launch
+++ b/uuv_gazebo_worlds/launch/ocean_waves.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0"?>
 <launch>
     <arg name="gui" default="true"/>
     <arg name="paused" default="false"/>
@@ -22,9 +23,6 @@
         <arg name="headless" value="false"/>
         <arg name="debug" value="$(var debug)"/>
         <arg name="verbose" value="true"/>
-        <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-        <!-- Also investigate why /gazebo ns has a strong bad perf impact-->
-        <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
     </include>
 
     <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_gazebo_worlds/launch/subsea_bop_panel.launch b/uuv_gazebo_worlds/launch/subsea_bop_panel.launch
index 3d3a8ab9..cf29693b 100644
--- a/uuv_gazebo_worlds/launch/subsea_bop_panel.launch
+++ b/uuv_gazebo_worlds/launch/subsea_bop_panel.launch
@@ -18,8 +18,6 @@
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
     <arg name="verbose" value="true" />
-    <!-- TODO Remove force_system in foxy, as it will be loaded by default -->
-    <arg name="extra_gazebo_args" value="-s libgazebo_ros_force_system.so --ros-args -r gazebo:__ns:=/gazebo"/>
   </include>
 
   <include file="$(find-pkg-share uuv_assistants)/launch/publish_world_ned_frame.launch">
diff --git a/uuv_sensor_plugins/uuv_sensor_ros_plugins/include/uuv_sensor_ros_plugins/ROSBaseModelPlugin.h b/uuv_sensor_plugins/uuv_sensor_ros_plugins/include/uuv_sensor_ros_plugins/ROSBaseModelPlugin.h
index c4d96c4e..28b27aca 100644
--- a/uuv_sensor_plugins/uuv_sensor_ros_plugins/include/uuv_sensor_ros_plugins/ROSBaseModelPlugin.h
+++ b/uuv_sensor_plugins/uuv_sensor_ros_plugins/include/uuv_sensor_ros_plugins/ROSBaseModelPlugin.h
@@ -36,6 +36,7 @@
 #include <tf2_msgs/msg/tf_message.hpp>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
 
diff --git a/uuv_teleop/scripts/finned_uuv_teleop.py b/uuv_teleop/scripts/finned_uuv_teleop.py
old mode 100755
new mode 100644
index 72109fdd..d2669ab2
--- a/uuv_teleop/scripts/finned_uuv_teleop.py
+++ b/uuv_teleop/scripts/finned_uuv_teleop.py
@@ -30,6 +30,14 @@
 from plankton_utils.param_helper import parse_nested_params_to_dict
 from plankton_utils.time import is_sim_time
 
+def to_int_list(l: list):
+    cp = list(l)
+    if(len(cp) != 0 and type(cp[0]) == str):
+        if(len(cp) == 1):
+            cp = ("".join(cp)).split(",")
+        cp = [int(i) for i in cp]
+    return cp
+
 
 class FinnedUUVControllerNode(Node):
     def __init__(self, **kwargs):
@@ -63,9 +71,10 @@ def __init__(self, **kwargs):
 
         # Read the vector for contribution of each fin on the change on
         # orientation
-        gain_roll = self.get_parameter('gain_roll').value
-        gain_pitch = self.get_parameter('gain_pitch').value
-        gain_yaw = self.get_parameter('gain_yaw').value
+        gain_roll = to_int_list(self.get_parameter('gain_roll').value)
+        gain_pitch = to_int_list(self.get_parameter('gain_pitch').value)
+        gain_yaw = to_int_list(self.get_parameter('gain_yaw').value)
+        print(self._n_fins, gain_pitch, gain_roll, gain_yaw)
 
         if len(gain_roll) != self._n_fins or len(gain_pitch) != self._n_fins \
             or len(gain_yaw) != self._n_fins:
diff --git a/uuv_world_plugins/uuv_world_ros_plugins/CMakeLists.txt b/uuv_world_plugins/uuv_world_ros_plugins/CMakeLists.txt
index c88803c7..146c0ba6 100644
--- a/uuv_world_plugins/uuv_world_ros_plugins/CMakeLists.txt
+++ b/uuv_world_plugins/uuv_world_ros_plugins/CMakeLists.txt
@@ -17,6 +17,7 @@ find_package(gazebo_dev REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(gazebo_ros REQUIRED)
 
 set(LIBS
   uuv_world_plugins
@@ -26,6 +27,7 @@ set(LIBS
   ament_cmake
   geometry_msgs
   rclcpp
+  gazebo_ros
 )
 
 include_directories(include)