diff --git a/CMakeLists.txt b/CMakeLists.txt
index 46a7290..803eb25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,7 +3,8 @@ project(heuristic_planners)
 
 add_compile_options(-std=c++17 -Wall -Werror -Wextra -pedantic )
 
-option(BUILD_DOC            "Build documentation   " OFF)
+option(BUILD_DOC            "Build documentation   " ON)
+option(BUILD_ROS_SUPPORT    "Build with ROS Support" ON)
 option(OPTIMIZE_FLAG        "Enable Compiler Optimization for Runtime Performance" ON)
 option(BUILD_DEBUG          "Build debug features  " OFF)
 option(BUILD_COMPUTE_STATS  "Build Algorithms with statistics" ON)
@@ -45,33 +46,21 @@ unset(BUILD_DOC CACHE)
 
 find_package(Boost)
 
-add_compile_definitions(ROS)
-  # find dependencies
-set(PROJECT_DEPENDENCIES
-  ament_cmake
-  ament_cmake_ros
-  rclcpp
-  std_msgs
-  std_srvs
-  geometry_msgs
-  nav_msgs
-  visualization_msgs
-  builtin_interfaces
-  pcl_conversions
-  pcl_ros
-  octomap_ros
-  nav2_costmap_2d
-  trajectory_msgs
-  tf2
-  tf2_ros
-  tf2_geometry_msgs
-)
-
-
-foreach(DEPENDENCY ${PROJECT_DEPENDENCIES})
-  find_package(${DEPENDENCY} REQUIRED)
-endforeach()
-
+if(BUILD_ROS_SUPPORT)
+  add_compile_definitions(ROS)
+  ## Find catkin macros and libraries
+  find_package(catkin REQUIRED COMPONENTS
+    std_msgs
+    geometry_msgs
+    nav_msgs
+    visualization_msgs
+    message_generation
+    roscpp
+    pcl_conversions
+    pcl_ros
+    octomap_ros
+    costmap_2d
+  )
   #Eigen is used to calculate metrics parameters
   find_package (Eigen3 REQUIRED NO_MODULE)
   find_package(OpenSSL REQUIRED)
@@ -83,35 +72,48 @@ endforeach()
 ################################################
 ## Declare ROS messages, services and actions ##
 ################################################
-# catkin_python_setup()
+catkin_python_setup()
 
 ## Generate services in the 'srv' folder
