Skip to content

Commit

Permalink
Configured computeLocalGrid and corrected grid size calculation
Browse files Browse the repository at this point in the history
  • Loading branch information
@guillermogilg99
guillermogilg99 committed Oct 8, 2024
1 parent f2d696d commit 56ec0c2
Show file tree
Hide file tree
Showing 5 changed files with 286 additions and 163 deletions.
317 changes: 199 additions & 118 deletions include/Grid3D/local_grid3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,11 @@

#include "utils/utils.hpp"

#include <torch/torch.h>
#include <torch/script.h>
#include <ros/ros.h>


#include <ctime>
#include <sstream> //for std::ostringstream
#include <fstream>
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -196,119 +201,188 @@ class Local_Grid3d
return m_grid[index].dist;
}

// JAC
void computeLocalGrid(const pcl::PointCloud<pcl::PointXYZ>::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<std::vector<float>> 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<m_gridSizeZ; iz++)
{
for(int iy=0; iy<m_gridSizeY; iy++)
{
for(int ix=0; ix<m_gridSizeX; ix++)
{
float xc = drone_x + (ix - (m_gridSizeX -1) / 2) * m_resolution;
float yc = drone_y + (iy - (m_gridSizeY -1) / 2) * m_resolution;
float zc = drone_z + (iz - (m_gridSizeZ -1) / 2) * m_resolution;

// 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;
coordinates_vector.push_back({xc, yc, zc});
}
}
}

// 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<int> 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<int> pointIdxNKNSearch(1);
std::vector<float> 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<float> 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<float> 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<float> 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<long>(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<double, std::milli> 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<m_gridSizeZ; iz++)
{
for(int iy=0; iy<m_gridSizeY; iy++)
{
for(int ix=0; ix<m_gridSizeX; ix++)
{
searchPoint.x = ix*m_resolution;
searchPoint.y = iy*m_resolution;
searchPoint.z = iz*m_resolution;
index = ix + iy*m_gridStepY + iz*m_gridStepZ;
++count;
// percent = count/size *100.0;
// ROS_INFO_THROTTLE(0.5,"Progress: %lf %%", percent);

// if(percent > 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<float>();
}
}
}
// 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<pcl::PointXYZ>::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<int> 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<int> pointIdxNKNSearch(1);
// std::vector<float> 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<m_gridSizeZ; iz++)
// {
// for(int iy=0; iy<m_gridSizeY; iy++)
// {
// for(int ix=0; ix<m_gridSizeX; ix++)
// {
// searchPoint.x = ix*m_resolution;
// searchPoint.y = iy*m_resolution;
// searchPoint.z = iz*m_resolution;
// index = ix + iy*m_gridStepY + iz*m_gridStepZ;
// ++count;
// // percent = count/size *100.0;
// // ROS_INFO_THROTTLE(0.5,"Progress: %lf %%", percent);

// // if(percent > 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<Planners::utils::Vec3i, double> getClosestObstacle(const Planners::utils::Vec3i& _coords){

Expand Down Expand Up @@ -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;

Expand Down
4 changes: 3 additions & 1 deletion include/utils/ros/ROSInterfaces.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,9 @@ namespace Planners
* @return true
* @return false
*/
bool configureLocalWorldCosts(const pcl::PointCloud<pcl::PointXYZ>::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<pcl::PointXYZ>::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm);

}
}
Expand Down
17 changes: 15 additions & 2 deletions include/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading

0 comments on commit 56ec0c2

Please sign in to comment.