diff --git a/.clang-format b/.clang-format
old mode 100644
new mode 100755
diff --git a/.github/workflows/ci-ros-lint.yaml b/.github/workflows/ci-ros-lint.yaml
index 6e1a06608..d3c094e93 100644
--- a/.github/workflows/ci-ros-lint.yaml
+++ b/.github/workflows/ci-ros-lint.yaml
@@ -5,7 +5,7 @@ on:
 jobs:
   ament_lint:
     name: ament_${{ matrix.linter }}
-    runs-on: ubuntu-20.04
+    runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
@@ -15,7 +15,7 @@ jobs:
     - uses: ros-tooling/setup-ros@v0.2
     - uses: ros-tooling/action-ros-lint@v0.1
       with:
-        distribution: galactic
+        distribution: humble
         linter: ${{ matrix.linter }}
         package-name:
           smacc2
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt
index bcbd5d62f..c39928bff 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt
+++ b/smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt
@@ -85,11 +85,11 @@ add_library(${PROJECT_NAME} SHARED
   src/${PROJECT_NAME}/components/amcl/cp_amcl.cpp
   src/${PROJECT_NAME}/components/slam_toolbox/cp_slam_toolbox.cpp
 
-  src/${PROJECT_NAME}/client_behaviors/cb_active_stop.cpp 
-  src/${PROJECT_NAME}/client_behaviors/cb_load_waypoints_file.cpp 
-  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_free.cpp 
+  src/${PROJECT_NAME}/client_behaviors/cb_active_stop.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_load_waypoints_file.cpp
+  src/${PROJECT_NAME}/client_behaviors/cb_navigate_next_waypoint_free.cpp
   src/${PROJECT_NAME}/client_behaviors/cb_position_control_free_space.cpp
-  src/${PROJECT_NAME}/client_behaviors/cb_pure_spinning.cpp 
+  src/${PROJECT_NAME}/client_behaviors/cb_pure_spinning.cpp
   src/${PROJECT_NAME}/client_behaviors/cb_save_slam_map.cpp
 )
 
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp
index 286662157..76395de1e 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_active_stop.hpp
@@ -23,8 +23,10 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-namespace cl_nav2z {
-struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior {
+namespace cl_nav2z
+{
+struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior
+{
 private:
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
 
@@ -35,4 +37,4 @@ struct CbActiveStop : public smacc2::SmaccAsyncClientBehavior {
 
   void onExit() override;
 };
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp
index 4844ffd59..4f85bc0f4 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_load_waypoints_file.hpp
@@ -23,10 +23,12 @@
 #include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
 #include <smacc2/smacc_client_behavior.hpp>
 
-namespace cl_nav2z {
-struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior {
+namespace cl_nav2z
+{
+struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior
+{
 public:
-  CbLoadWaypointsFile(std::string filepath) ;
+  CbLoadWaypointsFile(std::string filepath);
 
   CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce);
 
@@ -39,6 +41,6 @@ struct CbLoadWaypointsFile : public smacc2::SmaccAsyncClientBehavior {
   std::optional<std::string> parameterName_;
   std::optional<std::string> packageNamespace_;
 
-  cl_nav2z::CpWaypointNavigatorBase *waypointsNavigator_;
+  cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
 };
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp
index 26c90d76f..bcf5e392c 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp
@@ -18,13 +18,14 @@
  *
  ******************************************************************************************************************/
 #pragma once
-#include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
 #include <nav2z_client/client_behaviors/cb_position_control_free_space.hpp>
+#include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
 
-namespace cl_nav2z {
+namespace cl_nav2z
+{
 
-class CbNavigateNextWaypointFree
-    : public cl_nav2z::CbPositionControlFreeSpace {
+class CbNavigateNextWaypointFree : public cl_nav2z::CbPositionControlFreeSpace
+{
 public:
   CbNavigateNextWaypointFree();
 
@@ -37,7 +38,7 @@ class CbNavigateNextWaypointFree
   void onExit() override;
 
 protected:
-  cl_nav2z::CpWaypointNavigatorBase *waypointsNavigator_;
+  cl_nav2z::CpWaypointNavigatorBase * waypointsNavigator_;
 };
 
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp
index e71a260d4..a76b814bf 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_position_control_free_space.hpp
@@ -24,8 +24,10 @@
 #include <nav2z_client/components/pose/cp_pose.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-namespace cl_nav2z {
-struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior {
+namespace cl_nav2z
+{
+struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior
+{
 private:
   double targetYaw_;
   bool goalReached_;
@@ -39,9 +41,8 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior {
 
   // Limit the maximum linear velocity and angular velocity to avoid sudden
   // movements
-  double max_linear_velocity = 1.0; // Adjust this value according to your needs
-  double max_angular_velocity =
-      1.0; // Adjust this value according to your needs
+  double max_linear_velocity = 1.0;   // Adjust this value according to your needs
+  double max_angular_velocity = 1.0;  // Adjust this value according to your needs
 
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
 
@@ -60,4 +61,4 @@ struct CbPositionControlFreeSpace : public smacc2::SmaccAsyncClientBehavior {
 
   void onExit() override;
 };
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp
index cc830711f..ffbc312b5 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_pure_spinning.hpp
@@ -23,8 +23,10 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-namespace cl_nav2z {
-struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior {
+namespace cl_nav2z
+{
+struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior
+{
 private:
   double targetYaw__rads;
   bool goalReached_;
@@ -43,4 +45,4 @@ struct CbPureSpinning : public smacc2::SmaccAsyncClientBehavior {
 
   void onExit() override;
 };
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp
index 2f0356cf9..0ef5ae9b3 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/client_behaviors/cb_save_slam_map.hpp
@@ -23,25 +23,24 @@
 #include <memory>
 #include <nav2_msgs/srv/save_map.hpp>
 #include <slam_toolbox/srv/save_map.hpp>
-#include <slam_toolbox/srv/save_map.hpp>
 #include <smacc2/client_behaviors/cb_call_service.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-
-namespace cl_nav2z {
+namespace cl_nav2z
+{
 using namespace std::chrono_literals;
 template <typename TService>
 using CbServiceCall = smacc2::client_behaviors::CbServiceCall<TService>;
 
-
-struct CbSaveSlamMap : public CbServiceCall<nav2_msgs::srv::SaveMap> {
-
-public:  
+struct CbSaveSlamMap : public CbServiceCall<nav2_msgs::srv::SaveMap>
+{
+public:
   CbSaveSlamMap();
   // void onEntry() override {}
 
   void onExit() override;
 
-  std::shared_ptr<nav2_msgs::srv::SaveMap::Request> getRequest(/*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/) ;
+  std::shared_ptr<nav2_msgs::srv::SaveMap::Request> getRequest(
+    /*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/);
 };
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp
index 50bb1fd41..ea177ba37 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_active_stop.cpp
@@ -24,29 +24,30 @@
 
 #include <nav2z_client/client_behaviors/cb_active_stop.hpp>
 
-namespace cl_nav2z {
+namespace cl_nav2z
+{
 CbActiveStop::CbActiveStop() {}
 
-void CbActiveStop::onEntry() {
+void CbActiveStop::onEntry()
+{
   auto nh = this->getNode();
-  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>(
-      "/cmd_vel", rclcpp::QoS(1));
+  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
 
   rclcpp::Rate loop_rate(5);
   geometry_msgs::msg::Twist cmd_vel_msg;
-  while (!this->isShutdownRequested()) {
+  while (!this->isShutdownRequested())
+  {
     cmd_vel_msg.linear.x = 0;
     cmd_vel_msg.angular.z = 0;
 
     cmd_vel_pub_->publish(cmd_vel_msg);
     loop_rate.sleep();
   }
-  RCLCPP_INFO_STREAM(getLogger(),
-                     "[" << getName() << "] Finished behavior execution");
+  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution");
 
   this->postSuccessEvent();
 }
 
 void CbActiveStop::onExit() {}
 
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp
index 4bb3e7dd2..13de11be8 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_load_waypoints_file.cpp
@@ -18,34 +18,40 @@
  *
  ******************************************************************************************************************/
 
-#include <smacc2/smacc_client_behavior.hpp>
+#include <nav2z_client/client_behaviors/cb_load_waypoints_file.hpp>
 #include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
-#include <nav2z_client/client_behaviors/cb_load_waypoints_file.hpp>           
-
-namespace cl_nav2z {
+#include <smacc2/smacc_client_behavior.hpp>
 
-  CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {}
+namespace cl_nav2z
+{
 
-  CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce)
-      : parameterName_(parameter_name), packageNamespace_(packagenamesapce) {}
+CbLoadWaypointsFile::CbLoadWaypointsFile(std::string filepath) : filepath_(filepath) {}
 
-  void CbLoadWaypointsFile::onEntry() {
-    requiresComponent(waypointsNavigator_); // this is a component from the
-                                            // nav2z_client library
+CbLoadWaypointsFile::CbLoadWaypointsFile(std::string parameter_name, std::string packagenamesapce)
+: parameterName_(parameter_name), packageNamespace_(packagenamesapce)
+{
+}
 
-    if (filepath_) {
-      this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value());
-    } else {
-      RCLCPP_INFO(getLogger(), "Loading waypoints from parameter %s",
-                  parameterName_.value().c_str());
-      this->waypointsNavigator_->loadWaypointsFromYamlParameter(
-          parameterName_.value(), packageNamespace_.value());
-    }
+void CbLoadWaypointsFile::onEntry()
+{
+  requiresComponent(waypointsNavigator_);  // this is a component from the
+                                           // nav2z_client library
 
-    // change this to skip some points of the yaml file, default = 0
-    waypointsNavigator_->currentWaypoint_ = 0;
-    this->postSuccessEvent();
+  if (filepath_)
+  {
+    this->waypointsNavigator_->loadWayPointsFromFile(filepath_.value());
   }
+  else
+  {
+    RCLCPP_INFO(getLogger(), "Loading waypoints from parameter %s", parameterName_.value().c_str());
+    this->waypointsNavigator_->loadWaypointsFromYamlParameter(
+      parameterName_.value(), packageNamespace_.value());
+  }
+
+  // change this to skip some points of the yaml file, default = 0
+  waypointsNavigator_->currentWaypoint_ = 0;
+  this->postSuccessEvent();
+}
 
-  void CbLoadWaypointsFile::onExit() {}
-} // namespace cl_nav2z
+void CbLoadWaypointsFile::onExit() {}
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp
index f5637980a..02b7b8375 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.cpp
@@ -19,40 +19,36 @@
  ******************************************************************************************************************/
 #include <nav2z_client/client_behaviors/cb_navigate_next_waypoint_free.hpp>
 
-
-  namespace cl_nav2z {
-  
-  CbNavigateNextWaypointFree::CbNavigateNextWaypointFree() {}
-
-  CbNavigateNextWaypointFree::~CbNavigateNextWaypointFree() {}
-
-  void CbNavigateNextWaypointFree::onEntry() {
-    requiresComponent(this->waypointsNavigator_);
-    this->target_pose_ = this->waypointsNavigator_->getCurrentPose();
-
-    this->onSuccess(&CbNavigateNextWaypointFree::CbNavigateNextWaypointFree::onSucessCallback, this);
-    RCLCPP_INFO_STREAM(
-        getLogger(),
-        "[CbNavigateNextWaypoint] initial load file target pose: x: "
-            << this->target_pose_.position.x
-            << ", y: " << this->target_pose_.position.y);
-    CbPositionControlFreeSpace::onEntry();
-  }
-
-  void CbNavigateNextWaypointFree::onSucessCallback() {
-    RCLCPP_INFO_STREAM(
-        getLogger(),
-        "[CbNavigateNextWaypoint] Success on planning to next waypoint");
-    this->waypointsNavigator_->notifyGoalReached();
-    this->waypointsNavigator_->forward(1);
-    RCLCPP_INFO_STREAM(
-        getLogger(), "[CbNavigateNextWaypoint] next position index: "
-                         << this->waypointsNavigator_->getCurrentWaypointIndex()
-                         << "/"
-                         << this->waypointsNavigator_->getWaypoints().size());
-  }
-
-  void CbNavigateNextWaypointFree::onExit() {}
-
-
-} // namespace cl_nav2z
+namespace cl_nav2z
+{
+
+CbNavigateNextWaypointFree::CbNavigateNextWaypointFree() {}
+
+CbNavigateNextWaypointFree::~CbNavigateNextWaypointFree() {}
+
+void CbNavigateNextWaypointFree::onEntry()
+{
+  requiresComponent(this->waypointsNavigator_);
+  this->target_pose_ = this->waypointsNavigator_->getCurrentPose();
+
+  this->onSuccess(&CbNavigateNextWaypointFree::CbNavigateNextWaypointFree::onSucessCallback, this);
+  RCLCPP_INFO_STREAM(
+    getLogger(), "[CbNavigateNextWaypoint] initial load file target pose: x: "
+                   << this->target_pose_.position.x << ", y: " << this->target_pose_.position.y);
+  CbPositionControlFreeSpace::onEntry();
+}
+
+void CbNavigateNextWaypointFree::onSucessCallback()
+{
+  RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateNextWaypoint] Success on planning to next waypoint");
+  this->waypointsNavigator_->notifyGoalReached();
+  this->waypointsNavigator_->forward(1);
+  RCLCPP_INFO_STREAM(
+    getLogger(), "[CbNavigateNextWaypoint] next position index: "
+                   << this->waypointsNavigator_->getCurrentWaypointIndex() << "/"
+                   << this->waypointsNavigator_->getWaypoints().size());
+}
+
+void CbNavigateNextWaypointFree::onExit() {}
+
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp
index 7f789aba7..c0671a863 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_position_control_free_space.cpp
@@ -22,21 +22,24 @@
 #include <geometry_msgs/msg/twist.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
 
-#include <nav2z_client/components/pose/cp_pose.hpp>
 #include <nav2z_client/client_behaviors/cb_position_control_free_space.hpp>
+#include <nav2z_client/components/pose/cp_pose.hpp>
 
-namespace cl_nav2z {
+namespace cl_nav2z
+{
 CbPositionControlFreeSpace::CbPositionControlFreeSpace()
-    : targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0) {}
+: targetYaw_(0), k_betta_(1.0), max_angular_yaw_speed_(1.0)
+{
+}
 
 void CbPositionControlFreeSpace::updateParameters() {}
 
-void CbPositionControlFreeSpace::onEntry() {
+void CbPositionControlFreeSpace::onEntry()
+{
   auto nh = this->getNode();
-  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>(
-      "/cmd_vel", rclcpp::QoS(1));
+  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
 
-  cl_nav2z::Pose *pose;
+  cl_nav2z::Pose * pose;
 
   this->requiresComponent(pose);
 
@@ -58,18 +61,19 @@ void CbPositionControlFreeSpace::onEntry() {
   double ki_angular = 0.0;
   double kd_angular = 0.1;
 
-  while (rclcpp::ok() && !goalReached_) {
-    RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *getNode()->get_clock(), 200,
-                                "CbPositionControlFreeSpace, current pose: "
-                                    << currentPose.position.x << ", "
-                                    << currentPose.position.y << ", "
-                                    << tf2::getYaw(currentPose.orientation));
+  while (rclcpp::ok() && !goalReached_)
+  {
+    RCLCPP_INFO_STREAM_THROTTLE(
+      getLogger(), *getNode()->get_clock(), 200,
+      "CbPositionControlFreeSpace, current pose: " << currentPose.position.x << ", "
+                                                   << currentPose.position.y << ", "
+                                                   << tf2::getYaw(currentPose.orientation));
 
-    RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *getNode()->get_clock(), 200,
-                                "CbPositionControlFreeSpace, target pose: "
-                                    << target_pose_.position.x << ", "
-                                    << target_pose_.position.y << ", "
-                                    << tf2::getYaw(target_pose_.orientation));
+    RCLCPP_INFO_STREAM_THROTTLE(
+      getLogger(), *getNode()->get_clock(), 200,
+      "CbPositionControlFreeSpace, target pose: " << target_pose_.position.x << ", "
+                                                  << target_pose_.position.y << ", "
+                                                  << tf2::getYaw(target_pose_.orientation));
 
     tf2::Quaternion q;
     currentPose = pose->toPoseMsg();
@@ -82,15 +86,15 @@ void CbPositionControlFreeSpace::onEntry() {
     double error_y = target_pose_.position.y - currentPose.position.y;
 
     // Calculate the distance to the target pose
-    double distance_to_target =
-        std::sqrt(error_x * error_x + error_y * error_y);
+    double distance_to_target = std::sqrt(error_x * error_x + error_y * error_y);
 
-    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] distance to target: "
-                                        << distance_to_target << " ( th: "
-                                        << threshold_distance_ << ")");
+    RCLCPP_INFO_STREAM(
+      getLogger(), "[" << getName() << "] distance to target: " << distance_to_target
+                       << " ( th: " << threshold_distance_ << ")");
 
     // Check if the robot has reached the target pose
-    if (distance_to_target < threshold_distance_) {
+    if (distance_to_target < threshold_distance_)
+    {
       RCLCPP_INFO(getLogger(), "Goal reached!");
       // Stop the robot by setting the velocity commands to zero
       geometry_msgs::msg::Twist cmd_vel_msg;
@@ -98,27 +102,24 @@ void CbPositionControlFreeSpace::onEntry() {
       cmd_vel_msg.angular.z = 0.0;
       cmd_vel_pub_->publish(cmd_vel_msg);
       break;
-    } else {
+    }
+    else
+    {
       // Calculate the desired orientation angle
       double desired_yaw = std::atan2(error_y, error_x);
 
       // Calculate the difference between the desired orientation and the
       // current orientation
-      double yaw_error =
-          desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI);
+      double yaw_error = desired_yaw - (tf2::getYaw(currentPose.orientation) + M_PI);
 
       // Ensure the yaw error is within the range [-pi, pi]
-      while (yaw_error > M_PI)
-        yaw_error -= 2 * M_PI;
-      while (yaw_error < -M_PI)
-        yaw_error += 2 * M_PI;
+      while (yaw_error > M_PI) yaw_error -= 2 * M_PI;
+      while (yaw_error < -M_PI) yaw_error += 2 * M_PI;
 
       // Calculate the control signals (velocity commands) using PID controllers
-      double cmd_linear_x =
-          kp_linear * distance_to_target + ki_linear * integral_linear_ +
-          kd_linear * (distance_to_target - prev_error_linear_);
-      double cmd_angular_z = kp_angular * yaw_error +
-                             ki_angular * integral_angular_ +
+      double cmd_linear_x = kp_linear * distance_to_target + ki_linear * integral_linear_ +
+                            kd_linear * (distance_to_target - prev_error_linear_);
+      double cmd_angular_z = kp_angular * yaw_error + ki_angular * integral_angular_ +
                              kd_angular * (yaw_error - prev_error_angular_);
 
       if (cmd_linear_x > max_linear_velocity)
@@ -188,12 +189,11 @@ void CbPositionControlFreeSpace::onEntry() {
     }
   }
 
-  RCLCPP_INFO_STREAM(getLogger(),
-                     "[" << getName() << "] Finished behavior execution");
+  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Finished behavior execution");
 
   this->postSuccessEvent();
 }
 
 void CbPositionControlFreeSpace::onExit() {}
 
-} // namespace cl_nav2z
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp
index 5c264deaf..19de603b1 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_pure_spinning.cpp
@@ -20,85 +20,83 @@
 
 #include <angles/angles.h>
 #include <geometry_msgs/msg/twist.hpp>
+#include <nav2z_client/client_behaviors/cb_pure_spinning.hpp>
 #include <nav2z_client/components/pose/cp_pose.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
-#include <nav2z_client/client_behaviors/cb_pure_spinning.hpp>
-namespace cl_nav2z {
-
-
-  CbPureSpinning::CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed)
-      : targetYaw__rads(targetYaw_rads), k_betta_(1.0),
-        max_angular_yaw_speed_(max_angular_yaw_speed),
-        yaw_goal_tolerance_rads_(0.1) {}
-
-  void CbPureSpinning::updateParameters() {}
-
-  void CbPureSpinning::onEntry() {
-    auto nh = this->getNode();
-    cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>(
-        "/cmd_vel", rclcpp::QoS(1));
-
-    cl_nav2z::Pose *pose;
-    this->requiresComponent(pose);
-
-    geometry_msgs::msg::Twist cmd_vel;
-    goalReached_ = false;
-
-    geometry_msgs::msg::Pose currentPose = pose->toPoseMsg();
-
-    rclcpp::Rate loop_rate(10);
-    double countAngle = 0;
-    auto prevyaw = tf2::getYaw(currentPose.orientation);
-    while (rclcpp::ok() && !goalReached_) {
-      tf2::Quaternion q;
-      currentPose = pose->toPoseMsg();
-      tf2::fromMsg(currentPose.orientation, q);
-      auto currentYaw = tf2::getYaw(currentPose.orientation);
-      auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw);
-      countAngle += deltaAngle;
-
-      prevyaw = currentYaw;
-      double angular_error = targetYaw__rads - countAngle;
-
-      auto omega = angular_error * k_betta_;
+namespace cl_nav2z
+{
+
+CbPureSpinning::CbPureSpinning(double targetYaw_rads, double max_angular_yaw_speed)
+: targetYaw__rads(targetYaw_rads),
+  k_betta_(1.0),
+  max_angular_yaw_speed_(max_angular_yaw_speed),
+  yaw_goal_tolerance_rads_(0.1)
+{
+}
+
+void CbPureSpinning::updateParameters() {}
+
+void CbPureSpinning::onEntry()
+{
+  auto nh = this->getNode();
+  cmd_vel_pub_ = nh->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::QoS(1));
+
+  cl_nav2z::Pose * pose;
+  this->requiresComponent(pose);
+
+  geometry_msgs::msg::Twist cmd_vel;
+  goalReached_ = false;
+
+  geometry_msgs::msg::Pose currentPose = pose->toPoseMsg();
+
+  rclcpp::Rate loop_rate(10);
+  double countAngle = 0;
+  auto prevyaw = tf2::getYaw(currentPose.orientation);
+  while (rclcpp::ok() && !goalReached_)
+  {
+    tf2::Quaternion q;
+    currentPose = pose->toPoseMsg();
+    tf2::fromMsg(currentPose.orientation, q);
+    auto currentYaw = tf2::getYaw(currentPose.orientation);
+    auto deltaAngle = angles::shortest_angular_distance(prevyaw, currentYaw);
+    countAngle += deltaAngle;
+
+    prevyaw = currentYaw;
+    double angular_error = targetYaw__rads - countAngle;
+
+    auto omega = angular_error * k_betta_;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0;
+    cmd_vel.linear.z = 0;
+    cmd_vel.angular.z =
+      std::min(std::max(omega, -fabs(max_angular_yaw_speed_)), fabs(max_angular_yaw_speed_));
+
+    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] delta angle: " << deltaAngle);
+    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: " << countAngle);
+    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] k_betta_: " << k_betta_);
+
+    RCLCPP_INFO_STREAM(
+      getLogger(), "[" << getName() << "] angular error: " << angular_error << "("
+                       << yaw_goal_tolerance_rads_ << ")");
+    RCLCPP_INFO_STREAM(
+      getLogger(), "[" << getName() << "] command angular speed: " << cmd_vel.angular.z);
+
+    if (fabs(angular_error) < yaw_goal_tolerance_rads_)
+    {
+      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] GOAL REACHED. Sending stop command.");
+      goalReached_ = true;
       cmd_vel.linear.x = 0;
-      cmd_vel.linear.y = 0;
-      cmd_vel.linear.z = 0;
-      cmd_vel.angular.z =
-          std::min(std::max(omega, -fabs(max_angular_yaw_speed_)),
-                   fabs(max_angular_yaw_speed_));
-
-      RCLCPP_INFO_STREAM(getLogger(),
-                         "[" << getName() << "] delta angle: " << deltaAngle);
-      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] cummulated angle: "
-                                          << countAngle);
-      RCLCPP_INFO_STREAM(getLogger(),
-                         "[" << getName() << "] k_betta_: " << k_betta_);
-
-      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] angular error: "
-                                          << angular_error << "("
-                                          << yaw_goal_tolerance_rads_ << ")");
-      RCLCPP_INFO_STREAM(getLogger(), "[" << getName()
-                                          << "] command angular speed: "
-                                          << cmd_vel.angular.z);
-
-      if (fabs(angular_error) < yaw_goal_tolerance_rads_) {
-        RCLCPP_INFO_STREAM(getLogger(),
-                           "[" << getName()
-                               << "] GOAL REACHED. Sending stop command.");
-        goalReached_ = true;
-        cmd_vel.linear.x = 0;
-        cmd_vel.angular.z = 0;
-        break;
-      }
-
-      this->cmd_vel_pub_->publish(cmd_vel);
-
-      loop_rate.sleep();
+      cmd_vel.angular.z = 0;
+      break;
     }
 
-    this->postSuccessEvent();
+    this->cmd_vel_pub_->publish(cmd_vel);
+
+    loop_rate.sleep();
   }
 
-  void CbPureSpinning::onExit() {}
-} // namespace cl_nav2z
+  this->postSuccessEvent();
+}
+
+void CbPureSpinning::onExit() {}
+}  // namespace cl_nav2z
diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp
index 5b5e9d473..6500bc8f3 100644
--- a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp
+++ b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_save_slam_map.cpp
@@ -18,69 +18,67 @@
  *
  ******************************************************************************************************************/
 
-#include <memory>
-#include <chrono>
-#include <std_msgs/msg/string.hpp>
-#include <nav2_msgs/srv/save_map.hpp>
-#include <slam_toolbox/srv/save_map.hpp>
 #include <angles/angles.h>
+#include <chrono>
 #include <geometry_msgs/msg/twist.hpp>
+#include <memory>
+#include <nav2_msgs/srv/save_map.hpp>
+#include <nav2z_client/client_behaviors/cb_save_slam_map.hpp>
 #include <nav2z_client/components/pose/cp_pose.hpp>
 #include <slam_toolbox/srv/save_map.hpp>
 #include <smacc2/client_behaviors/cb_call_service.hpp>
 #include <smacc2/smacc_asynchronous_client_behavior.hpp>
-#include <nav2z_client/client_behaviors/cb_save_slam_map.hpp>
-
+#include <std_msgs/msg/string.hpp>
 
-namespace cl_nav2z {
+namespace cl_nav2z
+{
 using namespace std::chrono_literals;
 
-  
-  CbSaveSlamMap::CbSaveSlamMap(): CbServiceCall("/map_saver/save_map",
-                         getRequest()) {
-      // : CbServiceCall("/slam_toolbox/save_map",
-      //                   getRequest()) {
+CbSaveSlamMap::CbSaveSlamMap() : CbServiceCall("/map_saver/save_map", getRequest())
+{
+  // : CbServiceCall("/slam_toolbox/save_map",
+  //                   getRequest()) {
 
-    // map_name.data = "saved_map";
-    // auto request = getRequest(map_name);
-    // RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded");
-  }
+  // map_name.data = "saved_map";
+  // auto request = getRequest(map_name);
+  // RCLCPP_INFO_STREAM(getLogger(), "Save Slam Map builded");
+}
 
-  // void onEntry() override {}
+// void onEntry() override {}
 
-  void CbSaveSlamMap::onExit()  {}
+void CbSaveSlamMap::onExit() {}
 
-  std::shared_ptr<nav2_msgs::srv::SaveMap::Request> CbSaveSlamMap::getRequest(/*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/) {
-    nav2_msgs::srv::SaveMap_Request map_save;
-    std_msgs::msg::String map_name;
-    
-    // // map_name.data = "saved_map";
-    // map_save.map_topic = "map";
-    // map_save.map_url = "${workspacesFolder}/maps/saved_map";
-    // map_save.image_format = "png";
-    // map_save.occupied_thresh = 0.65;
-    // map_save.free_thresh = 0.25;
-    // map_save.map_mode = "trinary";
+std::shared_ptr<nav2_msgs::srv::SaveMap::Request> CbSaveSlamMap::getRequest(
+  /*slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type saved_map_name*/)
+{
+  nav2_msgs::srv::SaveMap_Request map_save;
+  std_msgs::msg::String map_name;
 
-    // // auto request = std::make_shared<slam_toolbox::srv::SaveMap::Request>();
-    // // // request->name = saved_map_name;
-    // // request->name = map_name;
-    // // return request;
-    // auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>(map_save);
+  // // map_name.data = "saved_map";
+  // map_save.map_topic = "map";
+  // map_save.map_url = "${workspacesFolder}/maps/saved_map";
+  // map_save.image_format = "png";
+  // map_save.occupied_thresh = 0.65;
+  // map_save.free_thresh = 0.25;
+  // map_save.map_mode = "trinary";
 
-    auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
-    request->map_topic = "map";
-    request->map_url = "/tmp/saved_map";
-    request->image_format = "png";
-    request->occupied_thresh = 0.65;
-    request->free_thresh = 0.25;
-    request->map_mode = "trinary";
-    
+  // // auto request = std::make_shared<slam_toolbox::srv::SaveMap::Request>();
+  // // // request->name = saved_map_name;
+  // // request->name = map_name;
+  // // return request;
+  // auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>(map_save);
 
-    return request;
-}
-} // namespace cl_nav2z
+  auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  request->map_topic = "map";
+  request->map_url = "/tmp/saved_map";
+  request->image_format = "png";
+  request->occupied_thresh = 0.65;
+  request->free_thresh = 0.25;
+  request->map_mode = "trinary";
 
+  return request;
+}
+}  // namespace cl_nav2z
 
 // slam_toolbox::srv::SaveMap_Request_<std::allocator<void> >::_name_type