From 933c7b1bd8ff29fac2cc89b169d40cb4731f6cb2 Mon Sep 17 00:00:00 2001
From: guillermogilg99 <guillermogilg99@gmail.com>
Date: Tue, 3 Sep 2024 13:50:37 +0000
Subject: [PATCH] Minor update

---
 include/Grid3D/local_grid3d.hpp    | 27 ++++++++++++++++-----------
 launch/local_planner.launch        |  6 +++---
 src/ROS/local_planner_ros_node.cpp |  2 +-
 src/utils/ros/ROSInterfaces.cpp    |  3 ++-
 4 files changed, 22 insertions(+), 16 deletions(-)

diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp
index 952ab15..cd12360 100644
--- a/include/Grid3D/local_grid3d.hpp
+++ b/include/Grid3D/local_grid3d.hpp
@@ -189,7 +189,7 @@ class Local_Grid3d
 	float getCellCost(const float &_x, const float &_y, const float &_z){
 		
 		if( !isIntoMap(_x, _y, _z) ){
-			std::cout << "OUT " << _x << "," << _y << "," << _z << std::endl;
+			std::cout << "OUT " << _x << "," << _y << "," << _z << "Expected: " << m_maxX << "," << m_maxY << "," << m_maxZ << std::endl;
 			return 0;
 		}
 			
@@ -330,7 +330,8 @@ class Local_Grid3d
 		m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ;
 		m_gridStepY = m_gridSizeX;
 		m_gridStepZ = m_gridSizeX*m_gridSizeY;
-
+		//std::cout << "m_gridSizeX = " << m_gridSizeX << "m_gridSizeY = " << m_gridSizeY << "m_gridSizeZ = " << m_gridSizeZ << std::endl;
+		//std::cout << "m_gridSize = " << m_gridSize << std::endl;
 		// JAC: Pongo los mismo datos que en global planner y no funciona
 		// m_gridSizeX = 341;
 		// m_gridSizeY = 241; 
@@ -364,7 +365,8 @@ class Local_Grid3d
 		std::vector<std::vector<float>> coordinates_vector;
         coordinates_vector.reserve(m_gridSizeX*m_gridSizeY*m_gridSizeZ);
 
-		std::cout << "m_resolution = " << m_resolution << std::endl;
+		//std::cout << "m_resolution = " << m_resolution << std::endl;
+		std::cout << "Drone position: " << "x = " << drone_x << " | y = " << drone_y << " | z = " << drone_z << std::endl;
 
 		for(int iz=0; iz<m_gridSizeZ; iz++)
 		{
@@ -382,7 +384,7 @@ class Local_Grid3d
 			}
 		}
 
-		std::cout << "---!!!--- Completed coordinates_vector ---!!!---" << std::endl;
+		//std::cout << "---!!!--- Completed coordinates_vector ---!!!---" << std::endl;
 
 		// Convertir el vector de puntos a un tensor de libtorch
         auto num_points = coordinates_vector.size();
@@ -393,19 +395,22 @@ class Local_Grid3d
             coordinates_tensor[i][2] = coordinates_vector[i][2];
         }
 
-		std::cout << "---!!!--- Completed coordinates_tensor ---!!!---" << std::endl;
-		std::cout << coordinates_tensor << std::endl;
+		//std::cout << "---!!!--- Completed coordinates_tensor ---!!!---" << std::endl;
+		//std::cout << coordinates_tensor << std::endl;
 
         // Pasar el tensor por la red neuronal
         auto start = std::chrono::high_resolution_clock::now();
-		std::cout << "---!!!--- Started timer ---!!!---" << std::endl;
+		//std::cout << "---!!!--- Started timer ---!!!---" << std::endl;
         torch::Tensor grid_output_tensor = loaded_sdf.forward({coordinates_tensor}).toTensor();
-		std::cout << "---!!!--- Query done ---!!!---" << std::endl;
+		//std::cout << "---!!!--- Query done ---!!!---" << std::endl;
         auto end = std::chrono::high_resolution_clock::now();
         std::chrono::duration<double, std::milli> duration = end - start;
+		// usleep(1e4);
+        // std::cout << "Please a key to go to the next iteration..." << std::endl;
+        // getchar(); // Comentar para no usar tecla.
         std::cout << "Points queried: " << num_points <<" |  Time taken to query model: " << duration.count() << " ms" << std::endl;
 
-		std::cout << "---!!!--- Completed query ---!!!---" << std::endl;
+		//std::cout << "---!!!--- Completed query ---!!!---" << std::endl;
 
 		int index;
 		//Asignar cada valor a la posición correcta del grid
@@ -474,9 +479,9 @@ class Local_Grid3d
         // Get map parameters: They have to take from local_world_size_x, local_world_size_y , local_world_size_z of the launch
 		double minX, minY, minZ, maxX, maxY, maxZ, res;
         
-		maxX = 10.0;
+		maxX = 8.0;
         minX = 0.0;
-        maxY = 10.0;
+        maxY = 8.0;
         minY = 0.0;
         maxZ = 4.0;
         minZ = 0.0;
diff --git a/launch/local_planner.launch b/launch/local_planner.launch
index 04abe4f..d4b1c2a 100644
--- a/launch/local_planner.launch
+++ b/launch/local_planner.launch
@@ -11,9 +11,9 @@
     <arg name="algorithm_name"      default="costlazythetastar"/>
 
     <!-- Size of the Local Map  -->
-    <arg name="local_world_size_x"        default="8"/>
-    <arg name="local_world_size_y"        default="8"/>
-    <arg name="local_world_size_z"        default="3"/>
+    <arg name="local_world_size_x"        default="4"/>
+    <arg name="local_world_size_y"        default="4"/>
+    <arg name="local_world_size_z"        default="2"/>
     <arg name="resolution"          default="0.2"/>
 
     <arg name="inflate_map"         default="true"/>
diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp
index 6a8f721..7b81434 100644
--- a/src/ROS/local_planner_ros_node.cpp
+++ b/src/ROS/local_planner_ros_node.cpp
@@ -499,7 +499,7 @@ class HeuristicLocalPlannerROS
         // lnh_.param("frame_id", frame_id, std::string("map"));		
         // configMarkers(algorithm_name, frame_id, resolution_);
 
-        lnh_.param("save_data_file", save_data_, (bool)true);		
+        lnh_.param("save_data_file", save_data_, (bool)true);
         lnh_.param("data_folder", data_folder_, std::string("planing_data.txt"));		
         if(save_data_)
             ROS_INFO_STREAM("Saving path planning data results to " << data_folder_);
diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp
index af5ae3f..4208470 100644
--- a/src/utils/ros/ROSInterfaces.cpp
+++ b/src/utils/ros/ROSInterfaces.cpp
@@ -190,12 +190,13 @@ namespace Planners
             torch::jit::script::Module loaded_sdf;
             loaded_sdf = torch::jit::load("/home/ros/exchange/weight_data/mod_70000p.pt", c10::kCPU);
             _grid.computeLocalGrid(loaded_sdf, drone_x, drone_y, drone_z);
-
+            
             // t1 = clock();
             // double time = (double(t1-t0)/CLOCKS_PER_SEC);
             // std::cout << "Execution Time: " << time << std::endl;
 
             auto world_size = _algorithm.getWorldSize();
+            std::cout << "World size: " << world_size << std::endl;
             auto resolution = _algorithm.getWorldResolution();
             // std::cout << "world size: " << world_size.x  << std::endl;  //200
             // std::cout << "resolution: " << resolution  << std::endl;  //0.05