ETFA2024 version

include/Grid3D/grid3d.hpp

@@ -602,7 +602,7 @@ bool loadSemanticGrid() {
 		// maximo=m_gridSizeX*m_gridSizeY*m_gridSizeZ;	
 		// std::cout << "maximo: " << maximo << std::endl;
 		int c0,c1,c2,c3,c4,c5,c6,c7,c8;
-		int ind1,ind2,ind3,ind4;
+		int ind1,ind2,ind3,ind4,ind5;
 
 		c0=0;
 		c1=0;
@@ -617,12 +617,27 @@ bool loadSemanticGrid() {
 		ind2=0;
 		ind3=0;
 		ind4=0;
+		ind5=0;
+
+		// CURRENT VERSION IN THE REPO: 2024-08-06
+		// for(int iz=0; iz<m_gridSizeZ; iz++)
+		// {
+		// 	for(int iy=0; iy<m_gridSizeY; iy++)
+		// 	{
+		// 		for(int ix=0; ix<m_gridSizeX; ix++)
+		// 		{	
+		// 			index = ix + iy*m_gridStepY + iz*m_gridStepZ;		
+		// 			m_grid[index].semantic = 0;
+		// 		}
+		// 	}
+		// }
 
-		for(int iz=0; iz<m_gridSizeZ; iz++)
+		// VERSION OF 2024-MARCH
+		for(int iz=0; iz<(m_gridSizeZ-2); iz++) //m_gridSizeZ-2?
 		{
-			for(int iy=0; iy<m_gridSizeY; iy++)
+			for(int iy=0; iy<(m_gridSizeY-3); iy++) //m_gridSizeY-3?
 			{
-				for(int ix=0; ix<m_gridSizeX; ix++)
+				for(int ix=0; ix<(m_gridSizeX); ix++)
 				{	
 					index = ix + iy*m_gridStepY + iz*m_gridStepZ;		
 					m_grid[index].semantic = 0;
@@ -715,7 +730,7 @@ bool loadSemanticGrid() {
 						dist = sqrt(pointNKNSquaredDistance[0]);
 
 						// std::cout << "dist: " << dist << std::endl;
-						if (dist < 300){
+						if (dist < 500){
 							m_grid[index].dist = dist;
 							if(!use_costmap_function){
 								// m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
@@ -728,17 +743,37 @@ bool loadSemanticGrid() {
 								// m_grid[index].prob = 100-dist;
 								m_grid[index].prob = dist;
 							}
-							if ((dist < 0.3) && (m_grid[index].semantic ==0)){
+							if ((dist < 0.2) && (m_grid[index].semantic ==0)){
 								ind1=ind1+1;
+								// std::cout << "IND 1 " << std::endl;
+								// std::cout << "index: " << index << std::endl;
+								// std::cout << "dist: " << dist << std::endl;
+								// std::cout << "value of semanticGrid: " << semanticGrid_v[index] << std::endl;
+								// std::cout << "searchPoint:" << searchPoint.x << "," << searchPoint.y << "," << searchPoint.z << std::endl;
+
+								// 	usleep(1e4);
+								// 	std::cout << "Please a key to go to the next iteration..." << std::endl;
+								// 	getchar(); // Comentar para no usar tecla.
 							}
-							if ((dist > 0.3) && (m_grid[index].semantic ==0)){
+							if ((dist > 0.2) && (m_grid[index].semantic ==0)){
 								ind2=ind2+1;
 							}
-							if ((dist < 0.3) && (m_grid[index].semantic !=0)){
+							if ((dist < 0.2) && (m_grid[index].semantic !=0)){
 								ind3=ind3+1;
 							}
-							if ((dist > 2) && (m_grid[index].semantic !=0)){
+							if ((dist > 0.2) && (dist < 0.5) && (m_grid[index].semantic !=0)){
 								ind4=ind4+1;
+								// std::cout << "IND 4 " << std::endl;
+								// std::cout << "index: " << index << std::endl;
+								// std::cout << "dist: " << dist << std::endl;
+								// std::cout << "value of semanticGrid: " << m_grid[index].semantic << std::endl;
+								// std::cout << "searchPoint:" << searchPoint.x << "," << searchPoint.y << "," << searchPoint.z << std::endl;
+								// usleep(1e4);
+								// std::cout << "Please a key to go to the next iteration..." << std::endl;
+								// getchar(); // Comentar para no usar tecla.
+							}
+							if ((dist > 0.5) && (m_grid[index].semantic !=0)){
+								ind5=ind5+1;
 							}
 
 						}
@@ -787,6 +822,7 @@ bool loadSemanticGrid() {
 		std::cout << "ind2: " << ind2 << std::endl;
 		std::cout << "ind3: " << ind3 << std::endl;
 		std::cout << "ind4: " << ind4 << std::endl;
+		std::cout << "ind5: " << ind5 << std::endl;
 
 	}
 

include/Planners/AlgorithmBase.hpp

@@ -135,6 +135,9 @@ namespace Planners
          */
         bool configureCellCost(const Vec3i &coordinates_, const double &_cost);
 
+        // ************************************************************************************************************************
+        // JAC --> Semantic layer
+
         /**
          * @brief Function to use in the future to configure the semantic cost of each node
          * 
@@ -145,6 +148,14 @@ namespace Planners
          */
         bool configureCellSemantic(const Vec3i &coordinates_, const int &_cost);
 
+        void inflateCellSemantic(const Vec3i &coordinates_, const int &_cost, bool do_inflate, unsigned int steps);
+
+        void inflateCellSemantic(const Vec3i &coordinates_, const int &_cost);
+
+        unsigned int getCellSemantic(const Vec3i &coordinates_);
+
+        // ************************************************************************************************************************
+
         /**
          * @brief Check if a set of discrete coordinates are marked as occupied
          * 
@@ -204,6 +215,18 @@ namespace Planners
         void inflateNodeAsCube(const Vec3i &_ref,
                                const CoordinateList &_directions,
                                const unsigned int &_inflate_steps);
+
+        /**
+         * @brief Basic inflation function for Semantic layer
+         * 
+         * @param _ref Discrete coordinates vector
+         * @param _directions Directions vector to inflate
+         * @param _inflate_steps number of cells to inflate in each direction
+         */
+        void inflateSemanticNodeAsCube(const Vec3i &_ref, 
+                                       const CoordinateList &_directions, 
+                                       const unsigned int &_inflate_steps,
+                                       const int &_cost);
 
         /**
          * @brief Create a Result Data Object object

include/Planners/ThetaStarSemanticCost.hpp

@@ -86,7 +86,7 @@ namespace Planners
          * @return unsigned int The G Value calculated by the function
          */
         virtual unsigned int computeG(const Node* _current, Node* _suc,  unsigned int _n_i, unsigned int _dirs) override;
-        int coef=10;
+        int coef=5;
 
     };
 

include/utils/ros/ROSInterfaces.hpp

@@ -139,7 +139,6 @@ namespace Planners
          */
         bool configureWorldCosts(Grid3d &_grid, AlgorithmBase &_algorithm);
         bool configureWorldSemantic(Grid3d &_grid, AlgorithmBase &_algorithm);
-        // bool configureWorldSemantic(Grid3d &_grid, AlgorithmBase &_algorithm, std::vector<int> &_semanticGrid);
 
     }
 }

include/utils/world.hpp

@@ -69,11 +69,16 @@ namespace utils
             resolution_   = _resolution;
             x_y_size_      = static_cast<long>(world_x_size_) * world_y_size_;
 
+            std::cout << "world_size_x: " << world_x_size_ << std::endl;
+            std::cout << "world_size_y: " << world_y_size_ << std::endl;
+            std::cout << "world_size_z: " << world_z_size_ << std::endl;
+
             discrete_world_vector_.clear();
             discrete_world_vector_.resize(static_cast<long>(world_x_size_) * world_y_size_ * _world_z_size);
             Node node;
             std::fill(discrete_world_vector_.begin(), discrete_world_vector_.end(), node);
 
+            std::cout << "discrete_world_vector: " << discrete_world_vector_.size() << std::endl;
             for(long unsigned int i = 0; i < discrete_world_vector_.size(); ++i){
                 discrete_world_vector_[i].coordinates =  getDiscreteWorldPositionFromIndex(i);
                 discrete_world_vector_[i].world_index = i;
@@ -114,14 +119,15 @@ namespace utils
             return true;
         }
         /**
-         * @brief Set the Node Cost object 
+         * @brief Set the Node Semantic Cost object 
          * 
          * @param _vec coordinates of the node
          * @param _cost cost value assigned to it
          * @return true if node is valid
          * @return false if requested coordinates correspond to invalid node (outside the world)
          */
         bool setNodeSemantic(const Vec3i &_vec, const int _cost){
+            // IF COMMENT THIS IF, THE COST ALWAYS WILL BE UPDATED BUT THE NODE IS BROKEN
 
             if(!checkValid(_vec))
                 return false;
@@ -130,6 +136,24 @@ namespace utils
 
             return true;
         }
+        /**
+         * @brief Get the Node Semantic Cost object 
+         * 
+         * @param _vec coordinates of the node
+         * @param _cost cost value assigned to it
+         * @return true if node is valid
+         * @return false if requested coordinates correspond to invalid node (outside the world)
+         */
+        unsigned int getNodeSemantic(const Vec3i &_vec){
+
+            if(!checkValid(_vec))
+                return false;          
+
+            // std::cout << "Semantic Cost: " << discrete_world_vector_[getWorldIndex(_vec)].semantic << std::endl;
+
+            return discrete_world_vector_[getWorldIndex(_vec)].semantic;
+        }
+
         /**
          * @brief Set to its default state the flags, cost values, and parent values inside 
          * each world's node
@@ -191,6 +215,60 @@ namespace utils
          * @param _y discrete coordinates
          * @param _z discrete coordinates
          */
+
+        /**
+         * @brief Function to check the semantic value of the node
+         * 
+         * @param _x discrete coordinate
+         * @param _y discrete coordinate
+         * @param _z discrete coordinate
+         * @return true if node valid and not occupied
+         * @return false if node is outside the workspace of is marked as occupied
+         */
+        int isOccupied_Semantic(const int _x, const int _y, const int _z) const
+        {
+            // if (!checkValid(_x, _y, _z))
+            //     return true;
+
+            // if (discrete_world_vector_[getWorldIndex(_x, _y, _z)].occuppied)
+            // {
+            //     return true;
+            // }
+
+            // return false;
+
+            // int cost_semantic;
+            // cost_semantic = discrete_world_vector_[getWorldIndex(_x, _y, _z)].semantic;
+            return discrete_world_vector_[getWorldIndex(_x, _y, _z)].semantic;
+        }
+        /**
+         * @brief Overloaded isOccupied function for Vec3i objects
+         * 
+         * @param _coord discrete coordinates vector
+         * @return true if node valid and not occupied 
+         * @return false if node is outside the workspace of is marked as occupied 
+         */
+        int isOccupied_Semantic(const Vec3i &_coord) const{
+            return isOccupied_Semantic(_coord.x, _coord.y, _coord.z);
+        }
+        /**
+         * @brief Overloaded isOccupied function for Node objects
+         * 
+         * @param _node node object
+         * @return true if node valid and not occupied 
+         * @return false if node is outside the workspace of is marked as occupied 
+         */
+        int isOccupied_Semantic(const Node &_node) const{
+            return isOccupied_Semantic(_node.coordinates.x, _node.coordinates.y, _node.coordinates.z);
+        }
+        /**
+         * @brief Set the world's node associated
+         * to these coordinates as occupied. 
+         * @param _x discrete coordinates
+         * @param _y discrete coordinates
+         * @param _z discrete coordinates
+         */
+
         void setOccupied(const int _x, const int _y, const int _z)
         {
 

launch/simulator_atlas.launch

@@ -23,11 +23,13 @@
     <!-- ATLAS -->
     <!-- <arg name="map_name"            default="Atlas_8_300"/> -->
     <arg name="map_name"            default="Atlas_8_puertas_02"/>
+    <!-- <arg name="map_name"            default="Atlas_8_floor_1kk"/> -->
     <!-- <arg name="map_name"            default="nube_de_puntos2"/> -->
     <!-- <arg name="map_name"            default="nube_de_puntos_con_etiquetas"/> -->
     <arg name="map"                 default="$(find heuristic_planners)/resources/3dmaps/$(arg map_name).bt"/>
 
-    <arg name="algorithm_name"      default="costlazythetastar"/>
+    <!-- <arg name="algorithm_name"      default="costlazythetastar"/> -->
+    <arg name="algorithm_name"      default="lazythetastar_semantic_cost"/>
 
     <!-- 212 -->
     <arg name="world_size_x"        default="211.8"/>