diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp index b97661f..184dd0a 100755 --- a/include/Grid3D/local_grid3d.hpp +++ b/include/Grid3D/local_grid3d.hpp @@ -62,10 +62,6 @@ class Local_Grid3d float m_resolution, m_oneDivRes; octomap::OcTree *m_octomap; - // 3D probabilistic grid cell - Planners::utils::gridCell *m_grid; - int m_gridSize, m_gridSizeX, m_gridSizeY, m_gridSizeZ; - int m_gridStepY, m_gridStepZ; // 3D point cloud representation of the map pcl::PointCloud::Ptr m_cloud, filter_cloud; @@ -86,6 +82,11 @@ class Local_Grid3d bool use_costmap_function; public: + // 3D probabilistic grid cell + Planners::utils::gridCell *m_grid; + int m_gridSize, m_gridSizeX, m_gridSizeY, m_gridSizeZ; + int m_gridStepY, m_gridStepZ; + // Local_Grid3d(): m_cloud(new pcl::PointCloud) Local_Grid3d(): m_cloud(new pcl::PointCloud) { @@ -266,8 +267,23 @@ class Local_Grid3d } } } + + // // Visualize the middle layer + // int midZ = m_gridSizeZ / 2; + // std::vector gridSlice(m_gridSizeX * m_gridSizeY); + + // for(int iy = 0; iy < m_gridSizeY; iy++) { + // for(int ix = 0; ix < m_gridSizeX; ix++) { + // int index = ix + iy * m_gridStepY + midZ * m_gridStepZ; + // gridSlice[iy * m_gridSizeX + ix] = m_grid[index].dist; + // } + // } + + // std::cout << "Stored grid: " << gridSlice << std::endl; + std::cout << "---!!!--- Exiting computeLocalGrid ---!!!---" << std::endl; + } // // JAC diff --git a/include/utils/world.hpp b/include/utils/world.hpp index 322575a..9613521 100755 --- a/include/utils/world.hpp +++ b/include/utils/world.hpp @@ -205,8 +205,10 @@ namespace utils */ bool isOccupied(const int _x, const int _y, const int _z) const { - if (!checkValid(_x, _y, _z)) + if (!checkValid(_x, _y, _z)){ + std::cout << "isOccupied failed because of checkValid" << std::endl; return true; + } if (discrete_world_vector_[getWorldIndex(_x, _y, _z)].occuppied) { diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp index 5242d7f..a83a2a6 100755 --- a/src/Planners/AStar.cpp +++ b/src/Planners/AStar.cpp @@ -211,8 +211,8 @@ void AStar::exploreNeighbours(Node* _current, const Vec3i &_target, node_by_posi } PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target, torch::jit::script::Module& loaded_sdf) { + std::cout << "ENTERED ASTAR FINDPATH" << std::endl; Node *current = nullptr; - bool solved{false}; discrete_world_.getNodePtr(_source)->parent = new Node(_source); @@ -222,12 +222,14 @@ PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target, torch::jit: utils::Clock main_timer; main_timer.tic(); + line_of_sight_checks_ = 0; node_by_cost& indexByCost = openSet_.get(); node_by_position& indexByWorldPosition = openSet_.get(); indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) { //Get the element at the start of the open set ordered by cost @@ -243,13 +245,13 @@ PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target, torch::jit: current->isInOpenList = false; current->isInClosedList = true; + #if defined(ROS) && defined(PUB_EXPLORED_NODES) publishROSDebugData(current, indexByCost, closedSet_); #endif exploreNeighbours(current, _target, indexByWorldPosition, loaded_sdf); } main_timer.toc(); - PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), solved, _source, line_of_sight_checks_); //Clear internal variables. This should be done diff --git a/src/Planners/LazyThetaStar.cpp b/src/Planners/LazyThetaStar.cpp index 62e2177..516b17f 100755 --- a/src/Planners/LazyThetaStar.cpp +++ b/src/Planners/LazyThetaStar.cpp @@ -49,6 +49,7 @@ namespace Planners PathData LazyThetaStar::findPath(const Vec3i &_source, const Vec3i &_target, torch::jit::script::Module& loaded_sdf) { + std::cout << "ENTERED LAZYTHETASTAR FINDPATH" << std::endl; Node *current = nullptr; bool solved{false}; diff --git a/src/Planners/ThetaStar.cpp b/src/Planners/ThetaStar.cpp index ca3be1c..67d8c30 100755 --- a/src/Planners/ThetaStar.cpp +++ b/src/Planners/ThetaStar.cpp @@ -64,8 +64,9 @@ namespace Planners if ( successor == nullptr || successor->isInClosedList || - successor->occuppied ) - continue; + successor->occuppied ) + continue; + if (! successor->isInOpenList ) { diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp index b041ba1..8ee4868 100755 --- a/src/ROS/local_planner_ros_node.cpp +++ b/src/ROS/local_planner_ros_node.cpp @@ -74,8 +74,8 @@ class HeuristicLocalPlannerROS // request_path_server_ = lnh_.advertiseService("request_path", &HeuristicLocalPlannerROS::requestPathService, this); // This is in planner_ros_node.cpp and the corresponding service defined. change_planner_server_ = lnh_.advertiseService("set_algorithm", &HeuristicLocalPlannerROS::setAlgorithm, this); - line_markers_pub_ = lnh_.advertise("path_line_markers", 1); - point_markers_pub_ = lnh_.advertise("path_points_markers", 1); + line_markers_pub_ = lnh_.advertise("local_path_line_markers", 1); + point_markers_pub_ = lnh_.advertise("local_path_points_markers", 1); cloud_test = lnh_.advertise >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloudm_grid[(m_local_grid3d_->m_gridSize-1)/2].dist > 0) + { + ROS_INFO("Starting point is FREE"); + std::cout << "Point index queried: " << (m_local_grid3d_->m_gridSize-1)/2 <<" | Value of dist: " << m_local_grid3d_->m_grid[(m_local_grid3d_->m_gridSize-1)/2].dist << std::endl; + } + else + { + ROS_INFO("Starting point is NOT FREE -> ABORTING"); + std::cout << "Point index queried: " << (m_local_grid3d_->m_gridSize-1)/2 <<" | Value of dist: " << m_local_grid3d_->m_grid[(m_local_grid3d_->m_gridSize-1)/2].dist << std::endl; + exit(EXIT_FAILURE); + } + + // 2. Calculate local goal from global goal -> Check furthest global goal in the local goal + Planners::utils::CoordinateList global_path_local; + + // 2.1 - Convert waypoints to local reference + + //Cálculo del origen del sistema local (a nivel de celda) + + int origen_local_x, origen_local_y, origen_local_z, drone_local_x, drone_local_y, drone_local_z; + drone_local_x = (m_local_grid3d_->m_gridSizeX - 1)/2; + drone_local_y = (m_local_grid3d_->m_gridSizeY - 1)/2; + drone_local_z = (m_local_grid3d_->m_gridSizeZ - 1)/2; + origen_local_x = round(drone_x_/resolution_) - drone_local_x; + origen_local_y = round(drone_y_/resolution_) - drone_local_y; + origen_local_z = round(drone_z_/resolution_) - drone_local_z; + + for (const auto& vec : global_path_) + { + Planners::utils::Vec3i newpoint; + newpoint.x = vec.x - origen_local_x; + newpoint.y = vec.y - origen_local_y; + newpoint.z = vec.z - origen_local_z; + + // Agregar el punto desplazado al nuevo vector + global_path_local.push_back(newpoint); + } + std::cout << "Global path (local reference): " << global_path_local << std::endl; + + // 2.2 - Find closest waypoint to the drone (which is supposed to be the "current" waypoint) + double min_dist = std::numeric_limits::infinity(); + int closest_index = -1; + + for (size_t i = 0; i < global_path_local.size(); ++i) + { + const Planners::utils::Vec3i& act_waypoint = global_path_local[i]; + + double dist_to_wayp = std::sqrt( + std::pow(act_waypoint.x - drone_local_x, 2) + + std::pow(act_waypoint.y - drone_local_y, 2) + + std::pow(act_waypoint.z - drone_local_z, 2) + ); + + if (dist_to_wayp < min_dist) + { + min_dist = dist_to_wayp; + closest_index = i; + } + } + std::cout << "Closest waypoint: " << global_path_local[closest_index] << std::endl; + + // 2.3 - Find furthest next waypoint that is still inside the local map (the drone will treat this waypoint as the local goal + bool points_in_range = true; + int it = closest_index; + Planners::utils::Vec3i local_goal; + + while (it <= (global_path_local.size() - 1) && points_in_range) + { + if (0 <= global_path_local[it].x && global_path_local[it].x < m_local_grid3d_->m_gridSizeX && + 0 <= global_path_local[it].y && global_path_local[it].y < m_local_grid3d_->m_gridSizeY && + 0 <= global_path_local[it].z && global_path_local[it].z < m_local_grid3d_->m_gridSizeZ) + { + it++; + } + else + { + local_goal.x = global_path_local[it - 1].x; + local_goal.y = global_path_local[it - 1].y; + local_goal.z = global_path_local[it - 1].z; + points_in_range = false; + } + + } + std::cout << "Local goal found: " << local_goal << std::endl; + + // 3 - Check if local goal accessible. If not, find closest point + + // 4 - Use path planner to find local waypoints + Planners::utils::Vec3i discrete_start, discrete_goal; + discrete_start.x = drone_local_x; + discrete_start.y = drone_local_y; + discrete_start.z = drone_local_z; + discrete_goal.x = local_goal.x; + discrete_goal.y = local_goal.y; + discrete_goal.z = local_goal.z; + + std::cout << "Discrete local start: " << discrete_start << " | Discrete local goal: " << discrete_goal << std::endl; + + if( algorithm_->detectCollision(discrete_start) ){ + std::cout << discrete_start << ": Start not valid" << std::endl; + } + if( algorithm_->detectCollision(discrete_goal) ){ + std::cout << discrete_goal << ": Goal not valid" << std::endl; + } + + + auto local_path_data = algorithm_->findPath(discrete_start, discrete_goal, loaded_sdf_); + std::cout << "LOCAL PATH CALCULATED SUCCESSFULLY" << std::endl; + Planners::utils::CoordinateList local_path = std::get(local_path_data["path"]); + std::cout << "Local path: " << local_path << std::endl; + + } bool setAlgorithm(heuristic_planners::SetAlgorithmRequest &_req, heuristic_planners::SetAlgorithmResponse &rep){ @@ -342,9 +461,9 @@ class HeuristicLocalPlannerROS local_world_size_meters.y=ws_y; local_world_size_meters.z=ws_z; - local_world_size_.x = std::floor((2*ws_x) / resolution_); - local_world_size_.y = std::floor((2*ws_y) / resolution_); - local_world_size_.z = std::floor((2*ws_z) / resolution_); + local_world_size_.x = std::floor((2*ws_x) / resolution_) + 1; + local_world_size_.y = std::floor((2*ws_y) / resolution_) + 1; + local_world_size_.z = std::floor((2*ws_z) / resolution_) + 1; lnh_.param("use3d", use3d_, (bool)true);