-find_package(rosidl_default_generators REQUIRED)
-
-set(SRVS_FILES
-  "srv/GetPath.srv"
-  "srv/SetAlgorithm.srv"
+add_service_files(
+  FILES
+  GetPath.srv
+  SetAlgorithm.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
-
-rosidl_generate_interfaces(${PROJECT_NAME}
-  ${SRVS_FILES} 
-  DEPENDENCIES geometry_msgs std_msgs
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs std_msgs
 )
 
-# list(APPEND PROJECT_DEPENDENCIES ${PROJECT_NAME}_msgs)
 
-##########
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF
+ # 
+ CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs roscpp message_runtime costmap_2d
+#  DEPENDS system_lib
+)
+endif()
+###########
 ## Build ##
 ###########
 
 include_directories(
   include
-  include/Planners
+  ${catkin_INCLUDE_DIRS}
   ${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6
 )
-
 if( BUILD_VOROCPP )
   list(APPEND VORO_SOURCES  ${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6/src/voro++.cc
                             ${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6/src/cell.cc
@@ -131,8 +133,6 @@ if( BUILD_VOROCPP )
   add_executable(voro++ include/voro++-0.4.6/src/cmd_line.cc)
   target_link_libraries(voro++ Voro++)
 endif()
-
-
 ##This is True by default but it's good to have it here as a reminder
 set(BUILD_SHARED_LIBS TRUE)
 list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/geometry_utils.cpp
@@ -141,7 +141,9 @@ list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/geometry_utils.cpp
                                           src/utils/utils.cpp       
                                           src/utils/metrics.cpp       
                                           )
+if(BUILD_ROS_SUPPORT)
   list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/ros/ROSInterfaces.cpp)
+endif()
 
 add_library(AlgorithmBase                   src/Planners/AlgorithmBase.cpp 
                                             ${${PROJECT_NAME}_UTILS_SOURCES})
@@ -151,6 +153,8 @@ add_library(AStar                  src/Planners/AStar.cpp
                                             ${${PROJECT_NAME}_UTILS_SOURCES}
                                             )
 
+add_library(AStar_Gradient            src/Planners/AStar_Gradient.cpp ) 
+add_library(AStar_EDF            src/Planners/AStar_EDF.cpp ) 
 add_library(AStarM1            src/Planners/AStarM1.cpp ) 
 add_library(AStarM2            src/Planners/AStarM2.cpp ) 
 add_library(ThetaStar          src/Planners/ThetaStar.cpp )
@@ -160,73 +164,69 @@ add_library(LazyThetaStar      src/Planners/LazyThetaStar.cpp )
 add_library(LazyThetaStarM1    src/Planners/LazyThetaStarM1.cpp )
 add_library(LazyThetaStarM1Mod src/Planners/LazyThetaStarM1Mod.cpp )
 add_library(LazyThetaStarM2    src/Planners/LazyThetaStarM2.cpp )
+add_library(LazyThetaStar_Gradient    src/Planners/LazyThetaStar_Gradient.cpp )
+add_library(LazyThetaStar_EDF    src/Planners/LazyThetaStar_EDF.cpp )
+
 
-list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2)  
-
-foreach(LIB ${${PROJECT_NAME}_LIBRARIES})
-  message("Building ${LIB}")
-  target_include_directories(${LIB}
-  PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>)
-  ament_target_dependencies(${LIB} ${PROJECT_DEPENDENCIES})
-  ament_export_targets(export_${LIB} HAS_LIBRARY_TARGET)
-  ament_export_dependencies(${LIB} ${PROJECT_DEPENDENCIES})
-  target_link_libraries(${LIB}  ${Boost_LIBRARIES})
-endforeach()
-
-
-add_executable(planner_ros_node src/ROS/planner_ros_node.cpp )
-ament_target_dependencies(planner_ros_node ${PROJECT_DEPENDENCIES})
-# rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
-# target_link_libraries(planner_ros_node "${cpp_typesupport_target}")
-rosidl_target_interfaces(planner_ros_node
-      ${PROJECT_NAME} "rosidl_typesupport_cpp")
-target_link_libraries(planner_ros_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
-
-#add_executable(local_planner_node src/ROS/local_planner_node.cpp src/Planners/LocalPlanner.cpp include/Planners/LocalPlanner.hpp)
-#ament_target_dependencies(local_planner_node ${PROJECT_DEPENDENCIES})
-#rosidl_target_interfaces(local_planner_node
-#      ${PROJECT_NAME} "rosidl_typesupport_cpp")
-#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
-
-#rosidl_target_interfaces(local_planner_node
-#      ${PROJECT_NAME} "rosidl_typesupport_cpp")
-#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
-
-install(TARGETS
-  planner_ros_node
-  #local_planner_node
-  DESTINATION lib/${PROJECT_NAME})
 
+list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF) 
+target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES})
+
+if(BUILD_ROS_SUPPORT)
+  add_executable(planner_ros_node src/ROS/planner_ros_node.cpp )
+  add_dependencies(planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES})
+  target_link_libraries(planner_ros_node ${catkin_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
+  list(APPEND ${PROJECT_NAME}_TARGETS planner_ros_node)
 #############
   ## Install ##
 #############
 
-ament_export_dependencies(${PROJECT_DEPENDENCIES})
 
-install(
-  DIRECTORY include/
-  DESTINATION include
-)
+## Mark executables and/or libraries for installation
+  install(TARGETS ${${PROJECT_NAME}_TARGETS}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+  
+  catkin_install_python(PROGRAMS  scripts/test_algorithms.py
+                                  scripts/test_algorithms_compare.py
+                                  scripts/test_algorithms_performance.py
+                                  scripts/test_algorithms_pseudo_random_paths.py
+                                  scripts/compare_trajectories_rviz.py
+                                  scripts/plan_caller.py
+                                  scripts/generate2d_random_map.py
+                                  setup.py
+                                  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
 
-foreach(LIB ${${PROJECT_NAME}_LIBRARIES})
-  install(
-    TARGETS ${LIB}
-    EXPORT export_${LIB}
-    ARCHIVE DESTINATION libs/${LIB}
-    LIBRARY DESTINATION libs/${LIB}
-    RUNTIME DESTINATION bin
+## Mark cpp header files for installation
+  install(DIRECTORY include/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
   )
-endforeach()
 
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME})
+## Mark other files for installation (e.g. launch and bag files, etc.)
 
-install(DIRECTORY
-  resources 
-  DESTINATION share/${PROJECT_NAME})
+  install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
+  install(DIRECTORY config/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
+  install(DIRECTORY rviz/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz)
+  install(DIRECTORY resources/ 
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/resources  
+  PATTERN ".gridm" EXCLUDE)
 
-ament_package()
+else()
+  
+  install(DIRECTORY include/
+    DESTINATION ${CMAKE_INSTALL_PREFIX}/include
+    FILES_MATCHING PATTERN "*.hpp"
+  )
+  
+  install(TARGETS ${${PROJECT_NAME}_TARGETS}
+    DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
+  )
 
+endif()
+unset(BUILD_ROS_SUPPORT CACHE)
diff --git a/config/costmap_for_rand_maps.yaml b/config/costmap_for_rand_maps.yaml
index b65149d..b3472a6 100644
--- a/config/costmap_for_rand_maps.yaml
+++ b/config/costmap_for_rand_maps.yaml
@@ -18,7 +18,7 @@ costmap:
       cost_scaling_factor: 2
       enabled: true
       inflate_unknown: false
-      inflation_radius: 3.0
+      _radius: 3.0
 
    static_layer:
       unknown_cost_value: -1
diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp
index bc552ee..8f62dea 100644
--- a/include/Grid3D/grid3d.hpp
+++ b/include/Grid3D/grid3d.hpp
@@ -1,4 +1,4 @@
-#ifndef __GRID3D_HPP__
+ #ifndef __GRID3D_HPP__
 #define __GRID3D_HPP__
 
 /**
@@ -7,25 +7,15 @@
  * @author Francisco J. Perez Grau and Fernando Caballero
  */
 
-// #include <ros/ros.h>
-#include <chrono>
-#include <pcl-1.10/pcl/impl/point_types.hpp>
-#include <pcl-1.10/pcl/kdtree/kdtree_flann.h>
-#include <pcl-1.10/pcl/point_cloud.h>
-#include <rclcpp/publisher.hpp>
-#include <rclcpp/rclcpp.hpp>
+#include <sys/time.h>
+#include <ros/ros.h>
 #include <octomap/octomap.h>
 #include <octomap/OcTree.h>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <rclcpp/timer.hpp>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-#include <std_msgs/msg/float32.hpp>
+#include <nav_msgs/OccupancyGrid.h>
+#include <std_msgs/Float32.h>
 #include <stdio.h> 
-
 // PCL
-#include <sys/time.h>
-// #include <pcl_ros/point_cloud.hpp>
-#include <pcl/common/common.h>
+#include <pcl/point_cloud.h>
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <cmath>
@@ -43,14 +33,13 @@
 // #include "voro++-0.4.6/src/voro++.hh"
 // #endif
 
-class Grid3d {
+class Grid3d
+{
 private:
-  rclcpp::Node* node_ptr_;
 	
 	// Ros parameters
-	// ros::NodeHandle m_nh;
+	ros::NodeHandle m_nh;
 	bool m_publishPc;
-  
 	std::string m_mapPath, m_nodeName;
 	std::string m_globalFrameId;
 	float m_sensorDev, m_gridSlice;
@@ -67,61 +56,55 @@ class Grid3d {
 	int m_gridStepY, m_gridStepZ;
 	
 	// 3D point clound representation of the map
-  	boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> m_cloud;
+	pcl::PointCloud<pcl::PointXYZI>::Ptr m_cloud;
 	pcl::KdTreeFLANN<pcl::PointXYZI> m_kdtree;
 	
 	// Visualization of the map as pointcloud
-	sensor_msgs::msg::PointCloud2 m_pcMsg;
-  	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr m_pcPub;
-  	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr percent_computed_pub_;
-  	rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr m_gridSlicePub;
-
-	// ros::Timer mapTimer;
-  	rclcpp::TimerBase::SharedPtr mapTimer;
+	sensor_msgs::PointCloud2 m_pcMsg;
+	ros::Publisher m_pcPub, percent_computed_pub_;
+	ros::Timer mapTimer;
 			
 	// Visualization of a grid slice as 2D grid map msg
-	nav_msgs::msg::OccupancyGrid m_gridSliceMsg;
-	// ros::Timer gridTimer;
-  	rclcpp::TimerBase::SharedPtr gridTimer;
+	nav_msgs::OccupancyGrid m_gridSliceMsg;
+	ros::Publisher m_gridSlicePub;
+	ros::Timer gridTimer;
 
 	//Parameters added to allow a new exp function to test different gridmaps
 	double cost_scaling_factor, robot_radius;
 	bool use_costmap_function;
 	
 public:
-	Grid3d(rclcpp::Node* node_ptr):node_ptr_(node_ptr), m_cloud(new pcl::PointCloud<pcl::PointXYZI>)
+	Grid3d(): m_cloud(new pcl::PointCloud<pcl::PointXYZI>)
 	{
-    
-
-    node_ptr_->declare_parameter<std::string>("name", "grid3d");
-    node_ptr_->declare_parameter<std::string>("global_frame_id", "map");
-    node_ptr_->declare_parameter<std::string>("map_path", "map.ot");
-    node_ptr_->declare_parameter<bool>("publish_point_cloud", true);
-    node_ptr_->declare_parameter<double>("publish_point_cloud_rate", 0.2);
-    node_ptr_->declare_parameter<double>("publish_grid_slice", 1.0);
-    node_ptr_->declare_parameter<double>("publish_grid_slice_rate", 0.2);
-    node_ptr_->declare_parameter<double>("sensor_dev", 0.2);
-    // node_ptr_->declare_parameter<double>("cost_scaling_factor", 0.8);
-    // node_ptr_->declare_parameter<double>("robot_radius", 0.4);
-    node_ptr_->declare_parameter<bool>("use_costmap_function", true);
-
-    node_ptr_->get_parameter("name", m_nodeName);
-    node_ptr_->get_parameter("global_frame_id", m_globalFrameId);
-    node_ptr_->get_parameter("map_path", m_mapPath);
-    node_ptr_->get_parameter("publish_point_cloud", m_publishPc);
-    node_ptr_->get_parameter("publish_point_cloud_rate", m_publishPointCloudRate);
-    node_ptr_->get_parameter("publish_grid_slice", m_gridSlice);
-    node_ptr_->get_parameter("publish_grid_slice_rate", m_publishGridSliceRate);
-    node_ptr_->get_parameter("sensor_dev", m_sensorDev);
-    node_ptr_->get_parameter("cost_scaling_factor", cost_scaling_factor);
-    node_ptr_->get_parameter("robot_radius", robot_radius);
-    node_ptr_->get_parameter("use_costmap_function", use_costmap_function);
+		// Load paraeters
+		double value;
+		ros::NodeHandle lnh("~");
+		lnh.param("name", m_nodeName, std::string("grid3d"));
+		if(!lnh.getParam("global_frame_id", m_globalFrameId))
+			m_globalFrameId = "map";	
+		if(!lnh.getParam("map_path", m_mapPath))
+			m_mapPath = "map.ot";
+		if(!lnh.getParam("publish_point_cloud", m_publishPc))
+			m_publishPc = true;
+		if(!lnh.getParam("publish_point_cloud_rate", m_publishPointCloudRate))
+			m_publishPointCloudRate = 0.2;	
+		if(!lnh.getParam("publish_grid_slice", value))
+			value = 1.0;
+		if(!lnh.getParam("publish_grid_slice_rate", m_publishGridSliceRate))
+			m_publishGridSliceRate = 0.2;
+		m_gridSlice = (float)value;
+		if(!lnh.getParam("sensor_dev", value))
+			value = 0.2;
+		m_sensorDev = (float)value;
+
+		lnh.param("cost_scaling_factor", cost_scaling_factor, 0.8); //0.8		
+		lnh.param("robot_radius", robot_radius, 0.4);		//0.4
+		lnh.param("use_costmap_function", use_costmap_function, (bool)true);		
 
 		// Load octomap 
 		m_octomap = NULL;
 		m_grid = NULL;
 
-    RCLCPP_WARN(node_ptr_->get_logger(), "Loading octomap from %s", m_mapPath.c_str());
 		if(loadOctomap(m_mapPath))
 		{
 			// Compute the point-cloud associated to the ocotmap
@@ -149,35 +132,17 @@ class Grid3d {
 			if(m_gridSlice >= 0 && m_gridSlice <= m_maxZ)
 			{
 				buildGridSliceMsg(m_gridSlice);
-        		m_gridSlicePub = node_ptr_->create_publisher<nav_msgs::msg::OccupancyGrid>(m_nodeName+"/grid_slice", 1);
-
-        		// gridTimer = node_ptr_->create_wall_timer(
-            	// std::chrono::seconds(1.0f/m_publishGridSliceRate), [this]() { node_ptr_->publishGridSlice();});
-            	// FIXME:
-        		gridTimer = node_ptr_->create_wall_timer(
-            	std::chrono::seconds(1), [this]() {this->publishGridSlice();});
-
+				m_gridSlicePub = m_nh.advertise<nav_msgs::OccupancyGrid>(m_nodeName+"/grid_slice", 1, true);
+				gridTimer      = m_nh.createTimer(ros::Duration(1.0/m_publishGridSliceRate), &Grid3d::publishGridSliceTimer, this);	
 			}
 			
 			// Setup point-cloud publisher
 			if(m_publishPc)
 			{
-				// m_pcPub  = m_nh.advertise<sensor_msgs::msg::PointCloud2>(m_nodeName+"/map_point_cloud", 1, true);
-        		m_pcPub = node_ptr_-> create_publisher<sensor_msgs::msg::PointCloud2>(m_nodeName+"/map_point_cloud", 1);
-        		
-				// mapTimer = node_ptr_->create_wall_timer(
-        		//     std::chrono::millisecondsseconds(1.0/m_publishPointCloudRate), [this]() { node_ptr_-> publishMapPointCloud();});
-        		//     FIXME:
-        		// mapTimer = node_ptr_->create_wall_timer(
-            	// std::chrono::seconds(1), [this]() { node_ptr_-> publishMapPointCloud();});	
-				//COMPILE			
-				// mapTimer = node_ptr_->create_wall_timer(
-            	// std::chrono::milliseconds(500), std::bind(&Grid3d::publishMapPointCloud, this));
-				
-				mapTimer = node_ptr_->create_wall_timer(std::chrono::duration<double>(1.0f/1),std::bind(&Grid3d::publishMapPointCloud, this));
+				m_pcPub  = m_nh.advertise<sensor_msgs::PointCloud2>(m_nodeName+"/map_point_cloud", 1, true);
+				mapTimer = m_nh.createTimer(ros::Duration(1.0/m_publishPointCloudRate), &Grid3d::publishMapPointCloudTimer, this);
 			}
-      		percent_computed_pub_ = node_ptr_->create_publisher<std_msgs::msg::Float32>(m_nodeName+"/percent_computed", 10);
-
+			percent_computed_pub_ = m_nh.advertise<std_msgs::Float32>(m_nodeName+"/percent_computed", 1, false);
 		}
 	}
 
@@ -226,14 +191,14 @@ class Grid3d {
   
 	void publishMapPointCloud(void)
 	{
-		m_pcMsg.header.stamp = node_ptr_->now();
-		m_pcPub->publish(m_pcMsg);
+		m_pcMsg.header.stamp = ros::Time::now();
+		m_pcPub.publish(m_pcMsg);
 	}
 	
 	void publishGridSlice(void)
 	{
-		m_gridSliceMsg.header.stamp = node_ptr_->now();
-		m_gridSlicePub->publish(m_gridSliceMsg);
+		m_gridSliceMsg.header.stamp = ros::Time::now();
+		m_gridSlicePub.publish(m_gridSliceMsg);
 	}
 	
 	bool isIntoMap(float x, float y, float z)
@@ -283,14 +248,15 @@ class Grid3d {
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
-	// void publishMapPointCloudTimer(const ros::TimerEvent& event)
-	// {
-	// 	publishMapPointCloud();
-	// }
-	// void publishGridSliceTimer(const ros::TimerEvent& event)
-	// {
-	// 	publishGridSlice();
-	// }
+	void publishMapPointCloudTimer(const ros::TimerEvent& event)
+	{
+		publishMapPointCloud();
+	}
+	
+	void publishGridSliceTimer(const ros::TimerEvent& event)
+	{
+		publishGridSlice();
+	}
 #pragma GCC diagnostic pop
 
 	bool loadOctomap(std::string &path)
@@ -300,7 +266,7 @@ class Grid3d {
 			delete m_octomap;
 		if(m_grid != NULL)
 			delete []m_grid;
-
+		
 		// Load octomap
 		octomap::AbstractOcTree *tree;
 		if(path.length() > 3 && (path.compare(path.length()-3, 3, ".bt") == 0))
@@ -314,12 +280,12 @@ class Grid3d {
 		else if(path.length() > 3 && (path.compare(path.length()-3, 3, ".ot") == 0))
 		{
 			tree = octomap::AbstractOcTree::read(path);
-			if(!tree){
+			if(!tree)
 				return false;
-			}
 		}	
 		else
 			return false;
+		
 		/*
 		// Load octomap
 		octomap::AbstractOcTree *tree = octomap::AbstractOcTree::read(path);
@@ -328,6 +294,7 @@ class Grid3d {
 		m_octomap = dynamic_cast<octomap::OcTree*>(tree);
 		std::cout << "Octomap loaded" << std::endl;
 
+		// Check loading and alloc momery for the grid
 		if(m_octomap == NULL)
 		{
 			std::cout << "Error: NULL octomap!!" << std::endl;
@@ -344,19 +311,11 @@ class Grid3d {
 		m_maxZ = (float)(maxZ-minZ);
 		m_resolution = (float)res;
 		m_oneDivRes = 1.0/m_resolution;
-		RCLCPP_INFO(node_ptr_->get_logger(),"Map size");
-		RCLCPP_INFO(node_ptr_->get_logger(),"\tx: %.2f to %.2f", minX, maxX);
-		RCLCPP_INFO(node_ptr_->get_logger(),"\ty: %.2f to %.2f", minY, maxY);
-		RCLCPP_INFO(node_ptr_->get_logger(),"\tz: %.2f to %.2f", minZ, maxZ);
-		RCLCPP_INFO(node_ptr_->get_logger(),"\tRes: %.2f", m_resolution);
-
-		// ROS_INFO("Map size:\n");
-		// ROS_INFO("\tx: %.2f to %.2f", minX, maxX);
-		// ROS_INFO("\ty: %.2f to %.2f", minZ, maxZ);
-		// ROS_INFO("\tz: %.2f to %.2f", minZ, maxZ);
-		// ROS_INFO("\tRes: %.2f" , m_resolution );
-
-		
+		ROS_INFO("Map size:\n");
+		ROS_INFO("\tx: %.2f to %.2f", minX, maxX);
+		ROS_INFO("\ty: %.2f to %.2f", minY, maxY);
+		ROS_INFO("\tz: %.2f to %.2f", minZ, maxZ);
+		ROS_INFO("\tRes: %.2f" , m_resolution );
 		
 		return true;
 	}
@@ -451,25 +410,21 @@ class Grid3d {
 		
 		std::ifstream input_sha;
 		auto sha_value = sha3_512((const char*)m_grid, m_gridSize);
-		RCLCPP_INFO(node_ptr_->get_logger(),"[Grid3D] Calculated SHA512 value: %s", sha_value.c_str());
-		// ROS_INFO("[Grid3D] Calculated SHA512 value: %s", sha_value.c_str());
+		ROS_INFO("[Grid3D] Calculated SHA512 value: %s", sha_value.c_str());
 
 		input_sha.open(fileName+"sha");
 		std::string readed_sha;
 		if(input_sha.is_open()){
 			getline(input_sha, readed_sha);
-			RCLCPP_INFO(node_ptr_->get_logger(),"[Grid3D] Readed SHA512 from file: %s", readed_sha.c_str());
-			// ROS_INFO("[Grid3D] Readed SHA512 from file: %s", readed_sha.c_str());
+			ROS_INFO("[Grid3D] Readed SHA512 from file: %s", readed_sha.c_str());
 			input_sha.close();
 		}else{
-			// ROS_ERROR("[Grid3D] Couldn't read SHA512 data of gridm file, recomputing grid");
-			RCLCPP_INFO(node_ptr_->get_logger(),"[Grid3D] Couldn't read SHA512 data of gridm file, recomputing grid");
+			ROS_ERROR("[Grid3D] Couldn't read SHA512 data of gridm file, recomputing grid");
 			return false;
 		}
 		//Check that both are the same
 		if(readed_sha.compare(sha_value) != 0){
-			// ROS_ERROR("[Grid3D] Found different SHA values between for gridm!");
-			RCLCPP_INFO(node_ptr_->get_logger(),"[Grid3D] Found different SHA values between for gridm!");
+			ROS_ERROR("[Grid3D] Found different SHA values between for gridm!");
 			return false;
 		}
 		
@@ -482,13 +437,15 @@ class Grid3d {
 		// Get map parameters
 		double minX, minY, minZ;
 		m_octomap->getMetricMin(minX, minY, minZ);
-		
+				
 		// Load the octomap in PCL for easy nearest neighborhood computation
 		// The point-cloud is shifted to have (0,0,0) as min values
 		int i = 0;
 		m_cloud->width = m_octomap->size();
+		// std::cout << "dim1: " << m_cloud->width << std::endl;
 		m_cloud->height = 1;
 		m_cloud->points.resize(static_cast<long>(m_cloud->width) * m_cloud->height);
+		std::cout << "dim1: " << m_cloud->width << std::endl;
 		for(octomap::OcTree::leaf_iterator it = m_octomap->begin_leafs(), end = m_octomap->end_leafs(); it != end; ++it)
 		{
 			if(it != NULL && m_octomap->isNodeOccupied(*it))
@@ -502,6 +459,7 @@ class Grid3d {
 		}
 		m_cloud->width = i;
 		m_cloud->points.resize(i);
+		// std::cout << "dim2: " << m_cloud->width << std::endl;
 		
 		// Create the point cloud msg for publication
 		pcl::toROSMsg(*m_cloud, m_pcMsg);
@@ -511,7 +469,7 @@ class Grid3d {
 	void computeGrid(void)
 	{
 		//Publish percent variable
-		std_msgs::msg::Float32 percent_msg;
+		std_msgs::Float32 percent_msg;
 		percent_msg.data = 0;
 		// Alloc the 3D grid
 		m_gridSizeX = (int)(m_maxX*m_oneDivRes);
@@ -528,8 +486,8 @@ class Grid3d {
 		// Compute the distance to the closest point of the grid
 		int index;
 		float dist;
-		float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI));
-		float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev);
+		// float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI));
+		// float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev);
 		pcl::PointXYZI searchPoint;
 		std::vector<int> pointIdxNKNSearch(1);
 		std::vector<float> pointNKNSquaredDistance(1);
@@ -548,24 +506,40 @@ class Grid3d {
 					index = ix + iy*m_gridStepY + iz*m_gridStepZ;
 					++count;
 					percent = count/size *100.0;
-					// ROS_INFO_THROTTLE(0.5,"Progress: %lf %%", percent);	
+					ROS_INFO_THROTTLE(0.5,"Progress: %lf %%", percent);	
 					if(percent > percent_msg.data + 0.5){
 						percent_msg.data = percent;
 						// percent_computed_pub_.publish(percent_msg);
 					}
-					
 					if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
 					{
-						dist = pointNKNSquaredDistance[0];
-						m_grid[index].dist = dist;
-						if(!use_costmap_function){
-							m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
-						}else{
-							double prob =  100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius)));
-							// ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob);
-							//JAC: Include the computation of prob considering the distance to the nearest voronoi edge.
-							m_grid[index].prob = prob;
+						// JAC: Always square distance has been considered!!!!!!
+						// dist = pointNKNSquaredDistance[0];
+						dist = sqrt(pointNKNSquaredDistance[0]);
+						
+						// std::cout << "dist: " << dist << std::endl;
+						if (dist < 200){
+							m_grid[index].dist = dist;
+							if(!use_costmap_function){
+								// m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
+								m_grid[index].prob = dist;
+							}else{
+								// double prob =  100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius)));
+								// ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob);
+								//JAC: Include the computation of prob considering the distance to the nearest voronoi edge.
+								// m_grid[index].prob = prob;
+								// m_grid[index].prob = 100-dist;
+								m_grid[index].prob = dist;
+							}
+						}
+						else{
+							// std::cout << "dist: " << dist << std::endl;
+							// m_grid[index].dist = -1.0;
+							m_grid[index].dist = 90.0;
+							m_grid[index].prob =  0.0;
 						}
+						
+						
 					}
 					else
 					{
@@ -577,20 +551,18 @@ class Grid3d {
 			}
 		}
 		percent_msg.data = 100;
-		// percent_computed_pub_->publish(percent_msg);
+		// percent_computed_pub_.publish(percent_msg);
 	}
 	
 	void buildGridSliceMsg(float z)
 	{
-		// static int _seq = 0;
+		static int seq = 0;
 		
 		// Setup grid msg
 		m_gridSliceMsg.header.frame_id = m_globalFrameId;
-		m_gridSliceMsg.header.stamp = node_ptr_->now();
-
-    // TODO: add seq
-		// m_gridSliceMsg.header.seq = _seq++;
-		m_gridSliceMsg.info.map_load_time = node_ptr_->now();
+		m_gridSliceMsg.header.stamp = ros::Time::now();
+		m_gridSliceMsg.header.seq = seq++;
+		m_gridSliceMsg.info.map_load_time = ros::Time::now();
 		m_gridSliceMsg.info.resolution = m_resolution;
 		m_gridSliceMsg.info.width = m_gridSizeX;
 		m_gridSliceMsg.info.height = m_gridSizeY;
diff --git a/include/Planners/AStar.hpp b/include/Planners/AStar.hpp
index 62a1bf6..e3f571f 100644
--- a/include/Planners/AStar.hpp
+++ b/include/Planners/AStar.hpp
@@ -12,8 +12,6 @@
  * 
  */
 #include <Planners/AlgorithmBase.hpp>
-#include <chrono>
-#include <sensor_msgs/msg/detail/point_cloud2__struct.hpp>
 
 
 /**
@@ -26,13 +24,11 @@
  * 
  */
 #ifdef ROS
-// #include <ros/ros.h>
-#include <rclcpp/rclcpp.hpp>
-#include <visualization_msgs/msg/marker.hpp>
+#include <ros/ros.h>
+#include <visualization_msgs/Marker.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
-#include <pcl_conversions/pcl_conversions.h>
-// #include <pcl_ros/point_cloud.h>
+#include <pcl_ros/point_cloud.h>
 #include "utils/ros/ROSInterfaces.hpp"
 #endif
 
@@ -99,7 +95,7 @@ namespace Planners{
          * (They are currently member functions of the class).
          */
         template<typename T, typename U>
-        void publishROSDebugData(const Planners::utils::Node* _node, const T &_open_set, const U &_closed_set);
+        void publishROSDebugData(const Node* _node, const T &_open_set, const U &_closed_set);
         
     protected:
 
@@ -129,7 +125,14 @@ namespace Planners{
          * This operation of erase and re-insert is performed in order to update the position
          * of the node in the container. 
          */
-        virtual void exploreNeighbours(Planners::utils::Node* _current, const Vec3i &_target,node_by_position &_index_by_pos);
+        virtual void exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos);
+
+        // virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos);
+
+        // virtual int chooseNeighbours(float angh, float angv);
+        // // virtual Vec3i chooseNeighbours(float angh, float angv);
+
+        // virtual void nodesToExplore(int node);
 
         /**
          * @brief This functions implements the algorithm G function. 
@@ -165,31 +168,27 @@ namespace Planners{
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        virtual unsigned int computeG(const Planners::utils::Node* _current, Planners::utils::Node* _suc, unsigned int _n_i, unsigned int _dirs);
+        virtual unsigned int computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs);
+
+        virtual float computeGradient(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs);
+
+        virtual Vec3i getVectorPull(const Vec3i &_source, const Vec3i &_target);
 
         unsigned int line_of_sight_checks_{0};  /*!< TODO Comment */
-        std::vector<Planners::utils::Node*> closedSet_; /*!< TODO Comment */
+        std::vector<Node*> closedSet_; /*!< TODO Comment */
         MagicalMultiSet openSet_; /*!< TODO Comment */
         
 #ifdef ROS
-
-        static std::shared_ptr<rclcpp::Node> ros_node_ptr_;
-
-        rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr explored_nodes_marker_pub_,
-                                                                openset_marker_pub_,
-                                                                closedset_marker_pub_,
-                                                                best_node_marker_pub_,
-                                                                aux_text_marker_pub_;
-        // rclcpp::Publisher<pcl::PointCloud<pcl::PointXYZ>>::SharedPtr occupancy_marker_pub_;
-        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr occupancy_marker_pub_;
-
-        visualization_msgs::msg::Marker explored_node_marker_, openset_markers_,  
-                                   closed_set_markers_, best_node_marker_, aux_text_marker_; 
-
-        rclcpp::Duration duration_pub_rate_nanoseconds_{std::chrono::milliseconds(10)};
-        rclcpp::Time last_publish_tamp_;
-        float resolution_;
-    	  pcl::PointCloud<pcl::PointXYZ>  occupancy_marker_;
+        ros::NodeHandle lnh_{"~"}; /*!< TODO Comment */
+        ros::Publisher explored_nodes_marker_pub_, occupancy_marker_pub_,  /*!< TODO Comment */
+                       openset_marker_pub_, closedset_marker_pub_, /*!< TODO Comment */
+                       best_node_marker_pub_, aux_text_marker_pub_; /*!< TODO Comment */
+        visualization_msgs::Marker explored_node_marker_, openset_markers_,  /*!< TODO Comment */
+                                   closed_set_markers_, best_node_marker_, aux_text_marker_; /*!< TODO Comment */
+        ros::Duration duration_pub_{0.001}; /*!< TODO Comment */
+        ros::Time last_publish_tamp_; /*!< TODO Comment */
+        float resolution_; /*!< TODO Comment */
+    	pcl::PointCloud<pcl::PointXYZ>  occupancy_marker_;  /*!< TODO Comment */
 
 #endif
     };
diff --git a/include/Planners/AStarM1.hpp b/include/Planners/AStarM1.hpp
index d48ecd8..23d70e7 100644
--- a/include/Planners/AStarM1.hpp
+++ b/include/Planners/AStarM1.hpp
@@ -4,63 +4,64 @@
  * @file AStarM1.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- *
+ * 
  * @brief This algorithm is a variation of the A*. The only
  * difference is that it reimplements the computeG method adding the
  * following cost term to the resturned result:
- *
- *  auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
- *
+ * 
+ *  auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc * dist_scale_factor_reduced_);
+ * 
  * @version 0.1
  * @date 2021-06-29
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
 #include <Planners/AStar.hpp>
 
-namespace Planners
-{
+namespace Planners{
 
     /**
-     * @brief
-     *
+     * @brief 
+     * 
      */
     class AStarM1 : public AStar
     {
-
+        
     public:
         /**
          * @brief Construct a new AStarM1 object
-         * @param _use_3d This parameter allows the user to choose between
-         * planning on a plane (8 directions possibles)
+         * @param _use_3d This parameter allows the user to choose between 
+         * planning on a plane (8 directions possibles) 
          * or in the 3D full space (26 directions)
          * @param _name Algorithm name stored internally
-         *
+         * 
          */
-        AStarM1(bool _use_3d, std::string _name);
+        AStarM1(bool _use_3d, std::string _name );
 
         /**
          * @brief Construct a new Cost Aware A Star M1 object
-         *
-         * @param _use_3d
+         * 
+         * @param _use_3d 
          */
         AStarM1(bool _use_3d);
 
     protected:
+        
         /**
-         * @brief Overrided ComputeG function.
-         *
+         * @brief Overrided ComputeG function. 
+         * 
          * @param _current Pointer to the current node
          * @param _suc Pointer to the successor node
-         * @param _n_i The index of the direction in the directions vector.
+         * @param _n_i The index of the direction in the directions vector. 
          * Depending on this index, the distance wi
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        inline virtual unsigned int computeG(const Planners::utils::Node *_current,  Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs) override;
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
     };
 
 }
 
-#endif
+#endif 
diff --git a/include/Planners/AStarM2.hpp b/include/Planners/AStarM2.hpp
index 5ff95fa..d75c350 100644
--- a/include/Planners/AStarM2.hpp
+++ b/include/Planners/AStarM2.hpp
@@ -4,61 +4,62 @@
  * @file AStarM2.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- *
+ * 
  * @brief This Algorithm is the same as the original A* with
  * the only difference in the ComputeG function that is re-implemented
  * It adds a edge-neighbour term to the total G value.
- *
+ * 
  * @version 0.1
  * @date 2021-09-21
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
 #include <Planners/AStar.hpp>
 
-namespace Planners
-{
+namespace Planners{
 
     /**
-     * @brief
-     *
+     * @brief 
+     * 
      */
     class AStarM2 : public AStar
     {
-
+        
     public:
         /**
          * @brief Construct a new AStarM2 object
-         * @param _use_3d This parameter allows the user to choose
-         * between planning on a plane (8 directions possibles)
+         * @param _use_3d This parameter allows the user to choose 
+         * between planning on a plane (8 directions possibles) 
          * or in the 3D full space (26 directions)
-         *
+         * 
          * @param _name Algorithm name stored internally
          */
-        AStarM2(bool _use_3d, std::string _name);
+        AStarM2(bool _use_3d, std::string _name );
 
         /**
          * @brief Construct a new AStarM2 object
-         *
-         * @param _use_3d
+         * 
+         * @param _use_3d 
          */
         AStarM2(bool _use_3d);
 
     protected:
+        
         /**
-         * @brief Overrided ComputeG function.
-         *
+         * @brief Overrided ComputeG function. 
+         * 
          * @param _current Pointer to the current node
          * @param _suc Pointer to the successor node
-         * @param _n_i The index of the direction in the directions vector.
+         * @param _n_i The index of the direction in the directions vector. 
          * Depending on this index, the distance wi
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        inline virtual unsigned int computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs) override;
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
     };
 
 }
 
-#endif
+#endif 
diff --git a/include/Planners/AlgorithmBase.hpp b/include/Planners/AlgorithmBase.hpp
index e3f3b96..3d4549a 100644
--- a/include/Planners/AlgorithmBase.hpp
+++ b/include/Planners/AlgorithmBase.hpp
@@ -4,31 +4,30 @@
  * @file AlgorithmBase.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- *
+ * 
  * @brief Generic AlgorithmBase Base Class. The algorithms should inherit from this class as far as possible.
  * It implements some generic functions used by all the algorithms
  * It could be extended to add functionalities required by other type of algorithms, not only heuristics ones
- *
+ * 
  * @version 0.1
  * @date 2021-06-29
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
-
-#include <functional>
 #include <iostream>
-#include <set>
 #include <vector>
+#include <set>
+#include <functional>
 
 #include <cmath>
 
-#include "utils/LineOfSight.hpp"
-#include "utils/geometry_utils.hpp"
+#include "utils/world.hpp"
 #include "utils/heuristic.hpp"
-#include "utils/time.hpp"
 #include "utils/utils.hpp"
-#include "utils/world.hpp"
+#include "utils/time.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/LineOfSight.hpp"
 
 namespace Planners
 {
@@ -39,7 +38,7 @@ namespace Planners
     class Clock;
 
     /**
-     * @brief Main base class that implements useful functions for children algorithm class
+     * @brief Main base class that implements useful functions for children algorithm class 
      * and provides a guide to implement any new algorithm.
      */
     class AlgorithmBase
@@ -48,8 +47,8 @@ namespace Planners
     public:
         /**
          * @brief Construct a new AlgorithmBase object
-         *
-         * @param _use_3d: This params allows the algorithm to choose a set of 3D directions of explorations
+         * 
+         * @param _use_3d: This params allows the algorithm to choose a set of 3D directions of explorations 
          * or a set or 2D directions of explorations. The 2D case is simply 3D but without the directions with Z!=0
          * directions2d = {
          *  { 0, 1, 0 }, {0, -1, 0}, { 1, 0, 0 }, { -1, 0, 0 }, //4 straight elements
@@ -60,48 +59,48 @@ namespace Planners
          *  { 0, 1, 0 }, {0, -1, 0}, { 1, 0, 0 }, { -1, 0, 0 }, { 0, 0, 1}, { 0, 0, -1}, //6 first elements
          *
          *  { 1, -1, 0 }, { -1, 1, 0 }, { -1, -1, 0 }, { 1, 1, 0 },  { -1, 0, -1 }, //7-18 inclusive
-         *  { 1, 0, 1 }, { 1, 0, -1 }, {-1, 0, 1}, { 0, -1, 1 }, { 0, 1, 1 }, { 0, 1, -1 },  { 0, -1, -1 },
+         *  { 1, 0, 1 }, { 1, 0, -1 }, {-1, 0, 1}, { 0, -1, 1 }, { 0, 1, 1 }, { 0, 1, -1 },  { 0, -1, -1 }, 
          *
-         *  { -1, -1, 1 }, { 1, 1, 1 },  { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 },
+         *  { -1, -1, 1 }, { 1, 1, 1 },  { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 }, 
          *};
          * Note that the the ordering is not trivial, the inner loop that explorates node take advantage of this order to directly use
          * pre-compiled distance depending if the squared norm of the vector is 1,2 or 3.
          * This pre compiled distances appears in the header utils.hpp
-         *
-         *
-         * @param _algorithm_name Algorithm name to uniquely identify the type of algorithm.
+         * 
+         * 
+         * @param _algorithm_name Algorithm name to uniquely identify the type of algorithm. 
          */
         AlgorithmBase(bool _use_3d, const std::string &_algorithm_name);
 
         /**
-         * @brief Set the World Size object. This method call the resizeWorld method
+         * @brief Set the World Size object. This method call the resizeWorld method 
          * from the internal discrete world object
-         *
+         * 
          * @param worldSize_ Discrete world size vector in units of resolution.
          * @param _resolution resolution to save inside the world object
          */
-        void setWorldSize(const Vec3i &worldSize_, const double _resolution);
+        void setWorldSize(const Vec3i &worldSize_,const double _resolution);
 
         /**
          * @brief Get the World Size, it simply call the getWorldSize method from the
          * discrete world internal object
-         *
+         * 
          * @return Vec3i discrete world bounds
          */
         Vec3i getWorldSize();
         /**
-         * @brief Get the World Resolution that is been used by the internal
+         * @brief Get the World Resolution that is been used by the internal 
          * discrete world object
-         * @return double resolution
+         * @return double resolution 
          */
         double getWorldResolution();
 
         /**
-         * @brief Get a pointer to the inner world object
-         *
-         * @return utils::DiscreteWorld*
+         * @brief Get a pointer to the inner world object 
+         * 
+         * @return utils::DiscreteWorld* 
          */
-        utils::DiscreteWorld *getInnerWorld();
+        utils::DiscreteWorld* getInnerWorld();
 
         /**
          * @brief Configure the heuristic from the list of static functions in the heuristic.hpp
@@ -110,36 +109,36 @@ namespace Planners
          * for example Heuristic::Euclidean
          */
         void setHeuristic(HeuristicFunction heuristic_);
-
+        
         /**
          * @brief Mark a set of coordinates of the map as occupied (blocked)
-         *
+         * 
          * @param coordinates_ Discrete vector of coordinates
          * @param do_inflate enable inflation (mark surrounding coordinates as occupied)
          * @param steps inflation steps (in multiples of the resolution value)
          */
         void addCollision(const Vec3i &coordinates_, bool do_inflate, unsigned int steps);
-
+        
         /**
          * @brief Calls the addCollision with the internal inflation configuration values
-         *
+         * 
          * @param coordinates_ Discrete coordinates vector
          */
         void addCollision(const Vec3i &coordinates_);
 
         /**
          * @brief Function to use in the future to configure the cost of each node
-         *
+         * 
          * @param coordinates_ Discrete coordinates
-         * @param _cost cost value
-         * @return true
-         * @return false
+         * @param _cost cost value 
+         * @return true 
+         * @return false 
          */
         bool configureCellCost(const Vec3i &coordinates_, const double &_cost);
 
         /**
          * @brief Check if a set of discrete coordinates are marked as occupied
-         *
+         * 
          * @param coordinates_ Discrete vector of coordinates
          * @return true Occupied
          * @return false not occupied
@@ -147,10 +146,10 @@ namespace Planners
         bool detectCollision(const Vec3i &coordinates_);
 
         /**
-         * @brief Main function that should be inherit by each algorithm.
+         * @brief Main function that should be inherit by each algorithm. 
          * This function should accept two VALID start and goal discrete coordinates and return
          * a PathData object containing the necessary information (path, time....)
-         *
+         * 
          * @param _source Start discrete coordinates
          * @param _target Goal discrete coordinates
          * @return PathData Results stored as PathData object
@@ -163,34 +162,32 @@ namespace Planners
          * @param _inflate If inflate by default
          * @param _inflation_steps The number of adjacent cells to inflate
          */
-        void setInflationConfig(const bool _inflate, const unsigned int _inflation_steps)
-        {
-            do_inflate_ = _inflate;
-            inflate_steps_ = _inflation_steps;
-        }
+        void setInflationConfig(const bool _inflate, const unsigned int _inflation_steps) 
+        { do_inflate_ = _inflate; inflate_steps_ = _inflation_steps;}
 
         /**
          * @brief Set the Cost Factor object
-         *
+         * 
          */
-        virtual void setCostFactor(const float &_factor) { cost_weight_ = _factor; }
+        virtual void setCostFactor(const float &_factor){ cost_weight_ = _factor; }
 
         /**
          * @brief Set the Max Line Of Sight object
-         *
-         * @param _max_line_of_sight
+         * 
+         * @param _max_line_of_sight 
          */
-        virtual void setMaxLineOfSight(const float &_max_line_of_sight) { max_line_of_sight_cells_ = std::floor(_max_line_of_sight / discrete_world_.getResolution()); }
+        virtual void setMaxLineOfSight(const float &_max_line_of_sight){ max_line_of_sight_cells_ = std::floor(_max_line_of_sight/discrete_world_.getResolution()); }
         /**
          * @brief Deleted function to be inherit from
-         *
+         * 
          */
         virtual void publishOccupationMarkersMap() = 0;
 
     protected:
+
         /**
-         * @brief Basic inflation function
-         *
+         * @brief Basic inflation function 
+         * 
          * @param _ref Discrete coordinates vector
          * @param _directions Directions vector to inflate
          * @param _inflate_steps number of cells to inflate in each direction
@@ -198,30 +195,31 @@ namespace Planners
         void inflateNodeAsCube(const Vec3i &_ref,
                                const CoordinateList &_directions,
                                const unsigned int &_inflate_steps);
-
+        
         /**
          * @brief Create a Result Data Object object
-         *
-         * @param _last
-         * @param _timer
-         * @param _explored_nodes
-         * @param _solved
-         * @param _start
-         * @param _sight_checks
-         * @return PathData
-         */
-        virtual PathData createResultDataObject(const Planners::utils::Node *_last, utils::Clock &_timer,
-                                                const size_t _explored_nodes, bool _solved,
+         * 
+         * @param _last 
+         * @param _timer 
+         * @param _explored_nodes 
+         * @param _solved 
+         * @param _start 
+         * @param _sight_checks 
+         * @return PathData 
+         */
+        virtual PathData createResultDataObject(const Node* _last, utils::Clock &_timer, 
+                                                const size_t _explored_nodes, bool _solved, 
                                                 const Vec3i &_start, const unsigned int _sight_checks);
 
+                                                        
         HeuristicFunction heuristic; /*!< TODO Comment */
-        CoordinateList direction;    /*!< TODO Comment */
+        CoordinateList direction; /*!< TODO Comment */
 
         utils::DiscreteWorld discrete_world_; /*!< TODO Comment */
-        unsigned int inflate_steps_{1};       /*!< TODO Comment */
-        bool do_inflate_{true};               /*!< TODO Comment */
+        unsigned int inflate_steps_{1}; /*!< TODO Comment */
+        bool do_inflate_{true}; /*!< TODO Comment */
 
-        double cost_weight_{0};                   /*!< TODO Comment */
+        double cost_weight_{0}; /*!< TODO Comment */
         unsigned int max_line_of_sight_cells_{0}; /*!< TODO Comment */
 
         const std::string algorithm_name_{""}; /*!< TODO Comment */
@@ -229,4 +227,4 @@ namespace Planners
     private:
     };
 }
-#endif
+#endif
\ No newline at end of file
diff --git a/include/Planners/LazyThetaStar.hpp b/include/Planners/LazyThetaStar.hpp
index 143be46..abdcb96 100644
--- a/include/Planners/LazyThetaStar.hpp
+++ b/include/Planners/LazyThetaStar.hpp
@@ -63,14 +63,14 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
          * @param _s_aux 
          */
-        virtual void SetVertex(Planners::utils::Node *_s_aux);
+        virtual void SetVertex(Node *_s_aux);
                 
     };
 
diff --git a/include/Planners/LazyThetaStarM1.hpp b/include/Planners/LazyThetaStarM1.hpp
index a5c1d1d..2418389 100644
--- a/include/Planners/LazyThetaStarM1.hpp
+++ b/include/Planners/LazyThetaStarM1.hpp
@@ -4,12 +4,12 @@
  * @file LazyThetaStarM1.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- * @brief
+ * @brief 
  * @version 0.1
  * @date 2021-06-29
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
 #include <Planners/LazyThetaStar.hpp>
 
@@ -17,56 +17,61 @@ namespace Planners
 {
     /**
      * @brief Lazy Theta* Algorithm Class
-     *
+     * 
      */
     class LazyThetaStarM1 : public LazyThetaStar
     {
 
     public:
+        
         /**
          * @brief Construct a new Lazy Theta Star object
-         *
-         * @param _use_3d This parameter allows the user to choose between planning on a plane
+         * 
+         * @param _use_3d This parameter allows the user to choose between planning on a plane 
          * (8 directions possibles) or in the 3D full space (26 directions)
          * @param _name Algorithm name stored internally
-         *
+         * 
          */
-        LazyThetaStarM1(bool _use_3d, std::string _name);
+        LazyThetaStarM1(bool _use_3d, std::string _name );
 
         /**
          * @brief Construct a new Cost Aware Lazy Theta Star object
-         *
-         * @param _use_3d
+         * 
+         * @param _use_3d 
          */
         LazyThetaStarM1(bool _use_3d);
 
     protected:
+
         /**
          * @brief Compute cost function of the Lazy version of the algorithm
-         *
+         * 
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        inline virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
-         * @param _s_aux
+         * @param _s_aux 
          */
-        virtual void SetVertex(Planners::utils::Node *_s_aux) override;
+        virtual void SetVertex(Node *_s_aux) override;
+
 
         /**
-         * @brief This functions implements the algorithm G function.
-         *
+         * @brief This functions implements the algorithm G function. 
+         * 
          * @param _current Pointer to the current node
          * @param _suc Pointer to the successor node
-         * @param _n_i The index of the direction in the directions vector.
+         * @param _n_i The index of the direction in the directions vector. 
          * Depending on this index, the distance wi
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        virtual unsigned int computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs) override;
+        virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
+                
     };
 
 }
diff --git a/include/Planners/LazyThetaStarM1Mod.hpp b/include/Planners/LazyThetaStarM1Mod.hpp
index 5c72d86..eeb08e9 100644
--- a/include/Planners/LazyThetaStarM1Mod.hpp
+++ b/include/Planners/LazyThetaStarM1Mod.hpp
@@ -49,7 +49,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
 
     };
 
diff --git a/include/Planners/LazyThetaStarM2.hpp b/include/Planners/LazyThetaStarM2.hpp
index 772d64a..301b8ee 100644
--- a/include/Planners/LazyThetaStarM2.hpp
+++ b/include/Planners/LazyThetaStarM2.hpp
@@ -4,12 +4,12 @@
  * @file LazyThetaStarM2.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- * @brief
+ * @brief 
  * @version 0.1
  * @date 2021-06-29
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
 #include <Planners/ThetaStarM2.hpp>
 
@@ -17,67 +17,70 @@ namespace Planners
 {
     /**
      * @brief Lazy Theta* Algorithm Class
-     *
+     * 
      */
     class LazyThetaStarM2 : public ThetaStarM2
     {
 
     public:
+        
         /**
          * @brief Construct a new Lazy Theta Star object
-         *
-         * @param _use_3d This parameter allows the user to choose between planning
+         * 
+         * @param _use_3d This parameter allows the user to choose between planning 
          * on a plane (8 directions possibles) or in the 3D full space (26 directions)
          * @param _name Algorithm name stored internally
-         *
+         * 
          */
-        LazyThetaStarM2(bool _use_3d, std::string _name);
+        LazyThetaStarM2(bool _use_3d, std::string _name );
 
         /**
          * @brief Construct a new Lazy Theta Star Safety Cost object
-         *
-         * @param _use_3d
+         * 
+         * @param _use_3d 
          */
         LazyThetaStarM2(bool _use_3d);
 
         /**
-         * @brief
-         *
-         * @param _source
-         * @param _target
-         * @return PathData
+         * @brief 
+         * 
+         * @param _source 
+         * @param _target 
+         * @return PathData 
          */
         virtual PathData findPath(const Vec3i &_source, const Vec3i &_target) override;
 
     protected:
+
         /**
          * @brief Compute cost function of the Lazy version of the algorithm
-         *
+         * 
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to second node
          */
-        inline virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
 
         /**
          * @brief SetVertex function
          * Line of sight is checked inside this function
-         * @param _s_aux
+         * @param _s_aux 
          */
-        virtual void SetVertex(Planners::utils::Node *_s_aux);
+        virtual void SetVertex(Node *_s_aux);
 
         /**
-         * @brief
-         *
-         * @param _current
-         * @param _suc
-         * @param _n_i
-         * @param _dirs
-         * @return unsigned int
+         * @brief 
+         * 
+         * @param _current 
+         * @param _suc 
+         * @param _n_i 
+         * @param _dirs 
+         * @return unsigned int 
          */
-        inline virtual unsigned int computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs) override;
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
 
         // Variable to ensure that the los is true between the parent of the current and one neighbour, so SetVertex function should not be executed
         bool los_neighbour_{false}; /*!< TODO Comment */
+
     };
 
 }
diff --git a/include/Planners/LocalPlanner.hpp b/include/Planners/LocalPlanner.hpp
deleted file mode 100755
index dedd282..0000000
--- a/include/Planners/LocalPlanner.hpp
+++ /dev/null
@@ -1,242 +0,0 @@
-#ifndef LOCALPLANNER_H_
-#define LOCALPLANNER_H_
-
-// #include <ros/ros.h>
-#include "rclcpp/rclcpp.hpp"
-#include <iostream>
-#include <cstdlib>
-#include <string>
-#include <math.h>
-#include <ctime>
-#include <sys/timeb.h>
-#include <fstream>
-
-#include "tf2/LinearMath/Transform.h"
-#include "tf2/exceptions.h"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/buffer.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "tf2_msgs/msg/tf_message.hpp"
-
-// #include <tf2_ros/transform_listener.h>
-// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-// #include "tf2_msgs/msg/tf_message.hpp"
-// #include "tf2/LinearMath/Transform.h"
-// #include "tf2/exceptions.h"
-// #include "tf2_ros/buffer.h"
-// #include <tf2_ros/message_filter.h>
-
-#include <std_msgs/msg/int32.hpp>
-#include <std_msgs/msg/bool.hpp>
-
-#include <std_srvs/srv/trigger.hpp>
-
-#include <geometry_msgs/msg/pose_stamped.hpp>
-
-// JAC: Add the algorithms needed
-#include "Planners/AStar.hpp"
-#include "Planners/AStarM2.hpp"
-#include "Planners/AStarM1.hpp"
-#include "Planners/ThetaStar.hpp"
-#include "Planners/ThetaStarM1.hpp"
-#include "Planners/ThetaStarM2.hpp"
-#include "Planners/LazyThetaStar.hpp"
-#include "Planners/LazyThetaStarM1.hpp"
-#include "Planners/LazyThetaStarM1Mod.hpp"
-#include "Planners/LazyThetaStarM2.hpp"
-#include "utils/ros/ROSInterfaces.hpp"
-#include "utils/SaveDataVariantToFile.hpp"
-#include "utils/misc.hpp"
-#include "utils/geometry_utils.hpp"
-#include "utils/metrics.hpp"
-
-#include "Grid3D/grid3d.hpp"
-#include <pcl/conversions.h>
-#include <pcl/impl/point_types.hpp>
-#include <pcl/point_cloud.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <rclcpp/logging.hpp>
-#include <visualization_msgs/msg/detail/marker__struct.hpp>
-#include <visualization_msgs/msg/marker.hpp>
-
-// #include <pcl_ros/point_cloud.h>
-#include <pcl_ros/transforms.hpp>
-
-#include <nav_msgs/msg/occupancy_grid.hpp>
-
-#include <heuristic_planners/srv/set_algorithm.hpp>
-#include <heuristic_planners/srv/get_path.hpp>
-
-// ROS1
-// #include <visualization_msgs/Marker.h>
-#include <trajectory_msgs/msg/multi_dof_joint_trajectory.hpp>
-#include <trajectory_msgs/msg/multi_dof_joint_trajectory_point.hpp>
-
-
-// #include <dynamic_reconfigure/server.h>
-// #include <theta_star_2d/LocalPlannerConfig.h>
-// #include <nix_common/CheckObstacles.h>
-// #include <actionlib/server/simple_action_server.h>
-// #include <actionlib/client/simple_action_client.h>
-
-// #include <upo_actions/ExecutePathAction.h>
-// #include <upo_actions/NavigateAction.h>
-// #include <upo_actions/Navigate3DAction.h>
-
-// #include <std_srvs/Empty.h>
-#include <std_srvs/srv/empty.hpp>
-
-
-// #include <std_msgs/Float32.h>
-#include <std_msgs/msg/float32.h>
-#include <std_msgs/msg/string.h>
-#include <std_msgs/msg/u_int8.hpp>
-
-// #include <sensor_msgs/PointCloud2.h>
-#include <sensor_msgs/msg/point_cloud2.hpp>
-// #include <octomap_msgs/Octomap.h>
-#include "octomap_msgs/msg/octomap.hpp"
-#include <octomap/octomap.h>
-#include <octomap/OcTree.h>
-// #include <pcl_ros/point_cloud.hpp>
-// #include <pcl_ros/transforms.h>
-#include <pcl/common/common.h>
-#include <pcl/kdtree/kdtree_flann.h>
-#include <pcl/conversions.h>
-#include <pcl/impl/point_types.hpp>
-#include <pcl/point_cloud.h>
-#include <pcl_conversions/pcl_conversions.h>
-
-namespace PathPlanners
-{
-class LocalPlanner : public rclcpp::Node {
-
-public:
-    LocalPlanner();
-
-    void plan();
-
-    // JAC: Removed when topics and/or services are confirmed about the reception of the global goal.
-    // void executePathGoalServerCB();
-    // void executePathPreemptCB();
-
-private:
-    void resetFlags();
-    void clearMarkers();
-    //These functions gets parameters from param server at startup if they exists, if not it passes default values
-
-    void configTopics();
-    //Config thetastar class parameters
-    void configServices();
-    // JAC: Decide if this is considered from AS2.
-    // void publishExecutePathFeedback();
-    geometry_msgs::msg::TransformStamped getTfMapToRobot();
-    // void publishTrajMarker3D();
-    void configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic);
-    bool calculateLocalGoal3D();
-    void calculatePath3D();
-
-    //Auxiliar functions
-    geometry_msgs::msg::Point makePoint(const geometry_msgs::msg::Vector3 &vec);
-
-    void pointsSub(const sensor_msgs::msg::PointCloud2::RawPtr &msg);
-    void configMarkers(std::string ns, std::string frame);
-
-    //Variables
-    // ros::NodeHandlePtr nh;
-    // ros::ServiceClient costmap_clean_srv;
-    // rclcpp::Service<std_srvs::srv::Trigger> costmap_clean_srv;
-
-    // ros::Subscriber local_map_sub;
-    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr local_map_sub_;
-    // rclcpp::Subscription local_map_sub_;
-
-    // ros::Publisher visMarkersPublisher, trajPub, costmap_inflated_pub_;
-    // JAC: trajPub with the corresponding message
-    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr visMarkersPublisher, trajPub;
-    // rclcpp::Publisher visMarkersPublisher, trajPub, costmap_inflated_pub_;
-
-    //Flow control flags
-    //To calculate planning time
-    // struct timeb startT, finishT;
-
-    //Theta star algorithm parameters
-    //Geometric params
-    //Algorithm params
-    float cost_weight;
-    float lof_distance;
-    float occ_threshold; 
-    
-    double initialSearchAround, finalSearchAround;
-
-    int occGoalCnt, badGoal;
-
-    int number_of_points;
-    bool debug;
-    float seconds, milliseconds;
-
-    std::string robot_base_frame, world_frame,traj_dest_frame;
-    
-    // JAC: Change the type of messafe
-    // trajectory_msgs::msg::MultiDOFJointTrajectory globalTrajectory, localTrajectory;
-    //Markers
-    visualization_msgs::msg::Marker lineMarker, waypointsMarker;
-
-    geometry_msgs::msg::Vector3 localGoal, robotPose;
-    tf2_ros::Buffer *tfBuffer;
-
-    std_msgs::msg::Bool is_running;
-
-    //action server stufff
-    //MFC: CHECK ALL THIS ACTION STUFF
-    /* std::unique_ptr<ExecutePathServer> execute_path_srv_ptr;
-
-    upo_actions::ExecutePathFeedback exec_path_fb;
-    std_msgs::msg::Float32 planningRate;
-    std_msgs::msg::UInt8 waypointGoingTo;
-    std_msgs::msg::String planningStatus;
-
-    upo_actions::ExecutePathResult action_result;
-
-    rclcpp::Time start_time;
-    std_msgs::msg::Float32 d2goal;
-    //action client to navigate
-    std::unique_ptr<NavigateClient> navigation_client_2d_ptr;
-    upo_actions::NavigateGoal nav_goal;
-
-    std::unique_ptr<Navigate3DClient> navigation3DClient;
-    upo_actions::Navigate3DGoal goal3D;
-    std::unique_ptr<actionlib::SimpleClientGoalState> state; */
-    
-
-    //!
-    //Theta star algorithm parameters
-    double ws_x_max; // 32.2
-    double ws_y_max; // 32.2
-    double ws_z_max;
-    double ws_x_min;
-    double ws_y_min;
-    double ws_z_min;
-    double map_resolution;
-    double inflate_map;
-
-    bool use3d;
-    bool mapReceived;
-
-    // JAC: Change to the right message of the goal. TODO
-    // trajectory_msgs::msg::MultiDOFJointTrajectoryPoint currentGoal; TODO
-    // std::vector<trajectory_msgs::msg::MultiDOFJointTrajectoryPoint> goals_vector,goals_vector_bl_frame;
-    int last;
-    // std::unique_ptr<tf2_ros::TransformListener> tf_list_ptr;
-    double timeout;
-
-    //ADDING DUE TO ROS2
-    std::unique_ptr<tf2_ros::Buffer> tfBuffer_;
-    std::shared_ptr<tf2_ros::TransformListener> tfListener_{nullptr};
-    
-    nav_msgs::msg::OccupancyGrid lcp;
-};
-
-} // namespace PathPlanners
-
-#endif
diff --git a/include/Planners/ThetaStar.hpp b/include/Planners/ThetaStar.hpp
index 42804ba..0ef0b66 100644
--- a/include/Planners/ThetaStar.hpp
+++ b/include/Planners/ThetaStar.hpp
@@ -53,7 +53,7 @@ namespace Planners
          * @param _s2 Pointer to node s2
          * @param _index_by_pos reference to openset to erase and insert the nodes in some cases
          */
-        virtual void UpdateVertex(Planners::utils::Node *_s, Planners::utils::Node *_s2, node_by_position &_index_by_pos);
+        virtual void UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos);
         
         /**
          * @brief Compute cost algorithm function
@@ -61,7 +61,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux);
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux);
 
         /**
          * @brief This function is the secondary inside the main loop of findPath 
@@ -80,9 +80,20 @@ namespace Planners
          * This operation of erase and re-insert is performed in order to update the position
          * of the node in the container. 
          */
-        virtual void exploreNeighbours(Planners::utils::Node* _current, const Vec3i &_target,node_by_position &_index_by_pos) override;
+        virtual void exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos) override;
+
+        virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos);
+
+        virtual int chooseNeighbours(float angh, float angv);
+        // virtual Vec3i chooseNeighbours(float angh, float angv);
+
+        // virtual Vec3i nodesToExplore(int _node);
+        // virtual void nodesToExplore(int _node, Vec3i &list_nodes_to_explore);
+        // virtual void nodesToExplore(int _node, CoordinateList &list_nodes_to_explore);
 
         utils::CoordinateListPtr checked_nodes, checked_nodes_current;  /*!< TODO Comment */
+        // float mod[26];
+        float angles_h[26], angles_v[26];
 
     };
 
diff --git a/include/Planners/ThetaStarM1.hpp b/include/Planners/ThetaStarM1.hpp
index 4eabf99..a12482b 100644
--- a/include/Planners/ThetaStarM1.hpp
+++ b/include/Planners/ThetaStarM1.hpp
@@ -4,18 +4,18 @@
  * @file ThetaStarM1.hpp
  * @author Rafael Rey (reyarcenegui@gmail.com)
  * @author Jose Antonio Cobano (jacobsua@upo.es)
- *
- * @brief This header declares the functions and class
+ * 
+ * @brief This header declares the functions and class 
  * associated to the Cost Aware Theta* Algorithm. It inherits
  * from the original Theta* algorithm and override two functions:
- *  1. ComputeCost
+ *  1. ComputeCost  
  *  2. ComputeG
- *
+ * 
  * @version 0.1
  * @date 2021-09-20
- *
+ * 
  * @copyright Copyright (c) 2021
- *
+ * 
  */
 #include <Planners/ThetaStar.hpp>
 
@@ -24,55 +24,60 @@ namespace Planners
 
     /**
      * @brief Theta* Algorithm Class
-     *
+     * 
      */
     class ThetaStarM1 : public ThetaStar
     {
 
     public:
+        
         /**
          * @brief Construct a new Theta Star M1 object
-         *
-         * @param _use_3d This parameter allows the user to choose between planning on
+         * 
+         * @param _use_3d This parameter allows the user to choose between planning on 
          * a plane (8 directions possibles) or in the 3D full space (26 directions)
          * @param _name Algorithm name stored internally
-         *
+         * 
          */
-        ThetaStarM1(bool _use_3d, std::string _name);
+        ThetaStarM1(bool _use_3d, std::string _name );
 
         /**
          * @brief Construct a new Cost Aware Theta Star object
-         *
-         * @param _use_3d
+         * 
+         * @param _use_3d 
          */
         ThetaStarM1(bool _use_3d);
 
+
     protected:
+
         /**
          * @brief Compute cost algorithm function
-         *
+         * 
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
+
 
         /**
          * @brief This functions implements the algorithm G function.  The difference
          * with the original A* function is that the returned G value here is composed of three terms:
          * 1. Dist between the current and successor (1, sqrt(2) or sqrt(3))
          * 2. The G value of the current node (How does it cost to reach to the current)
-         * 3. The edge_neighbour cost which is calculated as the averaged between the _current and _suc cost
+         * 3. The edge_neighbour cost which is calculated as the averaged between the _current and _suc cost 
          * scaled with the value dist_scale_factor_reduced_
-         *
+         * 
          *
          * @param _current Pointer to the current node
          * @param _suc Pointer to the successor node
-         * @param _n_i The index of the direction in the directions vector.
+         * @param _n_i The index of the direction in the directions vector. 
          * Depending on this index, the distance wi
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        inline virtual unsigned int computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs) override;
+        inline virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+
     };
 
 }
diff --git a/include/Planners/ThetaStarM2.hpp b/include/Planners/ThetaStarM2.hpp
index b57779b..32face3 100644
--- a/include/Planners/ThetaStarM2.hpp
+++ b/include/Planners/ThetaStarM2.hpp
@@ -57,7 +57,7 @@ namespace Planners
          * @param _s_aux Pointer to first node
          * @param _s2_aux Pointer to the second node
          */
-        inline virtual void ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux) override;
+        inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override;
 
         /**
          * @brief Compute edge distance
@@ -67,7 +67,7 @@ namespace Planners
          * @param _s2 
          * @return unsigned int 
          */
-        virtual unsigned int ComputeEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Planners::utils::Node* _s, const Planners::utils::Node* _s2);
+        virtual unsigned int ComputeEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2);
 
         /**
          * @brief This functions implements the algorithm G function.  The difference
@@ -84,7 +84,7 @@ namespace Planners
          * @param _dirs Number of directions used (to distinguish between 2D and 3D)
          * @return unsigned int The G Value calculated by the function
          */
-        virtual unsigned int computeG(const Planners::utils::Node* _current, Planners::utils::Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
+        virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
 
     
     };
diff --git a/include/utils/geometry_utils.hpp b/include/utils/geometry_utils.hpp
index 29446ac..aae0d73 100644
--- a/include/utils/geometry_utils.hpp
+++ b/include/utils/geometry_utils.hpp
@@ -130,6 +130,15 @@ namespace Planners
              */
             double angleBetweenThreePoints(const Vec3i &_v1, const Vec3i &_v2, const Vec3i &_v3);
 
+            /**
+             * @brief Returns the neighbour nodes chosen from the gradient: two options 9-17 nodes
+             * 
+             * @param _angh
+             * @param _angv
+             * @return double 
+             */
+            int chooseNeighbours(float _angh, float _angv);
+
 #ifdef ROS
             /**
              * @brief 
@@ -165,4 +174,4 @@ namespace Planners
     }//namespace utils
 }
 
-#endif
+#endif
\ No newline at end of file
diff --git a/include/utils/heuristic.hpp b/include/utils/heuristic.hpp
index e2c9ed9..e7c8708 100644
--- a/include/utils/heuristic.hpp
+++ b/include/utils/heuristic.hpp
@@ -36,7 +36,26 @@ namespace Planners
          */
         static Vec3i getDelta(const Vec3i &_source, const Vec3i &_target);
 
+        /**
+         * @brief Returns the absolute of unit vector of each neighbor and the tardet
+         * 
+         * @param _source 
+         * @param _target 
+         * @return Vec3i 
+         */
+        static Vec3i getVectorPull(const Vec3i &_source, const Vec3i &_target);
+
     public:
+
+         /**
+         * @brief Gaol pull to weight the heuristic in the selection of exploration neighbors.
+         * 
+         * @param _source 
+         * @param _target 
+         * @return unsigned int 
+         */
+        static unsigned int goal_pull(const Vec3i &_source, const Vec3i &_target);
+
         /**
          * @brief Manhattan heuristic
          * 
diff --git a/include/utils/ros/ROSInterfaces.hpp b/include/utils/ros/ROSInterfaces.hpp
index 141e3f2..c05f306 100644
--- a/include/utils/ros/ROSInterfaces.hpp
+++ b/include/utils/ros/ROSInterfaces.hpp
@@ -12,14 +12,14 @@
  * @copyright Copyright (c) 2021
  * 
  */
-#include <rclcpp/rclcpp.hpp>
-#include <geometry_msgs/msg/point.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-#include <nav_msgs/msg/occupancy_grid.hpp>
-#include <nav2_costmap_2d/cost_values.hpp>
+#include <ros/ros.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Pose.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <costmap_2d/cost_values.h>
 
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl_ros/transforms.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
 
 #include "utils/utils.hpp"
 #include "Grid3D/grid3d.hpp"
@@ -56,7 +56,7 @@ namespace Planners
          * @param _res resolution
          * @return Vec3i 
          */
-        Vec3i discretePoint(const geometry_msgs::msg::Point &_msg, const double &_res);
+        Vec3i discretePoint(const geometry_msgs::Point &_msg, const double &_res);
 
         /**
          * @brief Discretize a geomtry_msgs::Pose object
@@ -65,7 +65,7 @@ namespace Planners
          * @param _res resolution
          * @return Vec3i 
          */
-        Vec3i discretePose(const geometry_msgs::msg::Pose &_msg, const double &_res);
+        Vec3i discretePose(const geometry_msgs::Pose &_msg, const double &_res);
         /**
          * @brief get a geometry_msgs::Point from a discrete position and resolution
          * 
@@ -73,7 +73,7 @@ namespace Planners
          * @param _res resolution
          * @return geometry_msgs::Point 
          */
-        geometry_msgs::msg::Point continousPoint(const Vec3i &_vec, const double &_res);
+        geometry_msgs::Point continousPoint(const Vec3i &_vec, const double &_res);
 
         /**
          * @brief 
@@ -105,7 +105,7 @@ namespace Planners
          * @return true Currently always return true
          * @return false Never returns false at this version
          */
-        bool configureWorldFromOccupancy(const nav_msgs::msg::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size = false);
+        bool configureWorldFromOccupancy(const nav_msgs::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size = false);
         
         /**
          * @brief 
@@ -116,7 +116,7 @@ namespace Planners
          * @return true 
          * @return false 
          */
-        bool configureWorldFromOccupancyWithCosts(const nav_msgs::msg::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size = false);
+        bool configureWorldFromOccupancyWithCosts(const nav_msgs::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size = false);
 
         /**
          * @brief Same as configureWorldFromOccupancy but to use with a pcl::PointCloud
@@ -142,4 +142,4 @@ namespace Planners
     }
 }
 
-#endif
+#endif
\ No newline at end of file
diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp
index 1c5e031..62303b6 100644
--- a/include/utils/utils.hpp
+++ b/include/utils/utils.hpp
@@ -18,6 +18,7 @@
 #include <variant>
 #include <cmath>
 #include <memory>
+
 #ifdef ROS
 #include <Eigen/Dense>
 #endif
@@ -179,6 +180,13 @@ namespace Planners
             }
 #endif
         };
+
+        class Vec3f
+        {
+            public:
+            float x, y, z;    
+        };
+
         /**
          * @brief 
          * 
diff --git a/launch/planner.launch b/launch/planner.launch
index dcf5602..cd7eb3d 100644
--- a/launch/planner.launch
+++ b/launch/planner.launch
@@ -1,15 +1,43 @@
 <launch>
     <!-- Map name is the filename wihtouth the .bt extensionof the 3D Map -->
-    <arg name="map_name"            default="mbzirc_challenge3"/>
-    <!-- <arg name="map_name"            default="wall"/> -->
-    <!-- <arg name="map_name"            default="laberynth"/> -->
+    <!-- <arg name="map_name"            default="mbzirc_challenge3"/> -->
+    <arg name="map_name"            default="abu_dhabi_002_blender"/>
+    <!-- <arg name="map_name"            default="teatro"/> -->
+    <!-- <arg name="map_name"            default="maze"/> -->
+    <!-- U -->
+    <!-- <arg name="map_name"            default="Mapa4_Slide4_20x20_den100"/> -->
+    <!-- U without border-->
+    <!-- <arg name="map_name"            default="Mapa4_Slide4_20x20_sinborde_den100"/> -->
+    <!-- U almost closed: aperture 0,9m-->
+    <!-- <arg name="map_name"            default="Mapa_Slide5_conborde"/> -->
+    <!-- U almost closed with two holes-->
+    <!-- <arg name="map_name"            default="Mapa_Slide7_20x20_den100_conborde"/> -->
+    <!-- U almost closed with a greater hole: aperture 3m-->
+    <!-- <arg name="map_name"            default="Mapa_Slide6_20x20_den100_conborde"/> -->
+    <!-- U almost closed with a greater hole: aperture 1.5m-->
+    <!-- <arg name="map_name"            default="Mapa_Slide6_v2_20x20_den100_conborde"/> -->
+    <!-- U without border almost closed-->
+    <!-- <arg name="map_name"            default="Mapa_Slide5_sinborde"/> -->
+    <!-- H - Corridor with obstacle in the middle -->
+    <!-- <arg name="map_name"            default="Mapa_Slide3_20x20_den100"/> -->
+    <!-- <arg name="map_name"            default="manufacturing"/> -->
     <arg name="map"                 default="$(find heuristic_planners)/resources/3dmaps/$(arg map_name).bt"/>
 
     <arg name="algorithm_name"      default="costlazythetastar"/>
 
-    <arg name="world_size_x"        default="60"/>
-    <arg name="world_size_y"        default="60"/>
-    <arg name="world_size_z"        default="20"/>
+    <!-- <arg name="world_size_x"        default="60"/>
+    <arg name="world_size_y"        default="60"/> -->
+
+    <arg name="world_size_x"        default="200"/>
+    <arg name="world_size_y"        default="200"/>
+
+
+    <!-- maze.bt with world_size_z=5 -->
+    <!-- <arg name="world_size_z"        default="5"/> -->
+    <!-- <arg name="world_size_z"        default="2"/> manufacturing -->
+    <!-- mbzirc with world_size_z=20 -->
+    <!-- <arg name="world_size_z"        default="20"/>  -->
+    <arg name="world_size_z"        default="30"/>
     <arg name="resolution"          default="0.2"/>
 
     <arg name="inflate_map"         default="true"/>
@@ -17,17 +45,17 @@
     <!-- Possibles values are: euclidean, euclidean_optimized, manhattan, octogonal and dijkstra -->
     <arg name="heuristic"         default="euclidean"/>
 
-    <arg name="save_data"           default="false"/>
+    <arg name="save_data"           default="true"/>
     <!-- This should be a folder -->
     <arg name="data_folder"         default="$(env HOME)"/>
 
-    <arg name="overlay_markers"     default="false"/>
+    <arg name="overlay_markers"     default="true"/>
     
-    <arg name="cost_weight" default="4.0"/>
-    <arg name="max_line_of_sight_distance" default="2.0"/>
+    <arg name="cost_weight" default="500.0"/>
+    <arg name="max_line_of_sight_distance" default="1.0"/>
 
     <!-- GRIDM Generators Parameters -->
-    <arg name="cost_scaling_factor" default="2.0"/>  
+    <arg name="cost_scaling_factor" default="0.5"/>  
     <arg name="robot_radius"        default="0.4"/> 
     
     <arg name="output" default="screen"/>
diff --git a/package.xml b/package.xml
index 12095a8..ff176e5 100644
--- a/package.xml
+++ b/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="3">
+<package format="2">
   <name>heuristic_planners</name>
   <version>0.4.3</version>
   <description>The heuristic planners package </description>
@@ -7,39 +7,60 @@
   <maintainer email="reyarcenegui@gmail.com">RafaelRey</maintainer>
 
   <license>TODO</license>
-  
-  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>geometry_msgs</depend>
-  <depend>visualization_msgs</depend>
-  <depend>std_msgs</depend>
-  <depend>std_srvs</depend>
-  <depend>nav_msgs</depend>
-  <depend>pcl_ros</depend>
-  <depend>pcl_conversions</depend>
-  <depend>roscpp</depend>
-  <depend>octomap_ros</depend>
-  <depend>octomap_server</depend>
-  <depend>costmap_2d</depend>
-  <depend>message_generation</depend>
-  <depend>doxygen</depend>
-  <depend>libssl-dev</depend>
-  <depend>boost</depend>
-  <depend>eigen</depend>
-  <depend>trajectory_msgs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
 
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>octomap_ros</build_depend>
+  <build_depend>octomap_server</build_depend>
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>doxygen</build_depend>
+  <build_depend>libssl-dev</build_depend>
+  <build_depend>boost</build_depend>
+  <build_depend>eigen</build_depend>
+  
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>visualization_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>pcl_ros</build_export_depend>
+  <build_export_depend>pcl_conversions</build_export_depend>
+  <build_export_depend>octomap_ros</build_export_depend>
+  <build_export_depend>octomap_server</build_export_depend>
+  <build_export_depend>costmap_2d</build_export_depend>
+  <build_export_depend>doxygen</build_export_depend>
+  <build_export_depend>libssl-dev</build_export_depend>
+  <build_export_depend>boost</build_export_depend>
+  <build_export_depend>eigen</build_export_depend>
 
-  <build_depend>rosidl_default_generators</build_depend>
-  <build_depend>builtin_interfaces</build_depend>
-  <exec_depend>rosidl_default_runtime</exec_depend>
-  <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>visualization_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>octomap_ros</exec_depend>
+  <exec_depend>octomap_server</exec_depend>
+  <exec_depend>costmap_2d</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>map_server</exec_depend>
+  <exec_depend>doxygen</exec_depend>
+  <exec_depend>libssl-dev</exec_depend>
+  <exec_depend>boost</exec_depend>
+  <exec_depend>eigen</exec_depend>
 
-  <member_of_group>rosidl_interface_packages</member_of_group>
 
+  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <build_type>ament_cmake</build_type>
-  </export>
+    <!-- Other tools can request additional information be placed here -->
 
+  </export>
 </package>
diff --git a/rviz/planners.rviz b/rviz/planners.rviz
index 19e0c04..93730e2 100644
--- a/rviz/planners.rviz
+++ b/rviz/planners.rviz
@@ -1,11 +1,11 @@
 Panels:
   - Class: rviz/Displays
-    Help Height: 78
+    Help Height: 138
     Name: Displays
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.8294117450714111
-    Tree Height: 455
+    Tree Height: 270
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -21,7 +21,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: Map PointCloud
@@ -70,7 +69,7 @@ Visualization Manager:
       Marker Topic: /planner_ros_node/path_line_markers
       Name: Path Line Markers
       Namespaces:
-        costlazythetastar: true
+        lazythetastaredf: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
@@ -86,7 +85,7 @@ Visualization Manager:
       Marker Topic: /planner_ros_node/path_points_markers
       Name: Path Point Markers
       Namespaces:
-        costlazythetastar: true
+        lazythetastaredf: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
@@ -162,8 +161,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 20.100000381469727
-        Min Value: 0.10000000149011612
+        Max Value: 6.933859825134277
+        Min Value: 0.09765999764204025
         Value: true
       Axis: Z
       Channel Name: intensity
@@ -222,34 +221,28 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
-      Distance: 43.4847526550293
+      Angle: 0
+      Class: rviz/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Field of View: 0.7853981852531433
-      Focal Point:
-        X: 41.08955383300781
-        Y: 29.464345932006836
-        Z: -3.5244100093841553
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3747965693473816
+      Scale: 19.73822784423828
       Target Frame: <Fixed Frame>
-      Yaw: 3.4571502208709717
+      X: 25.470565795898438
+      Y: 24.96125030517578
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: true
-  Height: 1016
+  Height: 1366
   Hide Left Dock: true
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002520000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000450fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004500000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a00000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000008950000045000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -257,7 +250,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
-  Width: 924
-  X: 996
-  Y: 27
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 444
diff --git a/scripts/test_algorithms_compare.py b/scripts/test_algorithms_compare.py
index 409b1e2..c8d15a9 100755
--- a/scripts/test_algorithms_compare.py
+++ b/scripts/test_algorithms_compare.py
@@ -72,13 +72,15 @@ class colors: # You may need to change color settings
                     nargs='+', type=str, default=["planner.launch"],
                     choices=launch_list)
 parser.add_argument("--algorithm",  help="name of the algorithm",  
-                    nargs='*', type=str, required=True, default=["astar", "astarm1", "astarm2"], 
-                    choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"])
+                    # nargs='*', type=str, required=True, default=["astar", "astarm1", "astarm2"], 
+                    # choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"])
+                    nargs='*', type=str, required=True, default=["astar", "costastar", "astarsafetycost"], 
+                    choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"])
 
 parser.add_argument("--map-name",     help="name of the map to use. This map should be under the 3d/2d maps folder",     
-                    nargs='+', type=str, default=['mbzirc_challenge3.bt'],
+                    nargs='+', type=str, default=['maze.bt'],
                     choices=maps_list)
-
+# mbzirc_challenge3.bt
 parser.add_argument("--plots",     help="List of result variables to plot",     
                     nargs='+', type=str, required=True,
                     choices=plot_choices)
diff --git a/scripts/test_algorithms_performance.py b/scripts/test_algorithms_performance.py
index e905b45..fe019e4 100755
--- a/scripts/test_algorithms_performance.py
+++ b/scripts/test_algorithms_performance.py
@@ -28,7 +28,7 @@
                     choices=launch_list)
 parser.add_argument("--algorithm",  help="name of the algorithm",
                     nargs='+', type=str, required=True,
-                    choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"])
+                    choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"])
 parser.add_argument("--start-coords", help="start coordinates (x,y,z). Set z to 0 when testing with 2D",
                     nargs=3,   required=True)
 parser.add_argument("--goal-coords", help="goal coordinates (x,y,z). Set z to 0 when testing with 2D",
diff --git a/scripts/test_algorithms_pseudo_random_paths.py b/scripts/test_algorithms_pseudo_random_paths.py
index cd9def1..fe7d59d 100755
--- a/scripts/test_algorithms_pseudo_random_paths.py
+++ b/scripts/test_algorithms_pseudo_random_paths.py
@@ -41,7 +41,9 @@ def generate_centered_rnd_coords(center, margins):
                     choices=launch_list)
 parser.add_argument("--algorithm",  help="name of the algorithm",
                     nargs='+', type=str, required=True,
-                    choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"])
+                    # choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"])
+                    # choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastargradient"])
+                    choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"])
 parser.add_argument("--start-coords", help="start coordinates (x,y,z). Set z to 0 when testing with 2D",
                     nargs=3,   required=True)
 parser.add_argument("--goal-coords", help="goal coordinates (x,y,z). Set z to 0 when testing with 2D",
diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp
index 0ae7102..0fcd4d1 100644
--- a/src/Planners/AStar.cpp
+++ b/src/Planners/AStar.cpp
@@ -1,275 +1,289 @@
 #include "Planners/AStar.hpp"
-#include <pcl_conversions/pcl_conversions.h>
-#include <sensor_msgs/msg/detail/point_cloud2__struct.hpp>
 
-namespace Planners
+namespace Planners{
+    
+AStar::AStar(bool _use_3d = true, std::string _name = "astar" ): AlgorithmBase(_use_3d, _name){
+    configAlgorithm();
+}
+    
+AStar::AStar(bool _use_3d = true): AlgorithmBase(_use_3d, "astar")
 {
-  std::shared_ptr<rclcpp::Node> AStar::ros_node_ptr_ = nullptr;
-
-    AStar::AStar(bool _use_3d = true, std::string _name = "astar") : AlgorithmBase(_use_3d, _name)
-                                                                     
-    {
-        configAlgorithm();
-    }
-
-    AStar::AStar(bool _use_3d = true) : AlgorithmBase(_use_3d, "astar")
-    {
-        configAlgorithm();
-    }
-    void AStar::configAlgorithm()
-    {
+    configAlgorithm();
+}
+void AStar::configAlgorithm(){
 
-        closedSet_.reserve(50000);
-        openSet_.reserve(50000);
-        // If compiled with ros and visualization
+    closedSet_.reserve(50000);
+    openSet_.reserve(50000);
+    //If compiled with ros and visualization
 #ifdef ROS
-      if (ros_node_ptr_ == nullptr){
-        ros_node_ptr_ = std::make_shared<rclcpp::Node>("Algorithm_node");
-        explored_nodes_marker_pub_ = ros_node_ptr_->create_publisher<visualization_msgs::msg::Marker>("explored_nodes", 1);
-        openset_marker_pub_ = ros_node_ptr_->create_publisher<visualization_msgs::msg::Marker>("openset_nodes", 1);
-        closedset_marker_pub_ = ros_node_ptr_->create_publisher<visualization_msgs::msg::Marker>("closed_set_nodes", 1);
-        best_node_marker_pub_ = ros_node_ptr_->create_publisher<visualization_msgs::msg::Marker>("best_node_marker", 1);
-        aux_text_marker_pub_ = ros_node_ptr_->create_publisher<visualization_msgs::msg::Marker>("aux_text_marker", 1);
-        occupancy_marker_pub_ = ros_node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>("occupancy_markers", 1);
-
-        ros_node_ptr_->declare_parameter<std::string>("frame_id", "map");
-        ros_node_ptr_->declare_parameter<float>("resolution", 0.2);
-    }
-        std::string frame_id;
-
-        ros_node_ptr_->get_parameter("frame_id",frame_id);
-        ros_node_ptr_->get_parameter("resolution",resolution_);
-
-        occupancy_marker_.header.frame_id = frame_id; // "world";
-
-        explored_node_marker_.header.frame_id = frame_id; //"world";
-        explored_node_marker_.header.stamp = ros_node_ptr_->now();
-        explored_node_marker_.ns = "debug";
-        explored_node_marker_.id = 66;
-        explored_node_marker_.type = visualization_msgs::msg::Marker::CUBE_LIST;
-        explored_node_marker_.action = visualization_msgs::msg::Marker::ADD;
-        explored_node_marker_.pose.orientation.w = 1.0;
-        explored_node_marker_.scale.x = 1.0 * resolution_;
-        explored_node_marker_.scale.y = 1.0 * resolution_;
-        explored_node_marker_.scale.z = 1.0 * resolution_;
-        explored_node_marker_.color.a = 0.7;
-        explored_node_marker_.color.r = 0.0;
-        explored_node_marker_.color.g = 1.0;
-        explored_node_marker_.color.b = 0.0;
-
-        openset_markers_ = explored_node_marker_;
-        openset_markers_.color.b = 1.0;
-        openset_markers_.color.g = 0.0;
-        openset_markers_.id = 67;
-
-        closed_set_markers_ = explored_node_marker_;
-        closed_set_markers_.color.g = 0.0;
-        closed_set_markers_.color.r = 1.0;
-        explored_node_marker_.id = 68;
-
-        best_node_marker_ = explored_node_marker_;
-        best_node_marker_.color.g = 0.7;
-        best_node_marker_.color.b = 0.7;
-        best_node_marker_.id = 69;
-        best_node_marker_.type = visualization_msgs::msg::Marker::SPHERE;
-
-        aux_text_marker_ = explored_node_marker_;
-        aux_text_marker_.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
-        aux_text_marker_.id = 70;
-        aux_text_marker_.color.a = 0.7;
-        aux_text_marker_.color.g = 0.0;
-        aux_text_marker_.text = "";
-        aux_text_marker_.scale.z = 3.0 * resolution_;
-        last_publish_tamp_ = ros_node_ptr_->now();
+    explored_nodes_marker_pub_ = lnh_.advertise<visualization_msgs::Marker>("explored_nodes",   1);
+    openset_marker_pub_        = lnh_.advertise<visualization_msgs::Marker>("openset_nodes",    1);
+    closedset_marker_pub_      = lnh_.advertise<visualization_msgs::Marker>("closed_set_nodes", 1);
+    best_node_marker_pub_      = lnh_.advertise<visualization_msgs::Marker>("best_node_marker", 1);
+    aux_text_marker_pub_       = lnh_.advertise<visualization_msgs::Marker>("aux_text_marker",  1);
+	occupancy_marker_pub_ = lnh_.advertise<pcl::PointCloud<pcl::PointXYZ>>("occupancy_markers", 1, true);
+
+    std::string frame_id;
+    lnh_.param("frame_id", frame_id, std::string("map"));	
+    lnh_.param("resolution", resolution_, (float)0.2);
+	occupancy_marker_.header.frame_id = frame_id; // "world";
+
+    explored_node_marker_.header.frame_id = frame_id; //"world";
+	explored_node_marker_.header.stamp = ros::Time();
+	explored_node_marker_.ns = "debug";
+	explored_node_marker_.id = 66;
+	explored_node_marker_.type = visualization_msgs::Marker::CUBE_LIST;
+	explored_node_marker_.action = visualization_msgs::Marker::ADD;
+	explored_node_marker_.pose.orientation.w = 1.0;
+	explored_node_marker_.scale.x = 1.0 * resolution_;
+	explored_node_marker_.scale.y = 1.0 * resolution_;
+	explored_node_marker_.scale.z = 1.0 * resolution_;
+	explored_node_marker_.color.a = 0.7;
+	explored_node_marker_.color.r = 0.0;
+	explored_node_marker_.color.g = 1.0;
+	explored_node_marker_.color.b = 0.0;
+
+    openset_markers_    = explored_node_marker_;
+    openset_markers_.color.b = 1.0;
+    openset_markers_.color.g = 0.0;
+	openset_markers_.id      = 67;
+
+    closed_set_markers_ = explored_node_marker_;
+    closed_set_markers_.color.g = 0.0;
+    closed_set_markers_.color.r = 1.0;
+	explored_node_marker_.id = 68;
+
+    best_node_marker_ = explored_node_marker_;
+    best_node_marker_.color.g = 0.7;
+    best_node_marker_.color.b = 0.7;
+    best_node_marker_.id     = 69;
+	best_node_marker_.type = visualization_msgs::Marker::SPHERE;
+
+    aux_text_marker_   = explored_node_marker_;
+	aux_text_marker_.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+	aux_text_marker_.id = 70;
+	aux_text_marker_.color.a = 0.7;
+	aux_text_marker_.color.g = 0.0;
+    aux_text_marker_.text = "";
+	aux_text_marker_.scale.z = 3.0 * resolution_;
+    last_publish_tamp_ = ros::Time::now();
 #endif
-    }
-    void AStar::publishOccupationMarkersMap()
-    {
+
+}
+void AStar::publishOccupationMarkersMap()
+{
 #ifdef ROS
-        occupancy_marker_.clear();
-        for (const auto &it : discrete_world_.getElements())
-        {
-            if (!it.occuppied)
-                continue;
-            pcl::PointXYZ point;
-
-            point.x = it.coordinates.x * resolution_;
-            point.y = it.coordinates.y * resolution_;
-            point.z = it.coordinates.z * resolution_;
-            occupancy_marker_.push_back(point);
-        }
+	occupancy_marker_.clear();
+    for(const auto &it: discrete_world_.getElements()){
+        if(!it.occuppied) continue;
+        pcl::PointXYZ point;
+
+		point.x = it.coordinates.x * resolution_;
+		point.y = it.coordinates.y * resolution_;
+		point.z = it.coordinates.z * resolution_;
+		occupancy_marker_.push_back(point);
+    }
 
-        sensor_msgs::msg::PointCloud2 occupancy_marker_msg;
-        pcl::toROSMsg(occupancy_marker_, occupancy_marker_msg);
-        occupancy_marker_pub_->publish(occupancy_marker_msg);
+	occupancy_marker_pub_.publish(occupancy_marker_);
 #endif
-    }
+}
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
-    template <typename T, typename U>
-    void AStar::publishROSDebugData(const Planners::utils::Node *_node, const T &_open_set, const U &_closed_set)
-    {
+template<typename T, typename U>
+void AStar::publishROSDebugData(const Node* _node, const T &_open_set, const U &_closed_set)
+{
 #if defined(ROS) && defined(PUB_EXPLORED_NODES)
-        if ((ros::Time::now() - last_publish_tamp_).toSec() > duration_pub_.toSec())
-        {
-            last_publish_tamp_ = ros::Time::now();
-            explored_node_marker_.header.stamp = ros::Time();
-            explored_node_marker_.header.seq++;
-            openset_markers_.header.stamp = ros::Time();
-            openset_markers_.header.seq++;
-            closed_set_markers_.header.stamp = ros::Time();
-            closed_set_markers_.header.seq++;
-
-            openset_markers_.points.clear();
-            closed_set_markers_.points.clear();
-
-            explored_node_marker_.points.push_back(continousPoint(_node->coordinates, resolution_));
-
-            for (const auto &it : _open_set)
-                openset_markers_.points.push_back(continousPoint(it->coordinates, resolution_));
-
-            for (const auto &it : _closed_set)
-                closed_set_markers_.points.push_back(continousPoint(it->coordinates, resolution_));
-
-            best_node_marker_.pose.position = continousPoint(_node->coordinates, resolution_);
-            best_node_marker_.pose.position.z += resolution_;
-
-            aux_text_marker_.text = "Best node G = " + std::to_string(_node->G) + "\nBest node G+H = " + std::to_string(_node->G + _node->H) +
-                                    std::string("\nCost = ") + std::to_string(static_cast<int>(cost_weight_ * _node->cost * (dist_scale_factor_ / 100)));
-            aux_text_marker_.pose = best_node_marker_.pose;
-            aux_text_marker_.pose.position.z += 5 * resolution_;
-
-            closedset_marker_pub_.publish(closed_set_markers_);
-            openset_marker_pub_.publish(openset_markers_);
-            best_node_marker_pub_.publish(best_node_marker_);
-            explored_nodes_marker_pub_.publish(explored_node_marker_);
-            aux_text_marker_pub_.publish(aux_text_marker_);
-            // usleep(1e4);
-            // std::cout << "Please a key to go to the next iteration..." << std::endl;
-            // getchar(); // Comentar para no usar tecla.
-        }
+    if( (ros::Time::now() - last_publish_tamp_ ).toSec() > duration_pub_.toSec() ){
+        last_publish_tamp_ = ros::Time::now();
+        explored_node_marker_.header.stamp = ros::Time();
+        explored_node_marker_.header.seq++;
+        openset_markers_.header.stamp = ros::Time();
+        openset_markers_.header.seq++;
+        closed_set_markers_.header.stamp = ros::Time();
+        closed_set_markers_.header.seq++;
+
+        openset_markers_.points.clear();
+        closed_set_markers_.points.clear();
+
+        explored_node_marker_.points.push_back(continousPoint(_node->coordinates, resolution_));
+
+        for(const auto &it: _open_set)
+            openset_markers_.points.push_back(continousPoint(it->coordinates, resolution_));
+
+        for(const auto &it: _closed_set)
+            closed_set_markers_.points.push_back(continousPoint(it->coordinates, resolution_));
+
+        best_node_marker_.pose.position = continousPoint(_node->coordinates, resolution_);
+        best_node_marker_.pose.position.z += resolution_;
+
+        aux_text_marker_.text = "Best node G = " + std::to_string(_node->G) + "\nBest node G+H = " + std::to_string(_node->G+_node->H) +
+                     std::string("\nCost = ") + std::to_string(static_cast<int>(cost_weight_ * _node->cost * (dist_scale_factor_/100)));
+	    aux_text_marker_.pose = best_node_marker_.pose;
+        aux_text_marker_.pose.position.z += 5 * resolution_;
+
+        closedset_marker_pub_.publish(closed_set_markers_);
+        openset_marker_pub_.publish(openset_markers_);
+        best_node_marker_pub_.publish(best_node_marker_);
+        explored_nodes_marker_pub_.publish(explored_node_marker_);
+        aux_text_marker_pub_.publish(aux_text_marker_);
+        // usleep(1e4);
+        // std::cout << "Please a key to go to the next iteration..." << std::endl;
+        // getchar(); // Comentar para no usar tecla.
+    }
 
 #endif
-    }
 
-    inline unsigned int AStar::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
-        unsigned int cost = _current->G;
+}
 
-        if (_dirs == 8)
-        {
-            cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
-        }
+// JAC: Computation of the attractin vector
+inline Vec3i AStar::getVectorPull(const Vec3i &_source, const Vec3i &_target){
+    return {_target.x - _source.x, _target.y - _source.y, _target.z - _source.z};
+}
+
+// JAC: Create a file --> computeGradient
+inline float AStar::computeGradient(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
+    unsigned int cost;
+
+    float rest,grad;
+    // float div;
+
+    if(_dirs  == 8){
+        cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+        grad=(_current->cost - _suc->cost)/cost;
+    }else{
+        cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
+        rest=_current->cost-_suc->cost;
+
+        // // Casi lo mismo que multiplicar 500*grad????
+        // if (cost == 100)
+        //     div=0.2; // res
+        // else if (cost == 141)
+        //     div=0.2828; //sqrt(2)*res
+        // else if (cost == 173)
+        //     div = 0.3464; //sqrt(3)*res
+
+        // std::cout << "cost: " << cost << std::endl;
+        // std::cout << "div: " << div << std::endl;
+        // std::cout << "current cost: " << _current->cost << std::endl;
+        // std::cout << "suc cost: " << _suc->cost << std::endl;
+        // std::cout << "rest: " << rest << std::endl;
+        // std::cout << "G: " << _current->G << std::endl;
+
+        // grad=static_cast<float>((100/0.2)*(rest/cost)); // res=0,2
+        grad=static_cast<float>((500)*(rest/cost)); // res=0,2
+        // grad=static_cast<float>(rest/div);
+        // std::cout << "gradient: " << grad << std::endl;
+    }
+        
+    return grad;
+}
 
-        _suc->C = _suc->cost;
+inline unsigned int AStar::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
+    unsigned int cost = _current->G;
 
-        return cost;
+    if(_dirs  == 8){
+        cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+    }else{
+        cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
     }
+    
+    _suc->C = _suc->cost;
+    
+    return cost;
+}
 
 #pragma GCC diagnostic pop
 
-    void AStar::exploreNeighbours(Planners::utils::Node *_current, const Vec3i &_target, node_by_position &_index_by_pos)
-    {
-
-        for (unsigned int i = 0; i < direction.size(); ++i)
-        {
-
-            Vec3i newCoordinates = _current->coordinates + direction[i];
-            Planners::utils::Node *successor = discrete_world_.getNodePtr(newCoordinates);
-            // Skip the neighbour if it is not valid, occupied, or already in teh
-            // closed list
-            if (successor == nullptr ||
-                successor->isInClosedList ||
-                successor->occuppied)
-                continue;
-
-            unsigned int totalCost = computeG(_current, successor, i, direction.size());
-
-            if (!successor->isInOpenList)
-            {
-                successor->parent = _current;
-                successor->G = totalCost;
-                successor->H = heuristic(successor->coordinates, _target);
-                successor->gplush = successor->G + successor->H;
-                successor->isInOpenList = true;
-                _index_by_pos.insert(successor);
-            }
-            else if (totalCost < successor->G)
-            {
-                successor->parent = _current;
-                successor->G = totalCost;
-                successor->gplush = successor->G + successor->H;
-                auto found = _index_by_pos.find(successor->world_index);
-                _index_by_pos.erase(found);
-                _index_by_pos.insert(successor);
-            }
+void AStar::exploreNeighbours(Node* _current, const Vec3i &_target, node_by_position &_index_by_pos){
+    
+    for (unsigned int i = 0; i < direction.size(); ++i) {
+            
+        Vec3i newCoordinates = _current->coordinates + direction[i];
+        Node *successor = discrete_world_.getNodePtr(newCoordinates);
+        //Skip the neighbour if it is not valid, occupied, or already in the
+        //closed list
+        if ( successor == nullptr ||
+             successor->isInClosedList || 
+             successor->occuppied ) 
+            continue;
+ 
+        unsigned int totalCost = computeG(_current, successor, i, direction.size());
+            
+        if ( !successor->isInOpenList ) { 
+            successor->parent = _current;
+            successor->G = totalCost;
+            successor->H = heuristic(successor->coordinates, _target);
+            successor->gplush = successor->G + successor->H;
+            successor->isInOpenList = true;
+            _index_by_pos.insert(successor);
+        }
+        else if (totalCost < successor->G) {
+            successor->parent = _current;
+            successor->G = totalCost;
+            successor->gplush = successor->G + successor->H;
+            auto found = _index_by_pos.find(successor->world_index);
+            _index_by_pos.erase(found);
+            _index_by_pos.insert(successor);
         }
     }
-    PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target)
-    {
-        Planners::utils::Node *current = nullptr;
-
-        bool solved{false};
-
-        discrete_world_.getNodePtr(_source)->parent = new Planners::utils::Node(_source);
-        discrete_world_.setOpenValue(_source, true);
-        // Timer to record the execution time, not
-        // really important
-        utils::Clock main_timer;
-        main_timer.tic();
-
-        line_of_sight_checks_ = 0;
-
-        node_by_cost &indexByCost = openSet_.get<IndexByCost>();
-        node_by_position &indexByWorldPosition = openSet_.get<IndexByWorldPosition>();
-
-        indexByCost.insert(discrete_world_.getNodePtr(_source));
-
-        while (!indexByCost.empty())
-        {
-            // Get the element at the start of the open set ordered by cost
-            auto it = indexByCost.begin();
-            current = *it;
-            indexByCost.erase(indexByCost.begin());
-
-            if (current->coordinates == _target)
-            {
-                solved = true;
-                break;
-            }
+}
 
-            closedSet_.push_back(current);
-            // This flags are used to avoid search in the containers,
-            // for speed reasons.
-            current->isInOpenList = false;
-            current->isInClosedList = true;
+PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target)
+{
+    Node *current = nullptr;
+
+    bool solved{false};
+
+    discrete_world_.getNodePtr(_source)->parent = new Node(_source);
+    discrete_world_.setOpenValue(_source, true);
+    //Timer to record the execution time, not 
+    //really important
+    utils::Clock main_timer;
+    main_timer.tic();
+
+    line_of_sight_checks_ = 0;
+    
+    node_by_cost&     indexByCost          = openSet_.get<IndexByCost>();
+    node_by_position& indexByWorldPosition = openSet_.get<IndexByWorldPosition>();
+
+    indexByCost.insert(discrete_world_.getNodePtr(_source));
+    
+    while (!indexByCost.empty()) {
+        //Get the element at the start of the open set ordered by cost
+        auto it = indexByCost.begin();
+        current = *it;
+        indexByCost.erase(indexByCost.begin());
+        
+        if (current->coordinates == _target) { solved = true; break; }
+        
+        closedSet_.push_back(current);
+        //This flags are used to avoid search in the containers, 
+        //for speed reasons.
+        current->isInOpenList = false;
+        current->isInClosedList = true;
 
 #if defined(ROS) && defined(PUB_EXPLORED_NODES)
-            publishROSDebugData(current, indexByCost, closedSet_);
+        publishROSDebugData(current, indexByCost, closedSet_);
 #endif
 
-            exploreNeighbours(current, _target, indexByWorldPosition);
-        }
-        main_timer.toc();
-
-        PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(),
-                                                      solved, _source, line_of_sight_checks_);
-        // Clear internal variables. This should be done
-        // the same way in every new algorithm implemented.
+        exploreNeighbours(current, _target, indexByWorldPosition);     
+    }
+    main_timer.toc();
+    
+    PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), 
+                                                 solved, _source, line_of_sight_checks_);
+   //Clear internal variables. This should be done
+   //the same way in every new algorithm implemented.
 #if defined(ROS) && defined(PUB_EXPLORED_NODES)
-        explored_node_marker_.points.clear();
+    explored_node_marker_.points.clear();
 #endif
-        closedSet_.clear();
-        openSet_.clear();
-        delete discrete_world_.getNodePtr(_source)->parent;
-
-        discrete_world_.resetWorld();
-        return result_data;
-    }
+    closedSet_.clear();
+    openSet_.clear();
+    delete discrete_world_.getNodePtr(_source)->parent;
+    
+    discrete_world_.resetWorld();
+    return result_data;
+}
 
 }
diff --git a/src/Planners/AStarM1.cpp b/src/Planners/AStarM1.cpp
index 576f351..835a501 100644
--- a/src/Planners/AStarM1.cpp
+++ b/src/Planners/AStarM1.cpp
@@ -1,30 +1,28 @@
 #include "Planners/AStarM1.hpp"
 
-namespace Planners
-{
-
-    AStarM1::AStarM1(bool _use_3d) : AStar(_use_3d, "astarm1") {}
-    AStarM1::AStarM1(bool _use_3d, std::string _name = "astarm1") : AStar(_use_3d, _name) {}
-
-    inline unsigned int AStarM1::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
-
-        unsigned int cost = _current->G;
-
-        if (_dirs == 8)
-        {
-            cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
-        }
-
-        auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
-
-        cost += cost_term;
-        _suc->C = cost_term;
-
-        return cost;
+namespace Planners{
+    
+AStarM1::AStarM1(bool _use_3d):AStar(_use_3d, "astarm1") {}
+AStarM1::AStarM1(bool _use_3d, std::string _name = "astarm1" ):AStar(_use_3d, _name) {}
+
+inline unsigned int AStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){
+    
+    unsigned int cost = _current->G;
+
+    if(_dirs == 8){
+        cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+    }else{
+        cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
     }
+ 
+    // auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
+    // IROS
+    // auto cost_term = static_cast<unsigned int>(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc 
+    // RA-L
+    auto cost_term = static_cast<unsigned int>(cost_weight_ * (1/(((static_cast<double>(_current->cost) + static_cast<double>(_suc->cost))/2) * dist_scale_factor_reduced_))); 
+    cost += cost_term;
+    _suc->C = cost_term;
+    
+    return cost;
+}
 }
diff --git a/src/Planners/AStarM2.cpp b/src/Planners/AStarM2.cpp
index 26742f2..cc45ff5 100644
--- a/src/Planners/AStarM2.cpp
+++ b/src/Planners/AStarM2.cpp
@@ -1,33 +1,29 @@
 #include "Planners/AStarM2.hpp"
 
-namespace Planners
-{
+namespace Planners{
+    
+AStarM2::AStarM2(bool _use_3d):AStar(_use_3d, "astarm2") {}
+AStarM2::AStarM2(bool _use_3d, std::string _name = "astarm2" ):AStar(_use_3d, _name) {}
 
-    AStarM2::AStarM2(bool _use_3d) : AStar(_use_3d, "astarm2") {}
-    AStarM2::AStarM2(bool _use_3d, std::string _name = "astarm2") : AStar(_use_3d, _name) {}
 
-    inline unsigned int AStarM2::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
+inline unsigned int AStarM2::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){
+    
+    unsigned int cost = 0;
 
-        unsigned int cost = 0;
-
-        if (_dirs == 8)
-        {
-            cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
-        }
-
-        double cc = (_current->cost + _suc->cost) / 2;
-
-        auto edge_neighbour = static_cast<unsigned int>(cc * cost_weight_ * dist_scale_factor_reduced_);
+    if(_dirs == 8){
+        cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+    }else{
+        cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
+    }
 
-        cost += (_current->G + edge_neighbour);
+    double cc = ( _current->cost + _suc->cost ) / 2;
+    
+    auto edge_neighbour = static_cast<unsigned int>( cc *  cost_weight_ * dist_scale_factor_reduced_); 
 
-        _suc->C = edge_neighbour;
+    cost += ( _current->G + edge_neighbour );
 
-        return cost;
-    }
+    _suc->C = edge_neighbour;
+    
+    return cost;
+}
 }
diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp
index c1ca8a0..924f9d6 100644
--- a/src/Planners/AlgorithmBase.cpp
+++ b/src/Planners/AlgorithmBase.cpp
@@ -3,73 +3,41 @@
 namespace Planners
 {
 
-    AlgorithmBase::AlgorithmBase(bool _use_3d = true, const std::string &_algorithm_name = "generic_3d_algorithm") : algorithm_name_(_algorithm_name)
-    {
+    AlgorithmBase::AlgorithmBase(bool _use_3d = true, const std::string &_algorithm_name = "generic_3d_algorithm"): algorithm_name_(_algorithm_name){
         setHeuristic(&Heuristic::euclidean);
         CoordinateList directions2d, directions3d;
         directions2d = {
-            {0, 1, 0}, {0, -1, 0}, {1, 0, 0}, {-1, 0, 0}, // 4 straight elements
-            {1, -1, 0},
-            {-1, 1, 0},
-            {-1, -1, 0},
-            {1, 1, 0} // 4 diagonal elements
+            { 0, 1, 0 }, {0, -1, 0}, { 1, 0, 0 }, { -1, 0, 0 }, //4 straight elements
+            { 1, -1, 0 }, { -1, 1, 0 }, { -1, -1, 0 }, { 1, 1, 0 } //4 diagonal elements
         };
         directions3d = {
 
-            {0, 1, 0},
-            {0, -1, 0},
-            {1, 0, 0},
-            {-1, 0, 0},
-            {0, 0, 1},
-            {0, 0, -1}, // 6 first elements
-
-            {1, -1, 0},
-            {-1, 1, 0},
-            {-1, -1, 0},
-            {1, 1, 0},
-            {-1, 0, -1}, // 7-18 inclusive
-            {1, 0, 1},
-            {1, 0, -1},
-            {-1, 0, 1},
-            {0, -1, 1},
-            {0, 1, 1},
-            {0, 1, -1},
-            {0, -1, -1},
-
-            {-1, -1, 1},
-            {1, 1, 1},
-            {-1, 1, 1},
-            {1, -1, 1},
-            {-1, -1, -1},
-            {1, 1, -1},
-            {-1, 1, -1},
-            {1, -1, -1},
+            { 0, 1, 0 }, {0, -1, 0}, { 1, 0, 0 }, { -1, 0, 0 }, { 0, 0, 1}, { 0, 0, -1}, //6 first elements
+
+            { 1, -1, 0 }, { -1, 1, 0 }, { -1, -1, 0 }, { 1, 1, 0 },  { -1, 0, -1 }, //7-18 inclusive
+            { 1, 0, 1 }, { 1, 0, -1 }, {-1, 0, 1}, { 0, -1, 1 }, { 0, 1, 1 }, { 0, 1, -1 },  { 0, -1, -1 }, 
+
+            { -1, -1, 1 }, { 1, 1, 1 },  { -1, 1, 1 }, { 1, -1, 1 }, { -1, -1, -1 }, { 1, 1, -1 }, { -1, 1, -1 }, { 1, -1, -1 }, 
         };
-        if (_use_3d)
-        {
+        if(_use_3d){
             std::cout << "[Algorithm] Using 3D Directions" << std::endl;
             direction = directions3d;
-        }
-        else
-        {
+        }else{
             std::cout << "[Algorithm] Using 2D Directions" << std::endl;
             direction = directions2d;
         }
     }
-    void AlgorithmBase::setWorldSize(const Vec3i &_worldSize, const double _resolution)
+    void AlgorithmBase::setWorldSize(const Vec3i &_worldSize,const double _resolution)
     {
         discrete_world_.resizeWorld(_worldSize, _resolution);
     }
-    Vec3i AlgorithmBase::getWorldSize()
-    {
+    Vec3i AlgorithmBase::getWorldSize(){
         return discrete_world_.getWorldSize();
     }
-    double AlgorithmBase::getWorldResolution()
-    {
+    double AlgorithmBase::getWorldResolution(){
         return discrete_world_.getResolution();
     }
-    utils::DiscreteWorld *AlgorithmBase::getInnerWorld()
-    {
+    utils::DiscreteWorld* AlgorithmBase::getInnerWorld(){
         return &discrete_world_;
     }
 
@@ -77,8 +45,7 @@ namespace Planners
     {
         heuristic = std::bind(heuristic_, std::placeholders::_1, std::placeholders::_2);
     }
-    bool AlgorithmBase::configureCellCost(const Vec3i &coordinates_, const double &_cost)
-    {
+    bool AlgorithmBase::configureCellCost(const Vec3i &coordinates_, const double &_cost){
 
         return discrete_world_.setNodeCost(coordinates_, _cost);
     }
@@ -117,36 +84,31 @@ namespace Planners
         }
     }
 
-    PathData AlgorithmBase::createResultDataObject(const Planners::utils::Node *_last, utils::Clock &_timer,
-                                                   const size_t _explored_nodes, bool _solved,
-                                                   const Vec3i &_start, const unsigned int _sight_checks)
-    {
-
+    PathData AlgorithmBase::createResultDataObject(const Node* _last, utils::Clock &_timer, 
+                                                    const size_t _explored_nodes, bool _solved,
+                                                    const Vec3i &_start, const unsigned int _sight_checks){
+                                    
         PathData result_data;
 
-        result_data["solved"] = _solved;
-        result_data["goal_coords"] = _last->coordinates;
+        result_data["solved"]       = _solved;
+        result_data["goal_coords"]  = _last->coordinates;
         result_data["g_final_node"] = _last->G;
 
         CoordinateList path;
-
-        if (_solved)
-        {
-            while (_last != nullptr)
-            {
-
+        
+        if(_solved){
+            while (_last != nullptr) {
+                
                 path.push_back(_last->coordinates);
                 _last = _last->parent;
             }
-        }
-        else
-        {
+        }else{
             std::cout << "Error impossible to calculate a solution" << std::endl;
         }
-
+        
         unsigned int total_cost1{0};
         unsigned int total_cost2{0};
-
+        
         unsigned int total_H{0};
         unsigned int total_G1{0};
         unsigned int total_G2{0};
@@ -156,53 +118,51 @@ namespace Planners
         unsigned int total_grid_cost2{0};
 
 #ifdef COMPUTE_STATISTICS
-
+        
         auto adjacent_path = utils::geometry::getAdjacentPath(path, discrete_world_);
-
-        for (size_t i = 0; i < adjacent_path.size() - 1; ++i)
-        {
+        
+        for(size_t i = 0; i < adjacent_path.size() - 1; ++i){
             auto node_current = discrete_world_.getNodePtr(adjacent_path[i]);
-            auto node = discrete_world_.getNodePtr(adjacent_path[i + 1]);
-            if (node == nullptr)
+            auto node         = discrete_world_.getNodePtr(adjacent_path[i+1]);
+            if( node == nullptr )
                 continue;
+            
+            total_H         += node->H;
+            total_C         += node->C;
 
-            total_H += node->H;
-            total_C += node->C;
+            total_grid_cost1 += static_cast<unsigned int>( cost_weight_ * node_current->cost * dist_scale_factor_reduced_ );  // CAA*+M1
+            total_grid_cost2 += static_cast<unsigned int>( ( ( node->cost + node_current->cost ) / 2) *  cost_weight_ * dist_scale_factor_reduced_); ; //CAA*+M2         
 
-            total_grid_cost1 += static_cast<unsigned int>(cost_weight_ * node_current->cost * dist_scale_factor_reduced_); // CAA*+M1
-            total_grid_cost2 += static_cast<unsigned int>(((node->cost + node_current->cost) / 2) * cost_weight_ * dist_scale_factor_reduced_);
-            ; // CAA*+M2
+            unsigned int g_real1 = utils::geometry::distanceBetween2Nodes(adjacent_path[i], adjacent_path[i+1]); 
+            unsigned int g_real2 = utils::geometry::distanceBetween2Nodes(adjacent_path[i], adjacent_path[i+1]);  // Conmensurable
 
-            unsigned int g_real1 = utils::geometry::distanceBetween2Nodes(adjacent_path[i], adjacent_path[i + 1]);
-            unsigned int g_real2 = utils::geometry::distanceBetween2Nodes(adjacent_path[i], adjacent_path[i + 1]); // Conmensurable
-
-            total_G1 += g_real1;
-            total_G2 += g_real2;
+            total_G1 += g_real1; 
+            total_G2 += g_real2;            
         }
-
-        total_cost1 = total_G1 + total_grid_cost1;
-        total_cost2 = total_G2 + total_grid_cost2;
+        
+        total_cost1 = total_G1 + total_grid_cost1; 
+        total_cost2 = total_G2 + total_grid_cost2; 
 #endif
-        result_data["algorithm"] = algorithm_name_;
-        result_data["path"] = path;
-        result_data["time_spent"] = _timer.getElapsedMicroSeconds();
-        result_data["explored_nodes"] = _explored_nodes;
-        result_data["start_coords"] = _start;
-        result_data["path_length"] = geometry::calculatePathLength(path, discrete_world_.getResolution());
-
-        result_data["total_cost1"] = total_cost1;
-        result_data["total_cost2"] = total_cost2;
-        result_data["h_cost"] = total_H;
-        result_data["g_cost1"] = total_G1;
-        result_data["g_cost2"] = total_G2;
-        result_data["c_cost"] = total_C;
-        result_data["grid_cost1"] = total_grid_cost1;
-        result_data["grid_cost2"] = total_grid_cost2;
-
-        result_data["line_of_sight_checks"] = _sight_checks;
+        result_data["algorithm"]               = algorithm_name_;
+        result_data["path"]                    = path;
+        result_data["time_spent"]              = _timer.getElapsedMicroSeconds();
+        result_data["explored_nodes"]          = _explored_nodes;
+        result_data["start_coords"]            = _start;
+        result_data["path_length"]             = geometry::calculatePathLength(path, discrete_world_.getResolution());
+
+        result_data["total_cost1"]              = total_cost1;
+        result_data["total_cost2"]              = total_cost2;
+        result_data["h_cost"]                   = total_H;
+        result_data["g_cost1"]                  = total_G1;
+        result_data["g_cost2"]                  = total_G2;
+        result_data["c_cost"]                   = total_C;
+        result_data["grid_cost1"]               = total_grid_cost1;
+        result_data["grid_cost2"]               = total_grid_cost2;
+
+        result_data["line_of_sight_checks"]    = _sight_checks;
         result_data["max_line_of_sight_cells"] = max_line_of_sight_cells_;
-        result_data["cost_weight"] = cost_weight_;
-
+        result_data["cost_weight"]             = cost_weight_;
+    
         return result_data;
     }
 
diff --git a/src/Planners/LazyThetaStar.cpp b/src/Planners/LazyThetaStar.cpp
index 2a0bf66..9a87920 100644
--- a/src/Planners/LazyThetaStar.cpp
+++ b/src/Planners/LazyThetaStar.cpp
@@ -5,7 +5,7 @@ namespace Planners
     LazyThetaStar::LazyThetaStar(bool _use_3d):ThetaStar(_use_3d, "lazythetastar") {}
     LazyThetaStar::LazyThetaStar(bool _use_3d, std::string _name = "lazythetastar" ):ThetaStar(_use_3d, _name) {}
    
-    void LazyThetaStar::SetVertex(Planners::utils::Node *_s_aux)
+    void LazyThetaStar::SetVertex(Node *_s_aux)
     {
         line_of_sight_checks_++;
 
@@ -17,7 +17,7 @@ namespace Planners
             for (const auto &i: direction)
             {
                 Vec3i newCoordinates(_s_aux->coordinates + i);
-                Planners::utils::Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
+                Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
 
                 if (successor2 == nullptr || successor2->occuppied ) continue;
 
@@ -35,7 +35,7 @@ namespace Planners
             }
         }
     }
-    inline void LazyThetaStar::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void LazyThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
 
@@ -49,11 +49,11 @@ namespace Planners
 
     PathData LazyThetaStar::findPath(const Vec3i &_source, const Vec3i &_target)
     {
-        Planners::utils::Node *current = nullptr;
+        Node *current = nullptr;
 
         bool solved{false};
 
-        discrete_world_.getNodePtr(_source)->parent = new Planners::utils::Node(_source);
+        discrete_world_.getNodePtr(_source)->parent = new Node(_source);
         discrete_world_.setOpenValue(_source, true);
 
         utils::Clock main_timer;
@@ -90,6 +90,8 @@ namespace Planners
 #endif
 
             exploreNeighbours(current, _target, indexByWorldPosition);
+            // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT
+            // exploreNeighbours_Gradient(current, _target, indexByWorldPosition);
 
         }
         main_timer.toc();
diff --git a/src/Planners/LazyThetaStarM1.cpp b/src/Planners/LazyThetaStarM1.cpp
index a4febbd..0de7fdd 100644
--- a/src/Planners/LazyThetaStarM1.cpp
+++ b/src/Planners/LazyThetaStarM1.cpp
@@ -2,76 +2,140 @@
 
 namespace Planners
 {
-    LazyThetaStarM1::LazyThetaStarM1(bool _use_3d) : LazyThetaStar(_use_3d, "lazythetastarm1") {}
-    LazyThetaStarM1::LazyThetaStarM1(bool _use_3d, std::string _name = "lazythetastarm1") : LazyThetaStar(_use_3d, _name) {}
-
-    void LazyThetaStarM1::SetVertex(Planners::utils::Node *_s_aux)
-    {
+    LazyThetaStarM1::LazyThetaStarM1(bool _use_3d):LazyThetaStar(_use_3d, "lazythetastarm1") {}
+    LazyThetaStarM1::LazyThetaStarM1(bool _use_3d, std::string _name = "lazythetastarm1" ):LazyThetaStar(_use_3d, _name) {}
+    
+    void LazyThetaStarM1::SetVertex(Node *_s_aux)
+    {   
         line_of_sight_checks_++;
-        if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_))
+        if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ ))
         {
-            unsigned int G_max = std::numeric_limits<unsigned int>::max();
+            unsigned int G_max = std::numeric_limits<unsigned int>::max(); 
             unsigned int G_new;
 
-            for (const auto &i : direction)
+            for (const auto &i: direction)
             {
                 Vec3i newCoordinates(_s_aux->coordinates + i);
-                Planners::utils::Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
-                if (successor2 == nullptr || successor2->occuppied)
-                    continue;
+                Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
+                if (successor2 == nullptr || successor2->occuppied ) continue;
 
-                if (successor2->isInClosedList)
+                if ( successor2->isInClosedList ) 
                 {
-                    auto cost_term = static_cast<unsigned int>(cost_weight_ * successor2->cost * dist_scale_factor_reduced_);
-                    G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term;
+                    // auto cost_term = static_cast<unsigned int>(cost_weight_ * successor2->cost * dist_scale_factor_reduced_);
+                    // USE this cost_term when EDF decrease as distance increases
+                    // auto cost_term = static_cast<unsigned int>(cost_weight_ * ((static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper 
+                    // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE
+                    auto cost_term = static_cast<unsigned int>(cost_weight_ * (1/(((static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost))/2) * dist_scale_factor_reduced_)));
+                    // G_new = successor2->G +  geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; 
+                    // auto cost_term = static_cast<unsigned int>((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO  ¿Por qué se divide entre 100?
+                    G_new = successor2->G +  geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term;  //NUEVO //Estaba multiplicando por cost_term!!!!!
                     if (G_new < G_max)
                     {
+                        G_max = G_new;
                         _s_aux->parent = successor2;
-                        _s_aux->G = G_new;
-                        _s_aux->C = cost_term;
+                        _s_aux->G      = G_new;
+                        _s_aux->C      = cost_term;
                         _s_aux->gplush = _s_aux->G + _s_aux->H;
                     }
                 }
             }
         }
     }
-    inline void LazyThetaStarM1::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void LazyThetaStarM1::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
+        // std::cout << "distanceParent2: " << distanceParent2 << std::endl;
+        // ROS_INFO("Compute COST");
 
-        auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); // REVISAR _s_aux->parent o _s_aux
+        auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_);  //REVISAR _s_aux->parent o _s_aux
+        // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl;
 
-        if (distanceParent2_nodes == 0)
+        // No line of sight or distance greater than max_line_of_sight_cells
+        if ( distanceParent2_nodes == 0 ){
             distanceParent2_nodes = 1;
+        }
 
-        auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes;
-        if ((_s_aux->parent->G + distanceParent2 + cost_term) < _s2_aux->G)
-        {
-            _s2_aux->parent = _s_aux->parent;
-            _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term;
-            _s2_aux->C = cost_term;
+        // Line of sight
+        else{
+            // auto cost_term = static_cast<unsigned int>(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper 
+
+            // auto cost_term = static_cast<unsigned int>(cost_weight_ * (( static_cast<double>(_s_aux->parent->cost) + static_cast<double>(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper 
+            // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE
+            auto cost_term = static_cast<unsigned int>(cost_weight_ * (1/(((static_cast<double>(_s_aux->parent->cost) + static_cast<double>(_s2_aux->cost))/2) * dist_scale_factor_reduced_))) * distanceParent2_nodes; 
+            // auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes;
+            // auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2;
+            if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G )
+            {
+                _s2_aux->parent = _s_aux->parent;
+                // _s2_aux->G      = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) +  cost_term;
+                _s2_aux->G      = _s2_aux->parent->G + distanceParent2 +  cost_term;
+                _s2_aux->C      = cost_term;     
+                _s2_aux->gplush = _s2_aux->G + _s2_aux->H;   
+            }
         }
+
+        // ROS_INFO("Using resolution: [%lf]", _s2_aux->cost);
+
+        // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los
+        // CHANGED 30-Mar-2023
+        // auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes;
+        // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G )
+        // {
+        //     _s2_aux->parent = _s_aux->parent;
+        //     _s2_aux->G      = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) +  cost_term;
+        //     _s2_aux->C      = cost_term;       
+        // }
     }
 
-    unsigned int LazyThetaStarM1::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
+
+    unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
 
         unsigned int cost = _current->G;
+        // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost
+        // float grad_suc; // or _suc->grad
+        float dist_current_suc;
 
-        if (_dirs == 8)
-        {
-            cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
+
+        if(_dirs  == 8){
+            // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+            dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+            cost += dist_current_suc;
+            
+        }else{
+            // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
+            dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
+            cost += dist_current_suc;
         }
 
-        auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
+        // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc);
+        // grad_suc=(_suc->cost - _current->cost);
+        // std::cout << "current: " << _current->cost << std::endl;
+        // std::cout << "suc: " << _suc->cost << std::endl;
+        // std::cout << "gradiente: " << grad_suc << std::endl;
+        // auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); //+grad_suc
+
+        // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE
+        auto cost_term = static_cast<unsigned int>(cost_weight_ * (1/(((static_cast<double>(_current->cost) + static_cast<double>(_suc->cost))/2) * dist_scale_factor_reduced_))); 
         cost += cost_term;
         _suc->C = cost_term;
 
         return cost;
     }
+    // // NUEVO
+    // unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
+
+    //     unsigned int cost = _current->G;
+
+    //     auto cost_term = static_cast<unsigned int>((_suc->cost/100) * dist_scale_factor_reduced_);
+    //     if(_dirs  == 8){
+    //         cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_)*cost_term; //This is more efficient
+    //     }else{
+    //         cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_))*cost_term; //This is more efficient
+    //     }
+
+    //     // cost += cost_term;
+    //     _suc->C = cost_term;
 
-}
+    //     return cost;
+    // }
+}
\ No newline at end of file
diff --git a/src/Planners/LazyThetaStarM1Mod.cpp b/src/Planners/LazyThetaStarM1Mod.cpp
index 14197d5..2ad1c51 100644
--- a/src/Planners/LazyThetaStarM1Mod.cpp
+++ b/src/Planners/LazyThetaStarM1Mod.cpp
@@ -5,17 +5,42 @@ namespace Planners
     LazyThetaStarM1Mod::LazyThetaStarM1Mod(bool _use_3d):LazyThetaStarM1(_use_3d, "lazythetastarm1mod") {}
     LazyThetaStarM1Mod::LazyThetaStarM1Mod(bool _use_3d, std::string _name = "lazythetastarm1mod" ):LazyThetaStarM1(_use_3d, _name) {}
     
-    void LazyThetaStarM1Mod::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    void LazyThetaStarM1Mod::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
+        // JAC Prueba: Quitar para original
+        auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_);  //REVISAR _s_aux->parent o _s_aux
 
-        auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_);
-        if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G )
-        {
-            _s2_aux->parent = _s_aux->parent;
-            _s2_aux->G      = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) +  cost_term;
-            _s2_aux->C      = cost_term;       
+        // No line of sight or distance greater than max_line_of_sight_cells
+        if ( distanceParent2_nodes == 0 ){
+            distanceParent2_nodes = 1;
         }
+        // Line of sight
+        else{
+            auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_);
+            // auto cost_term = static_cast<unsigned int>((_s2_aux->cost/100) * dist_scale_factor_reduced_);  //NUEVO
+            // auto cost_term = static_cast<unsigned int>(cost_weight_ * ((_s2_aux->cost+_s_aux->parent->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // JAC: Quitar para original
+            if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G )
+            {
+                _s2_aux->parent = _s_aux->parent;
+                // _s2_aux->G      = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) +  cost_term;
+                _s2_aux->G      = _s2_aux->parent->G + distanceParent2 +  cost_term;
+                // _s2_aux->G      = _s2_aux->parent->G + distanceParent2 * cost_term; //NUEVO
+                _s2_aux->C      = cost_term;      
+                _s2_aux->gplush = _s2_aux->G + _s2_aux->H; 
+            }
+        }
+
+        // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los
+        // CHANGED 30-Mar-2023
+        // auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_);
+        // // auto cost_term = static_cast<unsigned int>(cost_weight_ * ((_s2_aux->cost+_s_aux->parent->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // JAC: Quitar para original
+        // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G )
+        // {
+        //     _s2_aux->parent = _s_aux->parent;
+        //     _s2_aux->G      = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) +  cost_term;
+        //     _s2_aux->C      = cost_term;       
+        // }
     }
     
-}
+}
\ No newline at end of file
diff --git a/src/Planners/LazyThetaStarM2.cpp b/src/Planners/LazyThetaStarM2.cpp
index d489a0e..e3f10c1 100644
--- a/src/Planners/LazyThetaStarM2.cpp
+++ b/src/Planners/LazyThetaStarM2.cpp
@@ -2,36 +2,34 @@
 
 namespace Planners
 {
-    LazyThetaStarM2::LazyThetaStarM2(bool _use_3d) : ThetaStarM2(_use_3d, "lazythetastarm2") {}
-    LazyThetaStarM2::LazyThetaStarM2(bool _use_3d, std::string _name = "lazythetastarm2") : ThetaStarM2(_use_3d, _name) {}
-
-    void LazyThetaStarM2::SetVertex(Planners::utils::Node *_s_aux)
-    {
-        if (!los_neighbour_)
-        {
-
-            unsigned int G_max = std::numeric_limits<unsigned int>::max();
+    LazyThetaStarM2::LazyThetaStarM2(bool _use_3d):ThetaStarM2(_use_3d, "lazythetastarm2") {}
+    LazyThetaStarM2::LazyThetaStarM2(bool _use_3d, std::string _name = "lazythetastarm2" ):ThetaStarM2(_use_3d, _name) {}
+    
+    void LazyThetaStarM2::SetVertex(Node *_s_aux)
+    {   
+        if( !los_neighbour_ ){
+
+            unsigned int G_max = std::numeric_limits<unsigned int>::max(); 
             unsigned int G_new;
 
-            for (const auto &i : direction)
+            for (const auto &i: direction)
             {
                 Vec3i newCoordinates(_s_aux->coordinates + i);
-                Planners::utils::Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
-                if (successor2 == nullptr || successor2->occuppied)
-                    continue;
+                Node *successor2 = discrete_world_.getNodePtr(newCoordinates);
+                if (successor2 == nullptr || successor2->occuppied ) continue;
 
-                if (successor2->isInClosedList)
+                if ( successor2->isInClosedList ) 
                 {
                     auto dist = geometry::distanceBetween2Nodes(successor2, _s_aux);
 
-                    G_new = static_cast<unsigned int>(successor2->G + dist +
-                                                      (static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost)) / 2);
+                    G_new  = static_cast<unsigned int>(  successor2-> G + dist +  
+                    ( static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost) ) / 2);
 
                     if (G_new < G_max)
                     {
                         _s_aux->parent = successor2;
-                        _s_aux->G = G_new;
-                        _s_aux->C = (static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost)) / 2;
+                        _s_aux->G      = G_new;
+                        _s_aux->C      = (static_cast<double>(_s_aux->cost) + static_cast<double>(successor2->cost)) / 2;
                         _s_aux->gplush = _s_aux->G + _s_aux->H;
                     }
                 }
@@ -40,49 +38,44 @@ namespace Planners
         los_neighbour_ = false;
     }
 
-    inline void LazyThetaStarM2::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void LazyThetaStarM2::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         line_of_sight_checks_++;
-        if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes))
-        {
-
+        if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes)) {
+            
             los_neighbour_ = true;
 
-            auto dist2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
-            auto edge2 = ComputeEdgeCost(checked_nodes, _s_aux->parent, _s2_aux);
+            auto dist2   = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
+            auto edge2   = ComputeEdgeCost(checked_nodes, _s_aux->parent, _s2_aux);
 
-            if ((_s_aux->parent->G + dist2 + edge2) < _s2_aux->G)
+            if ( ( _s_aux->parent->G + dist2 + edge2 ) < _s2_aux->G ) 
             {
                 _s2_aux->parent = _s_aux->parent;
-                _s2_aux->G = _s2_aux->parent->G + dist2 + edge2;
-                _s2_aux->C = edge2;
+                _s2_aux->G      = _s2_aux->parent->G + dist2 + edge2;
+                _s2_aux->C      = edge2;
                 _s2_aux->gplush = _s2_aux->G + _s2_aux->H;
-            }
-        }
+            }            
+        } 
         checked_nodes->clear();
     }
 
-    inline unsigned int LazyThetaStarM2::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
+    inline unsigned int LazyThetaStarM2::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
 
         unsigned int cost = 0;
 
-        if (_dirs == 8)
-        {
-            cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
+        if(_dirs == 8){
+            cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+        }else{
+            cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
         }
 
-        double cc = (_current->cost + _suc->cost) / 2;
-        auto edge_neighbour = static_cast<unsigned int>(cc * cost_weight_ * dist_scale_factor_reduced_);
-
-        cost += (_current->G + edge_neighbour);
+        double cc = ( _current->cost + _suc->cost ) / 2;
+        auto edge_neighbour = static_cast<unsigned int>( cc *  cost_weight_ * dist_scale_factor_reduced_); 
+    
+        cost += ( _current->G + edge_neighbour );
 
         _suc->C = edge_neighbour;
-
+        
         return cost;
     }
 
@@ -93,16 +86,16 @@ namespace Planners
 
         MagicalMultiSet openSet;
 
-        Planners::utils::Node *current = nullptr;
+        Node *current = nullptr;
         line_of_sight_checks_ = 0;
 
         bool solved{false};
 
-        discrete_world_.getNodePtr(_source)->parent = new Planners::utils::Node(_source);
+        discrete_world_.getNodePtr(_source)->parent = new Node(_source);
         discrete_world_.setOpenValue(_source, true);
 
-        node_by_cost &indexByCost = openSet.get<IndexByCost>();
-        node_by_position &indexByWorldPosition = openSet.get<IndexByWorldPosition>();
+        node_by_cost& indexByCost              = openSet.get<IndexByCost>();
+        node_by_position& indexByWorldPosition = openSet.get<IndexByWorldPosition>();
 
         indexByCost.insert(discrete_world_.getNodePtr(_source));
         while (!indexByCost.empty())
@@ -110,7 +103,7 @@ namespace Planners
             auto it = indexByCost.begin();
             current = *it;
             indexByCost.erase(indexByCost.begin());
-
+        
             if (current->coordinates == _target)
             {
                 solved = true;
@@ -128,9 +121,9 @@ namespace Planners
             exploreNeighbours(current, _target, indexByWorldPosition);
         }
         main_timer.toc();
-        PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(),
-                                                      solved, _source, line_of_sight_checks_);
-
+        PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), 
+                                                  solved, _source, line_of_sight_checks_);
+   
 #if defined(ROS) && defined(PUB_EXPLORED_NODES)
         explored_node_marker_.points.clear();
 #endif
diff --git a/src/Planners/LocalPlanner.cpp b/src/Planners/LocalPlanner.cpp
deleted file mode 100644
index 6dbfa99..0000000
--- a/src/Planners/LocalPlanner.cpp
+++ /dev/null
@@ -1,774 +0,0 @@
-/*
-Local Planner using the LazyThetaStar Class 
-José Antonio Cobano 
- */
-
-#include <LocalPlanner.hpp>
-
-namespace PathPlanners
-{ 
-    
-LocalPlanner::LocalPlanner(): rclcpp::Node("local_planner")
-{
-    // nh.reset(new ros::NodeHandle("~"));
-    // nh->param("mode3d", use3d, (bool)false); //JAC: It is not needed.
-
-    // JAC: Parameters from planner_ros_node. Decide these parameters. They are set in the launch file.
-    std::string algorithm_name;
-        
-    this->declare_parameter<std::string>("algorithm_name", "astar");
-    this->declare_parameter<std::string>("heuristic_name", "euclidean");
-    this->declare_parameter<float>("world_size_x", 5.0);
-    this->declare_parameter<float>("world_size_y", 5.0);
-    this->declare_parameter<float>("world_size_z", 5.0);
-    this->declare_parameter<float>("resolution", 0.05);
-    this->declare_parameter<bool>("inflate_map", true);
-    this->declare_parameter<bool>("use3d", (bool)true);  //JAC: Check if this parameter is needed.
-    this->declare_parameter<double>("cost_scaling_factor", 0.8);
-    this->declare_parameter<double>("robot_radius", 0.4);
-    this->declare_parameter<std::string>("frame_id", std::string("map"));
-    this->declare_parameter<std::string>("data_folder", std::string("planing_data.txt"));
-    this->declare_parameter<float>("max_line_of_sight_distance", (float)1000.0);
-    this->declare_parameter<float>("cost_weight", (float)0.0);
-    this->declare_parameter<bool>("overlay_markers", (bool)false);
-    this->declare_parameter<double>("inflation_size", 0.5);
-    
-    this->get_parameter("algorithm_name", algorithm_name); //JAC: The algorithm should be fixed.
-    this->get_parameter("heuristic_name", heuristic_);    
-
-    configureAlgorithm(algorithm_name, heuristic_);
-    
-    tf_list_ptr.reset(new tf::TransformListener(ros::Duration(5)));
-    // tfBuffer = tfBuffer_;
-    tfBuffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
-
-    configTopics();
-    configServices(); 
-}
-void LocalPlanner::clearMarkers()
-{
-    waypointsMarker.action = RVizMarker::DELETEALL;
-    lineMarker.action = RVizMarker::DELETEALL;
-
-    visMarkersPublisher.publish(lineMarker);
-    visMarkersPublisher.publish(waypointsMarker);
-
-    lineMarker.points.clear();
-    waypointsMarker.points.clear();
-
-    lineMarker.action = RVizMarker::ADD;
-    waypointsMarker.action = RVizMarker::ADD;
-}
-//Config standard services and action lib servers and clients
-void LocalPlanner::configServices()
-{
-    // JAC: This should be update in Aerostack2.
-    // execute_path_srv_ptr.reset(new ExecutePathServer(*nh, "/Execute_Plan", false));
-    // //JAC: This service provides the goal. Change in AS2.
-    // execute_path_srv_ptr->registerGoalCallback(boost::bind(&LocalPlanner::executePathGoalServerCB, this));
-    // execute_path_srv_ptr->registerPreemptCallback(boost::bind(&LocalPlanner::executePathPreemptCB, this));
-    // execute_path_srv_ptr->start();
-
-    // // JAC: This should be update in Aerostack2.
-    // navigation3DClient.reset(new Navigate3DClient("/Navigation3D", true));
-    // navigation3DClient->waitForServer();
-
-}
-void LocalPlanner::resetFlags()
-{
-    // mapReceived = false;
-    occGoalCnt = 0;
-    badGoal = 0;
-    last = 0;
-}
-void LocalPlanner::configMarkers(std::string ns, std::string frame)
-{
-    //Line strip marker use member points, only scale.x is used to control linea width
-    lineMarker.header.frame_id = frame;
-    lineMarker.header.stamp = ros::Time::now();
-    lineMarker.id = rand();
-    lineMarker.ns = ns;
-    lineMarker.lifetime = ros::Duration(500);
-    lineMarker.type = RVizMarker::LINE_STRIP;
-    lineMarker.action = RVizMarker::ADD;
-    lineMarker.pose.orientation.w = 1;
-    lineMarker.pose.position.z = 0.1;
-    lineMarker.color.r = 1.0;
-    lineMarker.color.g = 0.0;
-    lineMarker.color.b = 0.0;
-    lineMarker.color.a = 1.0;
-    lineMarker.scale.x = 0.1;
-
-    waypointsMarker.header.frame_id = frame;
-    waypointsMarker.header.stamp = ros::Time::now();
-    waypointsMarker.ns = ns;
-    waypointsMarker.id = lineMarker.id + 12;
-    waypointsMarker.lifetime = ros::Duration(500);
-    waypointsMarker.type = RVizMarker::POINTS;
-    waypointsMarker.action = RVizMarker::ADD;
-    waypointsMarker.pose.orientation.w = 1;
-    waypointsMarker.pose.position.z = 0.1;
-    waypointsMarker.color.r = 0.0;
-    waypointsMarker.color.g = 0.0;
-    waypointsMarker.color.b = 1.0;
-    waypointsMarker.color.a = 1.0;
-    waypointsMarker.scale.x = 0.15;
-    waypointsMarker.scale.y = 0.15;
-    waypointsMarker.scale.z = 0.4;
-}
-
-// void LocalPlanner::pointsSub(const PointCloud::ConstPtr &points)
-void LocalPlanner::pointsSub(const sensor_msgs::msg::PointCloud2::SharedPtr points)
-{
-    
-    mapReceived = true;
-    // // Callback in planner_ros_node
-    boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _points_ptr = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-    pcl::fromROSMsg(*(points.get()), *(_points_ptr.get()));
-    RCLCPP_INFO(this->get_logger(),"Loading map...");
-    Planners::utils::configureWorldFromPointCloud(_points_ptr, *algorithm_, resolution_); //ROSInterfaces.cpp
-    algorithm_->publishOccupationMarkersMap();
-    // JAC: Grid shoulb be built here: computeGrid in grid3d.hpp??? TODO
-    // JAC: configureWorldFromPointCloud and configureWorldCosts are run in configureAlgorithm in planner_ros_node 
-    // JAC: It could be something like this but it would not be needed load the octomap and compute the point cloud (computePointCloud).
-    // JAC: Directly computeGrid should be run.
-    // m_grid3d_ = std::make_unique<Grid3d>(this); 
-    // After computing the grid, the costs are added to each node with configureWorldCosts
-    Planners::utils::configureWorldCosts(*m_grid3d_, *algorithm_); //ROSInterfaces.cpp
-    RCLCPP_INFO(this->get_logger(),"Published occupation marker map");
-    cloud_ = *_points_ptr;
-    input_map_ = 2;
-    local_map_sub_.reset();    
-
-}
-// JAC: Removed when topics and/or services are confirmed about the reception of the global goal.
-// void LocalPlanner::executePathPreemptCB()
-// {
-//     // ROS_INFO_COND(debug, "Goal Preempted");
-//     RCLCPP_INFO(this->get_logger(),"Goal Preempted");
-//     execute_path_srv_ptr->setPreempted(); // set the action state to preempted
-
-//     // JAC: This should be update in Aerostack2.
-//     navigation3DClient->cancelAllGoals();
-
-//     resetFlags();
-//     clearMarkers();
-// }
-// JAC: Removed when topics and/or services are confirmed about the reception of the global goal.
-// void LocalPlanner::executePathGoalServerCB() // Note: "Action" is not appended to exe here
-// {
-//     resetFlags();
-//     clearMarkers();
-//     start_time = ros::Time::now();
-
-//     // ROS_INFO_COND(debug, "Local Planner Goal received in action server mode");
-//     RCLCPP_INFO(this->get_logger(),"Local Planner Goal received in action server mode");
-//     //upo_actions::ExecutePathGoalConstPtr path_shared_ptr;
-//     auto path_shared_ptr = execute_path_srv_ptr->acceptNewGoal();
-//     globalTrajectory = path_shared_ptr->path;
-//     auto size = globalTrajectory.points.size();
-
-//     goals_vector = globalTrajectory.points;
-//     goal3D.global_goal.pose.position.x = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].translation.x;
-//     goal3D.global_goal.pose.position.y = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].translation.y;
-//     goal3D.global_goal.pose.position.z = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].translation.z;
-        
-//     goal3D.global_goal.pose.orientation.x = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].rotation.x;
-//     goal3D.global_goal.pose.orientation.y = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].rotation.y;
-//     goal3D.global_goal.pose.orientation.z = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].rotation.z;
-//     goal3D.global_goal.pose.orientation.w = globalTrajectory.points.at(globalTrajectory.points.size() - 1).transforms[0].rotation.w;
-//     // JAC: Update --> This is done by publishing the local path.
-//     // navigation3DClient->sendGoal(goal3D);
-
-//     // ROS_INFO_COND(debug, "Local Planner: Processed goal message");
-// }
-void LocalPlanner::configTopics()
-{
-    // JAC: Subscription to the Point Cloud.
-    local_map_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>("/points", 1, std::bind(&LocalPlanner::pointsSub, this, std::placeholders::_1));
-    // JAC: This node is not published.
-    // occupancy_grid_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>("/grid", 1, std::bind(&HeuristicPlannerROS::occupancyGridCallback, this, std::placeholders::_1));
-
-    point_markers_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("path_points_markers", 1);
-    line_markers_pub_  = this->create_publisher<visualization_msgs::msg::Marker>("path_line_markers", 1);
-    // JAC: This has to be change. It has to publish to the corresponding message in AS2 (FollowPath) TODO
-    // trajPub = this->create_publisher<trajectory_msgs::MultiDOFJointTrajectory>("local_path", 1);
-}
-
-void LocalPlanner::plan()
-{
-    number_of_points = 0; //Reset variable
-
-    // // JAC: Review and modify in AS2. How do we know if execute paht is active?
-    // if (!execute_path_srv_ptr->isActive())
-    // {
-    //     clearMarkers();
-    //     return;
-    // }
-
-    // JAC: Here the solution local path is published.
-    calculatePath3D();
-    // JAC: This has to be decided to update. TODO
-    // seconds = finishT.time - startT.time - 1;
-    // milliseconds = (1000 - startT.millitm) + finishT.millitm;
-    // publishExecutePathFeedback();
-
-    // JAC: Review and modify in AS2
-    // if (*state == actionlib::SimpleClientGoalState::SUCCEEDED)
-    // {
-    //     clearMarkers();
-    //     action_result.arrived = true;
-    //     execute_path_srv_ptr->setSucceeded(action_result);
-    //     ROS_ERROR("LocalPlanner: Goal Succed");
-    //     return;
-    // }
-    // // JAC: Review and modify in AS2
-    // else if (*state == actionlib::SimpleClientGoalState::ABORTED)
-    // {
-    //     ROS_INFO_COND(debug, "Goal aborted by path tracker");
-    //     resetFlags();
-    // }
-    // // JAC: Review and modify in AS2
-    // else if (*state == actionlib::SimpleClientGoalState::PREEMPTED)
-    // {
-    //     ROS_INFO_COND(debug, "Goal preempted by path tracker");
-    //     resetFlags();
-    // }
-    //ROS_INFO_COND(debug, "Before start loop calculation");
-}
-
-void LocalPlanner::calculatePath3D()
-{
-    if (mapReceived) // It is true once a pointcloud is received.
-    {
-        ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Global trj received and pointcloud received");
-        mapReceived = false;
-
-        //TODO : Set to robot pose to the center of the workspace?
-        // if (theta3D.setValidInitialPosition(robotPose) || theta3D.searchInitialPosition3d(initialSearchAround))
-        // JAC: Decide if we will implement "Symetric search in nearest XY-Zup neighbours" with searchInitialPosition3d
-        if (!algorithm_->detectCollision(robotPose))
-        {
-            ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Calculating local goal");
-
-            if (calculateLocalGoal3D()) //TODO: VER CUAL SERIA EL LOCAL GOAL EN ESTE CASO
-            // geometry_msgs::msg::Vector3 localGoal, robotPose; --> from old localplanner
-            {
-                ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Local Goal calculated");
-
-                // ROS_INFO("%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z);
-                // JAC: geometry_msgs::msg::Vector3 localGoal
-                RCLCPP_INFO(this->get_logger(),"%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z);
-
-                // JAC: El local_goal debe ser de este tipo para pasarlo al findpath
-                // const auto discrete_goal =  Planners::utils::discretePoint(_req.goal, resolution_);
-
-                // if (!theta3D.isInside(localGoal)) --> checkValid (Planners::utils::)
-                if (!checkValid(localGoal))
-                {
-                    ROS_INFO("Returning, not inside :(");
-                    // JAC: Decide how communicate it. TODO
-
-                    // action_result.arrived=false;
-                    // execute_path_srv_ptr->setAborted(action_result, "Not inside after second check");
-                    // navigation3DClient->cancelAllGoals();
-                    return;
-                }
-
-                // if (theta3D.setValidFinalPosition(localGoal) || theta3D.searchFinalPosition3dAheadHorizontalPrior(finalSearchAround))
-                // JAC: Decide if implement "// Asymetric search in nearest XinFront-Y-Zup neighbours" with searchFinalPosition3dAheadHorizontalPrior
-                if (!algorithm_->detectCollision(localGoal))
-                {
-                    ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Computing Local Path");
-
-                    // JAC: We calculate the path with findpath
-                    // number_of_points = theta3D.computePath();
-                    // JAC: robotPose or start should be like in planner_ros_node. robotPose is the position (0,0,0)
-                    // const auto discrete_start =  Planners::utils::discretePoint(_req.start, resolution_);
-                    // JAC: discrete_start --> robotPose, discrete_goal --> localgoal
-                    auto path_data = algorithm_->findPath(discrete_start, discrete_goal);
-
-                    // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Path computed, %d points", number_of_points);
-                    occGoalCnt = 0;
-
-                    if( std::get<bool>(path_data["solved"]) ){
-                        Planners::utils::CoordinateList path;
-                        try{
-                            _rep.time_spent.data           = std::get<double>(path_data["time_spent"] );
-                            _rep.time_spent.data /= 1000;
-                            times.push_back(_rep.time_spent.data);
-
-                            if(_req.tries.data < 2 || i == ( _req.tries.data - 1) ){
-                                _rep.path_length.data          = std::get<double>(path_data["path_length"] );
-                                _rep.explored_nodes.data       = std::get<size_t>(path_data["explored_nodes"] );
-                                _rep.line_of_sight_checks.data = std::get<unsigned int>(path_data["line_of_sight_checks"] );
-
-                                _rep.total_cost1.data           = std::get<unsigned int>(path_data["total_cost1"] );
-                                _rep.total_cost2.data           = std::get<unsigned int>(path_data["total_cost2"] );
-                                _rep.h_cost.data               = std::get<unsigned int>(path_data["h_cost"]);
-                                _rep.g_cost1.data               = std::get<unsigned int>(path_data["g_cost1"]);
-                                _rep.g_cost2.data               = std::get<unsigned int>(path_data["g_cost2"]);
-                                _rep.c_cost.data               = std::get<unsigned int>(path_data["c_cost"]);
-
-                                _rep.cost_weight.data          = std::get<double>(path_data["cost_weight"]);
-                                _rep.max_los.data              = std::get<unsigned int>(path_data["max_line_of_sight_cells"]);
-                            }
-                            path = std::get<Planners::utils::CoordinateList>(path_data["path"]);
-
-                        }catch(std::bad_variant_access const& ex){
-                            std::cerr << "Bad variant error: " << ex.what() << std::endl;
-                        }
-                    }else{
-                        RCLCPP_INFO(this->get_logger(),"Could not calculate path between request points");
-                    }                    
-                }
-                else if (occGoalCnt > 2) //!Caso GOAL OCUPADO
-                {                        //If it cant find a free position near local goal, it means that there is something there.
-                    ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Pausing planning, final position busy");
-                    planningStatus.data = "Final position Busy, Cancelling goal";
-                    //TODO What to tell to the path tracker
-                    action_result.arrived=false;
-                    execute_path_srv_ptr->setAborted(action_result, "Local goal occupied");
-                    //In order to resume planning, someone must call the pause/resume planning Service that will change the flag to true
-                    occGoalCnt = 0;
-                }
-                else
-                {
-                    ++occGoalCnt;
-                }
-                // JAC: Here we have to convert the path to TrajectoryWaypoints.msg and publish
-                // JAC: Check if publisher should be changed because 
-                // trajPub.publish(localTrajectory);
-                // publishTrajMarker3D();
-            }
-            else if (badGoal < 3)
-            {
-                ROS_INFO_COND(debug, "Local Planner 3D: Bad Goal Calculated: [%.2f, %.2f]", localGoal.x, localGoal.y);
-
-                ++badGoal;
-            }
-            else
-            {
-                // JAC: How publish when local goal cannot be computed three times.
-                // action_result.arrived=false;
-                // execute_path_srv_ptr->setAborted(action_result,"Bad goal calculated 3 times");
-                badGoal = 0;
-                ROS_INFO("Bad goal calculated 3 times");
-            }
-        }
-        // JAC: No initial free position found
-        else
-        {
-            // planningStatus.data = "No initial position found...";
-            // action_result.arrived=false;
-            // execute_path_srv_ptr->setAborted(action_result,"No initial position found");
-
-            clearMarkers();
-            ROS_INFO_COND(debug, "Local Planner 3D: No initial free position found");
-        }
-    }
-}
-// JAC: Decide if this is considered from AS2.
-// void LocalPlanner::publishExecutePathFeedback()
-// {
-
-//     if (planningStatus.data.find("OK") > 0)
-//     {
-//         planningRate.data = 1000 / milliseconds;
-//     }
-//     else
-//     {
-//         planningRate.data = 0;
-//     }
-
-//     exec_path_fb.global_waypoint = waypointGoingTo;
-//     exec_path_fb.planning_rate = planningRate;
-//     exec_path_fb.status = planningStatus;
-//     execute_path_srv_ptr->publishFeedback(exec_path_fb);
-// }
-
-// Compute the local goal from the goals_vector (global path)
-// JAC: Now we can test the local planner with global goal as input from terminal.
-// JAC: localgoal should be like in planner_ros_node
-// JAC: const auto discrete_goal =  Planners::utils::discretePoint(_req.goal, resolution_);
-bool LocalPlanner::calculateLocalGoal3D()
-{
-
-    //Okey we have our std queu with the global waypoints
-    //? 1. Take front and calculate if it inside the workspace
-    //? 2. if it
-    geometry_msgs::msg::Vector3 currentGoalVec;
-    //geometry_msgs::TransformStamped robot = getTfMapToRobot();
-
-    // JAC: We have to initialize the global goal --> We have to know the type of message
-    // JAC: Once defined the type of message, we can compute the local goal.
-    // goals_vector_bl_frame = goals_vector;
-
-    geometry_msgs::msg::PoseStamped pose, poseout;
-    pose.header.frame_id = world_frame;
-    poseout.header.frame_id = robot_base_frame;
-    pose.header.stamp = ros::Time::now();
-    pose.header.seq = rand();
-
-    try
-    {
-        tf_list_ptr->waitForTransform(robot_base_frame, world_frame, ros::Time::now(), ros::Duration(1));
-    }
-    catch (tf::TransformException &ex)
-    {
-        ROS_ERROR("Transform exception : %s", ex.what());
-        return false;
-    }
-
-    //Transform the global trajectory to base link
-    ROS_INFO(PRINTF_CYAN "Calculating local goal 3D");
-    for (auto &it : goals_vector_bl_frame) // JAC: The bucle has to be updated from the type of message of the gloabl goal
-    {
-        pose.pose.position.x = it.transforms[0].translation.x;
-        pose.pose.position.y = it.transforms[0].translation.y;
-        pose.pose.position.z = it.transforms[0].translation.z;
-
-        pose.pose.orientation.x = it.transforms[0].rotation.x;
-        pose.pose.orientation.y = it.transforms[0].rotation.y;
-        pose.pose.orientation.z = it.transforms[0].rotation.z;
-        pose.pose.orientation.w = it.transforms[0].rotation.w;
-
-        double norm = pose.pose.orientation.x * pose.pose.orientation.x +
-                      pose.pose.orientation.y * pose.pose.orientation.y +
-                      pose.pose.orientation.z * pose.pose.orientation.z +
-                      pose.pose.orientation.w * pose.pose.orientation.w;
-        norm = sqrt(norm);
-        if (norm < 1e-6)
-        {
-            ROS_ERROR("Error, norm to small ");
-            return false;
-        }
-
-        pose.pose.orientation.x /= norm;
-        pose.pose.orientation.y /= norm;
-        pose.pose.orientation.z /= norm;
-        pose.pose.orientation.w /= norm;
-
-        try
-        {
-            tf_list_ptr->transformPose(robot_base_frame, pose, poseout);
-        }
-        catch (tf2::TransformException &ex)
-        {
-            ROS_ERROR("Tf error: %s", ex.what());
-            return false;
-        }
-
-        it.transforms[0].translation.x = poseout.pose.position.x;
-        it.transforms[0].translation.y = poseout.pose.position.y;
-        it.transforms[0].translation.z = poseout.pose.position.z;
-
-        it.transforms[0].rotation.x = poseout.pose.orientation.x;
-        it.transforms[0].rotation.y = poseout.pose.orientation.y;
-        it.transforms[0].rotation.z = poseout.pose.orientation.z;
-        it.transforms[0].rotation.w = poseout.pose.orientation.w;
-
-        //ROS_INFO_COND(debug, PRINTF_RED "Point Transformed: [%.2f,%.2f,%.2f]", poseout.pose.position.x, poseout.pose.position.y, poseout.pose.position.z);
-    }
-    int i = 0;
-    // JAC: In our case, do we receive only one waypoint? I mean, a global path to compute the global goal or a global goal?
-    // JAC: Update after deciding on the previous sentence.
-    if (!goals_vector_bl_frame.empty()) // JAC: It has to be updated from the type of message of the gloabl goal
-    {
-        for (auto it = goals_vector_bl_frame.begin(); it != goals_vector_bl_frame.end(); it++)
-        {
-            ++i;
-            if (i < last)
-                continue;
-
-            if (it == goals_vector_bl_frame.end() - 1)
-            {
-                currentGoal = *it;
-                currentGoalVec.x = currentGoal.transforms[0].translation.x;
-                currentGoalVec.y = currentGoal.transforms[0].translation.y;
-                currentGoalVec.z = currentGoal.transforms[0].translation.z;
-                // if (!theta3D.isInside(currentGoalVec))
-                if (!checkValid(currentGoalVec))
-                {
-                    last = 0;
-                    // JAC: This depends on the interfaces. TODO
-                    // action_result.arrived = false;
-                    // execute_path_srv_ptr->setAborted(action_result, "Preempted goal because global path does not fit into local workspace");
-                    
-                    // JAC: This is of the old localplanner
-                    /*if(goals_vector_bl_frame.size()==1)
-                        return false;
-
-                    currentGoal = *(it-1);//TODO COMPROBAR QUE NO ES UNA TRAYECTORIA DE UN UNICO PUNTO
-                    last = i-1;
-
-                }else{
-                    last=i;
-                }*/
-                }
-                //ROS_INFO_COND(debug, PRINTF_CYAN "Local Planner 3D: End of global trajectory queu");
-                break;
-            }
-            else
-            {
-
-                currentGoal = *(it + 1);
-                currentGoalVec.x = currentGoal.transforms[0].translation.x;
-                currentGoalVec.y = currentGoal.transforms[0].translation.y;
-                currentGoalVec.z = currentGoal.transforms[0].translation.z;
-                // ROS_INFO_COND(debug, "Local Planner 3D: i: Current goal bl frame: i: %d, last: %d [%.6f, %.6f, %.6f]", i, last, currentGoalVec.x, currentGoalVec.y, currentGoalVec.z);
-                //ROS_INFO_COND(debug, "Local Planner 3D: i: %d Local goal map frame: [%.2f, %.2f, %.2f]", i, currentGoal.transforms[0].translation.x, currentGoal.transforms[0].translation.y, currentGoal.transforms[0].translation.z);
-                // if (!theta3D.isInside(currentGoalVec))
-                if (!checkValid(currentGoalVec))
-                {
-                    currentGoal = *it;
-                    ROS_INFO_COND(debug, PRINTF_CYAN "Local Planner 3D: Passing Local Goal: [%.6f, %.6f, %.6f]", currentGoal.transforms[0].translation.x, currentGoal.transforms[0].translation.y, currentGoal.transforms[0].translation.z);
-                    last = i;
-                    break;
-                }
-            }
-        }
-
-        // JAC: geometry_msgs::msg::Vector3 localGoal;
-        localGoal.x = currentGoal.transforms[0].translation.x;
-        localGoal.y = currentGoal.transforms[0].translation.y;
-        localGoal.z = currentGoal.transforms[0].translation.z;
-
-        //ROS_INFO_COND(debug, "i: %d Local goal bl frame: [%.2f, %.2f, %.2f]", i, localGoal.x, localGoal.y, localGoal.z);
-        //ROS_INFO_COND(debug, "Local goalmap frame: [%.2f, %.2f, %.2f]", currentGoal.transforms[0].translation.x, currentGoal.transforms[0].translation.y, currentGoal.transforms[0].translation.z);
-
-        return true;
-    }
-    else
-    {
-        return false;
-    }
-}
-// JAC: This is used in publishTrajMarker3D. Do it later.
-// geometry_msgs::Point LocalPlanner::makePoint(const geometry_msgs::Vector3 &vec)
-// {
-
-//     geometry_msgs::Point p;
-//     p.x = vec.x;
-//     p.y = vec.y;
-//     p.z = vec.z;
-
-//     return p;
-// }
-
-// JAC: Used in calculateLocalGoal3D
-geometry_msgs::TransformStamped LocalPlanner::getTfMapToRobot()
-{
-    geometry_msgs::TransformStamped ret;
-
-    try
-    {
-        ret = tfBuffer->lookupTransform(world_frame, robot_base_frame, ros::Time(0));
-    }
-    catch (tf2::TransformException &ex)
-    {
-        ROS_WARN("%s", ex.what());
-    }
-
-    return ret;
-}
-
-// JAC: This shoud be done to show the computed local path, but after deciding in calculatePath3D.
-// void LocalPlanner::publishTrajMarker3D() //? DONE 3D
-// {
-//     //!This is done to clear out the previous markers
-//     waypointsMarker.action = RVizMarker::DELETEALL;
-//     lineMarker.action = RVizMarker::DELETEALL;
-
-//     visMarkersPublisher.publish(lineMarker);
-//     visMarkersPublisher.publish(waypointsMarker);
-
-//     lineMarker.points.clear();
-//     waypointsMarker.points.clear();
-
-//     lineMarker.action = RVizMarker::ADD;
-//     waypointsMarker.action = RVizMarker::ADD;
-
-//     lineMarker.header.stamp = ros::Time::now();
-//     waypointsMarker.header.stamp = ros::Time::now();
-
-//     geometry_msgs::Transform robot_pos; // = getTfMapToRobot().transform;
-//     robot_pos.translation.x = 0;
-//     robot_pos.translation.y = 0;
-//     robot_pos.translation.z = 0;
-
-//     if (traj_dest_frame != robot_base_frame)
-//     {
-//         try
-//         {
-//             geometry_msgs::PoseStamped pose;
-//             pose.header.frame_id = robot_base_frame;
-//             pose.pose.orientation.w = 1;
-//             tf_list_ptr->waitForTransform(traj_dest_frame, robot_base_frame, ros::Time::now(), ros::Duration(1));
-//             tf_list_ptr->transformPose(traj_dest_frame, pose, pose);
-//             robot_pos.translation.x = pose.pose.position.x;
-//             robot_pos.translation.y = pose.pose.position.y;
-//             robot_pos.translation.z = pose.pose.position.z;
-//         }
-//         catch (tf::TransformException &ex)
-//         {
-//             ROS_ERROR("Couldn't transform points %s", ex.what());
-//         }
-//     }
-
-//     lineMarker.points.push_back(makePoint(robot_pos.translation));
-//     waypointsMarker.points.push_back(makePoint(robot_pos.translation));
-
-//     for (auto it : localTrajectory.points)
-//     {
-//         lineMarker.points.push_back(makePoint(it.transforms[0].translation));
-//         waypointsMarker.points.push_back(makePoint(it.transforms[0].translation));
-//     }
-
-//     visMarkersPublisher.publish(lineMarker);
-//     visMarkersPublisher.publish(waypointsMarker);
-// }
-
-void LocalPlanner::configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic){
-
-        float ws_x, ws_y, ws_z;
-
-        this->get_parameter("world_size_x", ws_x);
-        this->get_parameter("world_size_y", ws_y);
-        this->get_parameter("world_size_z", ws_z);
-        this->get_parameter("resolution", resolution_);
-        this->get_parameter("inflate_map", inflate_);
-        this->get_parameter("use3d", use3d_);
-
-        world_size_.x = std::floor(ws_x / resolution_);
-        world_size_.y = std::floor(ws_y / resolution_);
-        world_size_.z = std::floor(ws_z / resolution_);
-
-        if( algorithm_name == "astar" ){
-            RCLCPP_INFO(this->get_logger(),"Using A*");
-            algorithm_.reset(new Planners::AStar(use3d_));
-        }else if( algorithm_name == "costastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware A*");
-            algorithm_.reset(new Planners::AStarM1(use3d_));
-        }else if( algorithm_name == "astarsafetycost" ){
-            RCLCPP_INFO(this->get_logger(),"Using A* Safety Cost");
-            algorithm_.reset(new Planners::AStarM2(use3d_));    
-        }else if ( algorithm_name == "thetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Theta*");
-            algorithm_.reset(new Planners::ThetaStar(use3d_));
-        }else if ( algorithm_name == "costhetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware Theta* ");
-            algorithm_.reset(new Planners::ThetaStarM1(use3d_));
-        }else if ( algorithm_name == "thetastarsafetycost" ){
-            RCLCPP_INFO(this->get_logger(),"Using Theta* Safety Cost");
-            algorithm_.reset(new Planners::ThetaStarM2(use3d_));
-        }else if( algorithm_name == "lazythetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using LazyTheta*");
-            algorithm_.reset(new Planners::LazyThetaStar(use3d_));
-        }else if( algorithm_name == "costlazythetastar"){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware LazyTheta*");
-            algorithm_.reset(new Planners::LazyThetaStarM1(use3d_));
-        }else if( algorithm_name == "costlazythetastarmodified"){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware LazyTheta*");
-            algorithm_.reset(new Planners::LazyThetaStarM1Mod(use3d_));
-        }else if( algorithm_name == "lazythetastarsafetycost"){
-            RCLCPP_INFO(this->get_logger(),"Using LazyTheta* Safety Cost");
-            algorithm_.reset(new Planners::LazyThetaStarM2(use3d_));
-        }else{
-            RCLCPP_WARN(this->get_logger(),"Wrong algorithm name parameter. Using ASTAR by default");
-            algorithm_.reset(new Planners::AStar(use3d_));
-        }
-
-        algorithm_->setWorldSize(world_size_, resolution_);
-
-        configureHeuristic(_heuristic);
-
-        RCLCPP_INFO(this->get_logger(),"Using discrete world size: [%d, %d, %d]", world_size_.x, world_size_.y, world_size_.z);
-        RCLCPP_INFO(this->get_logger(),"Using resolution: [%f]", resolution_);
-
-        if(inflate_){
-            double inflation_size;
-            this->get_parameter("inflation_size", inflation_size);
-            inflation_steps_ = std::round(inflation_size / resolution_);
-            RCLCPP_INFO(this->get_logger(),"Inflation size %.2f, using inflation step %d", inflation_size, inflation_steps_);
-        }
-        algorithm_->setInflationConfig(inflate_, inflation_steps_);
-
-
-        double cost_scaling_factor, robot_radius;
-
-        this->get_parameter("cost_scaling_factor", cost_scaling_factor);
-        this->get_parameter("robot_radius", robot_radius);
-        
-        m_grid3d_->setCostParams(cost_scaling_factor, robot_radius);
-        
-        std::string frame_id;
-        // lnh_.param("frame_id", frame_id, std::string("map"));		
-        this->get_parameter("frame_id", frame_id);
-
-        configMarkers(algorithm_name, frame_id, resolution_);
-
-        // JAC: This was in planne_ros_node but it should be done when the point cloud is received () because input_map_=2 as we use point cloud. 
-        //0: no map yet
-        //1: using occupancy
-        //2: using cloud
-        // if( input_map_ == 1 ){
-        //     Planners::utils::configureWorldFromOccupancyWithCosts(occupancy_grid_, *algorithm_);
-        // }else if( input_map_ == 2 ){
-        //     Planners::utils::configureWorldFromPointCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(cloud_), *algorithm_, resolution_);
-        //     Planners::utils::configureWorldCosts(*m_grid3d_, *algorithm_);
-        // }
-
-        //Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution)
-        float sight_dist, cost_weight;
-                                                                             //
-        this->get_parameter("max_line_of_sight_distance", sight_dist);
-        this->get_parameter("cost_weight", cost_weight);
-
-        algorithm_->setMaxLineOfSight(sight_dist);
-        algorithm_->setCostFactor(cost_weight);
-
-        this->get_parameter("overlay_markers", overlay_markers_);
-
-    }
-    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr local_map_sub_;
-    // rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_sub_;
-
-    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr line_markers_pub_, point_markers_pub_;
-    // JAC: Add trajPub
-    // rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr trajPub;
-
-    // JAC: Decide if topics or service will be used.
-    // rclcpp::Service<heuristic_planners::srv::GetPath>::SharedPtr request_path_server_;
-    // rclcpp::Service<heuristic_planners::srv::SetAlgorithm>::SharedPtr change_planner_server_;
-
-    std::unique_ptr<Grid3d> m_grid3d_;
-
-    std::unique_ptr<Planners::AlgorithmBase> algorithm_;
-        
-    visualization_msgs::msg::Marker path_line_markers_, path_points_markers_;
-    
-    //Parameters
-    Planners::utils::Vec3i world_size_; // Discrete
-    //float resolution_ = 0.2;
-    float resolution_;
-
-    bool use3d_{true};
-
-    bool inflate_{false};
-    unsigned int inflation_steps_{0};
-    std::string data_folder_;
-    bool overlay_markers_{0};
-    unsigned int color_id_{0};
-    // nav_msgs::msg::OccupancyGrid occupancy_grid_;
-    // TODO?: Change type of message --> sensor_msgs::msg::PointCloud2
-    pcl::PointCloud<pcl::PointXYZ> cloud_;
-    //sensor_msgs::msg::PointCloud2 cloud_; //JAC
-
-    //0: no map yet
-    //1: using occupancy
-    //2: using cloud
-    int input_map_{0};
-    std::string heuristic_;    
-
-} // namespace PathPlanners
diff --git a/src/Planners/ThetaStar.cpp b/src/Planners/ThetaStar.cpp
index eb6bfc6..ceb32f1 100644
--- a/src/Planners/ThetaStar.cpp
+++ b/src/Planners/ThetaStar.cpp
@@ -12,7 +12,7 @@ namespace Planners
         checked_nodes_current->reserve(5000);
     }
     
-    inline void ThetaStar::UpdateVertex(Planners::utils::Node *_s, Planners::utils::Node *_s2, node_by_position &_index_by_pos)
+    inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos)
     {
         unsigned int g_old = _s2->G;
 
@@ -30,12 +30,13 @@ namespace Planners
         }
     }
 
