diff --git a/CMakeLists.txt b/CMakeLists.txt index a46ece6..edb0902 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,7 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 + LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF # CATKIN_DEPENDS std_msgs visualization_msgs geometry_msgs nav_msgs roscpp message_runtime costmap_2d # DEPENDS system_lib @@ -154,6 +154,8 @@ add_library(AStar src/Planners/AStar.cpp ${${PROJECT_NAME}_UTILS_SOURCES} ) +add_library(AStar_Gradient src/Planners/AStar_Gradient.cpp ) +add_library(AStar_EDF src/Planners/AStar_EDF.cpp ) add_library(AStarM1 src/Planners/AStarM1.cpp ) add_library(AStarM2 src/Planners/AStarM2.cpp ) add_library(ThetaStar src/Planners/ThetaStar.cpp ) @@ -163,10 +165,12 @@ add_library(LazyThetaStar src/Planners/LazyThetaStar.cpp ) add_library(LazyThetaStarM1 src/Planners/LazyThetaStarM1.cpp ) add_library(LazyThetaStarM1Mod src/Planners/LazyThetaStarM1Mod.cpp ) add_library(LazyThetaStarM2 src/Planners/LazyThetaStarM2.cpp ) +add_library(LazyThetaStar_Gradient src/Planners/LazyThetaStar_Gradient.cpp ) +add_library(LazyThetaStar_EDF src/Planners/LazyThetaStar_EDF.cpp ) -list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2) +list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStar_Gradient AStar_EDF AStarM1 AStarM2 ThetaStar ThetaStarM1 ThetaStarM2 LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStar_Gradient LazyThetaStar_EDF) target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES}) diff --git a/config/costmap_for_rand_maps.yaml b/config/costmap_for_rand_maps.yaml index b65149d..b3472a6 100644 --- a/config/costmap_for_rand_maps.yaml +++ b/config/costmap_for_rand_maps.yaml @@ -18,7 +18,7 @@ costmap: cost_scaling_factor: 2 enabled: true inflate_unknown: false - inflation_radius: 3.0 + _radius: 3.0 static_layer: unknown_cost_value: -1 diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp index cdf8da0..c28bc44 100644 --- a/include/Grid3D/grid3d.hpp +++ b/include/Grid3D/grid3d.hpp @@ -366,7 +366,7 @@ bool loadSemanticGrid() { m_oneDivRes = 1.0/m_resolution; ROS_INFO("Map size:\n"); ROS_INFO("\tx: %.2f to %.2f", minX, maxX); - ROS_INFO("\ty: %.2f to %.2f", minZ, maxZ); + ROS_INFO("\ty: %.2f to %.2f", minY, maxY); ROS_INFO("\tz: %.2f to %.2f", minZ, maxZ); ROS_INFO("\tRes: %.2f" , m_resolution ); @@ -490,13 +490,15 @@ bool loadSemanticGrid() { // Get map parameters double minX, minY, minZ; m_octomap->getMetricMin(minX, minY, minZ); - + // Load the octomap in PCL for easy nearest neighborhood computation // The point-cloud is shifted to have (0,0,0) as min values int i = 0; m_cloud->width = m_octomap->size(); + // std::cout << "dim1: " << m_cloud->width << std::endl; m_cloud->height = 1; m_cloud->points.resize(static_cast(m_cloud->width) * m_cloud->height); + std::cout << "dim1: " << m_cloud->width << std::endl; for(octomap::OcTree::leaf_iterator it = m_octomap->begin_leafs(), end = m_octomap->end_leafs(); it != end; ++it) { if(it != NULL && m_octomap->isNodeOccupied(*it)) @@ -510,6 +512,7 @@ bool loadSemanticGrid() { } m_cloud->width = i; m_cloud->points.resize(i); + // std::cout << "dim2: " << m_cloud->width << std::endl; // Create the point cloud msg for publication pcl::toROSMsg(*m_cloud, m_pcMsg); @@ -536,8 +539,8 @@ bool loadSemanticGrid() { // 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); + // float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI)); + // float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev); pcl::PointXYZI searchPoint; std::vector pointIdxNKNSearch(1); std::vector pointNKNSquaredDistance(1); @@ -561,21 +564,34 @@ bool loadSemanticGrid() { percent_msg.data = percent; // percent_computed_pub_.publish(percent_msg); } - if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { - dist = pointNKNSquaredDistance[0]; - m_grid[index].dist = dist; - 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; - - //En sí esto no va aquí, pero no entiendo muy bien donde meterlo - m_grid[index].semantic = semanticGrid[index]; + // JAC: Always square distance has been considered!!!!!! + // dist = pointNKNSquaredDistance[0]; + dist = sqrt(pointNKNSquaredDistance[0]); + + // std::cout << "dist: " << dist << std::endl; + if (dist < 200){ + m_grid[index].dist = dist; + if(!use_costmap_function){ + // m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2); + m_grid[index].prob = dist; + }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; + // m_grid[index].prob = 100-dist; + m_grid[index].prob = dist; + m_grid[index].semantic = semanticGrid[index]; + } + } + else{ + // std::cout << "dist: " << dist << std::endl; + // m_grid[index].dist = -1.0; + m_grid[index].dist = 90.0; + m_grid[index].prob = 0.0; + } } else diff --git a/include/Grid3D/grid3d.zip b/include/Grid3D/grid3d.zip new file mode 100644 index 0000000..fe60b59 Binary files /dev/null and b/include/Grid3D/grid3d.zip differ diff --git a/include/Grid3D/grid3d_Santi.hpp b/include/Grid3D/grid3d_Santi.hpp new file mode 100755 index 0000000..23d6b60 --- /dev/null +++ b/include/Grid3D/grid3d_Santi.hpp @@ -0,0 +1,665 @@ + #ifndef __GRID3D_HPP__ +#define __GRID3D_HPP__ + +/** + * @file prob_map.cpp + * @brief This file includes the ROS node implementation. + * @author Francisco J. Perez Grau and Fernando Caballero + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +// PCL +#include +#include +#include +#include + +#include "utils/utils.hpp" + +#include //for std::ostringstream +#include +#include //for std::setw, std::hex, and std::setfill +#include //for all other OpenSSL function calls +#include //for SHA512_DIGEST_LENGTH +// #include "utils/ros/ROSInterfaces.hpp" + +// #ifdef BUILD_VORONOI +// #include "voro++-0.4.6/src/voro++.hh" +// #endif + + +class Grid3d +{ +private: + + // Ros parameters + ros::NodeHandle m_nh; + bool m_publishPc; + std::string m_mapPath, m_nodeName; + std::string m_globalFrameId; + float m_sensorDev, m_gridSlice; + double m_publishPointCloudRate, m_publishGridSliceRate; + + // Octomap parameters + float m_maxX, m_maxY, m_maxZ; + 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 clound representation of the map + pcl::PointCloud::Ptr m_cloud; + pcl::KdTreeFLANN m_kdtree; + + // Visualization of the map as pointcloud + sensor_msgs::PointCloud2 m_pcMsg; + ros::Publisher m_pcPub, percent_computed_pub_; + ros::Timer mapTimer; + + // Semantic Grid + + std_msgs::Int32MultiArray semantic_grid; + std::vector>> semanticGrid; + + // Visualization of a grid slice as 2D grid map msg + nav_msgs::OccupancyGrid m_gridSliceMsg; + ros::Publisher m_gridSlicePub; + ros::Timer gridTimer; + + //Parameters added to allow a new exp function to test different gridmaps + double cost_scaling_factor, robot_radius; + bool use_costmap_function; + +public: + Grid3d(): m_cloud(new pcl::PointCloud) + { + // Load paraeters + double value; + ros::NodeHandle lnh("~"); + lnh.param("name", m_nodeName, std::string("grid3d")); + if(!lnh.getParam("global_frame_id", m_globalFrameId)) + m_globalFrameId = "map"; + if(!lnh.getParam("map_path", m_mapPath)) + m_mapPath = "map.ot"; + if(!lnh.getParam("publish_point_cloud", m_publishPc)) + m_publishPc = true; + if(!lnh.getParam("publish_point_cloud_rate", m_publishPointCloudRate)) + m_publishPointCloudRate = 0.2; + if(!lnh.getParam("publish_grid_slice", value)) + value = 1.0; + if(!lnh.getParam("publish_grid_slice_rate", m_publishGridSliceRate)) + m_publishGridSliceRate = 0.2; + m_gridSlice = (float)value; + if(!lnh.getParam("sensor_dev", value)) + value = 0.2; + m_sensorDev = (float)value; + + lnh.param("cost_scaling_factor", cost_scaling_factor, 0.8); //0.8 + lnh.param("robot_radius", robot_radius, 0.4); //0.4 + lnh.param("use_costmap_function", use_costmap_function, (bool)true); + + // Load octomap + m_octomap = NULL; + m_grid = NULL; + + if(loadSemanticGrid()){ + std::cout<< "Grid Loaded\n"; + } + if(loadOctomap(m_mapPath)) + { + // Compute the point-cloud associated to the ocotmap + computePointCloud(); + + // Try to load tha associated grid-map from file + std::string path; + if(m_mapPath.compare(m_mapPath.length()-3, 3, ".bt") == 0) + path = m_mapPath.substr(0,m_mapPath.find(".bt"))+".gridm"; + if(m_mapPath.compare(m_mapPath.length()-3, 3, ".ot") == 0) + path = m_mapPath.substr(0,m_mapPath.find(".ot"))+".gridm"; + if(!loadGrid(path)) + { + // Compute the gridMap using kdtree search over the point-cloud + std::cout << "Computing 3D occupancy grid. This will take some time..." << std::endl; + computeGrid(); + std::cout << "\tdone!" << std::endl; + + // Save grid on file + if(saveGrid(path)) + std::cout << "Grid map successfully saved on " << path << std::endl; + } + + // Build the msg with a slice of the grid if needed + if(m_gridSlice >= 0 && m_gridSlice <= m_maxZ) + { + buildGridSliceMsg(m_gridSlice); + m_gridSlicePub = m_nh.advertise(m_nodeName+"/grid_slice", 1, true); + gridTimer = m_nh.createTimer(ros::Duration(1.0/m_publishGridSliceRate), &Grid3d::publishGridSliceTimer, this); + } + + // Setup point-cloud publisher + if(m_publishPc) + { + m_pcPub = m_nh.advertise(m_nodeName+"/map_point_cloud", 1, true); + mapTimer = m_nh.createTimer(ros::Duration(1.0/m_publishPointCloudRate), &Grid3d::publishMapPointCloudTimer, this); + } + percent_computed_pub_ = m_nh.advertise(m_nodeName+"/percent_computed", 1, false); + } + } + + ~Grid3d(void) + { + if(m_octomap != NULL) + delete m_octomap; + if(m_grid != NULL) + delete []m_grid; + } + bool setCostParams(const double &_cost_scaling_factor, const double &_robot_radius){ + + cost_scaling_factor = std::fabs(_cost_scaling_factor); + robot_radius = std::fabs(_robot_radius); + + return true; + } + + void setGridSliceHeight(const double _height){ + if(_height < m_maxZ && _height > 0.0 ){ + m_gridSlice = _height; + buildGridSliceMsg(m_gridSlice); + } + } + float computeCloudWeight(std::vector &points) + { + float weight = 0.; + int n = 0; + + for(long unsigned int i=0; i= 0.0 && p.y >= 0.0 && p.z >= 0.0 && p.x < m_maxX && p.y < m_maxY && p.z < m_maxZ) + { + int index = point2grid(p.x, p.y, p.z); + weight += m_grid[index].prob; + n++; + } + } + + if(n > 10) + return weight/n; + else + return 0; + } + + void publishMapPointCloud(void) + { + m_pcMsg.header.stamp = ros::Time::now(); + m_pcPub.publish(m_pcMsg); + } + + void publishGridSlice(void) + { + m_gridSliceMsg.header.stamp = ros::Time::now(); + m_gridSlicePub.publish(m_gridSliceMsg); + } + + bool isIntoMap(float x, float y, float z) + { + return (x >= 0.0 && y >= 0.0 && z >= 0.0 && x < m_maxX && y < m_maxY && z < m_maxZ); + } + // int getCellCost(const float &_x, const float &_y, const float &_z){ + // JAC Precision + float getCellCost(const float &_x, const float &_y, const float &_z){ + + if( !isIntoMap(_x, _y, _z) ) + return 0; + + int index = point2grid(_x, _y, _z); + + return m_grid[index].prob; + } + + std::pair getClosestObstacle(const Planners::utils::Vec3i& _coords){ + + pcl::PointXYZI searchPoint; + + searchPoint.x = _coords.x * m_resolution; + searchPoint.y = _coords.y * m_resolution; + searchPoint.z = _coords.z * m_resolution; + + int k = 1; + m_kdtree.setInputCloud(m_cloud); + std::vector pointIdxNKNSearch(k); + std::vector pointNKNSquaredDistance(k); + + if(m_kdtree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){ + + Planners::utils::Vec3i result; + + result.x = std::round(m_cloud->points[pointIdxNKNSearch[0]].x / m_resolution); + result.y = std::round(m_cloud->points[pointIdxNKNSearch[0]].y / m_resolution); + result.z = std::round(m_cloud->points[pointIdxNKNSearch[0]].z / m_resolution); + + return std::make_pair(result, std::sqrt(pointNKNSquaredDistance[k-1])); + }else{ + return std::make_pair(Planners::utils::Vec3i{}, std::numeric_limits::max()); + } + } + +protected: + + void semanticGridCallback(const std_msgs::Int32MultiArray msg) + { + int numRows = msg.layout.dim[0].size; + int numCols = msg.layout.dim[1].size; + int numLayers = msg.layout.dim[2].size; + std::vector data = msg.data; + std::cout << "CALLBACK" << std::endl; + semanticGrid.resize(numRows, std::vector>(numCols, std::vector(numLayers, 0))); + + for (int i = 0; i < numRows; ++i) + { + for (int j = 0; j < numCols; ++j) + { + for (int k = 0; k < numLayers; ++k) + { + semanticGrid[i][j][k] = data[i * numCols * numLayers + j * numLayers + k]; + } + } + } + // std::cout << "semanticGrid: " << semanticGrid[0][0][0] << std::endl; + } +bool loadSemanticGrid() { + ros::NodeHandle n; + + // Define a ROS service client + ros::ServiceClient client = n.serviceClient("get_semantic_grid"); + + IfcGrid::GetSemanticGrid srv; + + // Wait for the service to become available + if (!client.waitForExistence(ros::Duration(40.0))) { + ROS_ERROR("Service get_semantic_grid unavailable"); + return false; + } + + // Call the service + if (client.call(srv)) { + // Process the response + // The shape and semantic grid are available in srv.response.shape and srv.response.semantic_grid + std::vector shape = srv.response.shape; + std::vector int_shape(shape.begin(), shape.end()); + + std::vector semantic_grid = srv.response.semantic_grid; + + // Reshape the semantic_grid according to the received shape + std::vector>> three_d_grid; + int k = 0; + for (int i = 0; i < int_shape[0]; i++) { + std::vector> xy_plane; + for (int j = 0; j < int_shape[1]; j++) { + std::vector row; + for (int z = 0; z < int_shape[2]; z++) { + row.push_back(semantic_grid[k++]); + } + xy_plane.push_back(row); + } + three_d_grid.push_back(xy_plane); + } + + // Now three_d_grid is the desired 3D grid + semanticGrid = three_d_grid; + std::cout << "CAPA SEMANTICA" << std::endl; + + return !semanticGrid.empty(); + } else { + ROS_ERROR("Failed to call service get_semantic_grid"); + return false; + } +} + + + + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" + void publishMapPointCloudTimer(const ros::TimerEvent& event) + { + publishMapPointCloud(); + } + + void publishGridSliceTimer(const ros::TimerEvent& event) + { + publishGridSlice(); + } +#pragma GCC diagnostic pop + + bool loadOctomap(std::string &path) + { + // release previously loaded data + if(m_octomap != NULL) + delete m_octomap; + if(m_grid != NULL) + delete []m_grid; + + // Load octomap + octomap::AbstractOcTree *tree; + if(path.length() > 3 && (path.compare(path.length()-3, 3, ".bt") == 0)) + { + octomap::OcTree* binaryTree = new octomap::OcTree(0.1); + if (binaryTree->readBinary(path) && binaryTree->size() > 1) + tree = binaryTree; + else + return false; + } + else if(path.length() > 3 && (path.compare(path.length()-3, 3, ".ot") == 0)) + { + tree = octomap::AbstractOcTree::read(path); + if(!tree) + return false; + } + else + return false; + + /* + // Load octomap + octomap::AbstractOcTree *tree = octomap::AbstractOcTree::read(path); + if(!tree) + return false;*/ + m_octomap = dynamic_cast(tree); + std::cout << "Octomap loaded" << std::endl; + + // Check loading and alloc momery for the grid + if(m_octomap == NULL) + { + std::cout << "Error: NULL octomap!!" << std::endl; + return false; + } + + // Get map parameters + double minX, minY, minZ, maxX, maxY, maxZ, res; + m_octomap->getMetricMin(minX, minY, minZ); + m_octomap->getMetricMax(maxX, maxY, maxZ); + res = m_octomap->getResolution(); + m_maxX = (float)(maxX-minX); + m_maxY = (float)(maxY-minY); + m_maxZ = (float)(maxZ-minZ); + m_resolution = (float)res; + m_oneDivRes = 1.0/m_resolution; + ROS_INFO("Map size:\n"); + ROS_INFO("\tx: %.2f to %.2f", minX, maxX); + ROS_INFO("\ty: %.2f to %.2f", minZ, maxZ); + ROS_INFO("\tz: %.2f to %.2f", minZ, maxZ); + ROS_INFO("\tRes: %.2f" , m_resolution ); + + return true; + } + std::string bytes_to_hex_string(const std::vector& bytes) + { + std::ostringstream stream; + for (uint8_t b : bytes) + { + stream << std::setw(2) << std::setfill('0') << std::hex << static_cast(b); + } + return stream.str(); + } + //perform the SHA3-512 hash + std::string sha3_512(const char * _input,int _size) + { + uint32_t digest_length = SHA512_DIGEST_LENGTH; + const EVP_MD* algorithm = EVP_sha3_512(); + uint8_t* digest = static_cast(OPENSSL_malloc(digest_length)); + EVP_MD_CTX* context = EVP_MD_CTX_new(); + EVP_DigestInit_ex(context, algorithm, nullptr); + EVP_DigestUpdate(context, _input, _size); + EVP_DigestFinal_ex(context, digest, &digest_length); + EVP_MD_CTX_destroy(context); + std::string output = bytes_to_hex_string(std::vector(digest, digest + digest_length)); + OPENSSL_free(digest); + return output; + } + bool saveGrid(std::string &fileName) + { + FILE *pf; + + // Open file + pf = fopen(fileName.c_str(), "wb"); + if(pf == NULL) + { + std::cout << "Error opening file " << fileName << " for writing" << std::endl; + return false; + } + + // Write grid general info + fwrite(&m_gridSize, sizeof(int), 1, pf); + fwrite(&m_gridSizeX, sizeof(int), 1, pf); + fwrite(&m_gridSizeY, sizeof(int), 1, pf); + fwrite(&m_gridSizeZ, sizeof(int), 1, pf); + fwrite(&m_sensorDev, sizeof(float), 1, pf); + + // Write grid cells + fwrite(m_grid, sizeof(Planners::utils::gridCell), m_gridSize, pf); + + auto sha_value = sha3_512((const char*)m_grid, m_gridSize); + std::cout << "Sha512 value: " << sha_value << std::endl; + std::ofstream sha_file; + sha_file.open(fileName+"sha", std::ofstream::trunc); + sha_file << sha_value; + sha_file.close(); + // Close file + fclose(pf); + + return true; + } + + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" + bool loadGrid(std::string &fileName) + { + FILE *pf; + + // Open file + pf = fopen(fileName.c_str(), "rb"); + if(pf == NULL) + { + std::cout << "Error opening file " << fileName << " for reading" << std::endl; + return false; + } + + // Write grid general info + fread(&m_gridSize, sizeof(int), 1, pf); + fread(&m_gridSizeX, sizeof(int), 1, pf); + fread(&m_gridSizeY, sizeof(int), 1, pf); + fread(&m_gridSizeZ, sizeof(int), 1, pf); + fread(&m_sensorDev, sizeof(float), 1, pf); + m_gridStepY = m_gridSizeX; + m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // Write grid cells + if(m_grid != NULL) + delete []m_grid; + m_grid = new Planners::utils::gridCell[m_gridSize]; + fread(m_grid, sizeof(Planners::utils::gridCell), m_gridSize, pf); + fclose(pf); + + std::ifstream input_sha; + auto sha_value = sha3_512((const char*)m_grid, m_gridSize); + ROS_INFO("[Grid3D] Calculated SHA512 value: %s", sha_value.c_str()); + + input_sha.open(fileName+"sha"); + std::string readed_sha; + if(input_sha.is_open()){ + getline(input_sha, readed_sha); + ROS_INFO("[Grid3D] Readed SHA512 from file: %s", readed_sha.c_str()); + input_sha.close(); + }else{ + ROS_ERROR("[Grid3D] Couldn't read SHA512 data of gridm file, recomputing grid"); + return false; + } + //Check that both are the same + if(readed_sha.compare(sha_value) != 0){ + ROS_ERROR("[Grid3D] Found different SHA values between for gridm!"); + return false; + } + + return true; + } +#pragma GCC diagnostic pop + + void computePointCloud(void) + { + // Get map parameters + double minX, minY, minZ; + m_octomap->getMetricMin(minX, minY, minZ); + + // Load the octomap in PCL for easy nearest neighborhood computation + // The point-cloud is shifted to have (0,0,0) as min values + int i = 0; + m_cloud->width = m_octomap->size(); + m_cloud->height = 1; + m_cloud->points.resize(static_cast(m_cloud->width) * m_cloud->height); + for(octomap::OcTree::leaf_iterator it = m_octomap->begin_leafs(), end = m_octomap->end_leafs(); it != end; ++it) + { + if(it != NULL && m_octomap->isNodeOccupied(*it)) + { + m_cloud->points[i].x = it.getX()-minX; + m_cloud->points[i].y = it.getY()-minY; + m_cloud->points[i].z = it.getZ()-minZ; + + i++; + } + } + m_cloud->width = i; + m_cloud->points.resize(i); + + // Create the point cloud msg for publication + pcl::toROSMsg(*m_cloud, m_pcMsg); + m_pcMsg.header.frame_id = m_globalFrameId; + } + + void computeGrid(void) + { + //Publish percent variable + std_msgs::Float32 percent_msg; + percent_msg.data = 0; + // Alloc the 3D 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; + m_grid = new Planners::utils::gridCell[m_gridSize]; + + // Setup kdtree + m_kdtree.setInputCloud(m_cloud); + + // 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; + std::vector pointIdxNKNSearch(1); + std::vector pointNKNSquaredDistance(1); + double count=0; + double percent; + double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; + for(int iz=0; iz percent_msg.data + 0.5){ + percent_msg.data = percent; + // percent_computed_pub_.publish(percent_msg); + } + if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) + { + dist = pointNKNSquaredDistance[0]; + m_grid[index].dist = dist; + 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; + } + + } + } + } + percent_msg.data = 100; + // percent_computed_pub_.publish(percent_msg); + } + + void buildGridSliceMsg(float z) + { + static int seq = 0; + + // Setup grid msg + m_gridSliceMsg.header.frame_id = m_globalFrameId; + m_gridSliceMsg.header.stamp = ros::Time::now(); + m_gridSliceMsg.header.seq = seq++; + m_gridSliceMsg.info.map_load_time = ros::Time::now(); + m_gridSliceMsg.info.resolution = m_resolution; + m_gridSliceMsg.info.width = m_gridSizeX; + m_gridSliceMsg.info.height = m_gridSizeY; + m_gridSliceMsg.info.origin.position.x = 0.0; + m_gridSliceMsg.info.origin.position.y = 0.0; + m_gridSliceMsg.info.origin.position.z = z; + m_gridSliceMsg.info.origin.orientation.x = 0.0; + m_gridSliceMsg.info.origin.orientation.y = 0.0; + m_gridSliceMsg.info.origin.orientation.z = 0.0; + m_gridSliceMsg.info.origin.orientation.w = 1.0; + m_gridSliceMsg.data.resize(static_cast(m_gridSizeX)*m_gridSizeY); + + // Extract max probability + int offset = (int)(z*m_oneDivRes)*m_gridSizeX*m_gridSizeY; + int end = offset + m_gridSizeX*m_gridSizeY; + float maxProb = -1.0; + for(int i=offset; i maxProb) + maxProb = m_grid[i].prob; + + // Copy data into grid msg and scale the probability to [0,100] + if(maxProb < 0.000001) + maxProb = 0.000001; + maxProb = 100.0/maxProb; + for(int i=0; i closedSet_; /*!< TODO Comment */ MagicalMultiSet openSet_; /*!< TODO Comment */ diff --git a/include/Planners/AStarM1.hpp b/include/Planners/AStarM1.hpp index 9451485..23d70e7 100644 --- a/include/Planners/AStarM1.hpp +++ b/include/Planners/AStarM1.hpp @@ -9,7 +9,7 @@ * difference is that it reimplements the computeG method adding the * following cost term to the resturned result: * - * auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + * auto cost_term = static_cast(cost_weight_ * _suc * dist_scale_factor_reduced_); * * @version 0.1 * @date 2021-06-29 diff --git a/include/Planners/AStar_EDF.hpp b/include/Planners/AStar_EDF.hpp new file mode 100644 index 0000000..e763565 --- /dev/null +++ b/include/Planners/AStar_EDF.hpp @@ -0,0 +1,87 @@ +#ifndef ASTAREDF_HPP +#define ASTAREDF_HPP +/** + * @file AStarEDF.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This algorithm is a variation of the A*. The only + * difference is that it reimplements the exploreNeighbours method adding the + * computation of the gradient to choose the explored neighbours: + * + * @version 0.1 + * @date 2023-04-29 + * + * @copyright Copyright (c) 2023 + * + */ +#include + +namespace Planners{ + + /** + * @brief + * + */ + class AStarEDF : public AStar + { + + public: + /** + * @brief Construct a new AStarM1 object + * @param _use_3d This parameter allows the user to choose between + * planning on a plane (8 directions possibles) + * or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + * + */ + AStarEDF(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Cost Aware A Star M1 object + * + * @param _use_3d + */ + AStarEDF(bool _use_3d); + /** + * @brief Main function of the algorithm + * + * @param _source Start discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @param _target Goal discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @return PathData PathData Results stored as PathData object + * Reminder: + * PathData = std::map + * DataVariant = std::variant; + * TODO: Replace map here by unordered_map. Not much important, but it does not make sense to use a map. + */ + PathData findPath(const Vec3i &_source, const Vec3i &_target) override; + + protected: + + /** + * @brief Overrided ComputeG function. + * + * @param _current Pointer to the current node + * @param _suc Pointer to the successor node + * @param _n_i The index of the direction in the directions vector. + * Depending on this index, the distance wi + * @param _dirs Number of directions used (to distinguish between 2D and 3D) + * @return unsigned int The G Value calculated by the function + */ + inline virtual unsigned int computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs); + + virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos); + + virtual int chooseNeighbours(float angh, float angv); + // virtual Vec3i chooseNeighbours(float angh, float angv); + + virtual void nodesToExplore(int node); + + float angles_h[26], angles_v[26]; + + }; + +} + +#endif diff --git a/include/Planners/AStar_EDF_Backup.hpp b/include/Planners/AStar_EDF_Backup.hpp new file mode 100644 index 0000000..873820a --- /dev/null +++ b/include/Planners/AStar_EDF_Backup.hpp @@ -0,0 +1,85 @@ +#ifndef ASTAREDF_HPP +#define ASTAREDF_HPP +/** + * @file AStarEDF.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This algorithm is a variation of the A*. The only + * difference is that it reimplements the exploreNeighbours method adding the + * computation of the gradient to choose the explored neighbours: + * + * @version 0.1 + * @date 2023-04-29 + * + * @copyright Copyright (c) 2023 + * + */ +#include + +namespace Planners{ + + /** + * @brief + * + */ + class AStarEDF : public AStar + { + + public: + /** + * @brief Construct a new AStarM1 object + * @param _use_3d This parameter allows the user to choose between + * planning on a plane (8 directions possibles) + * or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + * + */ + AStarEDF(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Cost Aware A Star M1 object + * + * @param _use_3d + */ + AStarEDF(bool _use_3d); + /** + * @brief Main function of the algorithm + * + * @param _source Start discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @param _target Goal discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @return PathData PathData Results stored as PathData object + * Reminder: + * PathData = std::map + * DataVariant = std::variant; + * TODO: Replace map here by unordered_map. Not much important, but it does not make sense to use a map. + */ + PathData findPath(const Vec3i &_source, const Vec3i &_target) override; + + protected: + + /** + * @brief Overrided ComputeG function. + * + * @param _current Pointer to the current node + * @param _suc Pointer to the successor node + * @param _n_i The index of the direction in the directions vector. + * Depending on this index, the distance wi + * @param _dirs Number of directions used (to distinguish between 2D and 3D) + * @return unsigned int The G Value calculated by the function + */ + inline virtual unsigned int computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs) override; + + virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos); + + virtual int chooseNeighbours(float angh, float angv); + // virtual Vec3i chooseNeighbours(float angh, float angv); + + virtual void nodesToExplore(int node); + + }; + +} + +#endif diff --git a/include/Planners/AStar_Gradient.hpp b/include/Planners/AStar_Gradient.hpp new file mode 100644 index 0000000..51720ae --- /dev/null +++ b/include/Planners/AStar_Gradient.hpp @@ -0,0 +1,85 @@ +#ifndef ASTARGRADIENT_HPP +#define ASTAREDFGRADIENT_HPP +/** + * @file AStarGRADIENT.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This algorithm is a variation of the A*. The only + * difference is that it reimplements the exploreNeighbours method adding the + * computation of the gradient to choose the explored neighbours: + * + * @version 0.1 + * @date 2023-04-29 + * + * @copyright Copyright (c) 2023 + * + */ +#include + +namespace Planners{ + + /** + * @brief + * + */ + class AStarGradient : public AStar + { + + public: + /** + * @brief Construct a new AStarM1 object + * @param _use_3d This parameter allows the user to choose between + * planning on a plane (8 directions possibles) + * or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + * + */ + AStarGradient(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Cost Aware A Star M1 object + * + * @param _use_3d + */ + AStarGradient(bool _use_3d); + /** + * @brief Main function of the algorithm + * + * @param _source Start discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @param _target Goal discrete coordinates. It should be a valid coordinates, i.e. it should not + * be marked as occupied and it should be inside the configured workspace. + * @return PathData PathData Results stored as PathData object + * Reminder: + * PathData = std::map + * DataVariant = std::variant; + * TODO: Replace map here by unordered_map. Not much important, but it does not make sense to use a map. + */ + PathData findPath(const Vec3i &_source, const Vec3i &_target) override; + + protected: + + /** + * @brief Overrided ComputeG function. + * + * @param _current Pointer to the current node + * @param _suc Pointer to the successor node + * @param _n_i The index of the direction in the directions vector. + * Depending on this index, the distance wi + * @param _dirs Number of directions used (to distinguish between 2D and 3D) + * @return unsigned int The G Value calculated by the function + */ + inline virtual unsigned int computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs) override; + + virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos); + + virtual int chooseNeighbours(float angh, float angv); + // virtual Vec3i chooseNeighbours(float angh, float angv); + + virtual void nodesToExplore(int node); + + }; + +} + +#endif diff --git a/include/Planners/LazyThetaStar_EDF.hpp b/include/Planners/LazyThetaStar_EDF.hpp new file mode 100644 index 0000000..95487d0 --- /dev/null +++ b/include/Planners/LazyThetaStar_EDF.hpp @@ -0,0 +1,80 @@ +#ifndef LAZYTHETASTAREDF_HPP +#define LAZYTHETASTAREDF_HPP +/** + * @file LazyThetaStar_EDF.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This header contains the Lazy Theta* algorithm + * implementation. It inherits from the Theta* class + * and reimplement the findPath and the ComputeCosts + * function and implement the new SetVertex function + * + * @version 0.1 + * @date 2023-04-29 + * + * @copyright Copyright (c) 2023 + * + */ +#include + +namespace Planners +{ + /** + * @brief Lazy Theta* Algorithm Class considering the gradient and properties of the EDF described in IROS22 + * + */ + class LazyThetaStarEDF : public ThetaStar + { + + public: + + /** + * @brief Construct a new Lazy Theta Star object + * + * @param _use_3d This parameter allows the user to choose between planning on a plane ( + * 8 directions possibles) or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + * + */ + LazyThetaStarEDF(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Lazy Theta Star object + * + * @param _use_3d + */ + LazyThetaStarEDF(bool _use_3d); + + /** + * @brief Main function of the algorithm + * + * @param _source Start discrete coordinates + * @param _target Goal discrete coordinates + * @return PathData PathData Results stored as PathData object + */ + virtual PathData findPath(const Vec3i &_source, const Vec3i &_target) override; + + protected: + + /** + * @brief Compute cost function of the Lazy Theta* algorithm + * + * @param _s_aux Pointer to first node + * @param _s2_aux Pointer to second node + */ + virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override; + + /** + * @brief SetVertex function + * Line of sight is checked inside this function + * @param _s_aux + */ + virtual void SetVertex(Node *_s_aux); + + inline virtual unsigned int computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs) override; + + }; + +} + +#endif diff --git a/include/Planners/LazyThetaStar_Gradient.hpp b/include/Planners/LazyThetaStar_Gradient.hpp new file mode 100644 index 0000000..ef97fec --- /dev/null +++ b/include/Planners/LazyThetaStar_Gradient.hpp @@ -0,0 +1,78 @@ +#ifndef LAZYTHETASTARGRADIENT_HPP +#define LAZYTHETASTARGRADIENT_HPP +/** + * @file LazyThetaStar_Gradient.hpp + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This header contains the Lazy Theta* algorithm + * implementation. It inherits from the Theta* class + * and reimplement the findPath and the ComputeCosts + * function and implement the new SetVertex function + * + * @version 0.1 + * @date 2023-04-14 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +namespace Planners +{ + /** + * @brief Lazy Theta* Algorithm Class considering the gradient of the EDF + * + */ + class LazyThetaStarGradient : public ThetaStar + { + + public: + + /** + * @brief Construct a new Lazy Theta Star object + * + * @param _use_3d This parameter allows the user to choose between planning on a plane ( + * 8 directions possibles) or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + * + */ + LazyThetaStarGradient(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Lazy Theta Star object + * + * @param _use_3d + */ + LazyThetaStarGradient(bool _use_3d); + + /** + * @brief Main function of the algorithm + * + * @param _source Start discrete coordinates + * @param _target Goal discrete coordinates + * @return PathData PathData Results stored as PathData object + */ + virtual PathData findPath(const Vec3i &_source, const Vec3i &_target) override; + + protected: + + /** + * @brief Compute cost function of the Lazy Theta* algorithm + * + * @param _s_aux Pointer to first node + * @param _s2_aux Pointer to second node + */ + virtual void ComputeCost(Node *_s_aux, Node *_s2_aux) override; + + /** + * @brief SetVertex function + * Line of sight is checked inside this function + * @param _s_aux + */ + virtual void SetVertex(Node *_s_aux); + + }; + +} + +#endif diff --git a/include/Planners/ThetaStar.hpp b/include/Planners/ThetaStar.hpp index b893019..0ef0b66 100644 --- a/include/Planners/ThetaStar.hpp +++ b/include/Planners/ThetaStar.hpp @@ -82,7 +82,18 @@ namespace Planners */ virtual void exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos) override; + virtual void exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos); + + virtual int chooseNeighbours(float angh, float angv); + // virtual Vec3i chooseNeighbours(float angh, float angv); + + // virtual Vec3i nodesToExplore(int _node); + // virtual void nodesToExplore(int _node, Vec3i &list_nodes_to_explore); + // virtual void nodesToExplore(int _node, CoordinateList &list_nodes_to_explore); + utils::CoordinateListPtr checked_nodes, checked_nodes_current; /*!< TODO Comment */ + // float mod[26]; + float angles_h[26], angles_v[26]; }; diff --git a/include/Planners/ThetaStar_Gradient.hpp b/include/Planners/ThetaStar_Gradient.hpp new file mode 100644 index 0000000..b893019 --- /dev/null +++ b/include/Planners/ThetaStar_Gradient.hpp @@ -0,0 +1,91 @@ +#ifndef THETASTAR_HPP +#define THETASTAR_HPP +/** + * @file ThetaStar.hpp + * @author Rafael Rey (reyarcenegui@gmail.com) + * @author Jose Antonio Cobano (jacobsua@upo.es) + * + * @brief This class inherit from the AStar Algorithm and + * implements the UpdateVertex and ComputeCost functions and + * re-implements ExploreNeighbours method fromthe AStar class. + * + * @version 0.1 + * @date 2021-06-29 + * + * @copyright Copyright (c) 2021 + * + */ +#include + +namespace Planners +{ + + /** + * @brief Theta* Algorithm Class + * + */ + class ThetaStar : public AStar + { + + public: + + /** + * @brief Construct a new Theta Star object + * + * @param _use_3d This parameter allows the user to choose between + * planning on a plane (8 directions possibles) or in the 3D full space (26 directions) + * @param _name Algorithm name stored internally + */ + ThetaStar(bool _use_3d, std::string _name ); + + /** + * @brief Construct a new Theta Star object + * + * @param _use_3d + */ + ThetaStar(bool _use_3d); + + protected: + /** + * @brief Update Vertex function + * + * @param _s Pointer to node s + * @param _s2 Pointer to node s2 + * @param _index_by_pos reference to openset to erase and insert the nodes in some cases + */ + virtual void UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos); + + /** + * @brief Compute cost algorithm function + * + * @param _s_aux Pointer to first node + * @param _s2_aux Pointer to the second node + */ + inline virtual void ComputeCost(Node *_s_aux, Node *_s2_aux); + + /** + * @brief This function is the secondary inside the main loop of findPath + * function. This secondary loop iterates over the neighbours of a node, + * skipping the explored or occupied ones, and performs the appropiate operations + * and calculus of the H and G values, and the corresponding parents updates. + * + * @param _current A pointer to the current node, the loop will iterate over + * the neighbours of this node by using the directions vector in the AlgorithmBase + * class + * + * @param _target A reference to the target coordinates of the GOAL. + * + * @param _index_by_pos A reference to the openset to insert the non-explored nodes + * or to erase and insert the re-explored ones if a better path is found. + * This operation of erase and re-insert is performed in order to update the position + * of the node in the container. + */ + virtual void exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos) override; + + utils::CoordinateListPtr checked_nodes, checked_nodes_current; /*!< TODO Comment */ + + }; + +} + +#endif diff --git a/include/utils/geometry_utils.hpp b/include/utils/geometry_utils.hpp index fa508db..aae0d73 100644 --- a/include/utils/geometry_utils.hpp +++ b/include/utils/geometry_utils.hpp @@ -130,6 +130,15 @@ namespace Planners */ double angleBetweenThreePoints(const Vec3i &_v1, const Vec3i &_v2, const Vec3i &_v3); + /** + * @brief Returns the neighbour nodes chosen from the gradient: two options 9-17 nodes + * + * @param _angh + * @param _angv + * @return double + */ + int chooseNeighbours(float _angh, float _angv); + #ifdef ROS /** * @brief diff --git a/include/utils/heuristic.hpp b/include/utils/heuristic.hpp index e2c9ed9..e7c8708 100644 --- a/include/utils/heuristic.hpp +++ b/include/utils/heuristic.hpp @@ -36,7 +36,26 @@ namespace Planners */ static Vec3i getDelta(const Vec3i &_source, const Vec3i &_target); + /** + * @brief Returns the absolute of unit vector of each neighbor and the tardet + * + * @param _source + * @param _target + * @return Vec3i + */ + static Vec3i getVectorPull(const Vec3i &_source, const Vec3i &_target); + public: + + /** + * @brief Gaol pull to weight the heuristic in the selection of exploration neighbors. + * + * @param _source + * @param _target + * @return unsigned int + */ + static unsigned int goal_pull(const Vec3i &_source, const Vec3i &_target); + /** * @brief Manhattan heuristic * diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp index e64a54c..6269b89 100644 --- a/include/utils/utils.hpp +++ b/include/utils/utils.hpp @@ -18,6 +18,7 @@ #include #include #include + #ifdef ROS #include #endif @@ -180,6 +181,13 @@ namespace Planners } #endif }; + + class Vec3f + { + public: + float x, y, z; + }; + /** * @brief * diff --git a/launch/planner.launch b/launch/planner.launch index dcf5602..cd7eb3d 100644 --- a/launch/planner.launch +++ b/launch/planner.launch @@ -1,15 +1,43 @@ - - - + + + + + + + + + + + + + + + + + + + + + - - - + + + + + + + + + + + + @@ -17,17 +45,17 @@ - + - + - - + + - + diff --git a/launch/planner_bim.launch b/launch/planner_bim.launch new file mode 100644 index 0000000..376d3ea --- /dev/null +++ b/launch/planner_bim.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/simulator_atlas.launch b/launch/simulator_atlas.launch new file mode 100644 index 0000000..fe5bb4c --- /dev/null +++ b/launch/simulator_atlas.launch @@ -0,0 +1,83 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/simulator_mbzirc.launch b/launch/simulator_mbzirc.launch new file mode 100644 index 0000000..3f634de --- /dev/null +++ b/launch/simulator_mbzirc.launch @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/3dmaps/Atlas_8_300.bt b/resources/3dmaps/Atlas_8_300.bt new file mode 100644 index 0000000..5d174cd Binary files /dev/null and b/resources/3dmaps/Atlas_8_300.bt differ diff --git a/resources/3dmaps/abu_dhabi_002.bt b/resources/3dmaps/abu_dhabi_002.bt new file mode 100644 index 0000000..b8a339c Binary files /dev/null and b/resources/3dmaps/abu_dhabi_002.bt differ diff --git a/resources/3dmaps/abu_dhabi_002_blender.bt b/resources/3dmaps/abu_dhabi_002_blender.bt new file mode 100644 index 0000000..fcfe628 Binary files /dev/null and b/resources/3dmaps/abu_dhabi_002_blender.bt differ diff --git a/resources/3dmaps/abu_dhabi_025.bt b/resources/3dmaps/abu_dhabi_025.bt new file mode 100644 index 0000000..ffbf99d Binary files /dev/null and b/resources/3dmaps/abu_dhabi_025.bt differ diff --git a/resources/3dmaps/manufacturing.bt b/resources/3dmaps/manufacturing.bt deleted file mode 100755 index e54db80..0000000 Binary files a/resources/3dmaps/manufacturing.bt and /dev/null differ diff --git a/resources/3dmaps/mbzirc_challenge3.bt b/resources/3dmaps/mbzirc_challenge3.bt deleted file mode 100755 index 5f6b830..0000000 Binary files a/resources/3dmaps/mbzirc_challenge3.bt and /dev/null differ diff --git a/resources/3dmaps/tower.bt b/resources/3dmaps/tower.bt new file mode 100644 index 0000000..eb8927b Binary files /dev/null and b/resources/3dmaps/tower.bt differ diff --git a/rviz/planners.rviz b/rviz/planners.rviz index 19e0c04..93730e2 100644 --- a/rviz/planners.rviz +++ b/rviz/planners.rviz @@ -1,11 +1,11 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 138 Name: Displays Property Tree Widget: Expanded: ~ Splitter Ratio: 0.8294117450714111 - Tree Height: 455 + Tree Height: 270 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -21,7 +21,6 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 SyncSource: Map PointCloud @@ -70,7 +69,7 @@ Visualization Manager: Marker Topic: /planner_ros_node/path_line_markers Name: Path Line Markers Namespaces: - costlazythetastar: true + lazythetastaredf: true Queue Size: 100 Value: true - Class: rviz/Marker @@ -86,7 +85,7 @@ Visualization Manager: Marker Topic: /planner_ros_node/path_points_markers Name: Path Point Markers Namespaces: - costlazythetastar: true + lazythetastaredf: true Queue Size: 100 Value: true - Class: rviz/Marker @@ -162,8 +161,8 @@ Visualization Manager: - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: - Max Value: 20.100000381469727 - Min Value: 0.10000000149011612 + Max Value: 6.933859825134277 + Min Value: 0.09765999764204025 Value: true Axis: Z Channel Name: intensity @@ -222,34 +221,28 @@ Visualization Manager: Value: true Views: Current: - Class: rviz/Orbit - Distance: 43.4847526550293 + Angle: 0 + Class: rviz/TopDownOrtho Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 41.08955383300781 - Y: 29.464345932006836 - Z: -3.5244100093841553 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3747965693473816 + Scale: 19.73822784423828 Target Frame: - Yaw: 3.4571502208709717 + X: 25.470565795898438 + Y: 24.96125030517578 Saved: ~ Window Geometry: Displays: collapsed: true - Height: 1016 + Height: 1366 Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000252000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039c0000003efc0100000002fb0000000800540069006d006501000000000000039c000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039c0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000252fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002520000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000450fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004500000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a00000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000008950000045000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -257,7 +250,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 924 - X: 996 - Y: 27 + collapsed: false + Width: 2560 + X: 0 + Y: 444 diff --git a/scripts/test_algorithms_compare.py b/scripts/test_algorithms_compare.py index 409b1e2..c8d15a9 100755 --- a/scripts/test_algorithms_compare.py +++ b/scripts/test_algorithms_compare.py @@ -72,13 +72,15 @@ class colors: # You may need to change color settings nargs='+', type=str, default=["planner.launch"], choices=launch_list) parser.add_argument("--algorithm", help="name of the algorithm", - nargs='*', type=str, required=True, default=["astar", "astarm1", "astarm2"], - choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"]) + # nargs='*', type=str, required=True, default=["astar", "astarm1", "astarm2"], + # choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"]) + nargs='*', type=str, required=True, default=["astar", "costastar", "astarsafetycost"], + choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"]) parser.add_argument("--map-name", help="name of the map to use. This map should be under the 3d/2d maps folder", - nargs='+', type=str, default=['mbzirc_challenge3.bt'], + nargs='+', type=str, default=['maze.bt'], choices=maps_list) - +# mbzirc_challenge3.bt parser.add_argument("--plots", help="List of result variables to plot", nargs='+', type=str, required=True, choices=plot_choices) diff --git a/scripts/test_algorithms_performance.py b/scripts/test_algorithms_performance.py index e905b45..fe019e4 100755 --- a/scripts/test_algorithms_performance.py +++ b/scripts/test_algorithms_performance.py @@ -28,7 +28,7 @@ choices=launch_list) parser.add_argument("--algorithm", help="name of the algorithm", nargs='+', type=str, required=True, - choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"]) + choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"]) parser.add_argument("--start-coords", help="start coordinates (x,y,z). Set z to 0 when testing with 2D", nargs=3, required=True) parser.add_argument("--goal-coords", help="goal coordinates (x,y,z). Set z to 0 when testing with 2D", diff --git a/scripts/test_algorithms_pseudo_random_paths.py b/scripts/test_algorithms_pseudo_random_paths.py index cd9def1..fe7d59d 100755 --- a/scripts/test_algorithms_pseudo_random_paths.py +++ b/scripts/test_algorithms_pseudo_random_paths.py @@ -41,7 +41,9 @@ def generate_centered_rnd_coords(center, margins): choices=launch_list) parser.add_argument("--algorithm", help="name of the algorithm", nargs='+', type=str, required=True, - choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"]) + # choices=["astar", "astarm1", "astarm2", "thetastar", "thetastarm1", "thetastarm2", "lazythetastar", "lazythetastarm1", "lazythetastarm1mod", "lazythetastarm2"]) + # choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastargradient"]) + choices=["astar", "costastar", "astarsafetycost", "thetastar", "costhetastar", "thetastarsafetycost", "lazythetastar", "costlazythetastar", "costlazythetastarmodified", "lazythetastarsafetycost", "lazythetastaredf"]) parser.add_argument("--start-coords", help="start coordinates (x,y,z). Set z to 0 when testing with 2D", nargs=3, required=True) parser.add_argument("--goal-coords", help="goal coordinates (x,y,z). Set z to 0 when testing with 2D", diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp index 032d8bf..0fcd4d1 100644 --- a/src/Planners/AStar.cpp +++ b/src/Planners/AStar.cpp @@ -136,6 +136,49 @@ void AStar::publishROSDebugData(const Node* _node, const T &_open_set, const U & } +// JAC: Computation of the attractin vector +inline Vec3i AStar::getVectorPull(const Vec3i &_source, const Vec3i &_target){ + return {_target.x - _source.x, _target.y - _source.y, _target.z - _source.z}; +} + +// JAC: Create a file --> computeGradient +inline float AStar::computeGradient(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + unsigned int cost; + + float rest,grad; + // float div; + + if(_dirs == 8){ + cost = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + grad=(_current->cost - _suc->cost)/cost; + }else{ + cost = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + rest=_current->cost-_suc->cost; + + // // Casi lo mismo que multiplicar 500*grad???? + // if (cost == 100) + // div=0.2; // res + // else if (cost == 141) + // div=0.2828; //sqrt(2)*res + // else if (cost == 173) + // div = 0.3464; //sqrt(3)*res + + // std::cout << "cost: " << cost << std::endl; + // std::cout << "div: " << div << std::endl; + // std::cout << "current cost: " << _current->cost << std::endl; + // std::cout << "suc cost: " << _suc->cost << std::endl; + // std::cout << "rest: " << rest << std::endl; + // std::cout << "G: " << _current->G << std::endl; + + // grad=static_cast((100/0.2)*(rest/cost)); // res=0,2 + grad=static_cast((500)*(rest/cost)); // res=0,2 + // grad=static_cast(rest/div); + // std::cout << "gradient: " << grad << std::endl; + } + + return grad; +} + inline unsigned int AStar::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ unsigned int cost = _current->G; @@ -158,7 +201,7 @@ void AStar::exploreNeighbours(Node* _current, const Vec3i &_target, node_by_posi Vec3i newCoordinates = _current->coordinates + direction[i]; Node *successor = discrete_world_.getNodePtr(newCoordinates); - //Skip the neighbour if it is not valid, occupied, or already in teh + //Skip the neighbour if it is not valid, occupied, or already in the //closed list if ( successor == nullptr || successor->isInClosedList || @@ -185,6 +228,7 @@ void AStar::exploreNeighbours(Node* _current, const Vec3i &_target, node_by_posi } } } + PathData AStar::findPath(const Vec3i &_source, const Vec3i &_target) { Node *current = nullptr; diff --git a/src/Planners/AStarM1.cpp b/src/Planners/AStarM1.cpp index 5b30c7c..835a501 100644 --- a/src/Planners/AStarM1.cpp +++ b/src/Planners/AStarM1.cpp @@ -15,8 +15,11 @@ inline unsigned int AStarM1::computeG(const Node* _current, Node* _suc, unsigned cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient } - auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); - + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + // IROS + // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + // RA-L + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); cost += cost_term; _suc->C = cost_term; diff --git a/src/Planners/AStar_EDF.cpp b/src/Planners/AStar_EDF.cpp new file mode 100644 index 0000000..d108533 --- /dev/null +++ b/src/Planners/AStar_EDF.cpp @@ -0,0 +1,1758 @@ +#include "Planners/AStar_EDF.hpp" + +namespace Planners{ + +AStarEDF::AStarEDF(bool _use_3d):AStar(_use_3d, "astar_edf") {} +AStarEDF::AStarEDF(bool _use_3d, std::string _name = "astar_edf" ):AStar(_use_3d, _name) {} + +// This computeG is considering the distance cost. +inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + + // std::cout << "grad " << grad << std::endl; + // std::cout << "cost_weight " << cost_weight_ << std::endl; + // cost_weight_ = 1; + // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); + // std::cout << "cost_term " << cost_term << std::endl; + // std::cout << "cost before " << cost << std::endl; + cost += cost_term; + // std::cout << "cost after " << cost << std::endl; + _suc->C = cost_term; + + return cost; +} + +// // This computeG is the original AStar +// inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// _suc->C = _suc->cost; + +// return cost; +// } + + + +void AStarEDF::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + angles_h[0]=1.5708; + angles_v[0]=0; + angles_h[1]=-1.5708; + angles_v[1]=0; + angles_h[2]=0; + angles_v[2]=0; + angles_h[3]=3.14159; + angles_v[3]=0; + angles_h[4]=0; + angles_v[4]=1.5708; + angles_h[5]=0; + angles_v[5]=-1.5708; + angles_h[6]=-0.785398; + angles_v[6]=0; + angles_h[7]=2.35619; + angles_v[7]=0; + angles_h[8]=-2.35619; + angles_v[8]=0; + angles_h[9]=0.785398; + angles_v[9]=0; + angles_h[10]=3.14159; + angles_v[10]=-0.788391; + angles_h[11]=0; + angles_v[11]=0.788391; + angles_h[12]=0; + angles_v[12]=-0.788391; + angles_h[13]=3.14159; + angles_v[13]=0.788391; + angles_h[14]=-1.5708; + angles_v[14]=0.788391; + angles_h[15]=1.5708; + angles_v[15]=0.788391; + angles_h[16]=1.5708; + angles_v[16]=-0.788391; + angles_h[17]=-1.5708; + angles_v[17]=-0.788391; + angles_h[18]=-2.35619; + angles_v[18]=0.616318; + angles_h[19]=0.785398; + angles_v[19]=0.616318; + angles_h[20]=2.35619; + angles_v[20]=0.616318; + angles_h[21]=-0.785398; + angles_v[21]=0.616318; + angles_h[22]=-2.35619; + angles_v[22]=-0.616318; + angles_h[23]=0.785398; + angles_v[23]=-0.616318; + angles_h[24]=2.35619; + angles_v[24]=-0.616318; + angles_h[25]=-0.785398; + angles_v[25]=-0.616318; + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->occuppied ) + continue; + + if ( successor_grad->isInClosedList ){ + // ThetaStar2 + // gradient[i]=-1; + // ThetaStar1 + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + } + else { + + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i+1 << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + + } + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + int weight_gradient=1; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // std::cout << "ang: " << ang << std::endl; + angle_v=asin(ang); + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + /********************************************************************************************************/ + // // CONSIDERING THE DIFFERENCE BETWEEN THE ANGLE OF THE GRADIENT AND THE ANGLE OF THE GOAL. + // Angle of the gradient DESCOMENTAR + float angle_gradient_h=0; // horizontal angle X-Y + angle_gradient_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // std::cout << "angle gradient h: " << angle_gradient_h << std::endl; + + // Angle of the attraction DESCOMENTAR + float angle_attrac_h=0; // horizontal angle X-Y + angle_attrac_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // std::cout << "angle attrac h: " << angle_attrac_h << std::endl; + + unsigned int num_nodes; + Vec3i node_choosenCoordinates[13]; //11 + int index_nodes[13]; //11 + + if (angle_gradient_h != 0){ + if (abs(angle_attrac_h - angle_gradient_h) > (M_PI/2)){ //1.3962634: 80 + // It works with 9-10 and considering 75 degrees. + // if (abs(angle_attrac_h - angle_gradient_h) > (1.3089)){ //1.308996939: 75 + num_nodes=13; //11 + // Vec3i node_choosenCoordinates[11]; + // int index_nodes[11]; + // std::cout << "111: " << std::endl; + } + else { + num_nodes=11; //9 + // Vec3i node_choosenCoordinates[9]; + // int index_nodes[9]; + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "222: " << std::endl; + } + } + // If angle_gradient_h == 0 + else{ + num_nodes=13; //11 + // Vec3i node_choosenCoordinates[11]; + // int index_nodes[11]; + // std::cout << "333: " << std::endl; + } + /********************************************************************************************************/ + + // TO CHANGE TO EXPLORE 9,11, 13, 15 or 17 NODES + // Vec3i node_choosenCoordinates[11]; + // unsigned int num_nodes=11; + // int index_nodes[11]; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + if (num_nodes == 10){ + index_nodes[9]=1; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=11; + index_nodes[12]=13; + index_nodes[13]=10; + index_nodes[14]=12; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=0; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=12; + index_nodes[12]=13; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=12; + index_nodes[12]=13; + index_nodes[13]=10; + index_nodes[14]=11; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=3; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=8; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + if (num_nodes == 10){ + index_nodes[9]=2; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + if (num_nodes == 10){ + index_nodes[9]=5; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if (num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + index_nodes[13]=6; + index_nodes[14]=7; + } + else if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=4; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if (num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + index_nodes[13]=8; + index_nodes[14]=9; + } + else if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=7; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=4; + index_nodes[10]=5; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=18; + index_nodes[14]=23; + } + else if (num_nodes==17){ + index_nodes[9]=9; + index_nodes[10]=8; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=16; + if (num_nodes == 10){ + index_nodes[9]=6; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=23; + index_nodes[12]=18; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=23; + index_nodes[12]=18; + index_nodes[13]=19; + index_nodes[14]=22; + } + else if (num_nodes==17){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=9; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=20; + index_nodes[14]=25; + } + else if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=8; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=25; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=25; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=11; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=18; + index_nodes[12]=23; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=18; + index_nodes[12]=23; + index_nodes[13]=20; + index_nodes[14]=25; + } + else if (num_nodes==17){ + index_nodes[9]=23; + index_nodes[10]=12; + index_nodes[11]=25; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=20; + index_nodes[15]=13; + index_nodes[16]=18; + } + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes == 10){ + index_nodes[9]=10; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=25; + index_nodes[12]=20; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=25; + index_nodes[12]=20; + index_nodes[13]=18; + index_nodes[14]=23; + } + else if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=13; + index_nodes[11]=18; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=23; + index_nodes[15]=12; + index_nodes[16]=25; + } + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=13; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=10; + index_nodes[11]=22; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=19; + index_nodes[15]=11; + index_nodes[16]=21; + } + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes == 10){ + index_nodes[9]=12; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=19; + index_nodes[10]=11; + index_nodes[11]=21; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=24; + index_nodes[15]=10; + index_nodes[16]=22; + } + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + if (num_nodes == 10){ + index_nodes[9]=16; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=25; + index_nodes[14]=20; + } + else if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=15; + index_nodes[11]=19; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=22; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + if (num_nodes == 10){ + index_nodes[9]=17; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=23; + index_nodes[14]=18; + } + else if (num_nodes==17){ + index_nodes[9]=18; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=24; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=14; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=25; + index_nodes[14]=20; + } + else if (num_nodes==17){ + index_nodes[9]=22; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=20; + index_nodes[15]=15; + index_nodes[16]=19; + } + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=15; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=23; + index_nodes[14]=18; + } + else if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=16; + index_nodes[11]=23; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=18; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=14; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + if (num_nodes == 10){ + index_nodes[9]=23; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=11; + index_nodes[11]=19; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=10; + index_nodes[15]=22; + index_nodes[16]=17; + } + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=22; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=14; + index_nodes[11]=18; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=12; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + if (num_nodes == 10){ + index_nodes[9]=25; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=11; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=10; + index_nodes[15]=24; + index_nodes[16]=16; + } + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=24; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=15; + index_nodes[11]=20; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=12; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + if (num_nodes == 10){ + index_nodes[9]=19; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=16; + index_nodes[10]=12; + index_nodes[11]=23; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=13; + index_nodes[15]=14; + index_nodes[16]=18; + } + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=18; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=17; + index_nodes[11]=22; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=11; + index_nodes[15]=19; + index_nodes[16]=15; + } + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=7; + index_nodes[8]=0; + if (num_nodes == 10){ + index_nodes[9]=21; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=12; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=13; + index_nodes[15]=15; + index_nodes[16]=20; + } + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=20; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=16; + index_nodes[11]=24; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=11; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + for (unsigned int i = 0; i < num_nodes; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + // for (unsigned int i = 0; i < 9; ++i) { // when we consider the gradient to choose explored nodes + // for (unsigned int i = 0; i < direction.size(); ++i) { + for (unsigned int i = 0; i < num_nodes; ++i) { // when we consider the gradient to choose explored nodes + + // Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; // when we consider the gradient to choose explored nodes + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + unsigned int totalCost = computeG(_current, successor, index_nodes[i], direction.size()); + + if (! successor->isInOpenList ) { + + // successor->parent = _current; + // // successor->G = computeG(_current, successor, i, direction.size()); + // successor->G = computeG(_current, successor, gradient[i], index_nodes[i], direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // // atraction[i]=getVectorPull(successor->coordinates, _target); + // // std::cout << "atraction X: " << atraction[i].x << std::endl; + // // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // // std::cout << "atraction Z: " << atraction[i].z << std::endl; + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + + successor->parent = _current; + successor->G = totalCost; + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + else if (totalCost < successor->G) { + successor->parent = _current; + successor->G = totalCost; + successor->gplush = successor->G + successor->H; + auto found = _index_by_pos.find(successor->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + // } + } + + int AStarEDF::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // // Modules + // float mod[26]; + // float aux; + // for (unsigned int i = 0; i < direction.size(); ++i) + // { + // aux=0; + // mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // aux=mod[i]/100; + // mod[i]=aux; + // } + // // std::cout << "mod: " << mod[0] << std::endl; + + // // Angles + // float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = direction[i]; + + // angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + // angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void AStarEDF::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } + +PathData AStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) +{ + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + //Timer to record the execution time, not + //really important + 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 + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) { solved = true; break; } + + closedSet_.push_back(current); + //This flags are used to avoid search in the containers, + //for speed reasons. + current->isInOpenList = false; + current->isInClosedList = true; + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + } + 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 + //the same way in every new algorithm implemented. +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + openSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; +} +} diff --git a/src/Planners/AStar_EDF_9_17.cpp b/src/Planners/AStar_EDF_9_17.cpp new file mode 100644 index 0000000..bad1f9d --- /dev/null +++ b/src/Planners/AStar_EDF_9_17.cpp @@ -0,0 +1,1115 @@ +#include "Planners/AStar_EDF.hpp" + +namespace Planners{ + +AStarEDF::AStarEDF(bool _use_3d):AStar(_use_3d, "astar_edf") {} +AStarEDF::AStarEDF(bool _use_3d, std::string _name = "astar_edf" ):AStar(_use_3d, _name) {} + +// This computeG is considering the distance cost. +inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + + // std::cout << "grad " << grad << std::endl; + // std::cout << "cost_weight " << cost_weight_ << std::endl; + // cost_weight_ = 1; + // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); + // std::cout << "cost_term " << cost_term << std::endl; + // std::cout << "cost before " << cost << std::endl; + cost += cost_term; + // std::cout << "cost after " << cost << std::endl; + _suc->C = cost_term; + + return cost; +} + +// // This computeG is the original AStar +// inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// _suc->C = _suc->cost; + +// return cost; +// } + + + +void AStarEDF::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->occuppied ) + continue; + + if ( successor_grad->isInClosedList ){ + // ThetaStar2 + // gradient[i]=-1; + // ThetaStar1 + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + } + else { + + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i+1 << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + + } + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + int weight_gradient=1; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // std::cout << "ang: " << ang << std::endl; + angle_v=asin(ang); + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + unsigned int num_nodes=9; //17 + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=8; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + if (num_nodes==17){ + index_nodes[9]=9; + index_nodes[10]=8; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=16; + if (num_nodes==17){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes==17){ + index_nodes[9]=23; + index_nodes[10]=12; + index_nodes[11]=25; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=20; + index_nodes[15]=13; + index_nodes[16]=18; + } + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=13; + index_nodes[11]=18; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=23; + index_nodes[15]=12; + index_nodes[16]=25; + } + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=10; + index_nodes[11]=22; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=19; + index_nodes[15]=11; + index_nodes[16]=21; + } + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes==17){ + index_nodes[9]=19; + index_nodes[10]=11; + index_nodes[11]=21; + index_nodes[12]=0; + index_nodes[13]=2; + index_nodes[14]=24; + index_nodes[15]=10; + index_nodes[16]=22; + } + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=15; + index_nodes[11]=19; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=22; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + if (num_nodes==17){ + index_nodes[9]=18; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=24; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes==17){ + index_nodes[9]=22; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=20; + index_nodes[15]=15; + index_nodes[16]=19; + } + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=16; + index_nodes[11]=23; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=18; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=14; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=11; + index_nodes[11]=19; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=10; + index_nodes[15]=22; + index_nodes[16]=17; + } + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=14; + index_nodes[11]=18; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=12; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + if (num_nodes==17){ + index_nodes[9]=11; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=10; + index_nodes[15]=24; + index_nodes[16]=16; + } + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=15; + index_nodes[11]=20; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=12; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + if (num_nodes==17){ + index_nodes[9]=16; + index_nodes[10]=12; + index_nodes[11]=23; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=13; + index_nodes[15]=14; + index_nodes[16]=18; + } + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=17; + index_nodes[11]=22; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=11; + index_nodes[15]=19; + index_nodes[16]=15; + } + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=7; + index_nodes[8]=0; + if (num_nodes==17){ + index_nodes[9]=12; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=13; + index_nodes[15]=15; + index_nodes[16]=20; + } + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=16; + index_nodes[11]=24; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=11; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + + for (unsigned int i = 0; i < num_nodes; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + // for (unsigned int i = 0; i < 9; ++i) { // when we consider the gradient to choose explored nodes + // for (unsigned int i = 0; i < direction.size(); ++i) { + for (unsigned int i = 0; i < num_nodes; ++i) { // when we consider the gradient to choose explored nodes + + // Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; // when we consider the gradient to choose explored nodes + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + unsigned int totalCost = computeG(_current, successor, index_nodes[i], direction.size()); + + if (! successor->isInOpenList ) { + + // successor->parent = _current; + // // successor->G = computeG(_current, successor, i, direction.size()); + // successor->G = computeG(_current, successor, gradient[i], index_nodes[i], direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // // atraction[i]=getVectorPull(successor->coordinates, _target); + // // std::cout << "atraction X: " << atraction[i].x << std::endl; + // // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // // std::cout << "atraction Z: " << atraction[i].z << std::endl; + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + + successor->parent = _current; + successor->G = totalCost; + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + else if (totalCost < successor->G) { + successor->parent = _current; + successor->G = totalCost; + successor->gplush = successor->G + successor->H; + auto found = _index_by_pos.find(successor->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + // } + } + + int AStarEDF::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void AStarEDF::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } + +PathData AStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) +{ + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + //Timer to record the execution time, not + //really important + 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 + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) { solved = true; break; } + + closedSet_.push_back(current); + //This flags are used to avoid search in the containers, + //for speed reasons. + current->isInOpenList = false; + current->isInClosedList = true; + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + } + 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 + //the same way in every new algorithm implemented. +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + openSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; +} +} diff --git a/src/Planners/AStar_EDF_Backup.cpp b/src/Planners/AStar_EDF_Backup.cpp new file mode 100644 index 0000000..435398e --- /dev/null +++ b/src/Planners/AStar_EDF_Backup.cpp @@ -0,0 +1,826 @@ +#include "Planners/AStar_EDF.hpp" + +namespace Planners{ + +AStarEDF::AStarEDF(bool _use_3d):AStar(_use_3d, "astar_edf") {} +AStarEDF::AStarEDF(bool _use_3d, std::string _name = "astar_edf" ):AStar(_use_3d, _name) {} + +// This computeG is considering the distance cost. +inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + + cost += cost_term; + _suc->C = cost_term; + + return cost; +} + +// // This computeG is the original AStar +// inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// _suc->C = _suc->cost; + +// return cost; +// } + + + +void AStarEDF::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->occuppied ) + continue; + + if ( successor_grad->isInClosedList ){ + // ThetaStar2 + // gradient[i]=-1; + // ThetaStar1 + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + } + else { + + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i+1 << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + + } + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + int weight_gradient=1; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // std::cout << "ang: " << ang << std::endl; + angle_v=asin(ang); + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + // } + } + + int AStarEDF::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void AStarEDF::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } + +PathData AStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) +{ + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + //Timer to record the execution time, not + //really important + 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 + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) { solved = true; break; } + + closedSet_.push_back(current); + //This flags are used to avoid search in the containers, + //for speed reasons. + current->isInOpenList = false; + current->isInClosedList = true; + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + } + 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 + //the same way in every new algorithm implemented. +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + openSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; +} +} diff --git a/src/Planners/AStar_EDF_grad.cpp b/src/Planners/AStar_EDF_grad.cpp new file mode 100644 index 0000000..5533bf0 --- /dev/null +++ b/src/Planners/AStar_EDF_grad.cpp @@ -0,0 +1,852 @@ +#include "Planners/AStar_EDF.hpp" + +namespace Planners{ + +AStarEDF::AStarEDF(bool _use_3d):AStar(_use_3d, "astar_edf") {} +AStarEDF::AStarEDF(bool _use_3d, std::string _name = "astar_edf" ):AStar(_use_3d, _name) {} + +// This computeG is considering the distance cost. +inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, float grad, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + cost_weight_ = -(1*grad); + // std::cout << "grad " << grad << std::endl; + // std::cout << "cost_weight " << cost_weight_ << std::endl; + // cost_weight_ = 1; + // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + // std::cout << "cost_term " << cost_term << std::endl; + // std::cout << "cost before " << cost << std::endl; + cost += cost_term; + // std::cout << "cost after " << cost << std::endl; + _suc->C = cost_term; + + return cost; +} + +// // This computeG is the original AStar +// inline unsigned int AStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ +// unsigned int cost = _current->G; + +// if(_dirs == 8){ +// cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient +// }else{ +// cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient +// } + +// _suc->C = _suc->cost; + +// return cost; +// } + + + +void AStarEDF::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->occuppied ) + continue; + + if ( successor_grad->isInClosedList ){ + // ThetaStar2 + // gradient[i]=-1; + // ThetaStar1 + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + } + else { + + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i+1 << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + + } + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + int weight_gradient=1; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // std::cout << "ang: " << ang << std::endl; + angle_v=asin(ang); + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + // for (unsigned int i = 0; i < 9; ++i) { // when we consider the gradient to choose explored nodes + for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; // when we consider the gradient to choose explored nodes + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + unsigned int totalCost = computeG(_current, successor, gradient[i], i, direction.size()); //index_nodes[i] when we define the explored nodes + + if (! successor->isInOpenList ) { + + // successor->parent = _current; + // // successor->G = computeG(_current, successor, i, direction.size()); + // successor->G = computeG(_current, successor, gradient[i], index_nodes[i], direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // // atraction[i]=getVectorPull(successor->coordinates, _target); + // // std::cout << "atraction X: " << atraction[i].x << std::endl; + // // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // // std::cout << "atraction Z: " << atraction[i].z << std::endl; + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + + successor->parent = _current; + successor->G = totalCost; + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + else if (totalCost < successor->G) { + successor->parent = _current; + successor->G = totalCost; + successor->gplush = successor->G + successor->H; + auto found = _index_by_pos.find(successor->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + // } + } + + int AStarEDF::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void AStarEDF::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } + +PathData AStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) +{ + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + //Timer to record the execution time, not + //really important + 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 + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) { solved = true; break; } + + closedSet_.push_back(current); + //This flags are used to avoid search in the containers, + //for speed reasons. + current->isInOpenList = false; + current->isInClosedList = true; + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + } + 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 + //the same way in every new algorithm implemented. +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + openSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; +} +} diff --git a/src/Planners/AStar_Gradient.cpp b/src/Planners/AStar_Gradient.cpp new file mode 100644 index 0000000..ed21e73 --- /dev/null +++ b/src/Planners/AStar_Gradient.cpp @@ -0,0 +1,770 @@ +#include "Planners/AStar_Gradient.hpp" + +namespace Planners{ + +AStarGradient::AStarGradient(bool _use_3d):AStar(_use_3d, "astar_gradient") {} +AStarGradient::AStarGradient(bool _use_3d, std::string _name = "astar_gradient" ):AStar(_use_3d, _name) {} + +// This computeG is the original AStar +inline unsigned int AStarGradient::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + unsigned int cost = _current->G; + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + } + + _suc->C = _suc->cost; + + return cost; +} + + + +void AStarGradient::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->isInClosedList || + successor_grad->occuppied ) + continue; + + if (! successor_grad->isInOpenList ) { + + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i+1 << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + + } + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + int weight_gradient=1; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // std::cout << "ang: " << ang << std::endl; + angle_v=asin(ang); + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); --> nodesToExplore(direction[node_choosen]); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + } + } + + int AStarGradient::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void AStarGradient::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } + +PathData AStarGradient::findPath(const Vec3i &_source, const Vec3i &_target) +{ + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + //Timer to record the execution time, not + //really important + 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 + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) { solved = true; break; } + + closedSet_.push_back(current); + //This flags are used to avoid search in the containers, + //for speed reasons. + current->isInOpenList = false; + current->isInClosedList = true; + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + } + 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 + //the same way in every new algorithm implemented. +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + openSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; +} +} diff --git a/src/Planners/LazyThetaStar.cpp b/src/Planners/LazyThetaStar.cpp index e13cdc1..9a87920 100644 --- a/src/Planners/LazyThetaStar.cpp +++ b/src/Planners/LazyThetaStar.cpp @@ -90,6 +90,8 @@ namespace Planners #endif exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + // exploreNeighbours_Gradient(current, _target, indexByWorldPosition); } main_timer.toc(); diff --git a/src/Planners/LazyThetaStarM1.cpp b/src/Planners/LazyThetaStarM1.cpp index c0d7b7d..0de7fdd 100644 --- a/src/Planners/LazyThetaStarM1.cpp +++ b/src/Planners/LazyThetaStarM1.cpp @@ -21,10 +21,17 @@ namespace Planners if ( successor2->isInClosedList ) { - auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); - G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); + // USE this cost_term when EDF decrease as distance increases + // auto cost_term = static_cast(cost_weight_ * ((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_))); + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO ¿Por qué se divide entre 100? + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO //Estaba multiplicando por cost_term!!!!! if (G_new < G_max) { + G_max = G_new; _s_aux->parent = successor2; _s_aux->G = G_new; _s_aux->C = cost_term; @@ -37,37 +44,98 @@ namespace Planners inline void LazyThetaStarM1::ComputeCost(Node *_s_aux, Node *_s2_aux) { auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; - if ( distanceParent2_nodes == 0 ) + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ distanceParent2_nodes = 1; + } - auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; - if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) - { - _s2_aux->parent = _s_aux->parent; - _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; - _s2_aux->C = cost_term; + // Line of sight + else{ + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + + // auto cost_term = static_cast(cost_weight_ * (( static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost))/2) * dist_scale_factor_reduced_))) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } } + + // ROS_INFO("Using resolution: [%lf]", _s2_aux->cost); + + // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los + // CHANGED 30-Mar-2023 + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->C = cost_term; + // } } unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + float dist_current_suc; + if(_dirs == 8){ - cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + cost += dist_current_suc; + }else{ - cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + cost += dist_current_suc; } - auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); //+grad_suc + + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); cost += cost_term; _suc->C = cost_term; return cost; } - + // // NUEVO + // unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = _current->G; + + // auto cost_term = static_cast((_suc->cost/100) * dist_scale_factor_reduced_); + // if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_)*cost_term; //This is more efficient + // }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_))*cost_term; //This is more efficient + // } + + // // cost += cost_term; + // _suc->C = cost_term; + + // return cost; + // } } \ No newline at end of file diff --git a/src/Planners/LazyThetaStarM1Mod.cpp b/src/Planners/LazyThetaStarM1Mod.cpp index 9cac8e1..2ad1c51 100644 --- a/src/Planners/LazyThetaStarM1Mod.cpp +++ b/src/Planners/LazyThetaStarM1Mod.cpp @@ -8,14 +8,39 @@ namespace Planners void LazyThetaStarM1Mod::ComputeCost(Node *_s_aux, Node *_s2_aux) { auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // JAC Prueba: Quitar para original + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux - auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_); - if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) - { - _s2_aux->parent = _s_aux->parent; - _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; - _s2_aux->C = cost_term; + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; } + // Line of sight + else{ + auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_); + // auto cost_term = static_cast((_s2_aux->cost/100) * dist_scale_factor_reduced_); //NUEVO + // auto cost_term = static_cast(cost_weight_ * ((_s2_aux->cost+_s_aux->parent->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // JAC: Quitar para original + if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2 * cost_term; //NUEVO + _s2_aux->C = cost_term; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + + // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los + // CHANGED 30-Mar-2023 + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_); + // // auto cost_term = static_cast(cost_weight_ * ((_s2_aux->cost+_s_aux->parent->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // JAC: Quitar para original + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->C = cost_term; + // } } } \ No newline at end of file diff --git a/src/Planners/LazyThetaStarM1_Backup.cpp b/src/Planners/LazyThetaStarM1_Backup.cpp new file mode 100644 index 0000000..34194a5 --- /dev/null +++ b/src/Planners/LazyThetaStarM1_Backup.cpp @@ -0,0 +1,130 @@ +#include "Planners/LazyThetaStarM1.hpp" + +namespace Planners +{ + LazyThetaStarM1::LazyThetaStarM1(bool _use_3d):LazyThetaStar(_use_3d, "lazythetastarm1") {} + LazyThetaStarM1::LazyThetaStarM1(bool _use_3d, std::string _name = "lazythetastarm1" ):LazyThetaStar(_use_3d, _name) {} + + void LazyThetaStarM1::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO ¿Por qué se divide entre 100? + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO //Estaba multiplicando por cost_term!!!!! + if (G_new < G_max) + { + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_term; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + inline void LazyThetaStarM1::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + auto cost_term = static_cast(cost_weight_ * (( static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + } + } + + // ROS_INFO("Using resolution: [%lf]", _s2_aux->cost); + + // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los + // CHANGED 30-Mar-2023 + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->C = cost_term; + // } + } + + + unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + float dist_current_suc; + + + if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + cost += dist_current_suc; + + }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + cost += dist_current_suc; + } + + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * ((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + cost += cost_term; + _suc->C = cost_term; + + return cost; + } + // // NUEVO + // unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = _current->G; + + // auto cost_term = static_cast((_suc->cost/100) * dist_scale_factor_reduced_); + // if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_)*cost_term; //This is more efficient + // }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_))*cost_term; //This is more efficient + // } + + // // cost += cost_term; + // _suc->C = cost_term; + + // return cost; + // } +} \ No newline at end of file diff --git a/src/Planners/LazyThetaStarM1_IROS.cpp b/src/Planners/LazyThetaStarM1_IROS.cpp new file mode 100644 index 0000000..ad689fe --- /dev/null +++ b/src/Planners/LazyThetaStarM1_IROS.cpp @@ -0,0 +1,134 @@ +#include "Planners/LazyThetaStarM1.hpp" + +namespace Planners +{ + LazyThetaStarM1::LazyThetaStarM1(bool _use_3d):LazyThetaStar(_use_3d, "lazythetastarm1") {} + LazyThetaStarM1::LazyThetaStarM1(bool _use_3d, std::string _name = "lazythetastarm1" ):LazyThetaStar(_use_3d, _name) {} + + void LazyThetaStarM1::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); + // AHORA + auto cost_term = static_cast(cost_weight_ * ((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO ¿Por qué se divide entre 100? + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO //Estaba multiplicando por cost_term!!!!! + if (G_new < G_max) + { + G_max = G_new; + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_term; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + inline void LazyThetaStarM1::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + auto cost_term = static_cast(cost_weight_ * (( static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + + // ROS_INFO("Using resolution: [%lf]", _s2_aux->cost); + + // Before else was not so the cost_term and the rest were computed although there were not line of sight or the distance were greater than max_los + // CHANGED 30-Mar-2023 + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->C = cost_term; + // } + } + + + unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + float dist_current_suc; + + + if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + cost += dist_current_suc; + + }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + cost += dist_current_suc; + } + + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(cost_weight_ * _suc->cost * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * ((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + cost += cost_term; + _suc->C = cost_term; + + return cost; + } + // // NUEVO + // unsigned int LazyThetaStarM1::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = _current->G; + + // auto cost_term = static_cast((_suc->cost/100) * dist_scale_factor_reduced_); + // if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_)*cost_term; //This is more efficient + // }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_))*cost_term; //This is more efficient + // } + + // // cost += cost_term; + // _suc->C = cost_term; + + // return cost; + // } +} \ No newline at end of file diff --git a/src/Planners/LazyThetaStar_EDF.cpp b/src/Planners/LazyThetaStar_EDF.cpp new file mode 100644 index 0000000..0bc8e9f --- /dev/null +++ b/src/Planners/LazyThetaStar_EDF.cpp @@ -0,0 +1,340 @@ +#include "Planners/LazyThetaStar_EDF.hpp" + +namespace Planners +{ + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d):ThetaStar(_use_3d, "lazythetastaredf") {} + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d, std::string _name = "lazythetastaredf" ):ThetaStar(_use_3d, _name) {} + + // SetVertex original of the LazyThetaStar + // void LazyThetaStarGradient::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + + // if (!LineOfSight::bresenham3D((_s_aux->parent), _s_aux, discrete_world_)) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux); + // if (G_new < G_max) + // { + // G_max = G_new; + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + + // // This is the SetVertex of the IROS paper + // void LazyThetaStarEDF::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + // if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // cost_weight_ = (4*successor2->cost - _s_aux->cost); + // auto cost_term = static_cast(cost_weight_ * ((successor2->cost + _s_aux->cost)/2) * dist_scale_factor_reduced_); + // // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); // ESTABA ESTE 09-05-2023 + + // // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO + // if (G_new < G_max) + // { + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->C = cost_term; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + +// This is the SetVertex to test + void LazyThetaStarEDF::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + // auto cost_term = static_cast(successor2->cost * dist_scale_factor_reduced_); + // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); + // auto cost_term = static_cast(cost_weight_ * ((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + // auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_))); + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->cost) + static_cast(successor2->cost))*(0.5)) * dist_scale_factor_reduced_))); + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO + // G_new = successor2->G + (geometry::distanceBetween2Nodes(successor2, _s_aux) cost_term); //NUEVO + if (G_new < G_max) + { + G_max = G_new; + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_term; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + + // ComputeCost original of the LazyThetaStar + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + + // if ( (_s_aux->parent->G + distanceParent2) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + + // // This is the computeCost of the IROS paper + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // // ROS_INFO("Compute COST"); + + // auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // // No line of sight or distance greater than max_line_of_sight_cells + // if ( distanceParent2_nodes == 0 ){ + // distanceParent2_nodes = 1; + // } + + // // Line of sight + // else{ + // // From LazyThetaSatarM1 + // cost_weight_ = 4*(_s2_aux->cost -_s_aux->parent->cost); + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + // // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + // _s2_aux->C = cost_term; + // } + // } + // } + + // This is the computeCost to test + inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // From LazyThetaSatarM1 + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * (( static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + // auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost))/2) * dist_scale_factor_reduced_))) * distanceParent2_nodes; + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost))*(0.5)) * dist_scale_factor_reduced_))) * distanceParent2_nodes; + + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + (distanceParent2 + cost_term) ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + // ******************************************// + // TRIANGLE INEQUALITY + // ******************************************// + // // Cost from current (_s_aux) to succesor (_s2_aux) because the triangle inequality is not fulfilled + // auto cost_term2 = static_cast(cost_weight_ * (1/(((static_cast(_s_aux->cost) + static_cast(_s2_aux->cost))*(0.5)) * dist_scale_factor_reduced_))); //dist_scale_factor_reduced_=1 + // unsigned int G_new = _s2_aux->G + geometry::distanceBetween2Nodes(_s2_aux, _s_aux) + cost_term2; + + // // if ( (( _s_aux->parent->G + (distanceParent2 + cost_term) ) < _s2_aux->G) && (( _s_aux->parent->G + (distanceParent2 + cost_term) ) < G_new)) + // if (( _s_aux->parent->G + (distanceParent2 + cost_term) ) < _s2_aux->G) + // { + // // std::cout << "LoS" << std::endl; + // if (( _s_aux->parent->G + (distanceParent2 + cost_term) ) < G_new) + // { + // _s2_aux->parent = _s_aux->parent; + // // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + // _s2_aux->C = cost_term; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // // std::cout << "PARENT" << std::endl; + // } + // else{ + // std::cout << "NOOOOO PARENT" << std::endl; + // } + + // } + // else{ + // // std::cout << "ENTRA1" << std::endl; + // if (G_new < _s2_aux->G){ + // std::cout << "ENTRA2" << std::endl; // AQUI NO ENTRA NUNCA + // _s_aux->parent = _s2_aux; + // _s_aux->G = G_new; + // _s_aux->C = cost_term2; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // ******************************************// + } + } + + // This is the computeG to test: G*cost + unsigned int LazyThetaStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + // float dist_current_suc; + + + if(_dirs == 8){ + cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // cost += dist_current_suc; + + }else{ + cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // cost += dist_current_suc; + } + + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + // auto cost_term = static_cast(cost_weight_ * ((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + // COST CONSIDERING EDF INCREASING AS DISTANCE INCREASE + + // auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_))); + auto cost_term = static_cast(cost_weight_ * (1/(((static_cast(_current->cost) + static_cast(_suc->cost))*(0.5)) * dist_scale_factor_reduced_))); + cost += cost_term; + // cost += dist_scale_factor_ * cost_term; + _suc->C = cost_term; + + return cost; + } + + PathData LazyThetaStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) + { + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + utils::Clock main_timer; + main_timer.tic(); + + line_of_sight_checks_ = 0; + + MagicalMultiSet openSet; + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + // std::cout << "current G: " << current->G << std::endl; + // std::cout << "current cost: " << current->cost << std::endl; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + if (indexByCost.empty()){ + std::cout << "OPEN SET IS EMPTY" << std::endl; + // Call the requestPathService service with M1. + } + + + } + main_timer.toc(); + + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } + +} diff --git a/src/Planners/LazyThetaStar_EDF_1.cpp b/src/Planners/LazyThetaStar_EDF_1.cpp new file mode 100644 index 0000000..12f6a11 --- /dev/null +++ b/src/Planners/LazyThetaStar_EDF_1.cpp @@ -0,0 +1,321 @@ +#include "Planners/LazyThetaStar_EDF.hpp" + +namespace Planners +{ + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d):ThetaStar(_use_3d, "lazythetastaredf") {} + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d, std::string _name = "lazythetastaredf" ):ThetaStar(_use_3d, _name) {} + + // SetVertex original of the LazyThetaStar + // void LazyThetaStarGradient::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + + // if (!LineOfSight::bresenham3D((_s_aux->parent), _s_aux, discrete_world_)) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux); + // if (G_new < G_max) + // { + // G_max = G_new; + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + + // // This is the SetVertex of the IROS paper + // void LazyThetaStarEDF::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + // if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // cost_weight_ = (4*successor2->cost - _s_aux->cost); + // auto cost_term = static_cast(cost_weight_ * ((successor2->cost + _s_aux->cost)/2) * dist_scale_factor_reduced_); + // // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); // ESTABA ESTE 09-05-2023 + + // // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO + // if (G_new < G_max) + // { + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->C = cost_term; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + +// This is the SetVertex to test + void LazyThetaStarEDF::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + // auto cost_term = static_cast(successor2->cost * dist_scale_factor_reduced_); + // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); + auto cost_term = static_cast(cost_weight_ * ((static_cast(_s_aux->cost) + static_cast(successor2->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO + // G_new = successor2->G + (geometry::distanceBetween2Nodes(successor2, _s_aux) cost_term); //NUEVO + if (G_new < G_max) + { + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_term; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + + // ComputeCost original of the LazyThetaStar + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + + // if ( (_s_aux->parent->G + distanceParent2) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + + // // This is the computeCost of the IROS paper + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // // ROS_INFO("Compute COST"); + + // auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // // No line of sight or distance greater than max_line_of_sight_cells + // if ( distanceParent2_nodes == 0 ){ + // distanceParent2_nodes = 1; + // } + + // // Line of sight + // else{ + // // From LazyThetaSatarM1 + // cost_weight_ = 4*(_s2_aux->cost -_s_aux->parent->cost); + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + // // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + // _s2_aux->C = cost_term; + // } + // } + // } + + // This is the computeCost to test + inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // From LazyThetaSatarM1 + // auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + auto cost_term = static_cast(cost_weight_ * (( static_cast(_s_aux->parent->cost) + static_cast(_s2_aux->cost) ) /2) * dist_scale_factor_reduced_) * distanceParent2_nodes; // IROS Paper + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + (distanceParent2 + cost_term) ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + } + } + } + + // // This is the computeG of the IROS paper + // unsigned int LazyThetaStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = _current->G; + // // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // // float grad_suc; // or _suc->grad + // float dist_current_suc; + + + // if(_dirs == 8){ + // // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // cost += dist_current_suc; + + // }else{ + // // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // cost += dist_current_suc; + // } + + // // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // // grad_suc=(_suc->cost - _current->cost); + // // std::cout << "dist current suc: " << dist_current_suc << std::endl; + // // std::cout << "current cost: " << _current->cost << std::endl; + // // std::cout << "current G: " << _current->G << std::endl; + // // std::cout << "suc: " << _suc->cost << std::endl; + // // std::cout << "gradiente: " << grad_suc << std::endl; + // // CALCULAR COST_WEIGHT A PARTIR DEL GRADIENTE? + // cost_weight_ = 4*(_suc->cost - _current->cost); + // auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + // cost += cost_term; + // _suc->C = cost_term; + + // return cost; + // } + + // This is the computeG to test: G*cost + unsigned int LazyThetaStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + float dist_current_suc; + + + if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + cost += dist_current_suc; + + }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + cost += dist_current_suc; + } + + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + auto cost_term = static_cast(cost_weight_ * ((static_cast(_current->cost) + static_cast(_suc->cost))/2) * dist_scale_factor_reduced_); //+grad_suc // IROS Paper + cost += cost_term; + // cost += dist_scale_factor_ * cost_term; + _suc->C = cost_term; + + return cost; + } + + PathData LazyThetaStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) + { + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + utils::Clock main_timer; + main_timer.tic(); + + line_of_sight_checks_ = 0; + + MagicalMultiSet openSet; + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + // std::cout << "current G: " << current->G << std::endl; + // std::cout << "current cost: " << current->cost << std::endl; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + + } + main_timer.toc(); + + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } + +} diff --git a/src/Planners/LazyThetaStar_EDF_backup.cpp b/src/Planners/LazyThetaStar_EDF_backup.cpp new file mode 100644 index 0000000..bc15d9f --- /dev/null +++ b/src/Planners/LazyThetaStar_EDF_backup.cpp @@ -0,0 +1,313 @@ +#include "Planners/LazyThetaStar_EDF.hpp" + +namespace Planners +{ + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d):ThetaStar(_use_3d, "lazythetastaredf") {} + LazyThetaStarEDF::LazyThetaStarEDF(bool _use_3d, std::string _name = "lazythetastaredf" ):ThetaStar(_use_3d, _name) {} + + // SetVertex original of the LazyThetaStar + // void LazyThetaStarGradient::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + + // if (!LineOfSight::bresenham3D((_s_aux->parent), _s_aux, discrete_world_)) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux); + // if (G_new < G_max) + // { + // G_max = G_new; + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + + // This is the SetVertex of the IROS paper + void LazyThetaStarEDF::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + auto cost_term = static_cast(cost_weight_ * ((successor2->cost + _s_aux->cost)/2) * dist_scale_factor_reduced_); + // auto cost_term = static_cast(cost_weight_ * successor2->cost * dist_scale_factor_reduced_); // ESTABA ESTE 09-05-2023 + + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; + // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; //NUEVO + if (G_new < G_max) + { + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->C = cost_term; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + +// // This is the SetVertex to test +// void LazyThetaStarEDF::SetVertex(Node *_s_aux) +// { +// line_of_sight_checks_++; +// if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) +// { +// unsigned int G_max = std::numeric_limits::max(); +// unsigned int G_new; + +// for (const auto &i: direction) +// { +// Vec3i newCoordinates(_s_aux->coordinates + i); +// Node *successor2 = discrete_world_.getNodePtr(newCoordinates); +// if (successor2 == nullptr || successor2->occuppied ) continue; + +// if ( successor2->isInClosedList ) +// { +// auto cost_term = static_cast(successor2->cost * dist_scale_factor_reduced_); +// // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux) + cost_term; +// // auto cost_term = static_cast((successor2->cost/100) * dist_scale_factor_reduced_); //NUEVO +// G_new = successor2->G + (geometry::distanceBetween2Nodes(successor2, _s_aux) * cost_term); //NUEVO +// if (G_new < G_max) +// { +// _s_aux->parent = successor2; +// _s_aux->G = G_new; +// _s_aux->C = cost_term; +// _s_aux->gplush = _s_aux->G + _s_aux->H; +// } +// } +// } +// } +// } + + // ComputeCost original of the LazyThetaStar + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + + // if ( (_s_aux->parent->G + distanceParent2) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + + // This is the computeCost of the IROS paper + inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // From LazyThetaSatarM1 + auto cost_term = static_cast(cost_weight_ * ((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + _s2_aux->C = cost_term; + } + } + } + + // // This is the computeCost to test + // inline void LazyThetaStarEDF::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // // ROS_INFO("Compute COST"); + + // auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // // No line of sight or distance greater than max_line_of_sight_cells + // if ( distanceParent2_nodes == 0 ){ + // distanceParent2_nodes = 1; + // } + + // // Line of sight + // else{ + // // From LazyThetaSatarM1 + // auto cost_term = static_cast(((_s_aux->parent->cost + _s2_aux->cost)/2) * dist_scale_factor_reduced_) * distanceParent2_nodes; + // // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + // if ( ( _s_aux->parent->G + (distanceParent2 * cost_term) ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->G = _s2_aux->parent->G + (distanceParent2 * cost_term); + // _s2_aux->C = cost_term; + // } + // } + // } + + // This is the computeG of the IROS paper + unsigned int LazyThetaStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + unsigned int cost = _current->G; + // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // float grad_suc; // or _suc->grad + float dist_current_suc; + + + if(_dirs == 8){ + // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + cost += dist_current_suc; + + }else{ + // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + cost += dist_current_suc; + } + + // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // grad_suc=(_suc->cost - _current->cost); + // std::cout << "dist current suc: " << dist_current_suc << std::endl; + // std::cout << "current cost: " << _current->cost << std::endl; + // std::cout << "current G: " << _current->G << std::endl; + // std::cout << "suc: " << _suc->cost << std::endl; + // std::cout << "gradiente: " << grad_suc << std::endl; + auto cost_term = static_cast(cost_weight_ * ((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + cost += cost_term; + _suc->C = cost_term; + + return cost; + } + + // // This is the computeG to test: G*cost + // unsigned int LazyThetaStarEDF::computeG(const Node* _current, Node* _suc, unsigned int _n_i, unsigned int _dirs){ + + // unsigned int cost = _current->G; + // // EXT: Compute here the gradient from dd_2D_, dd_3D_ and each _suc->cost + // // float grad_suc; // or _suc->grad + // float dist_current_suc; + + + // if(_dirs == 8){ + // // cost += (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // dist_current_suc = (_n_i < 4 ? dist_scale_factor_ : dd_2D_); //This is more efficient + // cost += dist_current_suc; + + // }else{ + // // cost += (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // dist_current_suc = (_n_i < 6 ? dist_scale_factor_ : (_n_i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // cost += dist_current_suc; + // } + + // // grad_suc=(_suc->cost - _current->cost)/(dist_current_suc); + // // grad_suc=(_suc->cost - _current->cost); + // // std::cout << "current: " << _current->cost << std::endl; + // // std::cout << "suc: " << _suc->cost << std::endl; + // // std::cout << "gradiente: " << grad_suc << std::endl; + // auto cost_term = static_cast(((_current->cost + _suc->cost)/2) * dist_scale_factor_reduced_); //+grad_suc + // // cost += cost_term; + // cost += dist_scale_factor_ * cost_term; + // _suc->C = cost_term; + + // return cost; + // } + + PathData LazyThetaStarEDF::findPath(const Vec3i &_source, const Vec3i &_target) + { + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + utils::Clock main_timer; + main_timer.tic(); + + line_of_sight_checks_ = 0; + + MagicalMultiSet openSet; + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + std::cout << "current G: " << current->G << std::endl; + std::cout << "current cost: " << current->cost << std::endl; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + + } + main_timer.toc(); + + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } + +} diff --git a/src/Planners/LazyThetaStar_Gradient.cpp b/src/Planners/LazyThetaStar_Gradient.cpp new file mode 100644 index 0000000..663ceb0 --- /dev/null +++ b/src/Planners/LazyThetaStar_Gradient.cpp @@ -0,0 +1,180 @@ +#include "Planners/LazyThetaStar_Gradient.hpp" + +namespace Planners +{ + LazyThetaStarGradient::LazyThetaStarGradient(bool _use_3d):ThetaStar(_use_3d, "lazythetastargradient") {} + LazyThetaStarGradient::LazyThetaStarGradient(bool _use_3d, std::string _name = "lazythetastargradient" ):ThetaStar(_use_3d, _name) {} + + // SetVertex original of the LazyThetaStar + // void LazyThetaStarGradient::SetVertex(Node *_s_aux) + // { + // line_of_sight_checks_++; + + // if (!LineOfSight::bresenham3D((_s_aux->parent), _s_aux, discrete_world_)) + // { + // unsigned int G_max = std::numeric_limits::max(); + // unsigned int G_new; + + // for (const auto &i: direction) + // { + // Vec3i newCoordinates(_s_aux->coordinates + i); + // Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + + // if (successor2 == nullptr || successor2->occuppied ) continue; + + // if ( successor2->isInClosedList ) + // { + // G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux); + // if (G_new < G_max) + // { + // G_max = G_new; + // _s_aux->parent = successor2; + // _s_aux->G = G_new; + // _s_aux->gplush = _s_aux->G + _s_aux->H; + // } + // } + // } + // } + // } + + void LazyThetaStarGradient::SetVertex(Node *_s_aux) + { + line_of_sight_checks_++; + if (!LineOfSight::bresenham3DWithMaxThreshold(_s_aux->parent, _s_aux, discrete_world_, max_line_of_sight_cells_ )) + { + unsigned int G_max = std::numeric_limits::max(); + unsigned int G_new; + + for (const auto &i: direction) + { + Vec3i newCoordinates(_s_aux->coordinates + i); + Node *successor2 = discrete_world_.getNodePtr(newCoordinates); + if (successor2 == nullptr || successor2->occuppied ) continue; + + if ( successor2->isInClosedList ) + { + G_new = successor2->G + geometry::distanceBetween2Nodes(successor2, _s_aux); + if (G_new < G_max) + { + G_max = G_new; + _s_aux->parent = successor2; + _s_aux->G = G_new; + _s_aux->gplush = _s_aux->G + _s_aux->H; + } + } + } + } + } + + // ComputeCost original of the LazyThetaStar + // inline void LazyThetaStarGradient::ComputeCost(Node *_s_aux, Node *_s2_aux) + // { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + + // if ( (_s_aux->parent->G + distanceParent2) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2; + // _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + // } + // } + + inline void LazyThetaStarGradient::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // std::cout << "distanceParent2: " << distanceParent2 << std::endl; + // ROS_INFO("Compute COST"); + + auto distanceParent2_nodes = LineOfSight::nodesInLineBetweenTwoNodes(_s_aux->parent, _s2_aux, discrete_world_, max_line_of_sight_cells_); //REVISAR _s_aux->parent o _s_aux + // std::cout << "distanceParent2_nodes: " << distanceParent2_nodes << std::endl; + + // No line of sight or distance greater than max_line_of_sight_cells + if ( distanceParent2_nodes == 0 ){ + distanceParent2_nodes = 1; + } + + // Line of sight + else{ + // From LazyThetaSatarM1 + // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2_nodes; + // // auto cost_term = static_cast(cost_weight_ * _s2_aux->cost * dist_scale_factor_reduced_) * distanceParent2; + // if ( ( _s_aux->parent->G + distanceParent2 + cost_term ) < _s2_aux->G ) + // { + // _s2_aux->parent = _s_aux->parent; + // // _s2_aux->G = _s2_aux->parent->G + geometry::distanceBetween2Nodes(_s2_aux->parent, _s2_aux) + cost_term; + // _s2_aux->G = _s2_aux->parent->G + distanceParent2 + cost_term; + // _s2_aux->C = cost_term; + // } + + if ( (_s_aux->parent->G + distanceParent2) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s2_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + PathData LazyThetaStarGradient::findPath(const Vec3i &_source, const Vec3i &_target) + { + Node *current = nullptr; + + bool solved{false}; + + discrete_world_.getNodePtr(_source)->parent = new Node(_source); + discrete_world_.setOpenValue(_source, true); + + utils::Clock main_timer; + main_timer.tic(); + + line_of_sight_checks_ = 0; + + MagicalMultiSet openSet; + + node_by_cost& indexByCost = openSet.get(); + node_by_position& indexByWorldPosition = openSet.get(); + + indexByCost.insert(discrete_world_.getNodePtr(_source)); + while (!indexByCost.empty()) + { + + auto it = indexByCost.begin(); + current = *it; + indexByCost.erase(indexByCost.begin()); + + if (current->coordinates == _target) + { + solved = true; + break; + } + closedSet_.push_back(current); + + current->isInOpenList = false; + current->isInClosedList = true; + + SetVertex(current); +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + publishROSDebugData(current, indexByCost, closedSet_); +#endif + + // exploreNeighbours(current, _target, indexByWorldPosition); + // EXPLORING NEIGHBOURS CONSIDERING THE EDF GRADIENT + exploreNeighbours_Gradient(current, _target, indexByWorldPosition); + + } + main_timer.toc(); + + PathData result_data = createResultDataObject(current, main_timer, closedSet_.size(), + solved, _source, line_of_sight_checks_); + +#if defined(ROS) && defined(PUB_EXPLORED_NODES) + explored_node_marker_.points.clear(); +#endif + closedSet_.clear(); + delete discrete_world_.getNodePtr(_source)->parent; + + discrete_world_.resetWorld(); + return result_data; + } + +} diff --git a/src/Planners/ThetaStar (copy).cpp b/src/Planners/ThetaStar (copy).cpp new file mode 100644 index 0000000..9db3f35 --- /dev/null +++ b/src/Planners/ThetaStar (copy).cpp @@ -0,0 +1,955 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + // Vec3i cost_vector[26]; + // Vec3f cost_unit_vector[26]; + std::vector cost_vector{26}; + std::vector index_cost_vector{26}; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + // float max_gradient=0; + // int index_max_gradient=0; + // float min_dist_cost=100; + // int index_min_dist_cost=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + // float module_cost; + + // if ( successor_grad == nullptr || + // successor_grad->isInClosedList || + // successor_grad->occuppied ) { + // // gradient[i]=-1; // Dejar o quitar??? + // // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + if ( successor_grad == nullptr || + successor_grad->occuppied ) { + continue; + } + + if ( successor_grad->isInClosedList ){ + // ThetaStar2 + // gradient[i]=-1; + // ThetaStar1 + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + } + + // if (! successor_grad->isInOpenList ) { + else { + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + // NUEVO + // Compute the smallest distance cost + // if (successor_grad->cost < min_dist_cost){ + // min_dist_cost=successor_grad->cost; + // index_min_dist_cost=i; // cuidado porque se suma 1 por empezar de cero. + // } + // else if (successor_grad->cost == min_dist_cost){ + + // } + // cost_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + // module_cost=0; + // module_cost = sqrt(pow(cost_vector[i].x, 2) + pow(cost_vector[i].y, 2) + pow(cost_vector[i].z, 2)); + // cost_unit_vector[i].x = cost_vector[i].x/module_cost; + // cost_unit_vector[i].y = cost_vector[i].y/module_cost; + // cost_unit_vector[i].z = cost_vector[i].z/module_cost; + + // cost_vector[i]=successor_grad->cost; + // index_cost_vector[i]=i; + // std::cout << "INDEX cost vector: " << index_cost_vector[i] << std::endl; + // std::cout << "cost vector: " << cost_vector[i] << std::endl; + // std::cout << "index min cost: " << index_min_dist_cost << std::endl; + // NUEVO** + } + + // Aqui considera el gradiente de nodos que están en el CloseList + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + } + + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // NUEVO + // std::cout << "index min dist cost: " << index_min_dist_cost << std::endl; + // std::cout << "min dist cost: " << min_dist_cost << std::endl; + + // Ordenar el cost_vector de menor a mayor + // std::cout << "SIN ORDENAR " << std::endl; + // for (unsigned int i = 0; i < (direction.size()); ++i) { + // std::cout << "cost vector: " << cost_vector[i] << std::endl; + // std::cout << "index cost vector: " << index_cost_vector[i] << std::endl; + // } + // float aux1 = 0; + // float aux2 = 0; + // for (unsigned int i = 0; i < (direction.size()); ++i) { + // for (unsigned int j = 0; j < (direction.size()-1); ++j) { + // if (cost_vector[j] > cost_vector[j+1]){ + // aux1 = cost_vector[j]; + // cost_vector[j] = cost_vector[j+1]; + // cost_vector[j+1] = aux1; + // aux2 = index_cost_vector[j]; + // index_cost_vector[j] = index_cost_vector[j+1]; + // index_cost_vector[j+1] = aux2; + // } + // } + // } + // std::cout << "ORDENADO " << std::endl; + // for (unsigned int i = 0; i < (direction.size()); ++i) { + // std::cout << "cost vector: " << cost_vector[i] << std::endl; + // std::cout << "index cost vector: " << index_cost_vector[i] << std::endl; + // } + // int igual = 0; + // for (unsigned int i = 0; i < (direction.size()); ++i) { + // if (cost_vector[i] == cost_vector[0]){ + // igual = igual + 1; + // } + // } + // std::cout << "IGUAL: " << igual << std::endl; + // int aux_index; + // if (igual > 0){ + // // if ((index_cost_vector[0] == 0) || (index_cost_vector[0] == 1) || (index_cost_vector[0] == 2) || (index_cost_vector[0] == 3) || (index_cost_vector[0] == 4) || (index_cost_vector[0] == 5)) + // if ((index_cost_vector[0] == 6) || (index_cost_vector[0] == 7) || (index_cost_vector[0] == 8) || (index_cost_vector[0] == 9) || (index_cost_vector[0] == 10) || (index_cost_vector[0] == 11) || (index_cost_vector[0] == 12) || (index_cost_vector[0] == 13) || (index_cost_vector[0] == 14) || (index_cost_vector[0] == 15) || (index_cost_vector[0] == 16) || (index_cost_vector[0] == 17)) + // { + // if ((index_cost_vector[1] == 0) || (index_cost_vector[1] == 1) || (index_cost_vector[1] == 2) || (index_cost_vector[1] == 3) || (index_cost_vector[1] == 4) || (index_cost_vector[1] == 5)){ + // aux_index = index_cost_vector[0]; + // index_cost_vector[0] = index_cost_vector[1]; + // index_cost_vector[1] = aux_index; + // } + // } + // else if ((index_cost_vector[0] == 18) || (index_cost_vector[0] == 19) || (index_cost_vector[0] == 20) || (index_cost_vector[0] == 21) || (index_cost_vector[0] == 22) || (index_cost_vector[0] == 23) || (index_cost_vector[0] == 24) || (index_cost_vector[0] == 25)){ + // if ((index_cost_vector[1] == 0) || (index_cost_vector[1] == 1) || (index_cost_vector[1] == 2) || (index_cost_vector[1] == 3) || (index_cost_vector[1] == 4) || (index_cost_vector[1] == 5)){ + // aux_index = index_cost_vector[0]; + // index_cost_vector[0] = index_cost_vector[1]; + // index_cost_vector[1] = aux_index; + // } + // else if ((index_cost_vector[0] == 6) || (index_cost_vector[0] == 7) || (index_cost_vector[0] == 8) || (index_cost_vector[0] == 9) || (index_cost_vector[0] == 10) || (index_cost_vector[0] == 11) || (index_cost_vector[0] == 12) || (index_cost_vector[0] == 13) || (index_cost_vector[0] == 14) || (index_cost_vector[0] == 15) || (index_cost_vector[0] == 16) || (index_cost_vector[0] == 17)){ + // aux_index = index_cost_vector[0]; + // index_cost_vector[0] = index_cost_vector[1]; + // index_cost_vector[1] = aux_index; + // } + // } + // } + + // std::cout << "ORDENADO MEJOR" << std::endl; + // for (unsigned int i = 0; i < (direction.size()); ++i) { + // std::cout << "cost vector: " << cost_vector[i] << std::endl; + // std::cout << "index cost vector: " << index_cost_vector[i] << std::endl; + // } + + // NUEVO** + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node from the gradient + Vec3f vectorNeighbours[1]; + float weight_gradient=1.0; + // vectorNeighbours[0].x=0; + // vectorNeighbours[0].y=0; + // vectorNeighbours[0].z=0; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Compute vector to choose the node from the greatest cost + // Vec3f vectorNeighbours_cost[1]; + // vectorNeighbours_cost[0].x=weight_gradient*gradient_unit_vector[index_min_dist_cost].x+attraction_unit_current[0].x; + // vectorNeighbours_cost[0].y=weight_gradient*gradient_unit_vector[index_min_dist_cost].y+attraction_unit_current[0].y; + // vectorNeighbours_cost[0].z=weight_gradient*gradient_unit_vector[index_min_dist_cost].z+attraction_unit_current[0].z; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // module_vectorNeighbours = sqrt(pow(vectorNeighbours_cost[0].x, 2) + pow(vectorNeighbours_cost[0].y, 2) + pow(vectorNeighbours_cost[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + // angle_h=atan2(vectorNeighbours_cost[0].y,vectorNeighbours_cost[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + // ang=vectorNeighbours_cost[0].z/module_vectorNeighbours; + angle_v=asin(ang); + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + + // float angle_grad_h=0; + // float angle_att_h=0; + // float diff_angle_h=0; + // angle_grad_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // angle_att_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // diff_angle_h=angle_att_h-angle_grad_h; + // std::cout << "angle_grad h: " << angle_grad_h << std::endl; + // std::cout << "angle_att h: " << angle_att_h << std::endl; + // std::cout << "diff angle h: " << diff_angle_h << std::endl; + + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + // std::cout << "SIIIIIII" << std::endl; + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + + UpdateVertex(_current, successor, _index_by_pos); + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + + // UpdateVertex(_current, successor, _index_by_pos); + // } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void ThetaStar::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } +} diff --git a/src/Planners/ThetaStar.cpp b/src/Planners/ThetaStar.cpp index f52f292..ceb32f1 100644 --- a/src/Planners/ThetaStar.cpp +++ b/src/Planners/ThetaStar.cpp @@ -32,10 +32,11 @@ namespace Planners inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) { - auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); line_of_sight_checks_++; if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) { _s2_aux->parent = _s_aux->parent; @@ -80,4 +81,1665 @@ namespace Planners UpdateVertex(_current, successor, _index_by_pos); } } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // ROS_INFO("GRADIENT"); + // Modules + // float mod[26]; + // mod[0]=1; + // mod[1]=1; + // mod[2]=1; + // mod[3]=1; + // mod[4]=1; + // mod[5]=1; + // mod[6]=1.41; + // mod[7]=1.41; + // mod[8]=1.41; + // mod[9]=1.41; + // mod[10]=1.41; + // mod[11]=1.41; + // mod[12]=1.41; + // mod[13]=1.41; + // mod[14]=1.41; + // mod[15]=1.41; + // mod[16]=1.41; + // mod[17]=1.41; + // mod[18]=1.73; + // mod[19]=1.73; + // mod[20]=1.73; + // mod[21]=1.73; + // mod[22]=1.73; + // mod[23]=1.73; + // mod[24]=1.73; + // mod[25]=1.73; + + angles_h[0]=1.5708; + angles_v[0]=0; + angles_h[1]=-1.5708; + angles_v[1]=0; + angles_h[2]=0; + angles_v[2]=0; + angles_h[3]=3.14159; + angles_v[3]=0; + angles_h[4]=0; + angles_v[4]=1.5708; + angles_h[5]=0; + angles_v[5]=-1.5708; + angles_h[6]=-0.785398; + angles_v[6]=0; + angles_h[7]=2.35619; + angles_v[7]=0; + angles_h[8]=-2.35619; + angles_v[8]=0; + angles_h[9]=0.785398; + angles_v[9]=0; + angles_h[10]=3.14159; + angles_v[10]=-0.788391; + angles_h[11]=0; + angles_v[11]=0.788391; + angles_h[12]=0; + angles_v[12]=-0.788391; + angles_h[13]=3.14159; + angles_v[13]=0.788391; + angles_h[14]=-1.5708; + angles_v[14]=0.788391; + angles_h[15]=1.5708; + angles_v[15]=0.788391; + angles_h[16]=1.5708; + angles_v[16]=-0.788391; + angles_h[17]=-1.5708; + angles_v[17]=-0.788391; + angles_h[18]=-2.35619; + angles_v[18]=0.616318; + angles_h[19]=0.785398; + angles_v[19]=0.616318; + angles_h[20]=2.35619; + angles_v[20]=0.616318; + angles_h[21]=-0.785398; + angles_v[21]=0.616318; + angles_h[22]=-2.35619; + angles_v[22]=-0.616318; + angles_h[23]=0.785398; + angles_v[23]=-0.616318; + angles_h[24]=2.35619; + angles_v[24]=-0.616318; + angles_h[25]=-0.785398; + angles_v[25]=-0.616318; + + // Variables to compute the gradient of each node and the total one + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Variables to compute unit vector of the goal from each successor + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient (26 neighbours) + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->occuppied ) { + continue; + } + + gradient[i]=computeGradient(_current, successor_grad, i, direction.size()); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + + } + + // The minimum when EDF increases as distance increases + float max_gradient=0; + int index_max_gradient=1; + // int veces=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] < max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // i starts from 0 (in my notes from 1) + } + // if (gradient[i] == max_gradient) { + // veces=veces+1; + // } + + } + // std::cout << "veces: " << veces << std::endl; + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node from the gradient and the goal (attraction) + Vec3f vectorNeighbours[1]; + float weight_gradient=1; + + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // // // IF WE WANT TO CONSIDER THE ANGLES BETWEEN GOAL AND GRADIENT VECTOR TO CHOOSE THE NUMBER OF NEIGHBOURS + // // Angle of the gradient DESCOMENTAR + // float angle_gradient_h=0; // horizontal angle X-Y + // angle_gradient_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + + // float angle_gradient_v=0; // vertical angle + // float ang_gradient=0; + // float module_gradient=0; + // module_gradient = sqrt(pow(gradient_unit_vector[index_max_gradient].x, 2) + pow(gradient_unit_vector[index_max_gradient].y, 2) + pow(gradient_unit_vector[index_max_gradient].z, 2)); + // ang_gradient=gradient_unit_vector[index_max_gradient].z/module_gradient; + // angle_gradient_v=asin(ang_gradient); + + // std::cout << "angle gradient h: " << angle_gradient_h << std::endl; + + // // Angle of the attraction DESCOMENTAR + // float angle_attrac_h=0; // horizontal angle X-Y + // angle_attrac_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + + // float angle_attrac_v=0; // vertical angle + // float ang_attrac=0; + // float module_attrac=0; + // module_attrac = sqrt(pow(attraction_unit_current[0].x, 2) + pow(attraction_unit_current[0].y, 2) + pow(attraction_unit_current[0].z, 2)); + // ang_attrac=attraction_unit_current[0].z/module_attrac; + // angle_attrac_v=asin(ang_attrac); + + // std::cout << "angle attrac h: " << angle_attrac_h << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // module_vectorNeighbours = sqrt(pow(vectorNeighbours_cost[0].x, 2) + pow(vectorNeighbours_cost[0].y, 2) + pow(vectorNeighbours_cost[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector vectorNeighbours + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + // WITHOUT CONSIDERING THE DIFFERENCE BETWEEN THE ANGLE OF THE GRADIENT AND THE ANGLE OF THE GOAL. + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + angle_v=asin(ang); + + // /********************************************************************************************************/ + // // CONSIDERING THE DIFFERENCE BETWEEN THE ANGLE OF THE GRADIENT AND THE ANGLE OF THE GOAL. + // // Angle of the gradient DESCOMENTAR + // float angle_gradient_h=0; // horizontal angle X-Y + // angle_gradient_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // // std::cout << "angle gradient h: " << angle_gradient_h << std::endl; + + // // Angle of the attraction DESCOMENTAR + // float angle_attrac_h=0; // horizontal angle X-Y + // angle_attrac_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // // std::cout << "angle attrac h: " << angle_attrac_h << std::endl; + + // unsigned int num_nodes; + // Vec3i node_choosenCoordinates[13]; //11 + // int index_nodes[13]; //11 + + // if (angle_gradient_h != 0){ + // if (abs(angle_attrac_h - angle_gradient_h) > (M_PI/2)){ //1.3962634: 80 + // // It works with 9-10 and considering 75 degrees. + // // if (abs(angle_attrac_h - angle_gradient_h) > (1.3089)){ //1.308996939: 75 + // num_nodes=13; //11 + // // Vec3i node_choosenCoordinates[11]; + // // int index_nodes[11]; + // // std::cout << "111: " << std::endl; + // } + // else { + // num_nodes=11; //9 + // // Vec3i node_choosenCoordinates[9]; + // // int index_nodes[9]; + // // std::cout << "ang: " << ang << std::endl; + // // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // // std::cout << "222: " << std::endl; + // } + // } + // // If angle_gradient_h == 0 + // else{ + // num_nodes=13; //11 + // // Vec3i node_choosenCoordinates[11]; + // // int index_nodes[11]; + // // std::cout << "333: " << std::endl; + // } + // /********************************************************************************************************/ + + // // If we want to compute the angle_h depending on the difference between angle of gradient and the angle of goal + // /*******/ + // std::cout << "angle_attrac_h: " << angle_attrac_h << std::endl; + // std::cout << "angle_attrac_v: " << angle_attrac_v << std::endl; + // std::cout << "gradient_x: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + // std::cout << "angle_gradient_h: " << angle_gradient_h << std::endl; + // std::cout << "angle_gradient_v: " << angle_gradient_v << std::endl; + // if (angle_gradient_h != 0){ + // if (abs(angle_attrac_h - angle_gradient_h) > (M_PI/2)){ + // if (angle_attrac_h <= (angle_gradient_h + M_PI)){ + // angle_h = angle_gradient_h + (M_PI/2); + // if (angle_h > M_PI){ + // angle_h = angle_h - (2*M_PI); + // } + // ang=vectorNeighbours[0].z/module_vectorNeighbours; + // angle_v=asin(ang); + // } + // else { + // angle_h = angle_gradient_h - (M_PI/2); + // if (angle_h < (-M_PI)){ + // angle_h = (2*M_PI) + angle_h; + // } + // ang=vectorNeighbours[0].z/module_vectorNeighbours; + // angle_v=asin(ang); + // } + // } + // else { + // angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + // ang=vectorNeighbours[0].z/module_vectorNeighbours; + // angle_v=asin(ang); + // // std::cout << "ang: " << ang << std::endl; + // // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // } + // } + // // If angle_gradient_h == 0 + // else{ + // angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + // ang=vectorNeighbours[0].z/module_vectorNeighbours; + // angle_v=asin(ang); + // } + // /*******/ + + // std::cout << "angle h: " << angle_h << std::endl; + // std::cout << "angle v: " << angle_v << std::endl; + + // ///////////////////////////////////////////////// + // TO CHANGE TO EXPLORE 9,11, 13, 15 or 17 NODES + Vec3i node_choosenCoordinates[17]; + unsigned int num_nodes=17; + int index_nodes[17]; + // ///////////////////////////////////////////////// + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + CoordinateList _node_choosenCoordinates; + node_choosen=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen, _node_choosenCoordinates); // FUNCTION TO USE + // for (unsigned int i = 0; i < 9; ++i) { + // std::cout << "Node X" << _node_choosenCoordinates[i].x << std::endl; + // std::cout << "Node Y" << _node_choosenCoordinates[i].y << std::endl; + // std::cout << "Node Z" << _node_choosenCoordinates[i].z << std::endl; + // } + + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + if (num_nodes == 10){ + index_nodes[9]=1; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=11; + index_nodes[12]=13; + index_nodes[13]=10; + index_nodes[14]=12; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=0; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=12; + index_nodes[12]=13; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=12; + index_nodes[12]=13; + index_nodes[13]=10; + index_nodes[14]=11; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=4; + index_nodes[11]=11; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=10; + index_nodes[15]=5; + index_nodes[16]=12; + } + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=3; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=8; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + if (num_nodes == 10){ + index_nodes[9]=2; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=4; + index_nodes[11]=14; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=16; + index_nodes[15]=5; + index_nodes[16]=17; + } + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + if (num_nodes == 10){ + index_nodes[9]=5; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if (num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + index_nodes[13]=6; + index_nodes[14]=7; + } + else if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=4; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if (num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=2; + index_nodes[12]=3; + index_nodes[13]=8; + index_nodes[14]=9; + } + else if (num_nodes==17){ + index_nodes[9]=7; + index_nodes[10]=0; + index_nodes[11]=9; + index_nodes[12]=3; + index_nodes[13]=2; + index_nodes[14]=8; + index_nodes[15]=1; + index_nodes[16]=6; + } + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + if (num_nodes == 10){ + index_nodes[9]=7; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=4; + index_nodes[10]=5; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=18; + index_nodes[14]=23; + } + else if (num_nodes==17){ + index_nodes[9]=9; + index_nodes[10]=8; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=16; + if (num_nodes == 10){ + index_nodes[9]=6; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=23; + index_nodes[12]=18; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=23; + index_nodes[12]=18; + index_nodes[13]=19; + index_nodes[14]=22; + } + else if (num_nodes==17){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=18; + index_nodes[12]=19; + index_nodes[13]=22; + index_nodes[14]=23; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=9; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=20; + index_nodes[14]=25; + } + else if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=8; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=25; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=25; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=20; + index_nodes[12]=21; + index_nodes[13]=24; + index_nodes[14]=25; + index_nodes[15]=4; + index_nodes[16]=5; + } + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=11; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=18; + index_nodes[12]=23; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=18; + index_nodes[12]=23; + index_nodes[13]=20; + index_nodes[14]=25; + } + else if (num_nodes==17){ + index_nodes[9]=23; + index_nodes[10]=12; + index_nodes[11]=25; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=20; + index_nodes[15]=13; + index_nodes[16]=18; + } + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes == 10){ + index_nodes[9]=10; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=25; + index_nodes[12]=20; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=25; + index_nodes[12]=20; + index_nodes[13]=18; + index_nodes[14]=23; + } + else if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=13; + index_nodes[11]=18; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=23; + index_nodes[15]=12; + index_nodes[16]=25; + } + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + if (num_nodes == 10){ + index_nodes[9]=13; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=10; + index_nodes[11]=22; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=19; + index_nodes[15]=11; + index_nodes[16]=21; + } + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + if (num_nodes == 10){ + index_nodes[9]=12; + } + else if (num_nodes==11){ + index_nodes[9]=0; + index_nodes[10]=1; + } + else if(num_nodes==13){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=0; + index_nodes[10]=1; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=24; + index_nodes[14]=21; + } + else if (num_nodes==17){ + index_nodes[9]=19; + index_nodes[10]=11; + index_nodes[11]=21; + index_nodes[12]=0; + index_nodes[13]=1; + index_nodes[14]=24; + index_nodes[15]=10; + index_nodes[16]=22; + } + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + if (num_nodes == 10){ + index_nodes[9]=16; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=25; + index_nodes[14]=20; + } + else if (num_nodes==17){ + index_nodes[9]=20; + index_nodes[10]=15; + index_nodes[11]=19; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=22; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + if (num_nodes == 10){ + index_nodes[9]=17; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=23; + index_nodes[14]=18; + } + else if (num_nodes==17){ + index_nodes[9]=18; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=24; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=14; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=22; + index_nodes[12]=19; + index_nodes[13]=25; + index_nodes[14]=20; + } + else if (num_nodes==17){ + index_nodes[9]=22; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=20; + index_nodes[15]=15; + index_nodes[16]=19; + } + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + if (num_nodes == 10){ + index_nodes[9]=15; + } + else if (num_nodes==11){ + index_nodes[9]=2; + index_nodes[10]=3; + } + else if(num_nodes==13){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + } + else if(num_nodes==15){ + index_nodes[9]=2; + index_nodes[10]=3; + index_nodes[11]=24; + index_nodes[12]=21; + index_nodes[13]=23; + index_nodes[14]=18; + } + else if (num_nodes==17){ + index_nodes[9]=24; + index_nodes[10]=16; + index_nodes[11]=23; + index_nodes[12]=2; + index_nodes[13]=3; + index_nodes[14]=18; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=14; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + if (num_nodes == 10){ + index_nodes[9]=23; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=15; + index_nodes[10]=11; + index_nodes[11]=19; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=10; + index_nodes[15]=22; + index_nodes[16]=17; + } + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=22; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=14; + index_nodes[11]=18; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=12; + index_nodes[15]=16; + index_nodes[16]=23; + } + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + if (num_nodes == 10){ + index_nodes[9]=25; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=11; + index_nodes[10]=14; + index_nodes[11]=21; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=10; + index_nodes[15]=24; + index_nodes[16]=16; + } + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=24; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=13; + index_nodes[10]=15; + index_nodes[11]=20; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=12; + index_nodes[15]=17; + index_nodes[16]=25; + } + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + if (num_nodes == 10){ + index_nodes[9]=19; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=16; + index_nodes[12]=14; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=16; + index_nodes[10]=12; + index_nodes[11]=23; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=13; + index_nodes[15]=14; + index_nodes[16]=18; + } + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=18; + } + else if (num_nodes==11){ + index_nodes[9]=6; + index_nodes[10]=7; + } + else if(num_nodes==13){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=6; + index_nodes[10]=7; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=17; + index_nodes[14]=15; + } + else if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=17; + index_nodes[11]=22; + index_nodes[12]=6; + index_nodes[13]=7; + index_nodes[14]=11; + index_nodes[15]=19; + index_nodes[16]=15; + } + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=7; + index_nodes[8]=0; + if (num_nodes == 10){ + index_nodes[9]=21; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=17; + index_nodes[12]=15; + index_nodes[13]=12; + index_nodes[14]=13; + } + else if (num_nodes==17){ + index_nodes[9]=12; + index_nodes[10]=17; + index_nodes[11]=25; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=13; + index_nodes[15]=15; + index_nodes[16]=20; + } + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + if (num_nodes == 10){ + index_nodes[9]=20; + } + else if (num_nodes==11){ + index_nodes[9]=8; + index_nodes[10]=9; + } + else if(num_nodes==13){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + } + else if(num_nodes==15){ + index_nodes[9]=8; + index_nodes[10]=9; + index_nodes[11]=10; + index_nodes[12]=11; + index_nodes[13]=16; + index_nodes[14]=14; + } + else if (num_nodes==17){ + index_nodes[9]=10; + index_nodes[10]=16; + index_nodes[11]=24; + index_nodes[12]=8; + index_nodes[13]=9; + index_nodes[14]=11; + index_nodes[15]=14; + index_nodes[16]=21; + } + } + + for (unsigned int i = 0; i < num_nodes; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + // std::cout << "coord_X: " << node_choosenCoordinates[i].x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates[i].y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates[i].z << std::endl; + } + + // direction.size() should be substituted by the number of neighbours to explore + // num_nodes is the size of node_choosenCoordinates + for (unsigned int i = 0; i < num_nodes; ++i) { // when we consider the gradient to choose explored nodes + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + int node=0; + // // Modules + // float mod[26]; + // float aux; + // for (unsigned int i = 0; i < direction.size(); ++i) + // { + // aux=0; + // mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + // aux=mod[i]/100; + // mod[i]=aux; + // //std::cout << "mod: " << mod[i] << std::endl; + // } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + // float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = direction[i]; + // angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + // angles_v[i]=asin((newCoordinates.z/mod[i])); + // std::cout << "angles_h: " << i << angles_h[i] << std::endl; + // std::cout << "angles_v: " << i << angles_v[i] << std::endl; + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + } + diff --git a/src/Planners/ThetaStar11.cpp b/src/Planners/ThetaStar11.cpp new file mode 100644 index 0000000..d6a0acb --- /dev/null +++ b/src/Planners/ThetaStar11.cpp @@ -0,0 +1,853 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + // float max_gradient=0; + // int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + // if ( successor_grad == nullptr || + // successor_grad->isInClosedList || + // successor_grad->occuppied ) { + // // gradient[i]=-1; // Dejar o quitar??? + // // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + if ( successor_grad == nullptr || + successor_grad->occuppied ) { + // gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + if ( successor_grad->isInClosedList ){ + // gradient[i]=-1; // Dejar o quitar??? + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + + // if (! successor_grad->isInOpenList ) { + else { + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Aqui considera el gradiente de nodos que están en el CloseList + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + } + + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + float weight_gradient=1.0; + // vectorNeighbours[0].x=0; + // vectorNeighbours[0].y=0; + // vectorNeighbours[0].z=0; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + angle_v=asin(ang); + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + + // float angle_grad_h=0; + // float angle_att_h=0; + // float diff_angle_h=0; + // angle_grad_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // angle_att_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // diff_angle_h=angle_att_h-angle_grad_h; + // std::cout << "angle_grad h: " << angle_grad_h << std::endl; + // std::cout << "angle_att h: " << angle_att_h << std::endl; + // std::cout << "diff angle h: " << diff_angle_h << std::endl; + + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + + UpdateVertex(_current, successor, _index_by_pos); + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + + // UpdateVertex(_current, successor, _index_by_pos); + // } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void ThetaStar::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } +} diff --git a/src/Planners/ThetaStar_1.cpp b/src/Planners/ThetaStar_1.cpp new file mode 100644 index 0000000..3388eef --- /dev/null +++ b/src/Planners/ThetaStar_1.cpp @@ -0,0 +1,853 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + // float max_gradient=0; + // int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + // if ( successor_grad == nullptr || + // successor_grad->isInClosedList || + // successor_grad->occuppied ) { + // // gradient[i]=-1; // Dejar o quitar??? + // // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + if ( successor_grad == nullptr || + successor_grad->occuppied ) { + // gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + if ( successor_grad->isInClosedList ){ + // gradient[i]=-1; // Dejar o quitar??? + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + + // if (! successor_grad->isInOpenList ) { + else { + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Aqui considera el gradiente de nodos que están en el CloseList + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + } + + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + float weight_gradient=1.0; + // vectorNeighbours[0].x=0; + // vectorNeighbours[0].y=0; + // vectorNeighbours[0].z=0; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + angle_v=asin(ang); + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + + // float angle_grad_h=0; + // float angle_att_h=0; + // float diff_angle_h=0; + // angle_grad_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // angle_att_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // diff_angle_h=angle_att_h-angle_grad_h; + // std::cout << "angle_grad h: " << angle_grad_h << std::endl; + // std::cout << "angle_att h: " << angle_att_h << std::endl; + // std::cout << "diff angle h: " << diff_angle_h << std::endl; + + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + + UpdateVertex(_current, successor, _index_by_pos); + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + + // UpdateVertex(_current, successor, _index_by_pos); + // } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void ThetaStar::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } +} diff --git a/src/Planners/ThetaStar_2.cpp b/src/Planners/ThetaStar_2.cpp new file mode 100644 index 0000000..0fc25ef --- /dev/null +++ b/src/Planners/ThetaStar_2.cpp @@ -0,0 +1,853 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + // float max_gradient=0; + // int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + // if ( successor_grad == nullptr || + // successor_grad->isInClosedList || + // successor_grad->occuppied ) { + // // gradient[i]=-1; // Dejar o quitar??? + // // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + if ( successor_grad == nullptr || + successor_grad->occuppied ) { + // gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + if ( successor_grad->isInClosedList ){ + gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + + // if (! successor_grad->isInOpenList ) { + else { + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Aqui considera el gradiente de nodos que están en el CloseList + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + } + + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + float weight_gradient=1.0; + // vectorNeighbours[0].x=0; + // vectorNeighbours[0].y=0; + // vectorNeighbours[0].z=0; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + angle_v=asin(ang); + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + + // float angle_grad_h=0; + // float angle_att_h=0; + // float diff_angle_h=0; + // angle_grad_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // angle_att_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // diff_angle_h=angle_att_h-angle_grad_h; + // std::cout << "angle_grad h: " << angle_grad_h << std::endl; + // std::cout << "angle_att h: " << angle_att_h << std::endl; + // std::cout << "diff angle h: " << diff_angle_h << std::endl; + + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + + UpdateVertex(_current, successor, _index_by_pos); + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + + // UpdateVertex(_current, successor, _index_by_pos); + // } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void ThetaStar::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } +} diff --git a/src/Planners/ThetaStar_3.cpp b/src/Planners/ThetaStar_3.cpp new file mode 100644 index 0000000..101c669 --- /dev/null +++ b/src/Planners/ThetaStar_3.cpp @@ -0,0 +1,853 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } + + void ThetaStar::exploreNeighbours_Gradient(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + // ROS_INFO("GRADIENT"); + + // Compute the gradient of each node and the total one + // const int tam_grad=direction.size(); + float gradient[26]; + Vec3i gradient_vector[26]; + Vec3f gradient_unit_vector[26]; + + // Compute unit vector of the goal from each successor + // Vec3i attraction[26]; + // Vec3f attraction_unit[26]; + + Vec3i attraction_current[1]; + Vec3f attraction_unit_current[1]; + float module_current; + + // Compute unit vector of the goal from the current + Vec3i current_h = _current->coordinates; + Node *current_grad = discrete_world_.getNodePtr(current_h); + attraction_current[0]=getVectorPull(current_grad->coordinates, _target); + module_current=sqrt(pow(attraction_current[0].x, 2) + pow(attraction_current[0].y, 2) + pow(attraction_current[0].z, 2)); + attraction_unit_current[0].x = attraction_current[0].x/module_current; + attraction_unit_current[0].y = attraction_current[0].y/module_current; + attraction_unit_current[0].z = attraction_current[0].z/module_current; + + // std::cout << "current X: " << attraction_unit_current[0].x << std::endl; + // std::cout << "current Y: " << attraction_unit_current[0].y << std::endl; + // std::cout << "current Z: " << attraction_unit_current[0].z << std::endl; + + // Computation of each gradient + // float max_gradient=0; + // int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates_grad = _current->coordinates + direction[i]; + Node *successor_grad = discrete_world_.getNodePtr(newCoordinates_grad); + + // float module; + float module_grad; + + if ( successor_grad == nullptr || + successor_grad->isInClosedList || + successor_grad->occuppied ) { + // gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // module_grad=0; + // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + continue; + } + + // if ( successor_grad == nullptr || + // successor_grad->occuppied ) { + // // gradient[i]=-1; // Dejar o quitar??? + // // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + // if ( successor_grad->isInClosedList ){ + // // gradient[i]=-1; // Dejar o quitar??? + // gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + // // gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + // // module_grad=0; + // // module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + // // gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + // // gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + // // gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + // continue; + // } + + + if (! successor_grad->isInOpenList ) { + // else { + gradient[i]=500*(computeGradient(_current, successor_grad, i, direction.size())); //500 is given by 100 and 0.2 (res) --> 100/0.2 + gradient_vector[i]=getVectorPull(current_grad->coordinates, successor_grad->coordinates); + + module_grad=0; + module_grad = sqrt(pow(gradient_vector[i].x, 2) + pow(gradient_vector[i].y, 2) + pow(gradient_vector[i].z, 2)); + gradient_unit_vector[i].x = gradient_vector[i].x/module_grad; + gradient_unit_vector[i].y = gradient_vector[i].y/module_grad; + gradient_unit_vector[i].z = gradient_vector[i].z/module_grad; + + // std::cout << "gradient_vector X: " << gradient_unit_vector[i].x << std::endl; + // std::cout << "gradient_vector Y: " << gradient_unit_vector[i].y << std::endl; + // std::cout << "gradient_vector Z: " << gradient_unit_vector[i].z << std::endl; + + // Compute unit vector of the goal from each successor NOW IT IS USED THE ATTRACTION FROM THE CURRENT + // module = 0; + // attraction[i]=getVectorPull(successor_grad->coordinates, _target); + // module = sqrt(pow(attraction[i].x, 2) + pow(attraction[i].y, 2) + pow(attraction[i].z, 2)); + // // std::cout << "module: " << module << std::endl; + // // std::cout << "attraction X: " << attraction[i].x << std::endl; + // // std::cout << "attraction Y: " << attraction[i].y << std::endl; + // // std::cout << "attraction Z: " << attraction[i].z << std::endl; + + // attraction_unit[i].x = attraction[i].x/module; + // attraction_unit[i].y = attraction[i].y/module; + // attraction_unit[i].z = attraction[i].z/module; + // std::cout << "attraction_unit X: " << attraction_unit[i].x << std::endl; + // std::cout << "attraction_unit Y: " << attraction_unit[i].y << std::endl; + // std::cout << "attraction_unit Z: " << attraction_unit[i].z << std::endl; + + // std::cout << "current: " << _current->cost << std::endl; + // std::cout << "suc: " << successor_grad->cost << std::endl; + // std::cout << "Cell: " << i << std::endl; + // std::cout << "gradient: " << gradient[i] << std::endl; + } + } + + // Aqui considera el gradiente de nodos que están en el CloseList + // Compute unit vector of the gradient + float max_gradient=0; + int index_max_gradient=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + // std::cout << "gradient: " << gradient[i] << std::endl; + if (gradient[i] > max_gradient){ + max_gradient=gradient[i]; + index_max_gradient=i; // cuidado porque se suma 1 por empezar de cero. + } + } + + // std::cout << "index max gradient: " << index_max_gradient << std::endl; + // std::cout << "max gradient: " << max_gradient << std::endl; + + // std::cout << "gradient_unit_vector X: " << gradient_unit_vector[index_max_gradient].x << std::endl; + // std::cout << "gradient_unit_vector Y: " << gradient_unit_vector[index_max_gradient].y << std::endl; + // std::cout << "gradient_unit_vector Z: " << gradient_unit_vector[index_max_gradient].z << std::endl; + + // Compute vector to choose the node + Vec3f vectorNeighbours[1]; + float weight_gradient=1.0; + // vectorNeighbours[0].x=0; + // vectorNeighbours[0].y=0; + // vectorNeighbours[0].z=0; + vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x+attraction_unit_current[0].x; + vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y+attraction_unit_current[0].y; + vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z+attraction_unit_current[0].z; + + // WITHOUT CONSIDERING THE ATTRACTION OF THE GOAL + // vectorNeighbours[0].x=weight_gradient*gradient_unit_vector[index_max_gradient].x; + // vectorNeighbours[0].y=weight_gradient*gradient_unit_vector[index_max_gradient].y; + // vectorNeighbours[0].z=weight_gradient*gradient_unit_vector[index_max_gradient].z; + + // std::cout << "vector X: " << vectorNeighbours[0].x << std::endl; + // std::cout << "vector Y: " << vectorNeighbours[0].y << std::endl; + // std::cout << "vector Z: " << vectorNeighbours[0].z << std::endl; + + // Choose the neighbour nodes to explore + float module_vectorNeighbours=0; + module_vectorNeighbours = sqrt(pow(vectorNeighbours[0].x, 2) + pow(vectorNeighbours[0].y, 2) + pow(vectorNeighbours[0].z, 2)); + // std::cout << "module vectorNeighbours: " << module_vectorNeighbours << std::endl; + + // Angles of the vector + float angle_h=0; // horizontal angle X-Y + float angle_v=0; // vertical angle + float ang=0; + + angle_h=atan2(vectorNeighbours[0].y,vectorNeighbours[0].x); + ang=vectorNeighbours[0].z/module_vectorNeighbours; + angle_v=asin(ang); + // std::cout << "ang: " << ang << std::endl; + // angle_v=atan2(vectorNeighbours[0].z,vectorNeighbours[0].x); + // std::cout << "angle h: " << angle_h << std::endl; + + // float angle_grad_h=0; + // float angle_att_h=0; + // float diff_angle_h=0; + // angle_grad_h=atan2(gradient_unit_vector[index_max_gradient].y,gradient_unit_vector[index_max_gradient].x); + // angle_att_h=atan2(attraction_unit_current[0].y,attraction_unit_current[0].x); + // diff_angle_h=angle_att_h-angle_grad_h; + // std::cout << "angle_grad h: " << angle_grad_h << std::endl; + // std::cout << "angle_att h: " << angle_att_h << std::endl; + // std::cout << "diff angle h: " << diff_angle_h << std::endl; + + // std::cout << "angle v: " << angle_v << std::endl; + + // Function to compute the neighbours: Inputs --> angle_h and angle_v - Outputs --> List with neighbours + int node_choosen; + Vec3i node_choosenCoordinates[9]; + node_choosen=chooseNeighbours(angle_h, angle_v); + // node_choosenCoordinates=chooseNeighbours(angle_h, angle_v); + // std::cout << "node: " << node_choosen << std::endl; + // std::cout << "coord_X: " << node_choosenCoordinates.x << std::endl; + // std::cout << "coord_Y: " << node_choosenCoordinates.y << std::endl; + // std::cout << "coord_Z: " << node_choosenCoordinates.z << std::endl; + + // Vec3i nodes_to_explore[9]; + // TODO Function to calculate the nodes to explore + // nodesToExplore(node_choosen); + int index_nodes[9]; //={0,7,9,20,15,19,24,16,23}; + if (node_choosen == 0){ + //0,7,9,20,15,19,24,16,23 + index_nodes[0]=0; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=20; + index_nodes[4]=15; + index_nodes[5]=19; + index_nodes[6]=24; + index_nodes[7]=16; + index_nodes[8]=23; + } + else if (node_choosen == 1){ + // 1,6,8,14,18,21,17,22,25 + index_nodes[0]=1; + index_nodes[1]=6; + index_nodes[2]=8; + index_nodes[3]=14; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=17; + index_nodes[7]=22; + index_nodes[8]=25; + } + else if (node_choosen == 2){ + // 2,6,9,11,19,21,12,23,25 + index_nodes[0]=2; + index_nodes[1]=6; + index_nodes[2]=9; + index_nodes[3]=11; + index_nodes[4]=19; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=23; + index_nodes[8]=25; + } + else if (node_choosen == 3){ + // 3,7,9,13,18,20,10,22,24 + index_nodes[0]=3; + index_nodes[1]=7; + index_nodes[2]=9; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=20; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=24; + } + else if (node_choosen == 4){ + // 4,11,13,14,15,18,19,20,21 + index_nodes[0]=4; + index_nodes[1]=11; + index_nodes[2]=13; + index_nodes[3]=14; + index_nodes[4]=15; + index_nodes[5]=18; + index_nodes[6]=19; + index_nodes[7]=20; + index_nodes[8]=21; + } + else if (node_choosen == 5){ + // 5,10,12,16,17,22,23,24,25 + index_nodes[0]=5; + index_nodes[1]=10; + index_nodes[2]=12; + index_nodes[3]=16; + index_nodes[4]=17; + index_nodes[5]=22; + index_nodes[6]=23; + index_nodes[7]=24; + index_nodes[8]=25; + } + else if (node_choosen == 6){ + // 6,1,2,11,14,21,12,17,25 + index_nodes[0]=6; + index_nodes[1]=1; + index_nodes[2]=2; + index_nodes[3]=11; + index_nodes[4]=14; + index_nodes[5]=21; + index_nodes[6]=12; + index_nodes[7]=17; + index_nodes[8]=25; + } + else if (node_choosen == 7){ + // 7,0,3,13,20,15,10,24,17 + index_nodes[0]=7; + index_nodes[1]=0; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=20; + index_nodes[5]=15; + index_nodes[6]=10; + index_nodes[7]=24; + index_nodes[8]=17; + } + else if (node_choosen == 8){ + // 8,1,3,13,18,14,10,22,17 + index_nodes[0]=8; + index_nodes[1]=1; + index_nodes[2]=3; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=14; + index_nodes[6]=10; + index_nodes[7]=22; + index_nodes[8]=17; + } + else if (node_choosen == 9){ + // 9,0,2,15,19,11,16,23,12 + index_nodes[0]=9; + index_nodes[1]=0; + index_nodes[2]=2; + index_nodes[3]=15; + index_nodes[4]=19; + index_nodes[5]=11; + index_nodes[6]=16; + index_nodes[7]=23; + index_nodes[8]=12; + } + else if (node_choosen == 10){ + // 10,24,22,7,3,8,16,5,17 + index_nodes[0]=10; + index_nodes[1]=24; + index_nodes[2]=22; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 11){ + // 11,19,21,9,2,6,15,4,14 + index_nodes[0]=11; + index_nodes[1]=19; + index_nodes[2]=21; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 12){ + // 12,23,25,9,2,6,16,5,17 + index_nodes[0]=12; + index_nodes[1]=23; + index_nodes[2]=25; + index_nodes[3]=9; + index_nodes[4]=2; + index_nodes[5]=6; + index_nodes[6]=16; + index_nodes[7]=5; + index_nodes[8]=17; + } + else if (node_choosen == 13){ + // 13,20,18,7,3,8,15,4,14 + index_nodes[0]=13; + index_nodes[1]=20; + index_nodes[2]=18; + index_nodes[3]=7; + index_nodes[4]=3; + index_nodes[5]=8; + index_nodes[6]=15; + index_nodes[7]=4; + index_nodes[8]=14; + } + else if (node_choosen == 14){ + // 14, 4, 11, 13, 18, 21, 1, 6, 8 + index_nodes[0]=14; + index_nodes[1]=4; + index_nodes[2]=11; + index_nodes[3]=13; + index_nodes[4]=18; + index_nodes[5]=21; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=8; + } + else if (node_choosen == 15){ + // 15,19,20,7,0,9,13,4,11 + index_nodes[0]=15; + index_nodes[1]=19; + index_nodes[2]=20; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=13; + index_nodes[7]=4; + index_nodes[8]=11; + } + else if (node_choosen == 16){ + // 16,24,23,7,0,9,10,5,12 + index_nodes[0]=16; + index_nodes[1]=24; + index_nodes[2]=23; + index_nodes[3]=7; + index_nodes[4]=0; + index_nodes[5]=9; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 17){ + // 17,22,25,8,1,6,10,5,12 + index_nodes[0]=17; + index_nodes[1]=22; + index_nodes[2]=25; + index_nodes[3]=8; + index_nodes[4]=1; + index_nodes[5]=6; + index_nodes[6]=10; + index_nodes[7]=5; + index_nodes[8]=12; + } + else if (node_choosen == 18){ + // 18,13,15,3,8,1,20,4,21 + index_nodes[0]=18; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=3; + index_nodes[4]=8; + index_nodes[5]=1; + index_nodes[6]=20; + index_nodes[7]=4; + index_nodes[8]=21; + } + else if (node_choosen == 19){ + // 19,15,11,20,4,21,0,9,2 + index_nodes[0]=19; + index_nodes[1]=15; + index_nodes[2]=11; + index_nodes[3]=20; + index_nodes[4]=4; + index_nodes[5]=21; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 20){ + // 20,13,15,18,4,19,7,0,3 + index_nodes[0]=20; + index_nodes[1]=13; + index_nodes[2]=15; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=7; + index_nodes[7]=0; + index_nodes[8]=3; + } + else if (node_choosen == 21){ + // 21,11,14,18,4,19,1,6,2 + index_nodes[0]=21; + index_nodes[1]=11; + index_nodes[2]=14; + index_nodes[3]=18; + index_nodes[4]=4; + index_nodes[5]=19; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + else if (node_choosen == 22){ + // 22,10,17,24,5,25,3,8,1 + index_nodes[0]=22; + index_nodes[1]=10; + index_nodes[2]=17; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 23){ + // 23,16,12,24,5,25,0,9,2 + index_nodes[0]=23; + index_nodes[1]=16; + index_nodes[2]=12; + index_nodes[3]=24; + index_nodes[4]=5; + index_nodes[5]=25; + index_nodes[6]=0; + index_nodes[7]=9; + index_nodes[8]=2; + } + else if (node_choosen == 24){ + // 24,16,10,22,5,23,3,8,1 + index_nodes[0]=24; + index_nodes[1]=16; + index_nodes[2]=10; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=3; + index_nodes[7]=8; + index_nodes[8]=1; + } + else if (node_choosen == 25){ + // 25,17,12,22,5,23,1,6,2 + index_nodes[0]=25; + index_nodes[1]=17; + index_nodes[2]=12; + index_nodes[3]=22; + index_nodes[4]=5; + index_nodes[5]=23; + index_nodes[6]=1; + index_nodes[7]=6; + index_nodes[8]=2; + } + + for (unsigned int i = 0; i < 9; ++i) { + node_choosenCoordinates[i]=direction[index_nodes[i]]; + } + + // direction.size() should be substituted by the number of neighbours to explore + // 9 is the size of node_choosenCoordinates + for (unsigned int i = 0; i < 9; ++i) { + + Vec3i newCoordinates = _current->coordinates + node_choosenCoordinates[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + successor->G = computeG(_current, successor, index_nodes[i], direction.size()); + successor->H = heuristic(successor->coordinates, _target); + // atraction[i]=getVectorPull(successor->coordinates, _target); + // std::cout << "atraction X: " << atraction[i].x << std::endl; + // std::cout << "atraction Y: " << atraction[i].y << std::endl; + // std::cout << "atraction Z: " << atraction[i].z << std::endl; + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + // std::cout << "target X: " << _target.x << std::endl; + // std::cout << "target Y: " << _target.y << std::endl; + // std::cout << "target Z: " << _target.z << std::endl; + + UpdateVertex(_current, successor, _index_by_pos); + } + + // for (unsigned int i = 0; i < direction.size(); ++i) { + + // Vec3i newCoordinates = _current->coordinates + direction[i]; + // Node *successor = discrete_world_.getNodePtr(newCoordinates); + + // if ( successor == nullptr || + // successor->isInClosedList || + // successor->occuppied ) + // continue; + + // if (! successor->isInOpenList ) { + + // successor->parent = _current; + // successor->G = computeG(_current, successor, i, direction.size()); + // successor->H = heuristic(successor->coordinates, _target); + // successor->gplush = successor->G + successor->H; + // successor->isInOpenList = true; + // _index_by_pos.insert(successor); + // } + + // UpdateVertex(_current, successor, _index_by_pos); + // } + } + + int ThetaStar::chooseNeighbours(float angh, float angv){ + // Vec3i ThetaStar::chooseNeighbours(float angh, float angv){ + //choose_node.m (folder Gradients) + + // std::cout << "angh: " << angh << std::endl; + // std::cout << "angv: " << angv << std::endl; + int node=0; + // Vec3i nodeCoordinates; + + // Modules + float mod[26]; + float aux; + for (unsigned int i = 0; i < direction.size(); ++i) + { + aux=0; + mod[i]= (i < 6 ? dist_scale_factor_ : (i < 18 ? dd_2D_ : dd_3D_)); //This is more efficient + aux=mod[i]/100; + mod[i]=aux; + } + // std::cout << "mod: " << mod[0] << std::endl; + + // Angles + float angles_h[26], angles_v[26]; + float diff_h[26],diff_v[26]; + float ang_aux; //, ang_aux_v; + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = direction[i]; + + angles_h[i]=atan2(newCoordinates.y,newCoordinates.x); + angles_v[i]=asin((newCoordinates.z/mod[i])); + // dif_h + if (angh < 0){ + if (angles_h[i] < 0){ + diff_h[i]=abs(angh-angles_h[i]); + } + else if (angles_h[i] == 0){ + diff_h[i]=abs(angh); + } + else if (angles_h[i] == M_PI){ + diff_h[i]=abs(M_PI-angh); + } + else { + ang_aux=2*M_PI+angh; + diff_h[i]=abs(ang_aux-angles_h[i]); + } + } + else { + if (angles_h[i] < 0){ + ang_aux=2*M_PI+angles_h[i]; + diff_h[i]=abs(angh-ang_aux); + } + else { + diff_h[i]=abs(angh-angles_h[i]); + } + + } + // //dif_v + diff_v[i]=angv-angles_v[i]; + + // std::cout << "coord_X: " << newCoordinates.x << std::endl; + // std::cout << "coord_Y: " << newCoordinates.y << std::endl; + // std::cout << "indice: " << i << std::endl; + // std::cout << "angles_h: " << angles_h[i] << std::endl; + // std::cout << "angles_v: " << angles_v[i] << std::endl; + // std::cout << "diff_h: " << diff_h[i] << std::endl; + // std::cout << "diff_v: " << diff_v[i] << std::endl; + } + // std::cout << "diff_v: " << diff_v[0] << std::endl; + + + // Compute the minimum angles_h + float min_diff_h=M_PI; + int index_min_angle_h=0; + for (unsigned int i = 0; i < direction.size(); ++i) { + if (diff_h[i] < min_diff_h){ + min_diff_h=diff_h[i]; + index_min_angle_h=i; // cuidado porque se suma 1 por empezar de cero. + } + } + // std::cout << "min_diff_h: " << min_diff_h << std::endl; + // std::cout << "indicie elegido: " << index_min_angle_h << std::endl; + + if( index_min_angle_h == 0 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=0; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=15; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=16; + } + }else if( index_min_angle_h == 1 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=1; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=14; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=17; + } + }else if( index_min_angle_h == 2 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=2; // the same as index_min_angle_h + } + else if ((diff_v[index_min_angle_h] > (angles_v[11]/2)) && (diff_v[index_min_angle_h] < (3*(angles_v[11]/2)))){ + node=11; + } + else if ((diff_v[index_min_angle_h] < (-(angles_v[11]/2))) && (diff_v[index_min_angle_h] > (-3*(angles_v[11]/2)))){ + node=12; + } + else if (diff_v[index_min_angle_h] > (3*(angles_v[11]/2))){ + node=4; + } + else if (diff_v[index_min_angle_h] < (-3*(-angles_v[11]/2))){ + node=5; + } + }else if( index_min_angle_h == 3 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[11]/2)) && (diff_v[index_min_angle_h] > (-angles_v[11]/2))){ + node=3; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[11]/2)){ + node=13; + } + else if (diff_v[index_min_angle_h] < (-angles_v[11]/2)){ + node=10; + } + }else if( index_min_angle_h == 6 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=6; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=21; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=25; + } + }else if( index_min_angle_h == 7 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=7; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=20; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=24; + } + }else if( index_min_angle_h == 8 ){ + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=8; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=18; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=22; + } + // std::cout << "node: " << node << std::endl; + }else if( index_min_angle_h == 9 ){ + // std::cout << "node: " << node << std::endl; + if ((diff_v[index_min_angle_h] < (angles_v[19]/2)) && (diff_v[index_min_angle_h] > (-angles_v[19]/2))){ + node=9; // the same as index_min_angle_h + } + else if (diff_v[index_min_angle_h] > (angles_v[19]/2)){ + node=19; + } + else if (diff_v[index_min_angle_h] < (-angles_v[19]/2)){ + node=23; + } + } + // std::cout << "node: " << node << std::endl; + return (node); + // nodeCoordinates=direction[node]; + // return(nodeCoordinates); + } + + void ThetaStar::nodesToExplore(int node){ + std::cout << "node: " << node << std::endl; + } +} diff --git a/src/Planners/ThetaStar_Gradient.cpp b/src/Planners/ThetaStar_Gradient.cpp new file mode 100644 index 0000000..5a19a86 --- /dev/null +++ b/src/Planners/ThetaStar_Gradient.cpp @@ -0,0 +1,86 @@ +#include "Planners/ThetaStar.hpp" + +namespace Planners +{ + ThetaStar::ThetaStar(bool _use_3d):AStar(_use_3d, "thetastar") {} + + ThetaStar::ThetaStar(bool _use_3d, std::string _name = "thetastar" ):AStar(_use_3d, _name) { + checked_nodes.reset(new CoordinateList); + checked_nodes_current.reset(new CoordinateList); + + checked_nodes->reserve(5000); + checked_nodes_current->reserve(5000); + } + + inline void ThetaStar::UpdateVertex(Node *_s, Node *_s2, node_by_position &_index_by_pos) + { + unsigned int g_old = _s2->G; + + ComputeCost(_s, _s2); + if (_s2->G < g_old) + { + /* + The node is erased and after that inserted to simply + re-order the open list thus we can be sure that the node at + the front of the list will be the one with the lowest cost + */ + auto found = _index_by_pos.find(_s2->world_index); + _index_by_pos.erase(found); + _index_by_pos.insert(_s2); + } + } + + inline void ThetaStar::ComputeCost(Node *_s_aux, Node *_s2_aux) + { + // auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); + line_of_sight_checks_++; + if ( LineOfSight::bresenham3D(_s_aux->parent, _s2_aux, discrete_world_) ) + { + auto distanceParent2 = geometry::distanceBetween2Nodes(_s_aux->parent, _s2_aux); // It should be here--> more efficient + if ( ( _s_aux->parent->G + distanceParent2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux->parent; + _s2_aux->G = _s_aux->parent->G + distanceParent2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + + } else { + + auto distance2 = geometry::distanceBetween2Nodes(_s_aux, _s2_aux); + if ( ( _s_aux->G + distance2 ) < _s2_aux->G ) + { + _s2_aux->parent = _s_aux; + _s2_aux->G = _s_aux->G + distance2; + _s2_aux->gplush = _s2_aux->G + _s2_aux->H; + } + } + } + + void ThetaStar::exploreNeighbours(Node* _current, const Vec3i &_target,node_by_position &_index_by_pos){ + + // EXT: Check which node is each i. + + for (unsigned int i = 0; i < direction.size(); ++i) { + + Vec3i newCoordinates = _current->coordinates + direction[i]; + Node *successor = discrete_world_.getNodePtr(newCoordinates); + + if ( successor == nullptr || + successor->isInClosedList || + successor->occuppied ) + continue; + + if (! successor->isInOpenList ) { + + successor->parent = _current; + successor->G = computeG(_current, successor, i, direction.size()); + successor->H = heuristic(successor->coordinates, _target); + successor->gplush = successor->G + successor->H; + successor->isInOpenList = true; + _index_by_pos.insert(successor); + } + + UpdateVertex(_current, successor, _index_by_pos); + } + } +} diff --git a/src/ROS/planner_ros_node.cpp b/src/ROS/planner_ros_node.cpp index 1b56176..0111214 100644 --- a/src/ROS/planner_ros_node.cpp +++ b/src/ROS/planner_ros_node.cpp @@ -3,10 +3,14 @@ #include "Planners/AStar.hpp" #include "Planners/AStarM2.hpp" #include "Planners/AStarM1.hpp" +#include "Planners/AStar_Gradient.hpp" +#include "Planners/AStar_EDF.hpp" #include "Planners/ThetaStar.hpp" #include "Planners/ThetaStarM1.hpp" #include "Planners/ThetaStarM2.hpp" #include "Planners/LazyThetaStar.hpp" +#include "Planners/LazyThetaStar_Gradient.hpp" +#include "Planners/LazyThetaStar_EDF.hpp" #include "Planners/LazyThetaStarM1.hpp" #include "Planners/LazyThetaStarM1Mod.hpp" #include "Planners/LazyThetaStarM2.hpp" @@ -247,6 +251,12 @@ class HeuristicPlannerROS }else if( algorithm_name == "costastar" ){ ROS_INFO("Using Cost Aware A*"); algorithm_.reset(new Planners::AStarM1(use3d_)); + }else if( algorithm_name == "astar_gradient" ){ + ROS_INFO("Using A* Gradient"); + algorithm_.reset(new Planners::AStarGradient(use3d_)); + }else if( algorithm_name == "astar_edf" ){ + ROS_INFO("Using A* EDF"); + algorithm_.reset(new Planners::AStarEDF(use3d_)); }else if( algorithm_name == "astarsafetycost" ){ ROS_INFO("Using A* Safety Cost"); algorithm_.reset(new Planners::AStarM2(use3d_)); @@ -271,6 +281,12 @@ class HeuristicPlannerROS }else if( algorithm_name == "lazythetastarsafetycost"){ ROS_INFO("Using LazyTheta* Safety Cost"); algorithm_.reset(new Planners::LazyThetaStarM2(use3d_)); + }else if( algorithm_name == "lazythetastargradient"){ + ROS_INFO("Using LazyTheta* Gradient"); + algorithm_.reset(new Planners::LazyThetaStarGradient(use3d_)); + }else if( algorithm_name == "lazythetastaredf"){ + ROS_INFO("Using LazyTheta* EDF"); + algorithm_.reset(new Planners::LazyThetaStarEDF(use3d_)); }else{ ROS_WARN("Wrong algorithm name parameter. Using ASTAR by default"); algorithm_.reset(new Planners::AStar(use3d_)); diff --git a/src/utils/LineOfSight.cpp b/src/utils/LineOfSight.cpp index 0a29577..e8ccdda 100644 --- a/src/utils/LineOfSight.cpp +++ b/src/utils/LineOfSight.cpp @@ -121,14 +121,17 @@ namespace Planners } int nodesInLineBetweenTwoNodes(const Node *_lnode, const Node *_rnode, const DiscreteWorld &_world, const unsigned int _threshold){ if( utils::geometry::distanceBetween2Nodes(_lnode, _rnode) >= ( dist_scale_factor_ * _threshold ) ){ + // std::cout << "NO LOS" << std::endl; return 0; } utils::CoordinateListPtr nodes; nodes.reset(new CoordinateList); if(bresenham3D(_lnode, _rnode, _world, nodes)){ + // std::cout << "DEVUELVE 0 NODES" << std::endl; return nodes->size(); }else{ + // std::cout << "0 NODES" << std::endl; return 0; } } diff --git a/src/utils/heuristic.cpp b/src/utils/heuristic.cpp index 04647fd..546afe6 100644 --- a/src/utils/heuristic.cpp +++ b/src/utils/heuristic.cpp @@ -8,6 +8,17 @@ namespace Planners return {abs(_source.x - _target.x), abs(_source.y - _target.y), abs(_source.z - _target.z)}; } + Vec3i Heuristic::getVectorPull(const Vec3i &_source, const Vec3i &_target) + { + return {_target.x - _source.x, _target.y - _source.y, _target.z - _source.z}; + } + + unsigned int Heuristic::goal_pull(const Vec3i &_source, const Vec3i &_target) + { + auto delta = std::move(getDelta(_source, _target)); + return static_cast(dist_scale_factor_ * sqrt(pow(delta.x, 2) + pow(delta.y, 2) + pow(delta.z, 2))); + } + unsigned int Heuristic::manhattan(const Vec3i &_source, const Vec3i &_target) { auto delta = std::move(getDelta(_source, _target));