Complete local planner implementation

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
          * 

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

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 -->

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_))

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

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});
                     }
                 }
             }