-    inline void ThetaStar::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
-        auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
+        // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
         line_of_sight_checks_++;
         if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) )
         {
+            auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient
             if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G )
             {
                 _s2_aux->parent = _s_aux->parent;
@@ -55,12 +56,12 @@ namespace Planners
         }
     }
 
-    void ThetaStar::exploreNeighbours(Planners::utils::Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){
+    void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){
 
         for (unsigned int i = 0; i < direction.size(); ++i) {
 
             Vec3i newCoordinates = _current->coordinates + direction[i];
-            Planners::utils::Node *successor = discrete_world_.getNodePtr(newCoordinates);
+            Node *successor = discrete_world_.getNodePtr(newCoordinates);
 
             if ( successor == nullptr || 
                  successor->isInClosedList || 
@@ -80,4 +81,1665 @@ namespace Planners
             UpdateVertex(_current, successor, _index_by_pos); 
         }
     }
+
+    void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){
+
+        // ROS_INFO("GRADIENT");
+        // Modules
+        // float mod[26];
+        // mod[0]=1;
+        // mod[1]=1;
+        // mod[2]=1;
+        // mod[3]=1;
+        // mod[4]=1;
+        // mod[5]=1;
+        // mod[6]=1.41;
+        // mod[7]=1.41;
+        // mod[8]=1.41;
+        // mod[9]=1.41;
+        // mod[10]=1.41;
+        // mod[11]=1.41;
+        // mod[12]=1.41;
+        // mod[13]=1.41;
+        // mod[14]=1.41;
+        // mod[15]=1.41;
+        // mod[16]=1.41;
+        // mod[17]=1.41;
+        // mod[18]=1.73;
+        // mod[19]=1.73;
+        // mod[20]=1.73;
+        // mod[21]=1.73;
+        // mod[22]=1.73;
+        // mod[23]=1.73;
+        // mod[24]=1.73;
+        // mod[25]=1.73;
+
+        angles_h[0]=1.5708;
+        angles_v[0]=0;
+        angles_h[1]=-1.5708;
+        angles_v[1]=0;
+        angles_h[2]=0;
+        angles_v[2]=0;
+        angles_h[3]=3.14159;
+        angles_v[3]=0;
+        angles_h[4]=0;
+        angles_v[4]=1.5708;
+        angles_h[5]=0;
+        angles_v[5]=-1.5708;
+        angles_h[6]=-0.785398;
+        angles_v[6]=0;
+        angles_h[7]=2.35619;
+        angles_v[7]=0;
+        angles_h[8]=-2.35619;
+        angles_v[8]=0;
+        angles_h[9]=0.785398;
+        angles_v[9]=0;
+        angles_h[10]=3.14159;
+        angles_v[10]=-0.788391;
+        angles_h[11]=0;
+        angles_v[11]=0.788391;
+        angles_h[12]=0;
+        angles_v[12]=-0.788391;
+        angles_h[13]=3.14159;
+        angles_v[13]=0.788391;
+        angles_h[14]=-1.5708;
+        angles_v[14]=0.788391;
+        angles_h[15]=1.5708;
+        angles_v[15]=0.788391;
+        angles_h[16]=1.5708;
+        angles_v[16]=-0.788391;
+        angles_h[17]=-1.5708;
+        angles_v[17]=-0.788391;
+        angles_h[18]=-2.35619;
+        angles_v[18]=0.616318;
+        angles_h[19]=0.785398;
+        angles_v[19]=0.616318;
+        angles_h[20]=2.35619;
+        angles_v[20]=0.616318;
+        angles_h[21]=-0.785398;
+        angles_v[21]=0.616318;
+        angles_h[22]=-2.35619;
+        angles_v[22]=-0.616318;
+        angles_h[23]=0.785398;
+        angles_v[23]=-0.616318;
+        angles_h[24]=2.35619;
+        angles_v[24]=-0.616318;
+        angles_h[25]=-0.785398;
+        angles_v[25]=-0.616318;
+        
+        // Variables to compute the gradient of each node and the total one
+        float gradient[26];
+        Vec3i gradient_vector[26];
+        Vec3f gradient_unit_vector[26];
+        
+        // Variables to compute unit vector of the goal from each successor        
+        Vec3i attraction_current[1];
+        Vec3f attraction_unit_current[1];
+        float module_current;
+        
+        // Compute unit vector of the goal from the current
+        Vec3i current_h = _current->coordinates;
+        Node *current_grad = discrete_world_.getNodePtr(current_h);
+        attraction_current[0]=getVectorPull(current_grad->coordinates, _target);
+        module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2));
+        attraction_unit_current[0].x = attraction_current[0].x/module_current;
+        attraction_unit_current[0].y = attraction_current[0].y/module_current;
+        attraction_unit_current[0].z = attraction_current[0].z/module_current;
+        // std::cout << "current X: " << attraction_unit_current[0].x << std::endl;
+        // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl;
+        // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl;
+
+        // Computation of each gradient (26 neighbours)       
+        for (unsigned int i = 0; i < direction.size(); ++i) {
+
+            Vec3i newCoordinates_grad = _current->coordinates + direction[i];
+            Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad);
+            
+            float module_grad;
+
+            if ( successor_grad == nullptr || 
+                 successor_grad->occuppied ) {
+                    continue;
+                 }
+        
+            gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2
+            gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates);
+            module_grad=0;
+            module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2));
+            gradient_unit_vector[i].x = gradient_vector[i].x/module_grad;
+            gradient_unit_vector[i].y = gradient_vector[i].y/module_grad;
+            gradient_unit_vector[i].z = gradient_vector[i].z/module_grad;
+            // std::cout << "current: " << _current->cost << std::endl;
+            // std::cout << "suc: " << successor_grad->cost << std::endl;
+            // std::cout << "Cell: " << i << std::endl;
+            // std::cout << "gradient: " << gradient[i] << std::endl;
+            
+        }
+
+        // The minimum when EDF increases as distance increases
+        float max_gradient=0;
+        int index_max_gradient=1;
+        // int veces=0;
+        for (unsigned int i = 0; i < direction.size(); ++i) {
+            // std::cout << "gradient: " << gradient[i] << std::endl;
+            if (gradient[i] < max_gradient){
+                max_gradient=gradient[i];
+                index_max_gradient=i; // i starts from 0 (in my notes from 1)
+            }        
+            // if (gradient[i] == max_gradient) {
+            //     veces=veces+1;
+            // }
+            
+        }
+        // std::cout << "veces: " << veces << std::endl;
+        // std::cout << "index max gradient: " << index_max_gradient << std::endl;
+        // std::cout << "max gradient: " << max_gradient << std::endl;       
+
+        // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl;
+        // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl;
+        // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl;
+
+        // Compute vector to choose the node from the gradient and the goal (attraction)
+        Vec3f vectorNeighbours[1];
+        float weight_gradient=1;
+        
+        vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x;
+        vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y;
+        vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z;
+
+        // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl;
+        // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl;    
+        // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl;    
+
+        // // // IF WE WANT TO CONSIDER THE ANGLES BETWEEN GOAL AND GRADIENT VECTOR TO CHOOSE THE NUMBER OF NEIGHBOURS
+        // // Angle of the gradient DESCOMENTAR
+        // float angle_gradient_h=0; // horizontal angle X-Y
+        // angle_gradient_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x);
+
+        // float angle_gradient_v=0; // vertical angle  
+        // float ang_gradient=0;
+        // float module_gradient=0;
+        // module_gradient = sqrt(pow(gradient_unit_vector[index_max_gradient].x, 2) + pow(gradient_unit_vector[index_max_gradient].y, 2) + pow(gradient_unit_vector[index_max_gradient].z, 2));
+        // ang_gradient=gradient_unit_vector[index_max_gradient].z/module_gradient;
+        // angle_gradient_v=asin(ang_gradient);
+        
+        // std::cout << "angle gradient h: " << angle_gradient_h << std::endl;
+
+        // // Angle of the attraction DESCOMENTAR
+        // float angle_attrac_h=0; // horizontal angle X-Y
+        // angle_attrac_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x);
+
+        // float angle_attrac_v=0; // vertical angle  
+        // float ang_attrac=0;
+        // float module_attrac=0;
+        // module_attrac = sqrt(pow(attraction_unit_current[0].x, 2) + pow(attraction_unit_current[0].y, 2) + pow(attraction_unit_current[0].z, 2));
+        // ang_attrac=attraction_unit_current[0].z/module_attrac;
+        // angle_attrac_v=asin(ang_attrac);
+        
+        // std::cout << "angle attrac h: " << angle_attrac_h << std::endl;
+
+        // Choose the neighbour nodes to explore
+        float module_vectorNeighbours=0;
+        module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2));
+        // module_vectorNeighbours = sqrt(pow(vectorNeighbours_cost[0].x, 2) + pow(vectorNeighbours_cost[0].y, 2) + pow(vectorNeighbours_cost[0].z, 2));
+        // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl;
+
+        // Angles of the vector vectorNeighbours
+        float angle_h=0; // horizontal angle X-Y
+        float angle_v=0; // vertical angle  
+        float ang=0;
+
+        // WITHOUT CONSIDERING THE DIFFERENCE BETWEEN THE ANGLE OF THE GRADIENT AND THE ANGLE OF THE GOAL.
+        angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x);
+        ang=vectorNeighbours[0].z/module_vectorNeighbours;
+        angle_v=asin(ang);
+
+        // /********************************************************************************************************/
+        // // CONSIDERING THE DIFFERENCE BETWEEN THE ANGLE OF THE GRADIENT AND THE ANGLE OF THE GOAL.
+        // // Angle of the gradient DESCOMENTAR
+        // float angle_gradient_h=0; // horizontal angle X-Y
+        // angle_gradient_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x);
+        // // std::cout << "angle gradient h: " << angle_gradient_h << std::endl;
+
+        // // Angle of the attraction DESCOMENTAR
+        // float angle_attrac_h=0; // horizontal angle X-Y
+        // angle_attrac_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x);
+        // // std::cout << "angle attrac h: " << angle_attrac_h << std::endl;
+
+        // unsigned int num_nodes;
+        // Vec3i node_choosenCoordinates[13];  //11
+        // int index_nodes[13]; //11
+
+        // if (angle_gradient_h != 0){
+        //     if (abs(angle_attrac_h - angle_gradient_h) > (M_PI/2)){  //1.3962634: 80
+        //     // It works with 9-10 and considering 75 degrees.
+        //     // if (abs(angle_attrac_h - angle_gradient_h) > (1.3089)){  //1.308996939: 75
+        //         num_nodes=13; //11
+        //         // Vec3i node_choosenCoordinates[11]; 
+        //         // int index_nodes[11];
+        //         // std::cout << "111: " << std::endl;
+        //     }
+        //     else {
+        //         num_nodes=11; //9
+        //         // Vec3i node_choosenCoordinates[9]; 
+        //         // int index_nodes[9];
+        //         // std::cout << "ang: " << ang << std::endl;
+        //         // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x);
+        //         // std::cout << "222: " << std::endl;
+        //     }
+        // }
+        // // If angle_gradient_h == 0
+        // else{
+        //     num_nodes=13; //11
+        //     // Vec3i node_choosenCoordinates[11]; 
+        //     // int index_nodes[11];
+        //     // std::cout << "333: " << std::endl;
+        // }
+        // /********************************************************************************************************/
+
+        // // If we want to compute the angle_h depending on the difference between angle of gradient and the angle of goal
+        // /*******/
+        // std::cout << "angle_attrac_h: " << angle_attrac_h << std::endl;
+        // std::cout << "angle_attrac_v: " << angle_attrac_v << std::endl;
+        // std::cout << "gradient_x: " << gradient_unit_vector[index_max_gradient].x << std::endl;
+        // std::cout << "gradient_y: " << gradient_unit_vector[index_max_gradient].y << std::endl;
+        // std::cout << "gradient_z: " << gradient_unit_vector[index_max_gradient].z << std::endl;
+        // std::cout << "angle_gradient_h: " << angle_gradient_h << std::endl;
+        // std::cout << "angle_gradient_v: " << angle_gradient_v << std::endl;
+        // if (angle_gradient_h != 0){
+        //     if (abs(angle_attrac_h - angle_gradient_h) > (M_PI/2)){
+        //         if (angle_attrac_h <= (angle_gradient_h + M_PI)){
+        //             angle_h = angle_gradient_h + (M_PI/2);
+        //             if (angle_h > M_PI){
+        //                 angle_h = angle_h - (2*M_PI);
+        //             }
+        //             ang=vectorNeighbours[0].z/module_vectorNeighbours;
+        //             angle_v=asin(ang);
+        //         }
+        //         else {
+        //             angle_h = angle_gradient_h - (M_PI/2);
+        //             if (angle_h < (-M_PI)){
+        //                 angle_h = (2*M_PI) + angle_h;
+        //             }
+        //             ang=vectorNeighbours[0].z/module_vectorNeighbours;
+        //             angle_v=asin(ang);
+        //         }
+        //     }
+        //     else {
+        //         angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x);
+        //         ang=vectorNeighbours[0].z/module_vectorNeighbours;
+        //         angle_v=asin(ang);
+        //         // std::cout << "ang: " << ang << std::endl;
+        //         // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x);
+        //     }
+        // }
+        // // If angle_gradient_h == 0
+        // else{
+        //     angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x);
+        //     ang=vectorNeighbours[0].z/module_vectorNeighbours;
+        //     angle_v=asin(ang);
+        // }
+        // /*******/
+
+        // std::cout << "angle h: " << angle_h << std::endl;         
+        // std::cout << "angle v: " << angle_v << std::endl; 
+
+        // /////////////////////////////////////////////////
+        // TO CHANGE TO EXPLORE 9,11, 13, 15 or 17 NODES
+        Vec3i node_choosenCoordinates[17];
+        unsigned int num_nodes=17;
+        int index_nodes[17];
+        // /////////////////////////////////////////////////
+
+        // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours
+        int node_choosen;
+        CoordinateList _node_choosenCoordinates;
+        node_choosen=chooseNeighbours(angle_h, angle_v);
+        // std::cout << "node: " << node_choosen << std::endl;
+
+        // Vec3i nodes_to_explore[9];
+        // TODO Function to calculate the nodes to explore 
+        // nodesToExplore(node_choosen, _node_choosenCoordinates);  // FUNCTION TO USE
+        // for (unsigned int i = 0; i < 9; ++i) {
+        //     std::cout << "Node X" << _node_choosenCoordinates[i].x << std::endl;
+        //     std::cout << "Node Y" << _node_choosenCoordinates[i].y << std::endl;
+        //     std::cout << "Node Z" << _node_choosenCoordinates[i].z << std::endl;
+        // }
+
+        if (node_choosen == 0){
+            //0,7,9,20,15,19,24,16,23
+            index_nodes[0]=0;
+            index_nodes[1]=7;
+            index_nodes[2]=9;
+            index_nodes[3]=20;
+            index_nodes[4]=15;
+            index_nodes[5]=19;
+            index_nodes[6]=24;
+            index_nodes[7]=16;
+            index_nodes[8]=23;
+            if (num_nodes == 10){
+                index_nodes[9]=1;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=11;
+                index_nodes[12]=13;
+                index_nodes[13]=10;
+                index_nodes[14]=12;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=13;
+                index_nodes[10]=4;
+                index_nodes[11]=11;
+                index_nodes[12]=3;
+                index_nodes[13]=2;
+                index_nodes[14]=10;
+                index_nodes[15]=5;
+                index_nodes[16]=12;
+            }
+        }
+        else if (node_choosen == 1){
+            // 1,6,8,14,18,21,17,22,25
+            index_nodes[0]=1;
+            index_nodes[1]=6;
+            index_nodes[2]=8;
+            index_nodes[3]=14;
+            index_nodes[4]=18;
+            index_nodes[5]=21;
+            index_nodes[6]=17;
+            index_nodes[7]=22;
+            index_nodes[8]=25;
+            if (num_nodes == 10){
+                index_nodes[9]=0;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=12;
+                index_nodes[12]=13;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=12;
+                index_nodes[12]=13;
+                index_nodes[13]=10;
+                index_nodes[14]=11;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=13;
+                index_nodes[10]=4;
+                index_nodes[11]=11;
+                index_nodes[12]=3;
+                index_nodes[13]=2;
+                index_nodes[14]=10;
+                index_nodes[15]=5;
+                index_nodes[16]=12;
+            }
+        }
+        else if (node_choosen == 2){
+            // 2,6,9,11,19,21,12,23,25
+            index_nodes[0]=2;
+            index_nodes[1]=6;
+            index_nodes[2]=9;
+            index_nodes[3]=11;
+            index_nodes[4]=19;
+            index_nodes[5]=21;
+            index_nodes[6]=12;
+            index_nodes[7]=23;
+            index_nodes[8]=25;
+            if (num_nodes == 10){
+                index_nodes[9]=3;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+                index_nodes[13]=16;
+                index_nodes[14]=14;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=15;
+                index_nodes[10]=4;
+                index_nodes[11]=14;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=16;
+                index_nodes[15]=5;
+                index_nodes[16]=17;
+            }
+        }
+        else if (node_choosen == 3){
+            // 3,7,9,13,18,20,10,22,24
+            index_nodes[0]=3;
+            index_nodes[1]=7;
+            index_nodes[2]=8;
+            index_nodes[3]=13;
+            index_nodes[4]=18;
+            index_nodes[5]=20;
+            index_nodes[6]=10;
+            index_nodes[7]=22;
+            index_nodes[8]=24;
+            if (num_nodes == 10){
+                index_nodes[9]=2;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+                index_nodes[13]=17;
+                index_nodes[14]=15;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=15;
+                index_nodes[10]=4;
+                index_nodes[11]=14;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=16;
+                index_nodes[15]=5;
+                index_nodes[16]=17;
+            }
+        }
+        else if (node_choosen == 4){
+            // 4,11,13,14,15,18,19,20,21
+            index_nodes[0]=4;
+            index_nodes[1]=11;
+            index_nodes[2]=13;
+            index_nodes[3]=14;
+            index_nodes[4]=15;
+            index_nodes[5]=18;
+            index_nodes[6]=19;
+            index_nodes[7]=20;
+            index_nodes[8]=21;
+            if (num_nodes == 10){
+                index_nodes[9]=5;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if (num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=2;
+                index_nodes[12]=3;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=2;
+                index_nodes[12]=3;
+                index_nodes[13]=6;
+                index_nodes[14]=7;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=7;
+                index_nodes[10]=0;
+                index_nodes[11]=9;
+                index_nodes[12]=3;
+                index_nodes[13]=2;
+                index_nodes[14]=8;
+                index_nodes[15]=1;
+                index_nodes[16]=6;
+            }
+        }
+        else if (node_choosen == 5){
+            // 5,10,12,16,17,22,23,24,25
+            index_nodes[0]=5;
+            index_nodes[1]=10;
+            index_nodes[2]=12;
+            index_nodes[3]=16;
+            index_nodes[4]=17;
+            index_nodes[5]=22;
+            index_nodes[6]=23;
+            index_nodes[7]=24;
+            index_nodes[8]=25;
+            if (num_nodes == 10){
+                index_nodes[9]=4;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if (num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=2;
+                index_nodes[12]=3;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=2;
+                index_nodes[12]=3;
+                index_nodes[13]=8;
+                index_nodes[14]=9;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=7;
+                index_nodes[10]=0;
+                index_nodes[11]=9;
+                index_nodes[12]=3;
+                index_nodes[13]=2;
+                index_nodes[14]=8;
+                index_nodes[15]=1;
+                index_nodes[16]=6;
+            }
+        }
+        else if (node_choosen == 6){
+            // 6,1,2,11,14,21,12,17,25
+            index_nodes[0]=6;
+            index_nodes[1]=1;
+            index_nodes[2]=2;
+            index_nodes[3]=11;
+            index_nodes[4]=14;
+            index_nodes[5]=21;
+            index_nodes[6]=12;
+            index_nodes[7]=17;
+            index_nodes[8]=25;
+            if (num_nodes == 10){
+                index_nodes[9]=7;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=4;
+                index_nodes[10]=5;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+                index_nodes[13]=18;
+                index_nodes[14]=23;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=9;
+                index_nodes[10]=8;
+                index_nodes[11]=18;
+                index_nodes[12]=19;
+                index_nodes[13]=22;
+                index_nodes[14]=23;
+                index_nodes[15]=4;
+                index_nodes[16]=5;
+            }
+        }
+        else if (node_choosen == 7){
+            // 7,0,3,13,20,15,10,24,17
+            index_nodes[0]=7;
+            index_nodes[1]=0;
+            index_nodes[2]=3;
+            index_nodes[3]=13;
+            index_nodes[4]=20;
+            index_nodes[5]=15;
+            index_nodes[6]=10;
+            index_nodes[7]=24;
+            index_nodes[8]=16;
+            if (num_nodes == 10){
+                index_nodes[9]=6;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=23;
+                index_nodes[12]=18;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=23;
+                index_nodes[12]=18;
+                index_nodes[13]=19;
+                index_nodes[14]=22;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=18;
+                index_nodes[12]=19;
+                index_nodes[13]=22;
+                index_nodes[14]=23;
+                index_nodes[15]=4;
+                index_nodes[16]=5;
+            }
+        }
+        else if (node_choosen == 8){
+            // 8,1,3,13,18,14,10,22,17
+            index_nodes[0]=8;
+            index_nodes[1]=1;
+            index_nodes[2]=3;
+            index_nodes[3]=13;
+            index_nodes[4]=18;
+            index_nodes[5]=14;
+            index_nodes[6]=10;
+            index_nodes[7]=22;
+            index_nodes[8]=17;
+            if (num_nodes == 10){
+                index_nodes[9]=9;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+                index_nodes[13]=20;
+                index_nodes[14]=25;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=20;
+                index_nodes[12]=21;
+                index_nodes[13]=24;
+                index_nodes[14]=25;
+                index_nodes[15]=4;
+                index_nodes[16]=5;
+            }
+        }
+        else if (node_choosen == 9){
+            // 9,0,2,15,19,11,16,23,12
+            index_nodes[0]=9;
+            index_nodes[1]=0;
+            index_nodes[2]=2;
+            index_nodes[3]=15;
+            index_nodes[4]=19;
+            index_nodes[5]=11;
+            index_nodes[6]=16;
+            index_nodes[7]=23;
+            index_nodes[8]=12;
+            if (num_nodes == 10){
+                index_nodes[9]=8;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=20;
+                index_nodes[12]=25;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=20;
+                index_nodes[12]=25;
+                index_nodes[13]=24;
+                index_nodes[14]=21;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=20;
+                index_nodes[12]=21;
+                index_nodes[13]=24;
+                index_nodes[14]=25;
+                index_nodes[15]=4;
+                index_nodes[16]=5;
+            }
+        }
+        else if (node_choosen == 10){
+            // 10,24,22,7,3,8,16,5,17
+            index_nodes[0]=10;
+            index_nodes[1]=24;
+            index_nodes[2]=22;
+            index_nodes[3]=7;
+            index_nodes[4]=3;
+            index_nodes[5]=8;
+            index_nodes[6]=16;
+            index_nodes[7]=5;
+            index_nodes[8]=17;
+            if (num_nodes == 10){
+                index_nodes[9]=11;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=18;
+                index_nodes[12]=23;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=18;
+                index_nodes[12]=23;
+                index_nodes[13]=20;
+                index_nodes[14]=25;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=23;
+                index_nodes[10]=12;
+                index_nodes[11]=25;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=20;
+                index_nodes[15]=13;
+                index_nodes[16]=18;
+            }
+        }
+        else if (node_choosen == 11){
+            // 11,19,21,9,2,6,15,4,14
+            index_nodes[0]=11;
+            index_nodes[1]=19;
+            index_nodes[2]=21;
+            index_nodes[3]=9;
+            index_nodes[4]=2;
+            index_nodes[5]=6;
+            index_nodes[6]=15;
+            index_nodes[7]=4;
+            index_nodes[8]=14;
+            if (num_nodes == 10){
+                index_nodes[9]=10;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=25;
+                index_nodes[12]=20;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=25;
+                index_nodes[12]=20;
+                index_nodes[13]=18;
+                index_nodes[14]=23;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=20;
+                index_nodes[10]=13;
+                index_nodes[11]=18;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=23;
+                index_nodes[15]=12;
+                index_nodes[16]=25;
+            }
+        }
+        else if (node_choosen == 12){
+            // 12,23,25,9,2,6,16,5,17
+            index_nodes[0]=12;
+            index_nodes[1]=23;
+            index_nodes[2]=25;
+            index_nodes[3]=9;
+            index_nodes[4]=2;
+            index_nodes[5]=6;
+            index_nodes[6]=16;
+            index_nodes[7]=5;
+            index_nodes[8]=17;
+            if (num_nodes == 10){
+                index_nodes[9]=13;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+                index_nodes[13]=24;
+                index_nodes[14]=21;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=24;
+                index_nodes[10]=10;
+                index_nodes[11]=22;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=19;
+                index_nodes[15]=11;
+                index_nodes[16]=21;
+            }
+        }
+        else if (node_choosen == 13){
+            // 13,20,18,7,3,8,15,4,14
+            index_nodes[0]=13;
+            index_nodes[1]=20;
+            index_nodes[2]=18;
+            index_nodes[3]=7;
+            index_nodes[4]=3;
+            index_nodes[5]=8;
+            index_nodes[6]=15;
+            index_nodes[7]=4;
+            index_nodes[8]=14;
+            if (num_nodes == 10){
+                index_nodes[9]=12;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=0;
+                index_nodes[10]=1;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+                index_nodes[13]=24;
+                index_nodes[14]=21;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=19;
+                index_nodes[10]=11;
+                index_nodes[11]=21;
+                index_nodes[12]=0;
+                index_nodes[13]=1;
+                index_nodes[14]=24;
+                index_nodes[15]=10;
+                index_nodes[16]=22;
+            }
+        }
+        else if (node_choosen == 14){
+            // 14, 4, 11, 13, 18, 21, 1, 6, 8
+            index_nodes[0]=14;
+            index_nodes[1]=4;
+            index_nodes[2]=11;
+            index_nodes[3]=13;
+            index_nodes[4]=18;
+            index_nodes[5]=21;
+            index_nodes[6]=1;
+            index_nodes[7]=6;
+            index_nodes[8]=8;
+            if (num_nodes == 10){
+                index_nodes[9]=16;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+                index_nodes[13]=25;
+                index_nodes[14]=20;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=20;
+                index_nodes[10]=15;
+                index_nodes[11]=19;
+                index_nodes[12]=2;
+                index_nodes[13]=3;
+                index_nodes[14]=22;
+                index_nodes[15]=17;
+                index_nodes[16]=25;
+            }
+        }
+        else if (node_choosen == 15){
+            // 15,19,20,7,0,9,13,4,11
+            index_nodes[0]=15;
+            index_nodes[1]=19;
+            index_nodes[2]=20;
+            index_nodes[3]=7;
+            index_nodes[4]=0;
+            index_nodes[5]=9;
+            index_nodes[6]=13;
+            index_nodes[7]=4;
+            index_nodes[8]=11;
+            if (num_nodes == 10){
+                index_nodes[9]=17;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+                index_nodes[13]=23;
+                index_nodes[14]=18;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=18;
+                index_nodes[10]=14;
+                index_nodes[11]=21;
+                index_nodes[12]=2;
+                index_nodes[13]=3;
+                index_nodes[14]=24;
+                index_nodes[15]=16;
+                index_nodes[16]=23;
+            }
+        }
+        else if (node_choosen == 16){
+            // 16,24,23,7,0,9,10,5,12
+            index_nodes[0]=16;
+            index_nodes[1]=24;
+            index_nodes[2]=23;
+            index_nodes[3]=7;
+            index_nodes[4]=0;
+            index_nodes[5]=9;
+            index_nodes[6]=10;
+            index_nodes[7]=5;
+            index_nodes[8]=12;
+            if (num_nodes == 10){
+                index_nodes[9]=14;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=22;
+                index_nodes[12]=19;
+                index_nodes[13]=25;
+                index_nodes[14]=20;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=22;
+                index_nodes[10]=17;
+                index_nodes[11]=25;
+                index_nodes[12]=2;
+                index_nodes[13]=3;
+                index_nodes[14]=20;
+                index_nodes[15]=15;
+                index_nodes[16]=19;
+            }
+        }
+        else if (node_choosen == 17){
+            // 17,22,25,8,1,6,10,5,12
+            index_nodes[0]=17;
+            index_nodes[1]=22;
+            index_nodes[2]=25;
+            index_nodes[3]=8;
+            index_nodes[4]=1;
+            index_nodes[5]=6;
+            index_nodes[6]=10;
+            index_nodes[7]=5;
+            index_nodes[8]=12;
+            if (num_nodes == 10){
+                index_nodes[9]=15;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=2;
+                index_nodes[10]=3;
+                index_nodes[11]=24;
+                index_nodes[12]=21;
+                index_nodes[13]=23;
+                index_nodes[14]=18;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=24;
+                index_nodes[10]=16;
+                index_nodes[11]=23;
+                index_nodes[12]=2;
+                index_nodes[13]=3;
+                index_nodes[14]=18;
+                index_nodes[15]=14;
+                index_nodes[16]=21;
+            }
+        }
+        else if (node_choosen == 18){
+            // 18,13,15,3,8,1,20,4,21
+            index_nodes[0]=18;
+            index_nodes[1]=13;
+            index_nodes[2]=14;
+            index_nodes[3]=3;
+            index_nodes[4]=8;
+            index_nodes[5]=1;
+            index_nodes[6]=20;
+            index_nodes[7]=4;
+            index_nodes[8]=21;
+            if (num_nodes == 10){
+                index_nodes[9]=23;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+                index_nodes[13]=17;
+                index_nodes[14]=15;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=15;
+                index_nodes[10]=11;
+                index_nodes[11]=19;
+                index_nodes[12]=6;
+                index_nodes[13]=7;
+                index_nodes[14]=10;
+                index_nodes[15]=22;
+                index_nodes[16]=17;
+            }
+        }
+        else if (node_choosen == 19){
+            // 19,15,11,20,4,21,0,9,2
+            index_nodes[0]=19;
+            index_nodes[1]=15;
+            index_nodes[2]=11;
+            index_nodes[3]=20;
+            index_nodes[4]=4;
+            index_nodes[5]=21;
+            index_nodes[6]=0;
+            index_nodes[7]=9;
+            index_nodes[8]=2;
+            if (num_nodes == 10){
+                index_nodes[9]=22;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+                index_nodes[13]=12;
+                index_nodes[14]=13;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=13;
+                index_nodes[10]=14;
+                index_nodes[11]=18;
+                index_nodes[12]=6;
+                index_nodes[13]=7;
+                index_nodes[14]=12;
+                index_nodes[15]=16;
+                index_nodes[16]=23;
+            }
+        }
+        else if (node_choosen == 20){
+            // 20,13,15,18,4,19,7,0,3
+            index_nodes[0]=20;
+            index_nodes[1]=13;
+            index_nodes[2]=15;
+            index_nodes[3]=18;
+            index_nodes[4]=4;
+            index_nodes[5]=19;
+            index_nodes[6]=7;
+            index_nodes[7]=0;
+            index_nodes[8]=3;
+            if (num_nodes == 10){
+                index_nodes[9]=25;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+                index_nodes[13]=16;
+                index_nodes[14]=14;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=11;
+                index_nodes[10]=14;
+                index_nodes[11]=21;
+                index_nodes[12]=8;
+                index_nodes[13]=9;
+                index_nodes[14]=10;
+                index_nodes[15]=24;
+                index_nodes[16]=16;
+            }
+        }
+        else if (node_choosen == 21){
+            // 21,11,14,18,4,19,1,6,2
+            index_nodes[0]=21;
+            index_nodes[1]=11;
+            index_nodes[2]=14;
+            index_nodes[3]=18;
+            index_nodes[4]=4;
+            index_nodes[5]=19;
+            index_nodes[6]=1;
+            index_nodes[7]=6;
+            index_nodes[8]=2;
+            if (num_nodes == 10){
+                index_nodes[9]=24;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+                index_nodes[13]=12;
+                index_nodes[14]=13;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=13;
+                index_nodes[10]=15;
+                index_nodes[11]=20;
+                index_nodes[12]=8;
+                index_nodes[13]=9;
+                index_nodes[14]=12;
+                index_nodes[15]=17;
+                index_nodes[16]=25;
+            }
+        }
+        else if (node_choosen == 22){
+            // 22,10,17,24,5,25,3,8,1
+            index_nodes[0]=22;
+            index_nodes[1]=10;
+            index_nodes[2]=17;
+            index_nodes[3]=24;
+            index_nodes[4]=5;
+            index_nodes[5]=25;
+            index_nodes[6]=3;
+            index_nodes[7]=8;
+            index_nodes[8]=1;
+            if (num_nodes == 10){
+                index_nodes[9]=19;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=16;
+                index_nodes[12]=14;
+                index_nodes[13]=12;
+                index_nodes[14]=13;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=16;
+                index_nodes[10]=12;
+                index_nodes[11]=23;
+                index_nodes[12]=6;
+                index_nodes[13]=7;
+                index_nodes[14]=13;
+                index_nodes[15]=14;
+                index_nodes[16]=18;
+            }
+        }
+        else if (node_choosen == 23){
+            // 23,16,12,24,5,25,0,9,2
+            index_nodes[0]=23;
+            index_nodes[1]=16;
+            index_nodes[2]=12;
+            index_nodes[3]=24;
+            index_nodes[4]=5;
+            index_nodes[5]=25;
+            index_nodes[6]=0;
+            index_nodes[7]=9;
+            index_nodes[8]=2;
+            if (num_nodes == 10){
+                index_nodes[9]=18;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=6;
+                index_nodes[10]=7;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+                index_nodes[13]=17;
+                index_nodes[14]=15;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=10;
+                index_nodes[10]=17;
+                index_nodes[11]=22;
+                index_nodes[12]=6;
+                index_nodes[13]=7;
+                index_nodes[14]=11;
+                index_nodes[15]=19;
+                index_nodes[16]=15;
+            }
+        }
+        else if (node_choosen == 24){
+            // 24,16,10,22,5,23,3,8,1
+            index_nodes[0]=24;
+            index_nodes[1]=16;
+            index_nodes[2]=10;
+            index_nodes[3]=22;
+            index_nodes[4]=5;
+            index_nodes[5]=23;
+            index_nodes[6]=3;
+            index_nodes[7]=7;
+            index_nodes[8]=0;
+            if (num_nodes == 10){
+                index_nodes[9]=21;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=17;
+                index_nodes[12]=15;
+                index_nodes[13]=12;
+                index_nodes[14]=13;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=12;
+                index_nodes[10]=17;
+                index_nodes[11]=25;
+                index_nodes[12]=8;
+                index_nodes[13]=9;
+                index_nodes[14]=13;
+                index_nodes[15]=15;
+                index_nodes[16]=20;
+            }
+        }
+        else if (node_choosen == 25){
+            // 25,17,12,22,5,23,1,6,2
+            index_nodes[0]=25;
+            index_nodes[1]=17;
+            index_nodes[2]=12;
+            index_nodes[3]=22;
+            index_nodes[4]=5;
+            index_nodes[5]=23;
+            index_nodes[6]=1;
+            index_nodes[7]=6;
+            index_nodes[8]=2;
+            if (num_nodes == 10){
+                index_nodes[9]=20;
+            }
+            else if (num_nodes==11){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+            }
+            else if(num_nodes==13){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+            }
+            else if(num_nodes==15){
+                index_nodes[9]=8;
+                index_nodes[10]=9;
+                index_nodes[11]=10;
+                index_nodes[12]=11;
+                index_nodes[13]=16;
+                index_nodes[14]=14;
+            }
+            else if (num_nodes==17){
+                index_nodes[9]=10;
+                index_nodes[10]=16;
+                index_nodes[11]=24;
+                index_nodes[12]=8;
+                index_nodes[13]=9;
+                index_nodes[14]=11;
+                index_nodes[15]=14;
+                index_nodes[16]=21;
+            }
+        }
+
+        for (unsigned int i = 0; i < num_nodes; ++i) {
+            node_choosenCoordinates[i]=direction[index_nodes[i]];
+            // std::cout << "coord_X: " << node_choosenCoordinates[i].x << std::endl;
+            // std::cout << "coord_Y: " << node_choosenCoordinates[i].y << std::endl;
+            // std::cout << "coord_Z: " << node_choosenCoordinates[i].z << std::endl;
+        }
+
+        // direction.size() should be substituted by the number of neighbours to explore
+        // num_nodes is the size of node_choosenCoordinates
+        for (unsigned int i = 0; i < num_nodes; ++i) { // when we consider the gradient to choose explored nodes
+
+            Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i];            
+            Node *successor = discrete_world_.getNodePtr(newCoordinates);
+
+            if ( successor == nullptr || 
+                 successor->isInClosedList || 
+                 successor->occuppied ) 
+                continue;
+    
+            if (! successor->isInOpenList ) { 
+
+                successor->parent = _current;
+                successor->G = computeG(_current, successor, index_nodes[i], direction.size());
+                successor->H = heuristic(successor->coordinates, _target);
+                successor->gplush = successor->G + successor->H;
+                successor->isInOpenList = true;
+                _index_by_pos.insert(successor);
+            }
+         
+            UpdateVertex(_current, successor, _index_by_pos); 
+        }
+    }
+
+    int ThetaStar::chooseNeighbours(float angh, float angv){
+    // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){
+        //choose_node.m (folder Gradients)
+        
+        int node=0;
+        // // Modules
+        // float mod[26];
+        // float aux;
+        // for (unsigned int i = 0; i < direction.size(); ++i)
+        // {
+        //     aux=0;
+        //     mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
+        //     aux=mod[i]/100;
+        //     mod[i]=aux;
+        //     //std::cout << "mod: " << mod[i] << std::endl;
+        // }
+        // std::cout << "mod: " << mod[0] << std::endl;
+        
+        // Angles
+        // float angles_h[26], angles_v[26];
+        float diff_h[26],diff_v[26];
+        float ang_aux; //, ang_aux_v;
+
+        for (unsigned int i = 0; i < direction.size(); ++i) {
+
+            // Vec3i newCoordinates = direction[i];
+            // angles_h[i]=atan2(newCoordinates.y,newCoordinates.x);
+            // angles_v[i]=asin((newCoordinates.z/mod[i]));
+            // std::cout << "angles_h: " << i << angles_h[i] << std::endl;
+            // std::cout << "angles_v: " << i << angles_v[i] << std::endl;
+            // dif_h
+            if (angh < 0){
+                if (angles_h[i] < 0){
+                    diff_h[i]=abs(angh-angles_h[i]); 
+                }
+                else if (angles_h[i] == 0){
+                    diff_h[i]=abs(angh);
+                }
+                else if (angles_h[i] == M_PI){
+                    diff_h[i]=abs(M_PI-angh);
+                }
+                else {
+                    ang_aux=2*M_PI+angh;
+                    diff_h[i]=abs(ang_aux-angles_h[i]);
+                }
+            }
+            else {
+                if (angles_h[i] < 0){
+                    ang_aux=2*M_PI+angles_h[i];
+                    diff_h[i]=abs(angh-ang_aux);
+                }
+                else {
+                    diff_h[i]=abs(angh-angles_h[i]); 
+                }
+                
+            }
+            // //dif_v
+            diff_v[i]=angv-angles_v[i];
+
+            // std::cout << "coord_X: " << newCoordinates.x << std::endl;
+            // std::cout << "coord_Y: " << newCoordinates.y << std::endl;
+            // std::cout << "indice: " << i << std::endl;
+            // std::cout << "angles_h: " << angles_h[i] << std::endl;
+            // std::cout << "angles_v: " << angles_v[i] << std::endl;
+            // std::cout << "diff_h: " << diff_h[i] << std::endl;
+            // std::cout << "diff_v: " << diff_v[i] << std::endl;
+        }        
+        // std::cout << "diff_v: " << diff_v[0] << std::endl;
+        
+
+        // Compute the minimum angles_h
+        float min_diff_h=M_PI;
+        int index_min_angle_h=0;
+        for (unsigned int i = 0; i < direction.size(); ++i) {
+            if (diff_h[i] < min_diff_h){
+                min_diff_h=diff_h[i];
+                index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero.
+            }
+        }
+        // std::cout << "min_diff_h: " << min_diff_h << std::endl;
+        // std::cout << "indicie elegido: " << index_min_angle_h << std::endl;
+
+        if( index_min_angle_h == 0 ){
+            // std::cout << "node: " << node << std::endl;
+            if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){
+                node=0; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){
+                node=15;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){
+                node=16;
+            }
+        }else if( index_min_angle_h == 1 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){
+                node=1; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){
+                node=14;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){
+                node=17;
+            }
+        }else if( index_min_angle_h == 2 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){
+                node=2; // the same as index_min_angle_h
+            }
+            else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){
+                node=11;
+            }
+            else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){
+                node=12;
+            }
+            else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){
+                node=4;
+            }
+            else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){
+                node=5;
+            }
+        }else if( index_min_angle_h == 3 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){
+                node=3; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){
+                node=13;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){
+                node=10;
+            } 
+        }else if( index_min_angle_h == 6 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){
+                node=6; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){
+                node=21;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){
+                node=25;
+            }
+        }else if( index_min_angle_h == 7 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){
+                node=7; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){
+                node=20;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){
+                node=24;
+            }
+        }else if( index_min_angle_h == 8 ){
+            if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){
+                node=8; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){
+                node=18;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){
+                node=22;
+            }
+            // std::cout << "node: " << node << std::endl; 
+        }else if( index_min_angle_h == 9 ){
+            // std::cout << "node: " << node << std::endl; 
+            if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){
+                node=9; // the same as index_min_angle_h
+            }
+            else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){
+                node=19;
+            }
+            else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){
+                node=23;
+            }
+        }
+        // std::cout << "node: " << node << std::endl;
+        return (node);
+        // nodeCoordinates=direction[node];
+        // return(nodeCoordinates);
+    }
+
 }
