diff --git a/.github/workflows/doc.yml b/.github/workflows/doc.yml
index 775f0e8..870235f 100644
--- a/.github/workflows/doc.yml
+++ b/.github/workflows/doc.yml
@@ -14,10 +14,10 @@ jobs:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v3.3.0
-      - uses: ros-tooling/setup-ros@v0.6
+      - uses: ros-tooling/setup-ros@0.7.1
         with:
           required-ros-distributions: humble
-      - uses: ros-tooling/action-ros-ci@v0.3
+      - uses: ros-tooling/action-ros-ci@0.3.5
         id: action_ros_ci_step
         with:
           package-name: suave suave_msgs suave_missions suave_metacontrol system_modes mavros mavros_wrapper mros2_reasoner mros2_msgs
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 36172f8..6181e94 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -15,7 +15,7 @@ jobs:
       - name: Checkout
         uses: actions/checkout@v2
       - name: Build pkg
-        uses: ros-tooling/action-ros-ci@v0.2
+        uses: ros-tooling/action-ros-ci@0.3.5
         with:
           package-name: suave
           target-ros2-distro: humble
diff --git a/docker/dockerfile-suave b/docker/dockerfile-suave
index 1743c6f..68a1320 100644
--- a/docker/dockerfile-suave
+++ b/docker/dockerfile-suave
@@ -23,6 +23,7 @@ RUN add-apt-repository ppa:kisak/kisak-mesa \
     software-properties-common \
     mesa-utils \
     bash-completion \
+    python3-pip \
     && apt full-upgrade -y \
     && rm -rf /var/lib/apt/list/
 
@@ -45,17 +46,21 @@ RUN apt update && apt install -y \
   gz-garden \
 	&& rm -rf /var/lib/apt/lists/
 
+RUN pip uninstall empy && pip install empy==3.3.4 && pip install -U colcon-common-extensions
+
 ### Install ROS2
 RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
 RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
 
 RUN apt update && apt install -y \
   ros-humble-ros-base \
-  python3-colcon-common-extensions \
+  python3-empy=3.3.4-2 \
+  # python3-colcon-common-extensions \
   python3-rosdep \
   python3-vcstool \
   && rm -rf /var/lib/apt/lists/
 
+
 ###Add the USER env var
 RUN echo 'kasm-user ALL=(ALL) NOPASSWD: ALL' >> /etc/sudoers
 ENV HOME /home/kasm-user
@@ -121,6 +126,8 @@ RUN ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash \
 
 RUN ["/bin/bash", "-c", "sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh"]
 
+RUN pip uninstall empy -y && pip install empy==3.3.4
+
 # Build suave
 RUN ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash \
     && colcon build --symlink-install \
diff --git a/suave/suave/bluerov_gazebo.py b/suave/suave/bluerov_gazebo.py
index 68bceae..ef2c590 100644
--- a/suave/suave/bluerov_gazebo.py
+++ b/suave/suave/bluerov_gazebo.py
@@ -1,5 +1,4 @@
 from geometry_msgs.msg import Pose
-from std_msgs.msg import Bool
 from mavros_wrapper.ardusub_wrapper import BlueROVArduSubWrapper
 
 
diff --git a/suave/suave/follow_pipeline_lc.py b/suave/suave/follow_pipeline_lc.py
index c983fb9..5557e6b 100755
--- a/suave/suave/follow_pipeline_lc.py
+++ b/suave/suave/follow_pipeline_lc.py
@@ -1,15 +1,11 @@
 #!/usr/bin/env python
-from typing import Optional
-
 import math
 import rclpy
 import threading
 
 from rclpy.lifecycle import Node
-from rclpy.lifecycle import Publisher
 from rclpy.lifecycle import State
 from rclpy.lifecycle import TransitionCallbackReturn
-from rclpy.timer import Timer
 
 from suave_msgs.srv import GetPath
 from suave.bluerov_gazebo import BlueROVGazebo
@@ -88,7 +84,6 @@ def follow_pipeline(self):
             if self.abort_follow is True:
                 return
             timer.sleep()
-
         last_point = None
         self.distance_inspected = 0
         for gz_pose in pipe_path.result().path.poses:
diff --git a/suave/suave/spiral_search_lc.py b/suave/suave/spiral_search_lc.py
index 21bd3d4..069c056 100755
--- a/suave/suave/spiral_search_lc.py
+++ b/suave/suave/spiral_search_lc.py
@@ -99,8 +99,6 @@ def publish(self):
                     self.spiral_y,
                     resolution=0.1,
                     spiral_width=spiral_width)
-                self.get_logger().info(
-                        'setpoint_postion_local value {0}, {1}'.format(x, y))
 
                 self.ardusub.altitude = self.spiral_altitude + self.z_delta
                 self.goal_setpoint = self.ardusub.setpoint_position_local(
diff --git a/suave_metacontrol/launch/suave_metacontrol.launch.py b/suave_metacontrol/launch/suave_metacontrol.launch.py
index e285aa7..eb3e120 100644
--- a/suave_metacontrol/launch/suave_metacontrol.launch.py
+++ b/suave_metacontrol/launch/suave_metacontrol.launch.py
@@ -3,10 +3,8 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
 from launch.actions import IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
 
 from launch_ros.actions import Node