diff --git a/include/Planners/AlgorithmBase.hpp b/include/Planners/AlgorithmBase.hpp
index fbd40a1..e9067d2 100755
--- a/include/Planners/AlgorithmBase.hpp
+++ b/include/Planners/AlgorithmBase.hpp
@@ -145,6 +145,13 @@ namespace Planners
          */
         void addCollision(const Vec3i &coordinates_);
 
+        /**
+         * @brief Calls removeCollision for a given coordinate
+         * 
+         * @param coordinates_ Discrete coordinates vector
+         */
+        void removeCollision(const Vec3i &coordinates_);
+
         /**
          * @brief Function to use in the future to configure the cost of each node
          * 
diff --git a/include/utils/world.hpp b/include/utils/world.hpp
index 9613521..799e66f 100755
--- a/include/utils/world.hpp
+++ b/include/utils/world.hpp
@@ -261,6 +261,31 @@ namespace utils
 
             return setOccupied(_pos.x, _pos.y, _pos.z);
         }
+
+        /**
+         * @brief Set the world's node associated
+         * 
+         * @param _pos discrete node position vector
+         */
+        void setUnoccupied(const int _x, const int _y, const int _z){
+
+            if (!checkValid(_x, _y, _z))
+                return;
+
+            discrete_world_vector_[getWorldIndex(_x, _y, _z)].occuppied = false;
+            
+        }
+
+        /**
+         * @brief Set the world's node associated
+         * 
+         * @param _pos discrete node position vector
+         */
+        void setUnoccupied(const Vec3i &_pos){
+
+            return setUnoccupied(_pos.x, _pos.y, _pos.z);
+        }
+
         /**
          * @brief Checks the value of the internal flag of the node
          * that is used to mark that the node is in the open list
diff --git a/launch/local_planner.launch b/launch/local_planner.launch
index 2de58bb..44c8cf1 100755
--- a/launch/local_planner.launch
+++ b/launch/local_planner.launch
@@ -27,7 +27,7 @@
 
     <arg name="overlay_markers"     default="false"/>
     
-    <arg name="cost_weight" default="4.0"/>
+    <arg name="cost_weight" default="500.0"/>
     <arg name="max_line_of_sight_distance" default="2.0"/>+
 
     <!-- GRIDM Generators Parameters -->
diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp
index ae29b88..1f39684 100755
--- a/src/Planners/AlgorithmBase.cpp
+++ b/src/Planners/AlgorithmBase.cpp
@@ -77,6 +77,10 @@ namespace Planners
     {
         addCollision(coordinates_, do_inflate_, inflate_steps_);
     }
+    void AlgorithmBase::removeCollision(const Vec3i &coordinates_)
+    {
+        discrete_world_.setUnoccupied(coordinates_);
+    }
     bool AlgorithmBase::detectCollision(const Vec3i &coordinates_)
     {
         if (discrete_world_.isOccupied(coordinates_))
diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp
index e66ec42..3d15576 100755
--- a/src/ROS/local_planner_ros_node.cpp
+++ b/src/ROS/local_planner_ros_node.cpp
@@ -401,6 +401,9 @@ class HeuristicLocalPlannerROS
                 0 <= global_path_local[it].y && global_path_local[it].y < m_local_grid3d_->m_gridSizeY &&
                 0 <= global_path_local[it].z && global_path_local[it].z < m_local_grid3d_->m_gridSizeZ) 
             {
+                local_goal.x = global_path_local[it].x;
+                local_goal.y = global_path_local[it].y;
+                local_goal.z = global_path_local[it].z;
                 it++;
             }
             else
diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp
index b4a51fd..6379267 100755
--- a/src/utils/ros/ROSInterfaces.cpp
+++ b/src/utils/ros/ROSInterfaces.cpp
@@ -145,8 +145,10 @@ namespace Planners
                     {
                         float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution);
                         _algorithm.configureCellCost({i, j, k}, cost);
-                        if(cost <= 0)
+                        if(cost <= resolution)
                             _algorithm.addCollision({i, j, k});
+                        else
+                            _algorithm.removeCollision({i, j, k});
                     }
                 }
             }