+
diff --git a/src/Planners/ThetaStarM1.cpp b/src/Planners/ThetaStarM1.cpp
index 0b44cb7..7e2f4d1 100644
--- a/src/Planners/ThetaStarM1.cpp
+++ b/src/Planners/ThetaStarM1.cpp
@@ -2,10 +2,10 @@
 
 namespace Planners
 {
-    ThetaStarM1::ThetaStarM1(bool _use_3d) : ThetaStar(_use_3d, "thetastarm2") {}
-    ThetaStarM1::ThetaStarM1(bool _use_3d, std::string _name = "thetastarm1") : ThetaStar(_use_3d, _name) {}
+    ThetaStarM1::ThetaStarM1(bool _use_3d):ThetaStar(_use_3d, "thetastarm2") {}
+    ThetaStarM1::ThetaStarM1(bool _use_3d, std::string _name = "thetastarm1" ):ThetaStar(_use_3d, _name) {}
 
-    inline void ThetaStarM1::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void ThetaStarM1::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux);
 
@@ -13,53 +13,47 @@ namespace Planners
         if (LineOfSight::bresenham3D((_s_aux->parent), _s2_aux, discrete_world_, checked_nodes))
         {
             auto n_checked_nodes = checked_nodes->size();
-            if (n_checked_nodes == 0)
-                n_checked_nodes = 1;
+            if (n_checked_nodes==0)
+                    n_checked_nodes = 1;
 
-            auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * n_checked_nodes;
-            if ((_s_aux->parent->G + distanceParent2 + cost_term) < _s2_aux->G) // Conmensurable
+            auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_)* n_checked_nodes;
+            if ( (_s_aux->parent->G + distanceParent2 + cost_term) < _s2_aux->G )  // Conmensurable
             {
                 _s2_aux->parent = _s_aux->parent;
-                _s2_aux->G = _s_aux->parent->G + distanceParent2 + cost_term;
-                _s2_aux->C = cost_term;
-                _s2_aux->gplush = _s2_aux->H + _s2_aux->G;
+                _s2_aux->G      = _s_aux->parent->G + distanceParent2 + cost_term;
+                _s2_aux->C      = cost_term;
+                _s2_aux->gplush = _s2_aux->H + _s2_aux->G; 
             }
