diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp index 955629c..0c98b42 100644 --- 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(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 index d736cc5..4a16298 100644 --- a/launch/local_planner.launch +++ b/launch/local_planner.launch @@ -14,7 +14,7 @@ - + diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp index 3287db1..f080bc9 100644 --- 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 index cb34f60..826423a 100644 --- 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>(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 index b583d33..5dc387e 100644 --- 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();