diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp index 955629c..835a1b1 100755 --- a/include/Grid3D/local_grid3d.hpp +++ b/include/Grid3D/local_grid3d.hpp @@ -28,6 +28,11 @@ #include "utils/utils.hpp" +#include +#include +#include + + #include #include //for std::ostringstream #include @@ -116,7 +121,7 @@ class Local_Grid3d // configureParameters(); if(configureParameters()) { - computeLocalGrid(m_cloud); + //computeLocalGrid(m_cloud); // Build the msg with a slice of the grid if needed if(m_gridSlice >= 0 && m_gridSlice <= m_maxZ) @@ -196,119 +201,188 @@ class Local_Grid3d return m_grid[index].dist; } - // JAC - void computeLocalGrid(const pcl::PointCloud::ConstPtr &_cloud) //void + // NEW VERSION -- GUILLERMO + + void computeLocalGrid(torch::jit::script::Module& loaded_sdf, float drone_x, float drone_y, float drone_z) { - // unsigned t0, t1, t2, t3, t4, t5; + printf("-- Executing computeLocalGrid --\n"); - //Publish percent variable - // std_msgs::Float32 percent_msg; - // percent_msg.data = 0; - // 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); - m_gridSizeZ = (int)(m_maxZ*m_oneDivRes); - m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; - m_gridStepY = m_gridSizeX; - m_gridStepZ = m_gridSizeX*m_gridSizeY; + // Build global positions vector + std::vector> coordinates_vector; + coordinates_vector.reserve(m_gridSizeX*m_gridSizeY*m_gridSizeZ); + std::cout << "Drone position: " << "x = " << drone_x << " | y = " << drone_y << " | z = " << drone_z << std::endl; + for(int iz=0; iz ind; - // pcl::removeNaNFromPointCloud(*_cloud, *filter_cloud, ind); // JAC: Falla aqui - // std::cout << "no. of pts in input=" << _cloud->size() << std::endl; //JAC: Confirmado que el pointcloud se pasa bien - // std::cout << "no. of pts in output="<< filter_cloud->size() << std::endl; - // t2 = clock(); - m_kdtree.setInputCloud(_cloud); //Less than 1 millisecond - // t3 = clock(); - // double time2 = (double(t3-t2)/CLOCKS_PER_SEC); - // std::cout << "Execution Time2: " << time2 << std::endl; - - // Compute the distance to the closest point of the grid - int index; - float dist; - // float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI)); - // float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev); - // pcl::PointXYZI searchPoint; - pcl::PointXYZ searchPoint; - std::vector pointIdxNKNSearch(1); - std::vector pointNKNSquaredDistance(1); - double count=0; - // double percent; - // double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; - // t4 = clock(); - // JAC: 2-3 seconds + // DEBUGGING: PRINT FIRST, MIDDLE AND LAST POINT + + std::vector first_point = coordinates_vector.front(); // O coordinates_vector[0] + std::cout << "Primer punto: (" << first_point[0] << ", " << first_point[1] << ", " << first_point[2] << ")\n"; + + std::vector last_point = coordinates_vector.back(); // O coordinates_vector[coordinates_vector.size() - 1] + std::cout << "Último punto: (" << last_point[0] << ", " << last_point[1] << ", " << last_point[2] << ")\n"; + + size_t middle_index = coordinates_vector.size() / 2; + std::vector middle_point = coordinates_vector[middle_index]; + std::cout << "Punto medio: (" << middle_point[0] << ", " << middle_point[1] << ", " << middle_point[2] << ")\n"; + + // Convert vector tu libtorch array and query the neural network + auto num_points = coordinates_vector.size(); + torch::Tensor coordinates_tensor = torch::zeros({static_cast(num_points), 3}, torch::kFloat); + for (size_t i = 0; i < num_points; ++i) { + coordinates_tensor[i][0] = coordinates_vector[i][0]; + coordinates_tensor[i][1] = coordinates_vector[i][1]; + coordinates_tensor[i][2] = coordinates_vector[i][2]; + } + + auto start = std::chrono::high_resolution_clock::now(); + torch::Tensor grid_output_tensor = loaded_sdf.forward({coordinates_tensor}).toTensor(); + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Points queried: " << num_points <<" | Time taken to query model: " << duration.count() << " ms" << std::endl; + + // Place the queried values into the m_grid distance data field for(int iz=0; iz percent_msg.data + 0.5){ - // percent_msg.data = percent; - // // percent_computed_pub_.publish(percent_msg); - // } - - // JAC Error:Define correctly size of local map - // JAC: AQUI PETA CUANDO NO SE RECIBE POINTCLOUD - if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) - { - dist = pointNKNSquaredDistance[0]; - m_grid[index].dist = dist; // dist in the discrete world - // if(!use_costmap_function){ - // m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2); - // }else{ - // double prob = 100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius))); - // // ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob); - // //JAC: Include the computation of prob considering the distance to the nearest voronoi edge. - // m_grid[index].prob = prob; - // } - } - else - { - m_grid[index].dist = -1.0; - // m_grid[index].prob = 0.0; - } - + int index = ix + iy*m_gridStepY + iz*m_gridStepZ; + m_grid[index].dist = grid_output_tensor[index].item(); } } } - // t5 = clock(); - // double time3 = (double(t5-t4)/CLOCKS_PER_SEC); - // std::cout << "Execution Time3: " << time3 << std::endl; + std::cout << "---!!!--- Exiting computeLocalGrid ---!!!---" << std::endl; - // percent_msg.data = 100; - // percent_computed_pub_.publish(percent_msg); } + + // // JAC + // void computeLocalGrid(const pcl::PointCloud::ConstPtr &_cloud) //void + // { + // // unsigned t0, t1, t2, t3, t4, t5; + + // //Publish percent variable + // // std_msgs::Float32 percent_msg; + // // percent_msg.data = 0; + + // // 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); + // m_gridSizeZ = (int)(m_maxZ*m_oneDivRes); + // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // m_gridStepY = m_gridSizeX; + // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // // JAC: Pongo los mismo datos que en global planner y no funciona + // // m_gridSizeX = 341; + // // m_gridSizeY = 241; + // // m_gridSizeZ = 101; + // // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // // m_gridStepY = m_gridSizeX; + // // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // // JAC: Alloc the 3D local Grid from input parameters. These parameters should be defined once at the beginning. + // // release previously loaded data + // if(m_grid != NULL) + // delete []m_grid; + // // t0 = clock(); + // m_grid = new Planners::utils::gridCell[m_gridSize]; // 2-3 milliseconds + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + + // // std::cout << "_maxX: " << m_maxX << std::endl; + // // std::cout << "_gridSizeX: " << m_gridSizeX << std::endl; + + // // Setup kdtree + // // std::vector ind; + // // pcl::removeNaNFromPointCloud(*_cloud, *filter_cloud, ind); // JAC: Falla aqui + // // std::cout << "no. of pts in input=" << _cloud->size() << std::endl; //JAC: Confirmado que el pointcloud se pasa bien + // // std::cout << "no. of pts in output="<< filter_cloud->size() << std::endl; + // // t2 = clock(); + // m_kdtree.setInputCloud(_cloud); //Less than 1 millisecond + // // t3 = clock(); + // // double time2 = (double(t3-t2)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time2: " << time2 << std::endl; + + // // Compute the distance to the closest point of the grid + // int index; + // float dist; + // // float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI)); + // // float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev); + // // pcl::PointXYZI searchPoint; + // pcl::PointXYZ searchPoint; + // std::vector pointIdxNKNSearch(1); + // std::vector pointNKNSquaredDistance(1); + // double count=0; + // // double percent; + // // double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // // t4 = clock(); + // // JAC: 2-3 seconds + // for(int iz=0; iz percent_msg.data + 0.5){ + // // percent_msg.data = percent; + // // // percent_computed_pub_.publish(percent_msg); + // // } + + // // JAC Error:Define correctly size of local map + // // JAC: AQUI PETA CUANDO NO SE RECIBE POINTCLOUD + // if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) + // { + // dist = pointNKNSquaredDistance[0]; + // m_grid[index].dist = dist; // dist in the discrete world + // // if(!use_costmap_function){ + // // m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2); + // // }else{ + // // double prob = 100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius))); + // // // ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob); + // // //JAC: Include the computation of prob considering the distance to the nearest voronoi edge. + // // m_grid[index].prob = prob; + // // } + // } + // else + // { + // m_grid[index].dist = -1.0; + // // m_grid[index].prob = 0.0; + // } + + // } + // } + // } + // // t5 = clock(); + // // double time3 = (double(t5-t4)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time3: " << time3 << std::endl; + + // // percent_msg.data = 100; + // // percent_computed_pub_.publish(percent_msg); + // } std::pair getClosestObstacle(const Planners::utils::Vec3i& _coords){ @@ -360,30 +434,37 @@ class Local_Grid3d delete []m_grid; // 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; + double maxX, maxY, maxZ, res; - maxX = 10.0; - minX = 0.0; - maxY = 10.0; - minY = 0.0; - maxZ = 4.0; - minZ = 0.0; - res = 0.05; - - // maxX = 4.0; - // minX = 0.0; - // maxY = 4.0; - // minY = 0.0; - // maxZ = 2.0; - // minZ = 0.0; - // res = 0.1; - - m_maxX = (float)(maxX-minX); - m_maxY = (float)(maxY-minY); - m_maxZ = (float)(maxZ-minZ); + maxX = 1.0; // distancia a cada lado del dron (en x) + maxY = 1.0; // distancia a cada lado del dron (en y) + maxZ = 1.0; // distancia a cada lado del dron (en z) + res = 0.2; + + + m_maxX = (float)(maxX); + m_maxY = (float)(maxY); + m_maxZ = (float)(maxZ); m_resolution = (float)res; m_oneDivRes = 1.0/m_resolution; + // Grid parameters + m_gridSizeX = (int)(m_maxX*m_oneDivRes)*2 + 1; // drone is in the center + m_gridSizeY = (int)(m_maxY*m_oneDivRes)*2 + 1; + m_gridSizeZ = (int)(m_maxZ*m_oneDivRes)*2 + 1; + m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + m_gridStepY = m_gridSizeX; + m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // Delete previous data (if any) and create new grid + // if(m_grid != NULL) + // { + // printf("Deleting %p\n", m_grid); + // delete []m_grid; + // } + m_grid = new Planners::utils::gridCell[m_gridSize]; + printf("Creating %p, of size %d\n", m_grid, m_gridSize); + // float workspace_x, workspace_y, workspace_z; diff --git a/include/utils/ros/ROSInterfaces.hpp b/include/utils/ros/ROSInterfaces.hpp index 6b062ab..ecb9e24 100755 --- a/include/utils/ros/ROSInterfaces.hpp +++ b/include/utils/ros/ROSInterfaces.hpp @@ -149,7 +149,9 @@ namespace Planners * @return true * @return false */ - bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm); + bool configureLocalWorldCosts(Local_Grid3d &_grid, AlgorithmBase &_algorithm, float drone_x, float drone_y, float drone_z, torch::jit::script::Module& loaded_sdf); + + //bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm); } } diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp index 1c5e031..f521b25 100755 --- a/include/utils/utils.hpp +++ b/include/utils/utils.hpp @@ -58,8 +58,21 @@ namespace Planners */ struct gridCell { - float dist{0}; - float prob{0}; + gridCell(void) { + dist = 0; + prob = 0; + } + gridCell(const gridCell& other) { + dist = other.dist; + prob = other.prob; + } + gridCell& operator=(const gridCell& other) { + dist = other.dist; + prob = other.prob; + return *this; + } + float dist; + float prob; }; /** * @brief diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index 312f59f..b041ba1 100755 --- a/src/ROS/local_planner_ros_node.cpp +++ b/src/ROS/local_planner_ros_node.cpp @@ -145,7 +145,24 @@ class HeuristicLocalPlannerROS void localtimedCallback(const ros::TimerEvent& event) { - std::cout << "-----TIMED CALLBACK------" << std::endl; + printf("-----TIMED CALLBACK------\n"); + + // Only perform local planning if global path was received + if(globalPathReceived_ == 1){ + + // 1. Update Neural Network State (if new state available) + if(networkReceivedFlag_ == 1) + { + printf("Importing new neural network state\n"); + loaded_sdf_ = torch::jit::load("/home/ros/exchange/weight_data/model.pt", c10::kCPU); + networkReceivedFlag_ = 0; + } + + // 2. Update local map with the neural network information around the drone + Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_, drone_x_, drone_y_, drone_z_, loaded_sdf_); + printf("-- Exiting callback --\n"); + + } } // From lazy_theta_star_planners @@ -234,7 +251,7 @@ class HeuristicLocalPlannerROS algorithm_->publishLocalOccupationMarkersMap(); // Configure the distance grid and the cost grid - Planners::utils::configureLocalWorldCosts(boost::make_shared>(local_cloud_), *m_local_grid3d_, *algorithm_); // JAC: Is this correctly generated? + //!!!!!Planners::utils::configureLocalWorldCosts(boost::make_shared>(local_cloud_), *m_local_grid3d_, *algorithm_); // JAC: Is this correctly generated? // ROS_INFO("Published occupation marker local map"); @@ -433,6 +450,7 @@ class HeuristicLocalPlannerROS // Init internal variables: TF transform m_tfCache = false; + ROS_INFO("CONFIGURE ALGORITHM COMPLETED"); } void configureHeuristic(const std::string &_heuristic){ @@ -608,7 +626,8 @@ class HeuristicLocalPlannerROS Planners::utils::CoordinateList global_path_; int globalPathReceived_; - //Network Flag and subscriber + //Network, network flag, subscriber + torch::jit::script::Module loaded_sdf_; int networkReceivedFlag_; ros::Subscriber network_update_sub_; diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp index 4ae0d4b..2d34db8 100755 --- a/src/utils/ros/ROSInterfaces.cpp +++ b/src/utils/ros/ROSInterfaces.cpp @@ -121,54 +121,62 @@ namespace Planners float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); // std::cout << "Cost: " << cost << std::endl; _algorithm.configureCellCost({i, j, k}, cost); - } } + } } return true; } - - bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm) + bool configureLocalWorldCosts(Local_Grid3d &_grid, AlgorithmBase &_algorithm, float drone_x, float drone_y, float drone_z, torch::jit::script::Module& loaded_sdf) { - // JAC: Generate the local grid - // std::cout << "no. of pts=" << _points->size() << std::endl; - // unsigned t0, t1; - // t0 = clock(); - _grid.computeLocalGrid(_points); - // t1 = clock(); - // double time = (double(t1-t0)/CLOCKS_PER_SEC); - // std::cout << "Execution Time: " << time << std::endl; - - auto world_size = _algorithm.getWorldSize(); - auto resolution = _algorithm.getWorldResolution(); - // 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(); - for (int i = 0; i < world_size.x; i++) - { - for (int j = 0; j < world_size.y; j++) - { - for (int k = 0; k < world_size.z; k++) - { - //JAC: Precision - // auto cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); - float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); - // if (cost > 285.65) // JAC: cost is given in the discrete_world. - // std::cout << "Cost: " << cost << std::endl; - _algorithm.configureCellCost({i, j, k}, cost); - } - } - } - // t1 = clock(); - // double time = (double(t1-t0)/CLOCKS_PER_SEC); - // std::cout << "Execution Time: " << time << std::endl; + // Computar el grid local + _grid.computeLocalGrid(loaded_sdf, drone_x, drone_y, drone_z); + printf("-- Exiting computeLocalWorldCosts --\n"); return true; } + + // bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm) + // { + // // JAC: Generate the local grid + // // std::cout << "no. of pts=" << _points->size() << std::endl; + // // unsigned t0, t1; + // // t0 = clock(); + // _grid.computeLocalGrid(_points); + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + + // auto world_size = _algorithm.getWorldSize(); + // auto resolution = _algorithm.getWorldResolution(); + // // 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(); + // for (int i = 0; i < world_size.x; i++) + // { + // for (int j = 0; j < world_size.y; j++) + // { + // for (int k = 0; k < world_size.z; k++) + // { + // //JAC: Precision + // // auto cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + // float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + // // if (cost > 285.65) // JAC: cost is given in the discrete_world. + // // std::cout << "Cost: " << cost << std::endl; + // _algorithm.configureCellCost({i, j, k}, cost); + // } + // } + // } + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + // return true; + // } + } //ns utils } //ns planners \ No newline at end of file