-        }
-        else
-        {
-            auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux);
 
+        } else {
+            auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux);
+            
             auto cost_term = static_cast<unsigned int>(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_);
             unsigned int G_new = _s_aux->G + distance2 + cost_term;
 
-            if (G_new < _s2_aux->G)
-            {
+            if ( G_new < _s2_aux->G){
                 _s2_aux->parent = _s_aux;
-                _s2_aux->G = _s_aux->G + distance2 + cost_term;
-                _s2_aux->C = cost_term;
-                _s2_aux->gplush = _s2_aux->H + _s2_aux->G;
+                _s2_aux->G      = _s_aux->G + distance2 + cost_term;
+                _s2_aux->C      = cost_term; 
+                _s2_aux->gplush = _s2_aux->H + _s2_aux->G; 
             }
         }
         checked_nodes->clear();
     }
-    inline unsigned int ThetaStarM1::computeG(const Planners::utils::Node *_current, Planners::utils::Node *_suc, unsigned int _n_i, unsigned int _dirs)
-    {
-
+    inline unsigned int ThetaStarM1::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
+        
         unsigned int cost = _current->G;
 
-        if (_dirs == 8)
-        {
-            cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); // This is more efficient
-        }
-        else
-        {
-            cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); // This is more efficient
+        if(_dirs  == 8){
+            cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient
+        }else{
+            cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient
         }
 
         auto cost_term = static_cast<unsigned int>(cost_weight_ * _suc->cost * dist_scale_factor_reduced_);
