test for local planner sdfonline

robotics-upo · Sep 10, 2024 · 38a1ddf · 38a1ddf
1 parent 221081d
commit 38a1ddf
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 9 deletions.
diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp
@@ -116,7 +116,8 @@ class Local_Grid3d
 		// configureParameters();
 		if(configureParameters())
 		{
-            computeLocalGrid(m_cloud);
+            // computeLocalGrid(m_cloud);
+			std::cout << "\tENTRA COMPUTE GRID" << std::endl;
 
 			// Build the msg with a slice of the grid if needed
 			if(m_gridSlice >= 0 && m_gridSlice <= m_maxZ)
@@ -135,7 +136,7 @@ class Local_Grid3d
 			percent_computed_pub_ = m_nh.advertise<std_msgs::Float32>(m_nodeName+"/percent_computed", 1, false);
         }
         else{
-			// std::cout << "\tCOMPUTE GRID" << std::endl;
+			std::cout << "\tCOMPUTE GRID" << std::endl;
             // computeLocalGrid(m_cloud);
             // JAC: Here "Build the msg with a slice of the grid if needed" should be?
         }
@@ -205,6 +206,8 @@ class Local_Grid3d
 		// std_msgs::Float32 percent_msg;
 		// percent_msg.data = 0;
 
+		// std::cout << "\tSDF SDF" << std::endl;
+
 		// Alloc the 3D grid JAC: Alloc the size of the local grid
 		m_gridSizeX = (int)(m_maxX*m_oneDivRes);
 		m_gridSizeY = (int)(m_maxY*m_oneDivRes); 
@@ -231,7 +234,7 @@ class Local_Grid3d
 		// double time = (double(t1-t0)/CLOCKS_PER_SEC);
         // std::cout << "Execution Time: " << time << std::endl;
 
-		// std::cout << "_maxX: " << m_maxX  << std::endl;
+		// std::cout << "_maxX computeLocalGrid: " << m_maxX  << std::endl;
 		// std::cout << "_gridSizeX: " << m_gridSizeX  << std::endl;
 
 		// Setup kdtree
@@ -284,6 +287,10 @@ class Local_Grid3d
 					{	
 						dist = pointNKNSquaredDistance[0];
 						m_grid[index].dist = dist; // dist in the discrete world
+						// std::cout << "\tDIST" << std::endl;
+						// usleep(1e4);
+        				// std::cout << "Please a key to go to the next iteration..." << std::endl;
+        				// getchar(); // Comentar para no usar tecla.
 						// if(!use_costmap_function){
 						// 	m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2);
 						// }else{
@@ -368,7 +375,9 @@ class Local_Grid3d
         minY = 0.0;
         maxZ = 4.0;
         minZ = 0.0;
-        res = 0.05;
+        res = 0.2;
+
+		// std::cout << "maxX: " << maxX  << std::endl;
 
         // maxX = 4.0;
         // minX = 0.0;

diff --git a/launch/local_planner.launch b/launch/local_planner.launch
@@ -14,7 +14,7 @@
     <arg name="local_world_size_x"        default="5"/>
     <arg name="local_world_size_y"        default="5"/>
     <arg name="local_world_size_z"        default="2"/>
-    <arg name="resolution"          default="0.05"/>
+    <arg name="resolution"          default="0.2"/>
 
     <arg name="inflate_map"         default="true"/>
     <arg name="inflation_size"      default="$(arg resolution)"/>

diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp
@@ -35,6 +35,7 @@ namespace Planners
     // // JAC QUITAR setLocalWorldSize?
     void AlgorithmBase::setLocalWorldSize(const Vec3i &_worldSize,const double _resolution)
     {
+        // el resizeLocalWorld lo hace bien (sept-2024)
         discrete_world_.resizeLocalWorld(_worldSize, _resolution); // Hay un error y parece que proviene del getWorldIndex en el world.hpp porque toma los valores del world_x_size y x_y_size en lugar de los "local"
     }
 

diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp
@@ -488,13 +488,15 @@ class HeuristicLocalPlannerROS
         if(save_data_)
             ROS_INFO_STREAM("Saving path planning data results to " << data_folder_);
 
-        // JAC: This is not neccesary in local planner.
+        // JAC: This is not neccesary in local planner --> input_map_ = 0
+        // std::cout << "\timput_map" << input_map_ << std::endl; 
         if( input_map_ == 1 ){
             Planners::utils::configureWorldFromOccupancyWithCosts(occupancy_grid_, *algorithm_);
+            // ROS_INFO("CONFIGURED WORLD1");
         }else if( input_map_ == 2 ){
             Planners::utils::configureWorldFromPointCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>(cloud_), *algorithm_, resolution_);
             // Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_); //JAC: Discomment
-            ROS_INFO("CONFIGURED WORLD2");
+            // ROS_INFO("CONFIGURED WORLD2");
 
         }
         //Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution)

diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp
@@ -153,8 +153,10 @@ namespace Planners
 
             auto world_size = _algorithm.getWorldSize();
             auto resolution = _algorithm.getWorldResolution();
-            // std::cout << "world size: " << world_size.x  << std::endl;  //200
-            // std::cout << "resolution: " << resolution  << std::endl;  //0.05
+            // std::cout << "world size X: " << world_size.x  << std::endl;  //50
+            // std::cout << "world size Y: " << world_size.y  << std::endl;  //50
+            // std::cout << "world size Z: " << world_size.z  << std::endl;  //20
+            // std::cout << "resolution: " << resolution  << std::endl;  //0.2
 
             // JAC: 50-50 milliseconds --> CUDA
             // t0 = clock();