-        cost += cost_term;
+        cost   += cost_term;
         _suc->C = cost_term;
-
+        
         return cost;
     }
 }
diff --git a/src/Planners/ThetaStarM2.cpp b/src/Planners/ThetaStarM2.cpp
index 4c38c9d..50c6731 100644
--- a/src/Planners/ThetaStarM2.cpp
+++ b/src/Planners/ThetaStarM2.cpp
@@ -6,7 +6,7 @@ namespace Planners
     
     ThetaStarM2::ThetaStarM2(bool _use_3d, std::string _name = "thetastarm2" ):ThetaStar(_use_3d, _name) {}
     
-    inline void ThetaStarM2::ComputeCost(Planners::utils::Node *_s_aux, Planners::utils::Node *_s2_aux)
+    inline void ThetaStarM2::ComputeCost(Node *_s_aux, Node *_s2_aux)
     {
         line_of_sight_checks_++;
         if (LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_, checked_nodes))  
@@ -51,7 +51,7 @@ namespace Planners
         checked_nodes_current->clear();
     }
 
-    inline unsigned int ThetaStarM2::ComputeEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Planners::utils::Node* _s, const Planners::utils::Node* _s2){ 
+    inline unsigned int ThetaStarM2::ComputeEdgeCost(const utils::CoordinateListPtr _checked_nodes, const Node* _s, const Node* _s2){ 
         
         double dist_cost{0};
         double mean_dist_cost{0};
@@ -73,7 +73,7 @@ namespace Planners
         return static_cast<unsigned int>( mean_dist_cost * cost_weight_ * dist_scale_factor_reduced_);
     }
 
-    inline unsigned int ThetaStarM2::computeG(const Planners::utils::Node* _current, Planners::utils::Node* _suc,  unsigned int _n_i, unsigned int _dirs){
+    inline unsigned int ThetaStarM2::computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs){
         
         unsigned int cost = 0;
 
diff --git a/src/ROS/local_planner_node.cpp b/src/ROS/local_planner_node.cpp
deleted file mode 100644
index 366f843..0000000
--- a/src/ROS/local_planner_node.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include <rclcpp/rclcpp.hpp>
-#include <LocalPlanner.hpp>
-
-using namespace PathPlanners;
-
-
-int main(int argc, char **argv)
-{
-    rclcpp::init(argc, argv);
-
-    auto node = std::make_shared<LocalPlanner>();
-
-    rclcpp::spin(node);
-    rclcpp::shutdown();
-    return 0;
-
-/*
-//     tf2_ros::Buffer tfBuffer;
-//     tf2_ros::TransformListener tfListener(tfBuffer);
-    std::unique_ptr<tf2_ros::Buffer> tfBuffer_;
-    std::shared_ptr<tf2_ros::TransformListener> tfListener_{nullptr};
-    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
-
-    LocalPlanner lcPlanner(&tfBuffer);
-    
-    dynamic_reconfigure::Server<theta_star_2d::LocalPlannerConfig> server;
-  	dynamic_reconfigure::Server<theta_star_2d::LocalPlannerConfig>::CallbackType f;
-
-    // JAC: This will be executed in configureAlgorithm when LocalPlanner lcPlanner(&tfBuffer) is run, so it would not be needed.
-  	f = boost::bind(&LocalPlanner::dynRecCb,&lcPlanner,  _1, _2); 
-  	server.setCallback(f);
-
-	ros::Rate loop_rate(30);
-    while(ros::ok()){
-        
-        ros::spinOnce();
-        
-        // JAC: This is equivalent to requestPathService in planner_ros_node
-        // JAC: Define lcPlanner.requestPathService or use it from planner_ros_node
-        lcPlanner.plan();
-        
-        loop_rate.sleep();
-    } */
-    return 0;
-}
-
-
-
-
-//////////////////////////////
-//     auto node = std::make_shared<LocalPlanner>();
-
-//     // node->preset_loop_frequency(30);
-
-//     rclcpp::spin(node);
-//     // rclcpp::shutdown();
-
-//     return 0;
-// }
-
diff --git a/src/ROS/planner_ros_node.cpp b/src/ROS/planner_ros_node.cpp
index 822687e..0111214 100644
--- a/src/ROS/planner_ros_node.cpp
+++ b/src/ROS/planner_ros_node.cpp
@@ -3,10 +3,14 @@
 #include "Planners/AStar.hpp"
 #include "Planners/AStarM2.hpp"
 #include "Planners/AStarM1.hpp"
+#include "Planners/AStar_Gradient.hpp"
+#include "Planners/AStar_EDF.hpp"
 #include "Planners/ThetaStar.hpp"
 #include "Planners/ThetaStarM1.hpp"
 #include "Planners/ThetaStarM2.hpp"
 #include "Planners/LazyThetaStar.hpp"
+#include "Planners/LazyThetaStar_Gradient.hpp"
+#include "Planners/LazyThetaStar_EDF.hpp"
 #include "Planners/LazyThetaStarM1.hpp"
 #include "Planners/LazyThetaStarM1Mod.hpp"
 #include "Planners/LazyThetaStarM2.hpp"
@@ -18,120 +22,78 @@
 
 #include "Grid3D/grid3d.hpp"
 
-#include "rclcpp/rclcpp.hpp"
+#include <ros/ros.h>
 
-#include <pcl/conversions.h>
-#include <pcl/impl/point_types.hpp>
-#include <pcl/point_cloud.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <rclcpp/logging.hpp>
-#include <visualization_msgs/msg/detail/marker__struct.hpp>
-#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/Marker.h>
 
-// #include <pcl_ros/point_cloud.h>
-#include <pcl_ros/transforms.hpp>
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
 
-#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/OccupancyGrid.h>
 
-#include <heuristic_planners/srv/get_path.hpp>
-#include <heuristic_planners/srv/set_algorithm.hpp>
+#include <heuristic_planners/GetPath.h>
+#include <heuristic_planners/SetAlgorithm.h>
 
 /**
  * @brief Demo Class that demonstrate how to use the algorithms classes and utils 
  * with ROS 
  * 
  */
-class HeuristicPlannerROS : public rclcpp::Node {
+class HeuristicPlannerROS
+{
 
 public:
     HeuristicPlannerROS()
-      :rclcpp::Node("heuristic_planner_ros")
     {
 
         std::string algorithm_name;
+        lnh_.param("algorithm", algorithm_name, (std::string)"astar");
+        lnh_.param("heuristic", heuristic_, (std::string)"euclidean");
         
-        this->declare_parameter<std::string>("algorithm_name", "astar");
-        this->declare_parameter<std::string>("heuristic_name", "euclidean");
-
-        this->declare_parameter<float>("world_size_x", 100.0);
-        this->declare_parameter<float>("world_size_y", 100.0);
-        this->declare_parameter<float>("world_size_z", 0.0);
-        this->declare_parameter<float>("resolution", 0.2);
-        this->declare_parameter<bool>("inflate_map", true);
-        this->declare_parameter<bool>("use3d", (bool)true);
-        this->declare_parameter<double>("cost_scaling_factor", 0.8);
-        this->declare_parameter<double>("robot_radius", 0.4);
-        this->declare_parameter<std::string>("frame_id", std::string("map"));
-        this->declare_parameter<bool>("save_data_file", (bool)true);
-        this->declare_parameter<std::string>("data_folder", std::string("planing_data.txt"));
-        this->declare_parameter<float>("max_line_of_sight_distance", (float)1000.0);
-        this->declare_parameter<float>("cost_weight", (float)0.0);
-        this->declare_parameter<bool>("overlay_markers", (bool)false);
-        this->declare_parameter<double>("inflation_size", 0.5);
-        
-        this->get_parameter("algorithm_name", algorithm_name);
-        this->get_parameter("heuristic_name", heuristic_);
-
-        m_grid3d_ = std::make_unique<Grid3d>(this); //TODO Costs not implement yet 
         configureAlgorithm(algorithm_name, heuristic_);
 
-        point_markers_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("path_points_markers", 1);
-        line_markers_pub_  = this->create_publisher<visualization_msgs::msg::Marker>("path_line_markers", 1);
+        pointcloud_sub_     = lnh_.subscribe<pcl::PointCloud<pcl::PointXYZ>>("/points", 1, &HeuristicPlannerROS::pointCloudCallback, this);
+        occupancy_grid_sub_ = lnh_.subscribe<nav_msgs::OccupancyGrid>("/grid", 1, &HeuristicPlannerROS::occupancyGridCallback, this);
 
-        request_path_server_ = this->create_service<heuristic_planners::srv::GetPath>("request_path",std::bind(
-                                                                      &HeuristicPlannerROS::requestPathService, this,
-                                                                      std::placeholders::_1, // Corresponds to the 'request'  input
-                                                                      std::placeholders::_2  // Corresponds to the 'response' input
-                                                                      ));
+        request_path_server_   = lnh_.advertiseService("request_path",  &HeuristicPlannerROS::requestPathService, this);
+        change_planner_server_ = lnh_.advertiseService("set_algorithm", &HeuristicPlannerROS::setAlgorithm, this);
 
-        change_planner_server_ = this->create_service<heuristic_planners::srv::SetAlgorithm>("set_algorithm",std::bind(
-                                                                      &HeuristicPlannerROS::setAlgorithm, this,
-                                                                      std::placeholders::_1, // Corresponds to the 'request'  input
-                                                                      std::placeholders::_2  // Corresponds to the 'response' input
-                                                                      ));
+        line_markers_pub_  = lnh_.advertise<visualization_msgs::Marker>("path_line_markers", 1);
+        point_markers_pub_ = lnh_.advertise<visualization_msgs::Marker>("path_points_markers", 1);
 
-
-        pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>("/points", 1, std::bind(&HeuristicPlannerROS::pointCloudCallback, this, std::placeholders::_1));
-        occupancy_grid_sub_ = this->create_subscription<nav_msgs::msg::OccupancyGrid>("/grid", 1, std::bind(&HeuristicPlannerROS::occupancyGridCallback, this, std::placeholders::_1));
     }
 
 private:
 
-    void occupancyGridCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr _grid){
-        RCLCPP_INFO(this->get_logger(),"Loading OccupancyGrid map...");
+    void occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr &_grid){
+        ROS_INFO("Loading OccupancyGrid map...");
         Planners::utils::configureWorldFromOccupancyWithCosts(*_grid, *algorithm_);
         algorithm_->publishOccupationMarkersMap();
-        occupancy_grid_sub_.reset();
-        RCLCPP_INFO(this->get_logger(),"Occupancy Grid Loaded");
+        occupancy_grid_sub_.shutdown();
+        ROS_INFO("Occupancy Grid Loaded");
         occupancy_grid_ = *_grid;
         input_map_ = 1;
     }
 
-    // void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &_points)
-    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr points)
+    void pointCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &_points)
     {
-        boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> _points_ptr = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
-        pcl::fromROSMsg(*(points.get()), *(_points_ptr.get()));
 
-        RCLCPP_INFO(this->get_logger(),"Loading map...");
-        Planners::utils::configureWorldFromPointCloud(_points_ptr, *algorithm_, resolution_);
+        ROS_INFO("Loading map...");
+        Planners::utils::configureWorldFromPointCloud(_points, *algorithm_, resolution_);
         algorithm_->publishOccupationMarkersMap();
         Planners::utils::configureWorldCosts(*m_grid3d_, *algorithm_);
-        RCLCPP_INFO(this->get_logger(),"Published occupation marker map");
-        cloud_ = *_points_ptr;
+        ROS_INFO("Published occupation marker map");
+        cloud_ = *_points;
         input_map_ = 2;
-        pointcloud_sub_.reset();
+        pointcloud_sub_.shutdown();
     }   
-    bool setAlgorithm(const std::shared_ptr<heuristic_planners::srv::SetAlgorithm::Request> _req, 
-        std::shared_ptr<heuristic_planners::srv::SetAlgorithm::Response> rep){
+    bool setAlgorithm(heuristic_planners::SetAlgorithmRequest &_req, heuristic_planners::SetAlgorithmResponse &rep){
         
-        configureAlgorithm(_req->algorithm.data, _req->heuristic.data);
-        rep->result.data = true;
+        configureAlgorithm(_req.algorithm.data, _req.heuristic.data);
+        rep.result.data = true;
         return true;
     }
-    bool requestPathService(const std::shared_ptr<heuristic_planners::srv::GetPath::Request> req, std::shared_ptr<heuristic_planners::srv::GetPath::Response> rep){
-      auto _req = *(req.get());
-      auto _rep = *(rep.get());
+    bool requestPathService(heuristic_planners::GetPathRequest &_req, heuristic_planners::GetPathResponse &_rep){
 
         if( !_req.algorithm.data.empty() ){
             if( !_req.heuristic.data.empty() ){
@@ -143,7 +105,7 @@ class HeuristicPlannerROS : public rclcpp::Node {
             configureHeuristic(_req.heuristic.data);
         }
 
-        RCLCPP_INFO(this->get_logger(),"Path requested, computing path");
+        ROS_INFO("Path requested, computing path");
         //delete previous markers
         publishMarker(path_line_markers_, line_markers_pub_);
         publishMarker(path_points_markers_, point_markers_pub_);
@@ -235,9 +197,9 @@ class HeuristicPlannerROS : public rclcpp::Node {
                     if(saver.savePathDataToFile(path_data, data_folder_ + "/planning.txt") && 
                        saver.savePathDistancesToFile(adjacent_path, result_distances, data_folder_ + "/path_metrics.txt") &&
                        saver.saveAnglesToFile(angles, data_folder_ + "/angles.txt") ){
-                        RCLCPP_INFO(this->get_logger(),"Path data metrics saved");
+                        ROS_INFO("Path data metrics saved");
                     }else{
-                        RCLCPP_ERROR(this->get_logger(),"Couldn't save path data metrics. Path and results does not have same size");
+                        ROS_ERROR("Couldn't save path data metrics. Path and results does not have same size");
                     }
                 }
 
@@ -254,10 +216,10 @@ class HeuristicPlannerROS : public rclcpp::Node {
                     path_line_markers_.points.clear();
                     path_points_markers_.points.clear();
 
-                    RCLCPP_INFO(this->get_logger(),"Path calculated succesfully");
+                    ROS_INFO("Path calculated succesfully");
                 }
             }else{
-                RCLCPP_INFO(this->get_logger(),"Could not calculate path between request points");
+                ROS_INFO("Could not calculate path between request points");
             }
         }  
         if(_req.tries.data > 2){
@@ -271,50 +233,62 @@ class HeuristicPlannerROS : public rclcpp::Node {
 
         float ws_x, ws_y, ws_z;
 
-        this->get_parameter("world_size_x", ws_x);
-        this->get_parameter("world_size_y", ws_y);
-        this->get_parameter("world_size_z", ws_z);
-        this->get_parameter("resolution", resolution_);
-        this->get_parameter("inflate_map", inflate_);
-        this->get_parameter("use3d", use3d_);
+        lnh_.param("world_size_x", ws_x, (float)100.0); // In meters
+        lnh_.param("world_size_y", ws_y, (float)100.0); // In meters
+        lnh_.param("world_size_z", ws_z, (float)100.0); // In meters
+        lnh_.param("resolution", resolution_, (float)0.2);
+        lnh_.param("inflate_map", inflate_, (bool)true);
 
         world_size_.x = std::floor(ws_x / resolution_);
         world_size_.y = std::floor(ws_y / resolution_);
         world_size_.z = std::floor(ws_z / resolution_);
-
+        
+        lnh_.param("use3d", use3d_, (bool)true);
 
         if( algorithm_name == "astar" ){
-            RCLCPP_INFO(this->get_logger(),"Using A*");
+            ROS_INFO("Using A*");
             algorithm_.reset(new Planners::AStar(use3d_));
         }else if( algorithm_name == "costastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware A*");
+            ROS_INFO("Using Cost Aware A*");
             algorithm_.reset(new Planners::AStarM1(use3d_));
+        }else if( algorithm_name == "astar_gradient" ){
+            ROS_INFO("Using A* Gradient");
+            algorithm_.reset(new Planners::AStarGradient(use3d_));
+        }else if( algorithm_name == "astar_edf" ){
+            ROS_INFO("Using A* EDF");
+            algorithm_.reset(new Planners::AStarEDF(use3d_));
         }else if( algorithm_name == "astarsafetycost" ){
-            RCLCPP_INFO(this->get_logger(),"Using A* Safety Cost");
+            ROS_INFO("Using A* Safety Cost");
             algorithm_.reset(new Planners::AStarM2(use3d_));    
         }else if ( algorithm_name == "thetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Theta*");
+            ROS_INFO("Using Theta*");
             algorithm_.reset(new Planners::ThetaStar(use3d_));
         }else if ( algorithm_name == "costhetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware Theta* ");
+            ROS_INFO("Using Cost Aware Theta* ");
             algorithm_.reset(new Planners::ThetaStarM1(use3d_));
         }else if ( algorithm_name == "thetastarsafetycost" ){
-            RCLCPP_INFO(this->get_logger(),"Using Theta* Safety Cost");
+            ROS_INFO("Using Theta* Safety Cost");
             algorithm_.reset(new Planners::ThetaStarM2(use3d_));
         }else if( algorithm_name == "lazythetastar" ){
-            RCLCPP_INFO(this->get_logger(),"Using LazyTheta*");
+            ROS_INFO("Using LazyTheta*");
             algorithm_.reset(new Planners::LazyThetaStar(use3d_));
         }else if( algorithm_name == "costlazythetastar"){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware LazyTheta*");
+            ROS_INFO("Using Cost Aware LazyTheta*");
             algorithm_.reset(new Planners::LazyThetaStarM1(use3d_));
         }else if( algorithm_name == "costlazythetastarmodified"){
-            RCLCPP_INFO(this->get_logger(),"Using Cost Aware LazyTheta*");
+            ROS_INFO("Using Cost Aware LazyTheta*");
             algorithm_.reset(new Planners::LazyThetaStarM1Mod(use3d_));
         }else if( algorithm_name == "lazythetastarsafetycost"){
-            RCLCPP_INFO(this->get_logger(),"Using LazyTheta* Safety Cost");
+            ROS_INFO("Using LazyTheta* Safety Cost");
             algorithm_.reset(new Planners::LazyThetaStarM2(use3d_));
+        }else if( algorithm_name == "lazythetastargradient"){
+            ROS_INFO("Using LazyTheta* Gradient");
+            algorithm_.reset(new Planners::LazyThetaStarGradient(use3d_));
+        }else if( algorithm_name == "lazythetastaredf"){
+            ROS_INFO("Using LazyTheta* EDF");
+            algorithm_.reset(new Planners::LazyThetaStarEDF(use3d_));
         }else{
-            RCLCPP_WARN(this->get_logger(),"Wrong algorithm name parameter. Using ASTAR by default");
+            ROS_WARN("Wrong algorithm name parameter. Using ASTAR by default");
             algorithm_.reset(new Planners::AStar(use3d_));
         }
 
@@ -322,36 +296,32 @@ class HeuristicPlannerROS : public rclcpp::Node {
 
         configureHeuristic(_heuristic);
 
-        RCLCPP_INFO(this->get_logger(),"Using discrete world size: [%d, %d, %d]", world_size_.x, world_size_.y, world_size_.z);
-        RCLCPP_INFO(this->get_logger(),"Using resolution: [%f]", resolution_);
+        ROS_INFO("Using discrete world size: [%d, %d, %d]", world_size_.x, world_size_.y, world_size_.z);
+        ROS_INFO("Using resolution: [%f]", resolution_);
 
         if(inflate_){
             double inflation_size;
-            this->get_parameter("inflation_size", inflation_size);
+            lnh_.param("inflation_size", inflation_size, 0.5);
             inflation_steps_ = std::round(inflation_size / resolution_);
-            RCLCPP_INFO(this->get_logger(),"Inflation size %.2f, using inflation step %d", inflation_size, inflation_steps_);
+            ROS_INFO("Inflation size %.2f, using inflation step %d", inflation_size, inflation_steps_);
         }
         algorithm_->setInflationConfig(inflate_, inflation_steps_);
 
-
+        m_grid3d_.reset(new Grid3d); //TODO Costs not implement yet
         double cost_scaling_factor, robot_radius;
-
-        this->get_parameter("cost_scaling_factor", cost_scaling_factor);
-        this->get_parameter("robot_radius", robot_radius);
+        lnh_.param("cost_scaling_factor", cost_scaling_factor, 0.8);		
+		lnh_.param("robot_radius", robot_radius, 0.4);		
         
         m_grid3d_->setCostParams(cost_scaling_factor, robot_radius);
         
         std::string frame_id;
-        // lnh_.param("frame_id", frame_id, std::string("map"));		
-        this->get_parameter("frame_id", frame_id);
-
+        lnh_.param("frame_id", frame_id, std::string("map"));		
         configMarkers(algorithm_name, frame_id, resolution_);
 
-        this->get_parameter("save_data_file", save_data_);
-        this->get_parameter("data_folder", data_folder_);
-
+        lnh_.param("save_data_file", save_data_, (bool)true);		
+        lnh_.param("data_folder", data_folder_, std::string("planing_data.txt"));		
         if(save_data_)
-            RCLCPP_INFO_STREAM(this->get_logger(),"Saving path planning data results to " << data_folder_);
+            ROS_INFO_STREAM("Saving path planning data results to " << data_folder_);
 
         //
         if( input_map_ == 1 ){
@@ -362,36 +332,33 @@ class HeuristicPlannerROS : public rclcpp::Node {
         }
         //Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution)
         float sight_dist, cost_weight;
-                                                                             //
-        this->get_parameter("max_line_of_sight_distance", sight_dist);
-        this->get_parameter("cost_weight", cost_weight);
-
+        lnh_.param("max_line_of_sight_distance", sight_dist, (float)1000.0); // In meters
+        lnh_.param("cost_weight", cost_weight, (float)0.0);
         algorithm_->setMaxLineOfSight(sight_dist);
         algorithm_->setCostFactor(cost_weight);
 
-        this->get_parameter("overlay_markers", overlay_markers_);
-
+        lnh_.param("overlay_markers", overlay_markers_, (bool)false);
     }
     void configureHeuristic(const std::string &_heuristic){
         
         if( _heuristic == "euclidean" ){
             algorithm_->setHeuristic(Planners::Heuristic::euclidean);
-            RCLCPP_INFO(this->get_logger(),"Using Euclidean Heuristics");
+            ROS_INFO("Using Euclidean Heuristics");
         }else if( _heuristic == "euclidean_optimized" ){
             algorithm_->setHeuristic(Planners::Heuristic::euclideanOptimized);
-            RCLCPP_INFO(this->get_logger(),"Using Optimized Euclidean Heuristics");
+            ROS_INFO("Using Optimized Euclidean Heuristics");
         }else if( _heuristic == "manhattan" ){
             algorithm_->setHeuristic(Planners::Heuristic::manhattan);
-            RCLCPP_INFO(this->get_logger(),"Using Manhattan Heuristics");
+            ROS_INFO("Using Manhattan Heuristics");
         }else if( _heuristic == "octogonal" ){
             algorithm_->setHeuristic(Planners::Heuristic::octagonal);
-            RCLCPP_INFO(this->get_logger(),"Using Octogonal Heuristics");
+            ROS_INFO("Using Octogonal Heuristics");
         }else if( _heuristic == "dijkstra" ){
             algorithm_->setHeuristic(Planners::Heuristic::dijkstra);     
-            RCLCPP_INFO(this->get_logger(),"Using Dijkstra Heuristics");
+            ROS_INFO("Using Dijkstra Heuristics");
         }else{
             algorithm_->setHeuristic(Planners::Heuristic::euclidean);
-            RCLCPP_WARN(this->get_logger(),"Wrong Heuristic param. Using Euclidean Heuristics by default");
+            ROS_WARN("Wrong Heuristic param. Using Euclidean Heuristics by default");
         }
     }
     std::vector<std::pair<Planners::utils::Vec3i, double>> getClosestObstaclesToPathPoints(const Planners::utils::CoordinateList &_path){
@@ -413,11 +380,11 @@ class HeuristicPlannerROS : public rclcpp::Node {
 
         path_line_markers_.ns = _ns;
         path_line_markers_.header.frame_id = _frame;
-        path_line_markers_.header.stamp = this->now();
+        path_line_markers_.header.stamp = ros::Time::now();
         path_line_markers_.id = rand();
-        path_line_markers_.lifetime = rclcpp::Duration::from_seconds(500);
-        path_line_markers_.type = visualization_msgs::msg::Marker::LINE_STRIP;
-        path_line_markers_.action = visualization_msgs::msg::Marker::ADD;
+        path_line_markers_.lifetime = ros::Duration(500);
+        path_line_markers_.type = visualization_msgs::Marker::LINE_STRIP;
+        path_line_markers_.action = visualization_msgs::Marker::ADD;
         path_line_markers_.pose.orientation.w = 1;
 
         path_line_markers_.color.r = 0.0;
@@ -429,11 +396,11 @@ class HeuristicPlannerROS : public rclcpp::Node {
 
         path_points_markers_.ns = _ns;
         path_points_markers_.header.frame_id = _frame;
-        path_points_markers_.header.stamp = this->now();
+        path_points_markers_.header.stamp = ros::Time::now();
         path_points_markers_.id = rand();
-        path_points_markers_.lifetime = rclcpp::Duration::from_seconds(500);
-        path_points_markers_.type = visualization_msgs::msg::Marker::POINTS;
-        path_points_markers_.action = visualization_msgs::msg::Marker::ADD;
+        path_points_markers_.lifetime = ros::Duration(500);
+        path_points_markers_.type = visualization_msgs::Marker::POINTS;
+        path_points_markers_.action = visualization_msgs::Marker::ADD;
         path_points_markers_.pose.orientation.w = 1;
         path_points_markers_.color.r = 0.0;
         path_points_markers_.color.g = 1.0;
@@ -444,26 +411,25 @@ class HeuristicPlannerROS : public rclcpp::Node {
         path_points_markers_.scale.z = _scale;
 
     }
-    template <typename T>
-    void publishMarker(visualization_msgs::msg::Marker &_marker, const T &_pub ){
+    void publishMarker(visualization_msgs::Marker &_marker, const ros::Publisher &_pub){
         
         //Clear previous marker
         if( !overlay_markers_ ){
-            _marker.action = visualization_msgs::msg::Marker::DELETEALL;
-            _pub->publish(_marker);
+            _marker.action = visualization_msgs::Marker::DELETEALL;
+            _pub.publish(_marker);
         }else{
             path_points_markers_.id           = rand();
-            path_points_markers_.header.stamp = this->now();
+            path_points_markers_.header.stamp = ros::Time::now();
             setRandomColor(path_points_markers_.color);
 
             path_line_markers_.id             = rand();
-            path_line_markers_.header.stamp   = this->now();
+            path_line_markers_.header.stamp   = ros::Time::now();
             setRandomColor(path_line_markers_.color);
         }
-        _marker.action = visualization_msgs::msg::Marker::ADD;
-        _pub->publish(_marker);
+        _marker.action = visualization_msgs::Marker::ADD;
+        _pub.publish(_marker);
     }
-    void setRandomColor(std_msgs::msg::ColorRGBA &_color, unsigned int _n_div = 20){
+    void setRandomColor(std_msgs::ColorRGBA &_color, unsigned int _n_div = 20){
         //Using golden angle approximation
         const double golden_angle = 180 * (3 - sqrt(5));
         double hue = color_id_ * golden_angle + 60;
@@ -479,28 +445,20 @@ class HeuristicPlannerROS : public rclcpp::Node {
     }
 
 
-    // ros::NodeHandle lnh_{"~"};
-    // ros::ServiceServer request_path_server_, change_planner_server_;
-
-    // ros::Subscriber pointcloud_sub_, occupancy_grid_sub_;
-
-    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
-    rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_sub_;
-
-    rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr line_markers_pub_, point_markers_pub_;
-
-    rclcpp::Service<heuristic_planners::srv::GetPath>::SharedPtr request_path_server_;
-    rclcpp::Service<heuristic_planners::srv::SetAlgorithm>::SharedPtr change_planner_server_;
+    ros::NodeHandle lnh_{"~"};
+    ros::ServiceServer request_path_server_, change_planner_server_;
+    ros::Subscriber pointcloud_sub_, occupancy_grid_sub_;
+    //TODO Fix point markers
+    ros::Publisher line_markers_pub_, point_markers_pub_;
 
     std::unique_ptr<Grid3d> m_grid3d_;
 
     std::unique_ptr<Planners::AlgorithmBase> algorithm_;
         
-    visualization_msgs::msg::Marker path_line_markers_, path_points_markers_;
+    visualization_msgs::Marker path_line_markers_, path_points_markers_;
     
     //Parameters
     Planners::utils::Vec3i world_size_; // Discrete
-    //float resolution_ = 0.2;
     float resolution_;
 
     bool save_data_;
@@ -511,11 +469,8 @@ class HeuristicPlannerROS : public rclcpp::Node {
     std::string data_folder_;
     bool overlay_markers_{0};
     unsigned int color_id_{0};
-    nav_msgs::msg::OccupancyGrid occupancy_grid_;
-    // TODO?: Change type of message --> sensor_msgs::msg::PointCloud2
+    nav_msgs::OccupancyGrid occupancy_grid_;
     pcl::PointCloud<pcl::PointXYZ> cloud_;
-    //sensor_msgs::msg::PointCloud2 cloud_; //JAC
-
     //0: no map yet
     //1: using occupancy
     //2: using cloud
@@ -525,14 +480,10 @@ class HeuristicPlannerROS : public rclcpp::Node {
 };
 int main(int argc, char **argv)
 {
-    // ros::init(argc, argv, "heuristic_planner_ros_node");
-    rclcpp::init(argc, argv);
-
-    auto node = std::make_shared<HeuristicPlannerROS>();
-    rclcpp::spin(node);
+    ros::init(argc, argv, "heuristic_planner_ros_node");
 
-    // HeuristicPlannerROS heuristic_planner_ros;
-    // ros::spin();
+    HeuristicPlannerROS heuristic_planner_ros;
+    ros::spin();
 
 return 0;
-}
+}
\ No newline at end of file
diff --git a/src/utils/LineOfSight.cpp b/src/utils/LineOfSight.cpp
index 0a29577..e8ccdda 100644
--- a/src/utils/LineOfSight.cpp
+++ b/src/utils/LineOfSight.cpp
@@ -121,14 +121,17 @@ namespace Planners
             }
             int nodesInLineBetweenTwoNodes(const Node *_lnode, const Node *_rnode, const DiscreteWorld &_world, const unsigned int _threshold){
                 if( utils::geometry::distanceBetween2Nodes(_lnode, _rnode) >= ( dist_scale_factor_ * _threshold ) ){
+                    // std::cout << "NO LOS" << std::endl;
                     return 0;
                 }
                 utils::CoordinateListPtr nodes;
                 nodes.reset(new CoordinateList);
 
                 if(bresenham3D(_lnode, _rnode, _world, nodes)){
+                    // std::cout << "DEVUELVE 0 NODES" << std::endl;
                     return nodes->size();
                 }else{
+                    // std::cout << "0 NODES" << std::endl;
                     return 0;
                 }
             }
diff --git a/src/utils/heuristic.cpp b/src/utils/heuristic.cpp
index 04647fd..546afe6 100644
--- a/src/utils/heuristic.cpp
+++ b/src/utils/heuristic.cpp
@@ -8,6 +8,17 @@ namespace Planners
         return {abs(_source.x - _target.x), abs(_source.y - _target.y), abs(_source.z - _target.z)};
     }
 
+    Vec3i Heuristic::getVectorPull(const Vec3i &_source, const Vec3i &_target)
+    {
+        return {_target.x - _source.x, _target.y - _source.y, _target.z - _source.z};
+    }
+
+    unsigned int Heuristic::goal_pull(const Vec3i &_source, const Vec3i &_target)
+    {
+        auto delta = std::move(getDelta(_source, _target));
+        return static_cast<unsigned int>(dist_scale_factor_ * sqrt(pow(delta.x, 2) + pow(delta.y, 2) + pow(delta.z, 2)));
+    }
+
     unsigned int Heuristic::manhattan(const Vec3i &_source, const Vec3i &_target)
     {
         auto delta = std::move(getDelta(_source, _target));
diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp
index 5b22f5d..d54c33d 100644
--- a/src/utils/ros/ROSInterfaces.cpp
+++ b/src/utils/ros/ROSInterfaces.cpp
@@ -12,23 +12,23 @@ namespace Planners
                     static_cast<int>(std::round(_point.y / _res)),
                     static_cast<int>(std::round(_point.z / _res))};
         }
-        Vec3i discretePoint(const geometry_msgs::msg::Point &_msg, const double &_res)
+        Vec3i discretePoint(const geometry_msgs::Point &_msg, const double &_res)
         {
 
             return {static_cast<int>(std::round(_msg.x / _res)),
                     static_cast<int>(std::round(_msg.y / _res)),
                     static_cast<int>(std::round(_msg.z / _res))};
         }
-        Vec3i discretePose(const geometry_msgs::msg::Pose &_msg, const double &_res)
+        Vec3i discretePose(const geometry_msgs::Pose &_msg, const double &_res)
         {
 
             return {static_cast<int>(std::round(_msg.position.x / _res)),
                     static_cast<int>(std::round(_msg.position.y / _res)),
                     static_cast<int>(std::round(_msg.position.z / _res))};
         }
-        geometry_msgs::msg::Point continousPoint(const Vec3i &_vec, const double &_res)
+        geometry_msgs::Point continousPoint(const Vec3i &_vec, const double &_res)
         {
-            geometry_msgs::msg::Point ret;
+            geometry_msgs::Point ret;
 
             ret.x = _vec.x * _res;
             ret.y = _vec.y * _res;
@@ -44,7 +44,7 @@ namespace Planners
                     0};
         }
 
-        bool configureWorldFromOccupancy(const nav_msgs::msg::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size)
+        bool configureWorldFromOccupancy(const nav_msgs::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size)
         {
 
             if (_set_size)
@@ -69,7 +69,7 @@ namespace Planners
 
             return true;
         }
-        bool configureWorldFromOccupancyWithCosts(const nav_msgs::msg::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size)
+        bool configureWorldFromOccupancyWithCosts(const nav_msgs::OccupancyGrid &_grid, AlgorithmBase &_algorithm, bool _set_size)
         {
 
             if (_set_size)
@@ -129,4 +129,4 @@ namespace Planners
         }
 
     } //ns utils
-} //ns planners
+} //ns planners
\ No newline at end of file