diff --git a/CMakeLists.txt b/CMakeLists.txt index 942b53f..8c0f01e 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,6 @@ if(BUILD_DOC) set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile) set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) - # request to configure the file configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY) message("Doxygen build started") @@ -189,6 +188,10 @@ if(BUILD_ROS_SUPPORT) add_dependencies(planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES}) target_link_libraries(planner_ros_node ${catkin_LIBRARIES} ${TORCH_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl) list(APPEND ${PROJECT_NAME}_TARGETS planner_ros_node) + add_executable(local_planner_ros_node src/ROS/local_planner_ros_node.cpp ) + add_dependencies(local_planner_ros_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_LIBRARIES}) + target_link_libraries(local_planner_ros_node ${catkin_LIBRARIES} ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl) + list(APPEND ${PROJECT_NAME}_TARGETS local_planner_ros_node) ############# ## Install ## ############# diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp new file mode 100644 index 0000000..952ab15 --- /dev/null +++ b/include/Grid3D/local_grid3d.hpp @@ -0,0 +1,599 @@ + #ifndef __LOCAL_GRID3D_HPP__ +#define __LOCAL_GRID3D_HPP__ + +/** + * @file prob_map.cpp + * @brief This file includes the ROS node implementation. + * @author Francisco J. Perez Grau, Fernando Caballero and José Antonio Cobano + */ + +#include +#include +#include +#include +#include +#include +#include +// PCL +#include +#include +#include +#include + +#include // for PCL_EXPORTS +#include // for pcl::isFinite +#include +#include +#include + +#include "utils/utils.hpp" + +#include +#include +#include + +#include +#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 Local_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 cloud representation of the map + pcl::PointCloud::Ptr m_cloud, filter_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; + + // 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: + // Local_Grid3d(): m_cloud(new pcl::PointCloud) + Local_Grid3d(): m_cloud(new pcl::PointCloud) + { + // Load parameters INCLUDE PARAMETERS OF THE SIZE OF THE LOCAL SIZE + double value; + ros::NodeHandle lnh("~"); + lnh.param("name", m_nodeName, std::string("local_grid3d")); + if(!lnh.getParam("global_frame_id", m_globalFrameId)) // JAC: Local planner --> base_link or occupancy map? + 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); + + m_grid = NULL; + if(m_grid != NULL) + delete []m_grid; + + // configureParameters(); + if(configureParameters()) + { + //computeLocalGrid(m_cloud); + + // 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), &Local_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), &Local_Grid3d::publishMapPointCloudTimer, this); + } + percent_computed_pub_ = m_nh.advertise(m_nodeName+"/percent_computed", 1, false); + } + else{ + // std::cout << "\tCOMPUTE GRID" << std::endl; + // computeLocalGrid(m_cloud); + // JAC: Here "Build the msg with a slice of the grid if needed" should be? + } + + } + + ~Local_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); + } + } + + 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) ){ + std::cout << "OUT " << _x << "," << _y << "," << _z << std::endl; + return 0; + } + + + int index = point2grid(_x, _y, _z); + + // return m_grid[index].prob; + return m_grid[index].dist; + } + + // // JAC - OLD VERSION + // void computeLocalGrid(const pcl::PointCloud::ConstPtr &_cloud) //void + // { + // // unsigned t0, t1, t2, t3, t4, t5; + + // //Publish percent variable + // // std_msgs::Float32 percent_msg; + // // percent_msg.data = 0; + + // // Alloc the 3D grid JAC: Alloc the size of the local grid + // m_gridSizeX = (int)(m_maxX*m_oneDivRes); + // m_gridSizeY = (int)(m_maxY*m_oneDivRes); + // m_gridSizeZ = (int)(m_maxZ*m_oneDivRes); + // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // m_gridStepY = m_gridSizeX; + // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // // JAC: Pongo los mismo datos que en global planner y no funciona + // // m_gridSizeX = 341; + // // m_gridSizeY = 241; + // // m_gridSizeZ = 101; + // // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // // m_gridStepY = m_gridSizeX; + // // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // // JAC: Alloc the 3D local Grid from input parameters. These parameters should be defined once at the beginning. + // // release previously loaded data + // if(m_grid != NULL) + // delete []m_grid; + // // t0 = clock(); + // m_grid = new Planners::utils::gridCell[m_gridSize]; // 2-3 milliseconds + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + + // // std::cout << "_maxX: " << m_maxX << std::endl; + // // std::cout << "_gridSizeX: " << m_gridSizeX << std::endl; + + // // Setup kdtree + // // std::vector ind; + // // pcl::removeNaNFromPointCloud(*_cloud, *filter_cloud, ind); // JAC: Falla aqui + // // std::cout << "no. of pts in input=" << _cloud->size() << std::endl; //JAC: Confirmado que el pointcloud se pasa bien + // // std::cout << "no. of pts in output="<< filter_cloud->size() << std::endl; + // // t2 = clock(); + // m_kdtree.setInputCloud(_cloud); //Less than 1 millisecond + // // t3 = clock(); + // // double time2 = (double(t3-t2)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time2: " << time2 << std::endl; + + // // Compute the distance to the closest point of the grid + // int index; + // float dist; + // // float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI)); + // // float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev); + // // pcl::PointXYZI searchPoint; + // pcl::PointXYZ searchPoint; + // std::vector pointIdxNKNSearch(1); + // std::vector pointNKNSquaredDistance(1); + // double count=0; + // // double percent; + // // double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // // t4 = clock(); + // // JAC: 2-3 seconds + // for(int iz=0; iz percent_msg.data + 0.5){ + // // percent_msg.data = percent; + // // // percent_computed_pub_.publish(percent_msg); + // // } + + // // JAC Error:Define correctly size of local map + // // JAC: AQUI PETA CUANDO NO SE RECIBE POINTCLOUD + // if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) + // { + // dist = pointNKNSquaredDistance[0]; + // m_grid[index].dist = dist; // dist in the discrete world + // // if(!use_costmap_function){ + // // m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2); + // // }else{ + // // double prob = 100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius))); + // // // ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob); + // // //JAC: Include the computation of prob considering the distance to the nearest voronoi edge. + // // m_grid[index].prob = prob; + // // } + // } + // else + // { + // m_grid[index].dist = -1.0; + // // m_grid[index].prob = 0.0; + // } + + // } + // } + // } + // // t5 = clock(); + // // double time3 = (double(t5-t4)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time3: " << time3 << std::endl; + + // // percent_msg.data = 100; + // // percent_computed_pub_.publish(percent_msg); + // } + + // GUILLERMO - NEW VERSION WITH NN SDF + void computeLocalGrid(torch::jit::script::Module& loaded_sdf, float drone_x, float drone_y, float drone_z) //void + { + // unsigned t0, t1, t2, t3, t4, t5; + + //Publish percent variable + // std_msgs::Float32 percent_msg; + // percent_msg.data = 0; + std::cout << "---!!!--- Entered computeLocalGrid ---!!!---" << std::endl; + // Alloc the 3D grid JAC: Alloc the size of the local grid + m_gridSizeX = (int)(m_maxX*m_oneDivRes); + m_gridSizeY = (int)(m_maxY*m_oneDivRes); + m_gridSizeZ = (int)(m_maxZ*m_oneDivRes); + m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + m_gridStepY = m_gridSizeX; + m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // JAC: Pongo los mismo datos que en global planner y no funciona + // m_gridSizeX = 341; + // m_gridSizeY = 241; + // m_gridSizeZ = 101; + // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // m_gridStepY = m_gridSizeX; + // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // JAC: Alloc the 3D local Grid from input parameters. These parameters should be defined once at the beginning. + // release previously loaded data + if(m_grid != NULL) + delete []m_grid; + // t0 = clock(); + m_grid = new Planners::utils::gridCell[m_gridSize]; // 2-3 milliseconds + // t1 = clock(); + // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // std::cout << "Execution Time: " << time << std::endl; + + // std::cout << "_maxX: " << m_maxX << std::endl; + // std::cout << "_gridSizeX: " << m_gridSizeX << std::endl; + + // Setup kdtree + // std::vector ind; + // pcl::removeNaNFromPointCloud(*_cloud, *filter_cloud, ind); // JAC: Falla aqui + // std::cout << "no. of pts in input=" << _cloud->size() << std::endl; //JAC: Confirmado que el pointcloud se pasa bien + // std::cout << "no. of pts in output="<< filter_cloud->size() << std::endl; + // t2 = clock(); + + + //Construir vector de posiciones globales para pasar a la red + std::vector> coordinates_vector; + coordinates_vector.reserve(m_gridSizeX*m_gridSizeY*m_gridSizeZ); + + std::cout << "m_resolution = " << m_resolution << std::endl; + + for(int iz=0; iz(num_points), 3}, torch::kFloat); + for (size_t i = 0; i < num_points; ++i) { + coordinates_tensor[i][0] = coordinates_vector[i][0]; + coordinates_tensor[i][1] = coordinates_vector[i][1]; + coordinates_tensor[i][2] = coordinates_vector[i][2]; + } + + std::cout << "---!!!--- Completed coordinates_tensor ---!!!---" << std::endl; + std::cout << coordinates_tensor << std::endl; + + // Pasar el tensor por la red neuronal + auto start = std::chrono::high_resolution_clock::now(); + std::cout << "---!!!--- Started timer ---!!!---" << std::endl; + torch::Tensor grid_output_tensor = loaded_sdf.forward({coordinates_tensor}).toTensor(); + std::cout << "---!!!--- Query done ---!!!---" << std::endl; + auto end = std::chrono::high_resolution_clock::now(); + std::chrono::duration duration = end - start; + std::cout << "Points queried: " << num_points <<" | Time taken to query model: " << duration.count() << " ms" << std::endl; + + std::cout << "---!!!--- Completed query ---!!!---" << std::endl; + + int index; + //Asignar cada valor a la posición correcta del grid + for(int iz=0; iz(); + } + } + } + } + + std::pair getClosestObstacle(const Planners::utils::Vec3i& _coords){ + + // pcl::PointXYZI searchPoint; + pcl::PointXYZ 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: + +#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 + + // JAC: Configure parameters of the size of the Local Grid + bool configureParameters(void) + { + if(m_grid != NULL) + delete []m_grid; + + // Get map parameters: They have to take from local_world_size_x, local_world_size_y , local_world_size_z of the launch + double minX, minY, minZ, maxX, maxY, maxZ, res; + + maxX = 10.0; + minX = 0.0; + maxY = 10.0; + minY = 0.0; + maxZ = 4.0; + minZ = 0.0; + res = 0.2; + + // maxX = 4.0; + // minX = 0.0; + // maxY = 4.0; + // minY = 0.0; + // maxZ = 2.0; + // minZ = 0.0; + // res = 0.1; + + m_maxX = (float)(maxX-minX); + m_maxY = (float)(maxY-minY); + m_maxZ = (float)(maxZ-minZ); + m_resolution = (float)res; + m_oneDivRes = 1.0/m_resolution; + + + // float workspace_x, workspace_y, workspace_z; + + // m_nh.param("local_world_size_x", workspace_x, (float)2.0); // In meters + // m_nh.param("local_world_size_y", workspace_y, (float)2.0); // In meters + // m_nh.param("local_world_size_z", workspace_z, (float)1.0); // In meters + // m_nh.param("resolution", m_resolution, (float)0.1); + + // maxX = (float)(2*workspace_x); + // minX = (float)(0.0); + // maxY = (float)(2*workspace_y); + // minY = (float)(0.0); + // maxZ = (float)(2*workspace_z); + // minZ = (float)(0.0); + + // std::cout << "workspace_x: " << workspace_x << std::endl; + // std::cout << "maxX: " << maxX << std::endl; + + // 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; + + return false; + } + + 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; + } + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" +#pragma GCC diagnostic pop + + 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 occupancy_marker_; /*!< TODO Comment */ + pcl::PointCloud occupancy_marker_, local_occupancy_marker_; /*!< TODO Comment */ #endif }; diff --git a/include/Planners/AlgorithmBase.hpp b/include/Planners/AlgorithmBase.hpp old mode 100755 new mode 100644 index abd9edb..4587f9f --- a/include/Planners/AlgorithmBase.hpp +++ b/include/Planners/AlgorithmBase.hpp @@ -76,13 +76,29 @@ namespace Planners AlgorithmBase(bool _use_3d, const std::string &_algorithm_name); /** - * @brief Set the World Size object. This method call the resizeWorld method + * @brief Set the World Size object. This method call the method * from the internal discrete world object * * @param worldSize_ Discrete world size vector in units of resolution. * @param _resolution resolution to save inside the world object */ void setWorldSize(const Vec3i &worldSize_,const double _resolution); + + /** + * @brief Set the Local World Size object. This method call the method + * from the internal discrete world object + * JAC + * @param worldSize_ Discrete local world size vector in units of resolution. + * @param _resolution resolution to save inside the world object + */ + void setLocalWorldSize(const Vec3i &_worldSize,const double _resolution); + + /** + * @brief Clean the World Size object. This method call the method + * from the internal discrete world object + * JAC + */ + void cleanLocalWorld(); /** * @brief Get the World Size, it simply call the getWorldSize method from the @@ -186,6 +202,8 @@ namespace Planners */ virtual void publishOccupationMarkersMap() = 0; + virtual void publishLocalOccupationMarkersMap() = 0; + protected: /** @@ -219,6 +237,7 @@ namespace Planners CoordinateList direction; /*!< TODO Comment */ utils::DiscreteWorld discrete_world_; /*!< TODO Comment */ + // utils::DiscreteWorld discrete_local_world_; /*!< TODO Comment */ unsigned int inflate_steps_{1}; /*!< TODO Comment */ bool do_inflate_{true}; /*!< TODO Comment */ diff --git a/include/utils/ros/ROSInterfaces.hpp b/include/utils/ros/ROSInterfaces.hpp old mode 100755 new mode 100644 index c05f306..8722e46 --- a/include/utils/ros/ROSInterfaces.hpp +++ b/include/utils/ros/ROSInterfaces.hpp @@ -23,10 +23,15 @@ #include "utils/utils.hpp" #include "Grid3D/grid3d.hpp" +#include "Grid3D/local_grid3d.hpp" #include "Planners/AlgorithmBase.hpp" +#include #include +#include +#include + namespace Planners { namespace utils @@ -139,6 +144,19 @@ namespace Planners */ bool configureWorldCosts(Grid3d &_grid, AlgorithmBase &_algorithm); + /** + * @brief + * + * @param _grid + * @param _algorithm + * @return true + * @return false + */ + bool configureLocalWorldCosts(Local_Grid3d &_grid, AlgorithmBase &_algorithm, float drone_x, float drone_y, float drone_z); + //bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm); + + + } } diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp old mode 100755 new mode 100644 diff --git a/include/utils/world.hpp b/include/utils/world.hpp old mode 100755 new mode 100644 index 5c4bf7b..9735c13 --- a/include/utils/world.hpp +++ b/include/utils/world.hpp @@ -37,6 +37,22 @@ namespace utils DiscreteWorld() { } + /** + * @brief Clean the World + * JAC + */ + void cleanWorld() + { + for(long unsigned int i = 0; i < discrete_world_vector_.size(); ++i){ + discrete_world_vector_[i].occuppied = false; + } + // for(auto &it: discrete_world_vector_){ + // it.isInClosedList = false; + // it.isInOpenList = false; + // it.H = it.G = it.C = 0; + // it.parent = nullptr; + // } + } /** * @brief Overloaded resizeWorld function for Vec3i objects * @@ -69,17 +85,67 @@ namespace utils resolution_ = _resolution; x_y_size_ = static_cast(world_x_size_) * world_y_size_; - discrete_world_vector_.clear(); - discrete_world_vector_.resize(static_cast(world_x_size_) * world_y_size_ * _world_z_size); + discrete_world_vector_.clear(); + discrete_world_vector_.resize(static_cast(world_x_size_) * world_y_size_ * world_z_size_); // JAC Corrected: _world_z_size should be world_z_size_ Node node; std::fill(discrete_world_vector_.begin(), discrete_world_vector_.end(), node); + + // std::cout << "WORLD SIZE: " << discrete_world_vector_.size() << std::endl; + for(long unsigned int i = 0; i < discrete_world_vector_.size(); ++i){ + discrete_world_vector_[i].coordinates = getDiscreteWorldPositionFromIndex(i); + discrete_world_vector_[i].world_index = i; + } + } + + // JAC + /** + * @brief Overloaded resizeLocalWorld function for Vec3i objects + * + * @param _local_world_size Vec3i object with world size data + * @param _resolution resolution to create the internal world vector + */ + void resizeLocalWorld(const Vec3i &_local_world_size, const double &_resolution){ + return resizeLocalWorld(_local_world_size.x, _local_world_size.y, _local_world_size.z, _resolution); + } + // /** + // * @brief It configures the inner world vector. Internally the world coordinates + // * goes from [0, world_x_size], [0, world_y_size], [0, world_z_size] + // * It clears the previous world and create a new one. + // * JAC + // * @param _local_world_x_size + // * @param _local_world_y_size + // * @param _local_world_z_size + // * @param _resolution + // */ + void resizeLocalWorld(const unsigned int &_local_world_x_size, + const unsigned int &_local_world_y_size, + const unsigned int &_local_world_z_size, + const double &_resolution) + { + if( _resolution <= 0.005 ) + throw std::out_of_range("Resolution too small, it should be > 0.005"); + + world_x_size_ = _local_world_x_size; // Debe ser el world_x_size because it is necessary in CheckValid and getworldIndex. + world_y_size_ = _local_world_y_size; + world_z_size_ = _local_world_z_size; + resolution_ = _resolution; + x_y_size_ = static_cast(world_x_size_) * world_y_size_; // Change x_y_size --> x_y_local_size? + + discrete_world_vector_.clear(); + discrete_world_vector_.resize(static_cast(world_x_size_) * world_y_size_ * world_z_size_); // JAC Corrected: _world_z_size should be world_z_size_ + + Node node; + std::fill(discrete_world_vector_.begin(), discrete_world_vector_.end(), node); + + // std::cout << "LOCAL WORLD SIZE: " << discrete_world_vector_.size() << std::endl; for(long unsigned int i = 0; i < discrete_world_vector_.size(); ++i){ discrete_world_vector_[i].coordinates = getDiscreteWorldPositionFromIndex(i); discrete_world_vector_[i].world_index = i; - } + } } + /** * @brief Set the Node Cost object overloaded function for continous coordinates * @@ -176,11 +242,26 @@ namespace utils */ void setOccupied(const int _x, const int _y, const int _z) { + // IF LOCAL PLANNER + // JAC: _x, _y, _z have negative and positive values + int x_aux, y_aux, z_aux; + x_aux = _x+(world_x_size_/2); // JAC: 100 is given by the world_size_z/res + y_aux = _y+(world_y_size_/2); + z_aux = _z+(world_z_size_/2); + + if (!checkValid(x_aux, y_aux, z_aux)) + return; + + // std::cout << "AQUI" << std::endl; + discrete_world_vector_[getWorldIndex(x_aux, y_aux, z_aux)].occuppied = true; - if (!checkValid(_x, _y, _z)) - return; + // IF GLOBAL PLANNER + // // Original function + // if (!checkValid(_x, _y, _z)) + // return; - discrete_world_vector_[getWorldIndex(_x, _y, _z)].occuppied = true; + // discrete_world_vector_[getWorldIndex(_x, _y, _z)].occuppied = true; + } /** * @brief Set the world's node associated @@ -188,7 +269,7 @@ namespace utils * @param _pos discrete node position vector */ void setOccupied(const Vec3i &_pos){ - + // JAC: _pos has negative and positive values return setOccupied(_pos.x, _pos.y, _pos.z); } /** @@ -401,9 +482,10 @@ namespace utils * @return true if position inside the workspace * @return false if any of the coordinates is bigger than the associated world size dimension */ + inline bool checkValid(const unsigned int _x, const unsigned int _y, - const unsigned int _z) const{ + const unsigned int _z) const{ if ( _x >= world_x_size_ || _y >= world_y_size_ || @@ -412,6 +494,7 @@ namespace utils return true; } + /** * @brief getWorldIndex overloaded function for Vec3i coordinates * @@ -419,6 +502,13 @@ namespace utils * @return unsigned int world index associated to the requested discrete position */ inline long unsigned int getWorldIndex(const Vec3i &_pos) const{ + if (_pos.x<0){ + // std::cout << "X NEGATIVE" << std::endl; + std::cout << "X: " << _pos.x << std::endl; + // std::cout << "INDEX: " << _z * x_y_size_ + _y * world_x_size_ + _x << std::endl; + } + // else + // std::cout << "AQUI" << std::endl; return getWorldIndex(_pos.x, _pos.y, _pos.z); } @@ -456,11 +546,13 @@ namespace utils } std::vector discrete_world_vector_; + // std::vector discrete_local_world_vector_; - long unsigned int x_y_size_; + long unsigned int x_y_size_, x_y_local_size_; unsigned int world_x_size_, world_y_size_, world_z_size_; - double resolution_; + unsigned int local_world_x_size_, local_world_y_size_, local_world_z_size_; + double resolution_; // JAC: Should I define another variable for the local planner (resolution_local_)? }; } diff --git a/launch/local_planner.launch b/launch/local_planner.launch new file mode 100644 index 0000000..04abe4f --- /dev/null +++ b/launch/local_planner.launch @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp index 89e9c67..81aa9c6 100755 --- a/src/Planners/AStar.cpp +++ b/src/Planners/AStar.cpp @@ -88,6 +88,23 @@ void AStar::publishOccupationMarkersMap() #endif } +void AStar::publishLocalOccupationMarkersMap() +{ +#ifdef ROS + local_occupancy_marker_.clear(); + for(const auto &it: discrete_world_.getElements()){ + if(!it.occuppied) continue; + pcl::PointXYZ point; + + point.x = it.coordinates.x * resolution_; + point.y = it.coordinates.y * resolution_; + point.z = it.coordinates.z * resolution_; + local_occupancy_marker_.push_back(point); + } + local_occupancy_marker_pub_.publish(local_occupancy_marker_); +#endif +} + #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-parameter" template diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp old mode 100755 new mode 100644 index 924f9d6..3287db1 --- a/src/Planners/AlgorithmBase.cpp +++ b/src/Planners/AlgorithmBase.cpp @@ -31,6 +31,18 @@ namespace Planners { discrete_world_.resizeWorld(_worldSize, _resolution); } + + // // JAC QUITAR setLocalWorldSize? + void AlgorithmBase::setLocalWorldSize(const Vec3i &_worldSize,const double _resolution) + { + discrete_world_.resizeLocalWorld(_worldSize, _resolution); // Hay un error y parece que proviene del getWorldIndex en el world.hpp porque toma los valores del world_x_size y x_y_size en lugar de los "local" + } + + // JAC + void AlgorithmBase::cleanLocalWorld() + { + discrete_world_.cleanWorld(); + } Vec3i AlgorithmBase::getWorldSize(){ return discrete_world_.getWorldSize(); } diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp new file mode 100644 index 0000000..6a8f721 --- /dev/null +++ b/src/ROS/local_planner_ros_node.cpp @@ -0,0 +1,727 @@ +#include + +#include "Planners/AStar.hpp" +#include "Planners/AStarM2.hpp" +#include "Planners/AStarM1.hpp" +#include "Planners/ThetaStar.hpp" +#include "Planners/ThetaStarM1.hpp" +#include "Planners/ThetaStarM2.hpp" +#include "Planners/LazyThetaStar.hpp" +#include "Planners/LazyThetaStarM1.hpp" +#include "Planners/LazyThetaStarM1Mod.hpp" +#include "Planners/LazyThetaStarM2.hpp" +#include "utils/ros/ROSInterfaces.hpp" +#include "utils/SaveDataVariantToFile.hpp" +#include "utils/misc.hpp" +#include "utils/geometry_utils.hpp" +#include "utils/metrics.hpp" + +#include "Grid3D/local_grid3d.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + + +/** + * @brief Demo Class that demonstrate how to use the algorithms classes and utils + * with ROS + * + */ +class HeuristicLocalPlannerROS +{ + +public: + HeuristicLocalPlannerROS() + { + + std::string algorithm_name; + lnh_.param("algorithm", algorithm_name, (std::string)"astar"); + lnh_.param("heuristic", heuristic_, (std::string)"euclidean"); + + configureAlgorithm(algorithm_name, heuristic_); + + // pointcloud_local_sub_ = lnh_.subscribe>("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + // !!!!!!!!!!!!!!! LAS ITERACIONES NO DEBEN HACERSE CADA VEZ QUE SE PUBLIQUEN PUNTOS, SINO CADA X TIEMPO (cambiar en algún momento) + pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + + //GLOBAL POSITIONING SUBSCRIBER - for SDF query + globalposition_local_sub_ = lnh_.subscribe("/drone_position", 1, &HeuristicLocalPlannerROS::globalPositionCallback, this); + // pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); //compile + occupancy_grid_local_sub_ = lnh_.subscribe("/grid", 1, &HeuristicLocalPlannerROS::occupancyGridCallback, this); + + // request_path_server_ = lnh_.advertiseService("request_path", &HeuristicLocalPlannerROS::requestPathService, this); // This is in planner_ros_node.cpp and the corresponding service defined. + change_planner_server_ = lnh_.advertiseService("set_algorithm", &HeuristicLocalPlannerROS::setAlgorithm, this); + + line_markers_pub_ = lnh_.advertise("path_line_markers", 1); + point_markers_pub_ = lnh_.advertise("path_points_markers", 1); + cloud_test = lnh_.advertise >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloudisActive()) + // { + // clearMarkers(); + // return; + // } + + calculatePath3D(); + // state.reset(new actionlib::SimpleClientGoalState(navigation3DClient->getState())); + + // seconds = finishT.time - startT.time - 1; + // milliseconds = (1000 - startT.millitm) + finishT.millitm; + // publishExecutePathFeedback(); + + // if (*state == actionlib::SimpleClientGoalState::SUCCEEDED) + // { + // clearMarkers(); + // action_result.arrived = true; + // execute_path_srv_ptr->setSucceeded(action_result); + // ROS_ERROR("LocalPlanner: Goal Succed"); + // return; + // } + // else if (*state == actionlib::SimpleClientGoalState::ABORTED) + // { + // ROS_INFO_COND(debug, "Goal aborted by path tracker"); + // resetFlags(); + // } + // else if (*state == actionlib::SimpleClientGoalState::PREEMPTED) + // { + // ROS_INFO_COND(debug, "Goal preempted by path tracker"); + // resetFlags(); + // } + //ROS_INFO_COND(debug, "Before start loop calculation"); + } + + +private: + + void occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr &_grid){ + ROS_INFO("Loading OccupancyGrid map..."); + Planners::utils::configureWorldFromOccupancyWithCosts(*_grid, *algorithm_); + algorithm_->publishOccupationMarkersMap(); + occupancy_grid_local_sub_.shutdown(); + ROS_INFO("Occupancy Grid Loaded"); + occupancy_grid_ = *_grid; + input_map_ = 1; + } + + void globalPositionCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){ + double drone_x = msg->pose.position.x; + double drone_y = msg->pose.position.y; + double drone_z = msg->pose.position.z; + + // Store as class members + this->drone_x_ = drone_x; + this->drone_y_ = drone_y; + this->drone_z_ = drone_z; + } + + // From lazy_theta_star_planners + // void pointCloudCallback(const PointCloud::ConstPtr &points) + // void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &_points) + void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud) + { + // // ROS_INFO_COND(debug, PRINTF_MAGENTA "Collision Map Received"); + mapReceived = true; // At the end to run the calculate3Dpath after generating PointCloud? + sensor_msgs::PointCloud2 base_cloud; + + // laser0 in bag + // std::cout << "frame cloud: " << cloud->header.frame_id << std::endl; + + // Pre-cache transform for point-cloud to base frame and transform the pc + if(!m_tfCache) + { + try + { + // Providing ros::Time(0) will just get us the latest available transform. + // ros::Duration(2.0) --> Duration before timeout + + m_tfListener.waitForTransform(baseFrameId, cloud->header.frame_id, ros::Time(0), ros::Duration(2.0)); + // Get the transform between two frames by frame ID. + // lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const + m_tfListener.lookupTransform(baseFrameId, cloud->header.frame_id, ros::Time(0), pclTf); + m_tfCache = true; + } + catch (tf::TransformException &ex) + { + ROS_ERROR("%s",ex.what()); + return; + } + } + + // base_cloud.header.frame_id is laser0 (like cloud) + // base_cloud=*cloud; + // base_cloud.header.frame_id = + + // transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) + + // Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + pcl_ros::transformPointCloud(baseFrameId, pclTf, *cloud, base_cloud); // Transform pointcloud to our TF --> baseCloud + // pcl_ros::transformPointCloud(globalFrameId, pclTf, *cloud, base_cloud); // Transform pointcloud to map/world (globalFrameId) --> baseCloud JAC + + // std::cout << "frame base_cloud: " << base_cloud.header.frame_id << std::endl; + // std::cout << "Height: " << base_cloud.height << std::endl; + // std::cout << "Width: " << base_cloud.width << std::endl; //width is the length of the point cloud + // std::cout << "Size: " << base_cloud.size() << std::endl; + // ROS_INFO("PointCloud received"); + + // PointCloud2 to PointXYZ conversion (base_cloud --> downCloud), with range limits [0,5000]: 5000 is defined in PointCloud2_to_PointXYZ + // std::vector downCloud; + // PointCloud2_to_PointXYZ(base_cloud, downCloud); + + // JAC: Decide if convert std::vector to pcl::PointCloud after obtaining downCloud or use PointCloud2_to_PointXYZ_pcl. Now, the latter because in ROSInterfaces.cpp is used. + + + // Convert std::vector to pcl::PointCloud + // std::vector downCloud; + // pcl::PointCloud local_cloud_; + // for(unsigned int i=0; i local_cloud_; + local_cloud_.header.frame_id = baseFrameId; + + // sensor_msgs::PointCloud2 --> pcl::PointCloud to configure the local map. Only points inside local map are considered. + PointCloud2_to_PointXYZ_pcl(base_cloud,local_cloud_); + // std::cout << "Size: " << local_cloud_.size() << std::endl; + // std::cout << "frame local_cloud: " << local_cloud_.header.frame_id << std::endl; + + // Publisher to show that local_cloud_ is correctly generated and it corresponds to the point cloud from velodyne. + cloud_test.publish(local_cloud_); + + // Clean the world before generating it for each iteration + algorithm_->cleanLocalWorld(); + // Configure the local world from the PointCloud (for the local map) + Planners::utils::configureWorldFromPointCloud(boost::make_shared>(local_cloud_), *algorithm_, resolution_); + // Publish the Occupancy Map + algorithm_->publishLocalOccupationMarkersMap(); + + // Configure the distance grid and the cost grid + //Planners::utils::configureLocalWorldCosts(boost::make_shared>(local_cloud_), *m_local_grid3d_, *algorithm_, float drone_x_, float drone_y_, float drone_z_); // JAC: Is this correctly generated? + Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_, drone_x_, drone_y_, drone_z_); // GUILLERMO: NEW VERSION + + // ROS_INFO("Published occupation marker local map"); + + + // cloud_ = *_points; + // input_map_ = 2; + // pointcloud_local_sub_.shutdown(); + + // std::cout << "Size: " << downCloud.size() << std::endl; + // for(unsigned int i=0; i &out) + { + sensor_msgs::PointCloud2Iterator iterX(in, "x"); + sensor_msgs::PointCloud2Iterator iterY(in, "y"); + sensor_msgs::PointCloud2Iterator iterZ(in, "z"); + out.clear(); + for(unsigned int i=0; i=-(size_x) && p.y<=size_y && p.y>=-(size_y) && p.z<=size_z && p.z>=-(size_z)) + out.push_back(p); + } + + return true; + } + + bool PointCloud2_to_PointXYZ(sensor_msgs::PointCloud2 &in, std::vector &out) + { + sensor_msgs::PointCloud2Iterator iterX(in, "x"); + sensor_msgs::PointCloud2Iterator iterY(in, "y"); + sensor_msgs::PointCloud2Iterator iterZ(in, "z"); + out.clear(); + for(unsigned int i=0; i 1 && d2 < 5000) + out.push_back(p); + } + + return true; + } + + void calculatePath3D() +{ + if (mapReceived) // It is true once a pointcloud is received. + { + // ROS_INFO("Local Planner 3D: Global trj received and pointcloud received"); + mapReceived = false; + + // //TODO : Set to robot pose to the center of the workspace? + // if (theta3D.setValidInitialPosition(robotPose) || theta3D.searchInitialPosition3d(initialSearchAround)) + // { + // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Calculating local goal"); + + // if (calculateLocalGoal3D()) //TODO: VER CUAL SERIA EL LOCAL GOAL EN ESTE CASO + // { + // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Local Goal calculated"); + + // // ROS_INFO("%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); + // RCLCPP_INFO(this->get_logger(),"%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); + + // if (!theta3D.isInside(localGoal)) + // { + // ROS_INFO("Returning, not inside :("); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Not inside after second check"); + // navigation3DClient->cancelAllGoals(); + // return; + // } + + // if (theta3D.setValidFinalPosition(localGoal) || theta3D.searchFinalPosition3dAheadHorizontalPrior(finalSearchAround)) + // { + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Computing Local Path"); + + // // JAC: Should we calculate the path with findpath (which return the path instead of the number of points)? + // number_of_points = theta3D.computePath(); + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Path computed, %d points", number_of_points); + // occGoalCnt = 0; + + // if (number_of_points > 0) + // { + // buildAndPubTrayectory3D(); + // planningStatus.data = "OK"; + // if (impossibleCnt > 0) //If previously the local planner couldn t find solution, reset + // impossibleCnt = 0; + // } + // else if (number_of_points == 0) //!Esto es lo que devuelve el algoritmo cuando NO HAY SOLUCION + // { + + // impossibleCnt++; + // //ROS_INFO_COND(debug,"Local: +1 impossible"); + // if (impossibleCnt > 2) + // { + + // clearMarkers(); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Requesting new global path, navigation cancelled"); + // navigation3DClient->cancelAllGoals(); + // planningStatus.data = "Requesting new global path, navigation cancelled"; + // impossibleCnt = 0; + // } + // } + // } + // else if (occGoalCnt > 2) //!Caso GOAL OCUPADO + // { //If it cant find a free position near local goal, it means that there is something there. + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Pausing planning, final position busy"); + // planningStatus.data = "Final position Busy, Cancelling goal"; + // //TODO What to tell to the path tracker + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Local goal occupied"); + // //In order to resume planning, someone must call the pause/resume planning Service that will change the flag to true + // occGoalCnt = 0; + // } + // else + // { + // ++occGoalCnt; + // } + // } + // else if (badGoal < 3) + // { + // ROS_INFO_COND(debug, "Local Planner 3D: Bad Goal Calculated: [%.2f, %.2f]", localGoal.x, localGoal.y); + + // ++badGoal; + // } + // else + // { + // navigation3DClient->cancelAllGoals(); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result,"Bad goal calculated 3 times"); + // badGoal = 0; + // ROS_INFO("Bad goal calculated 3 times"); + // } + // } + // else + // { + // planningStatus.data = "No initial position found..."; + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result,"No initial position found"); + // navigation3DClient->cancelAllGoals(); + + // clearMarkers(); + // ROS_INFO_COND(debug, "Local Planner 3D: No initial free position found"); + // } + } + // else + // { + // ROS_INFO("Map not received"); + // } +} + + bool setAlgorithm(heuristic_planners::SetAlgorithmRequest &_req, heuristic_planners::SetAlgorithmResponse &rep){ + + configureAlgorithm(_req.algorithm.data, _req.heuristic.data); + rep.result.data = true; + return true; + } + + void configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic){ + + float ws_x, ws_y, ws_z; + + lnh_.param("local_world_size_x", ws_x, (float)2.0); // In meters + lnh_.param("local_world_size_y", ws_y, (float)2.0); // In meters + lnh_.param("local_world_size_z", ws_z, (float)1.0); // In meters + lnh_.param("resolution", resolution_, (float)0.1); + lnh_.param("inflate_map", inflate_, (bool)true); + + // world_size_.x = std::floor(ws_x / resolution_); + // world_size_.y = std::floor(ws_y / resolution_); + // world_size_.z = std::floor(ws_z / resolution_); + + local_world_size_meters.x=ws_x; + local_world_size_meters.y=ws_y; + local_world_size_meters.z=ws_z; + + local_world_size_.x = std::floor((2*ws_x) / resolution_); + local_world_size_.y = std::floor((2*ws_y) / resolution_); + local_world_size_.z = std::floor((2*ws_z) / resolution_); + + lnh_.param("use3d", use3d_, (bool)true); + + if( algorithm_name == "astar" ){ + ROS_INFO("Using A*"); + algorithm_.reset(new Planners::AStar(use3d_)); + }else if( algorithm_name == "costastar" ){ + ROS_INFO("Using Cost Aware A*"); + algorithm_.reset(new Planners::AStarM1(use3d_)); + }else if( algorithm_name == "astarsafetycost" ){ + ROS_INFO("Using A* Safety Cost"); + algorithm_.reset(new Planners::AStarM2(use3d_)); + }else if ( algorithm_name == "thetastar" ){ + ROS_INFO("Using Theta*"); + algorithm_.reset(new Planners::ThetaStar(use3d_)); + }else if ( algorithm_name == "costhetastar" ){ + ROS_INFO("Using Cost Aware Theta* "); + algorithm_.reset(new Planners::ThetaStarM1(use3d_)); + }else if ( algorithm_name == "thetastarsafetycost" ){ + ROS_INFO("Using Theta* Safety Cost"); + algorithm_.reset(new Planners::ThetaStarM2(use3d_)); + }else if( algorithm_name == "lazythetastar" ){ + ROS_INFO("Using LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStar(use3d_)); + }else if( algorithm_name == "costlazythetastar"){ + ROS_INFO("Using Cost Aware LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarM1(use3d_)); + }else if( algorithm_name == "costlazythetastarmodified"){ + ROS_INFO("Using Cost Aware LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarM1Mod(use3d_)); + }else if( algorithm_name == "lazythetastarsafetycost"){ + ROS_INFO("Using LazyTheta* Safety Cost"); + algorithm_.reset(new Planners::LazyThetaStarM2(use3d_)); + }else{ + ROS_WARN("Wrong algorithm name parameter. Using ASTAR by default"); + algorithm_.reset(new Planners::AStar(use3d_)); + } + + // algorithm_->setWorldSize(local_world_size_, resolution_); + // JAC: QUITAR? + algorithm_->setLocalWorldSize(local_world_size_, resolution_); + + configureHeuristic(_heuristic); + + ROS_INFO("Using discrete local world size: [%d, %d, %d]", local_world_size_.x, local_world_size_.y, local_world_size_.z); + ROS_INFO("Using resolution: [%f]", resolution_); + + if(inflate_){ + double inflation_size; + lnh_.param("inflation_size", inflation_size, 0.05); + inflation_steps_ = std::round(inflation_size / resolution_); + ROS_INFO("Inflation size %.2f, using inflation step %d", inflation_size, inflation_steps_); + } + algorithm_->setInflationConfig(inflate_, inflation_steps_); + + m_local_grid3d_.reset(new Local_Grid3d); //TODO Costs not implement yet // Is this necessary in the Local Planner? + double cost_scaling_factor, robot_radius; + lnh_.param("cost_scaling_factor", cost_scaling_factor, 0.8); + lnh_.param("robot_radius", robot_radius, 0.4); + + m_local_grid3d_->setCostParams(cost_scaling_factor, robot_radius); + + // Read node parameters + // if(!lnh.getParam("in_cloud", m_inCloudTopic)) + // m_inCloudTopic = "/pointcloud"; + if(!lnh_.getParam("base_frame_id", baseFrameId)) + baseFrameId = "base_link"; + if(!lnh_.getParam("odom_frame_id", odomFrameId)) + odomFrameId = "odom"; + if(!lnh_.getParam("global_frame_id", globalFrameId)) + globalFrameId = "map"; + + // configMarkers(algorithm_name, globalFrameId, resolution_); // Not influence by baseFrameId + configMarkers(algorithm_name, baseFrameId, resolution_); + + // From planner_ros_node + // std::string frame_id; + // lnh_.param("frame_id", frame_id, std::string("map")); + // configMarkers(algorithm_name, frame_id, resolution_); + + lnh_.param("save_data_file", save_data_, (bool)true); + lnh_.param("data_folder", data_folder_, std::string("planing_data.txt")); + if(save_data_) + ROS_INFO_STREAM("Saving path planning data results to " << data_folder_); + + // JAC: This is not neccesary in local planner. + if( input_map_ == 1 ){ + Planners::utils::configureWorldFromOccupancyWithCosts(occupancy_grid_, *algorithm_); + }else if( input_map_ == 2 ){ + Planners::utils::configureWorldFromPointCloud(boost::make_shared>(cloud_), *algorithm_, resolution_); + // Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_); //JAC: Discomment + ROS_INFO("CONFIGURED WORLD2"); + + } + //Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution) + float sight_dist, cost_weight; + lnh_.param("max_line_of_sight_distance", sight_dist, (float)1000.0); // In meters + lnh_.param("cost_weight", cost_weight, (float)0.0); + algorithm_->setMaxLineOfSight(sight_dist); + algorithm_->setCostFactor(cost_weight); + + lnh_.param("overlay_markers", overlay_markers_, (bool)false); + + // Init internal variables: TF transform + m_tfCache = false; + } + void configureHeuristic(const std::string &_heuristic){ + + if( _heuristic == "euclidean" ){ + algorithm_->setHeuristic(Planners::Heuristic::euclidean); + ROS_INFO("Using Euclidean Heuristics"); + }else if( _heuristic == "euclidean_optimized" ){ + algorithm_->setHeuristic(Planners::Heuristic::euclideanOptimized); + ROS_INFO("Using Optimized Euclidean Heuristics"); + }else if( _heuristic == "manhattan" ){ + algorithm_->setHeuristic(Planners::Heuristic::manhattan); + ROS_INFO("Using Manhattan Heuristics"); + }else if( _heuristic == "octogonal" ){ + algorithm_->setHeuristic(Planners::Heuristic::octagonal); + ROS_INFO("Using Octogonal Heuristics"); + }else if( _heuristic == "dijkstra" ){ + algorithm_->setHeuristic(Planners::Heuristic::dijkstra); + ROS_INFO("Using Dijkstra Heuristics"); + }else{ + algorithm_->setHeuristic(Planners::Heuristic::euclidean); + ROS_WARN("Wrong Heuristic param. Using Euclidean Heuristics by default"); + } + } + std::vector> getClosestObstaclesToPathPoints(const Planners::utils::CoordinateList &_path){ + + std::vector> result; + if ( use3d_ ){ + //TODO grid3d distances does not take into account the inflation added internally by the algorithm + + for(const auto &it: _path) + result.push_back( m_local_grid3d_->getClosestObstacle(it) ); + } + + else{//TODO IMplement for 2d + result.push_back(std::make_pair(Planners::utils::Vec3i{0,0,0}, 0.0)); + } + return result; + } + void configMarkers(const std::string &_ns, const std::string &_frame, const double &_scale){ + + path_line_markers_.ns = _ns; + path_line_markers_.header.frame_id = _frame; + path_line_markers_.header.stamp = ros::Time::now(); + path_line_markers_.id = rand(); + path_line_markers_.lifetime = ros::Duration(500); + path_line_markers_.type = visualization_msgs::Marker::LINE_STRIP; + path_line_markers_.action = visualization_msgs::Marker::ADD; + path_line_markers_.pose.orientation.w = 1; + + path_line_markers_.color.r = 0.0; + path_line_markers_.color.g = 1.0; + path_line_markers_.color.b = 0.0; + + path_line_markers_.color.a = 1.0; + path_line_markers_.scale.x = _scale; + + path_points_markers_.ns = _ns; + path_points_markers_.header.frame_id = _frame; + path_points_markers_.header.stamp = ros::Time::now(); + path_points_markers_.id = rand(); + path_points_markers_.lifetime = ros::Duration(500); + path_points_markers_.type = visualization_msgs::Marker::POINTS; + path_points_markers_.action = visualization_msgs::Marker::ADD; + path_points_markers_.pose.orientation.w = 1; + path_points_markers_.color.r = 0.0; + path_points_markers_.color.g = 1.0; + path_points_markers_.color.b = 1.0; + path_points_markers_.color.a = 1.0; + path_points_markers_.scale.x = _scale; + path_points_markers_.scale.y = _scale; + path_points_markers_.scale.z = _scale; + + } + void publishMarker(visualization_msgs::Marker &_marker, const ros::Publisher &_pub){ + + //Clear previous marker + if( !overlay_markers_ ){ + _marker.action = visualization_msgs::Marker::DELETEALL; + _pub.publish(_marker); + }else{ + path_points_markers_.id = rand(); + path_points_markers_.header.stamp = ros::Time::now(); + setRandomColor(path_points_markers_.color); + + path_line_markers_.id = rand(); + path_line_markers_.header.stamp = ros::Time::now(); + setRandomColor(path_line_markers_.color); + } + _marker.action = visualization_msgs::Marker::ADD; + _pub.publish(_marker); + } + void setRandomColor(std_msgs::ColorRGBA &_color, unsigned int _n_div = 20){ + //Using golden angle approximation + const double golden_angle = 180 * (3 - sqrt(5)); + double hue = color_id_ * golden_angle + 60; + color_id_++; + if(color_id_ == _n_div) + color_id_ = 1; + + auto random_color = Planners::Misc::HSVtoRGB(hue, 100, 100); + + _color.r = random_color.x; + _color.g = random_color.y; + _color.b = random_color.z; + } + + + ros::NodeHandle lnh_{"~"}; + ros::ServiceServer request_path_server_, change_planner_server_; + ros::Subscriber pointcloud_local_sub_, occupancy_grid_local_sub_, globalposition_local_sub_; + //TODO Fix point markers + ros::Publisher line_markers_pub_, point_markers_pub_, cloud_test; + + tf::TransformListener m_tfListener; + + std::unique_ptr m_local_grid3d_; + + std::unique_ptr algorithm_; + + visualization_msgs::Marker path_line_markers_, path_points_markers_; + + //Parameters + Planners::utils::Vec3i world_size_; // Discrete + // JAC: local_world_size_meters: size of the local world provided by the launch + // JAC: local_world_size_: size of the discrete local world from local_world_size_meters (of the launch) and resolution + Planners::utils::Vec3i local_world_size_, local_world_size_meters; // Discrete + float resolution_; + + bool save_data_; + bool use3d_{true}; + + bool inflate_{false}; + unsigned int inflation_steps_{0}; + std::string data_folder_; + bool overlay_markers_{0}; + unsigned int color_id_{0}; + nav_msgs::OccupancyGrid occupancy_grid_; + pcl::PointCloud cloud_; + + //! Indicates that the local transfrom for the pointcloud is cached + bool m_tfCache; + tf::StampedTransform pclTf; + + //! Node parameters + // std::string m_inCloudTopic; + std::string baseFrameId; + std::string odomFrameId; + std::string globalFrameId; + // std::string m_baseFrameId; + + // Visualization of the map as pointcloud: measurements of 3D LIDAR + // sensor_msgs::PointCloud2 local_cloud; + //0: no map yet + //1: using occupancy + //2: using cloud + int input_map_{0}; + std::string heuristic_; + + int number_of_points; + bool mapReceived; + + //drone global position + double drone_x_ = 0.0; + double drone_y_ = 0.0; + double drone_z_ = 0.0; + +}; +// int main(int argc, char **argv) +// { +// ros::init(argc, argv, "heuristic_local_planner_ros_node"); + +// HeuristicLocalPlannerROS heuristic_local_planner_ros; +// ros::spin(); + +// return 0; +// } + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "heuristic_local_planner_ros_node"); + + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener tfListener(tfBuffer); + + HeuristicLocalPlannerROS heuristic_local_planner_ros; + + // f = boost::bind(&LocalPlanner::dynRecCb,&lcPlanner, _1, _2); + // server.setCallback(f); + + ros::Rate loop_rate(30); + while(ros::ok()){ + + ros::spinOnce(); + + // Call to Local Plan + heuristic_local_planner_ros.plan(); + + loop_rate.sleep(); + } + return 0; +} \ No newline at end of file diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp old mode 100755 new mode 100644 index d54c33d..af5ae3f --- a/src/utils/ros/ROSInterfaces.cpp +++ b/src/utils/ros/ROSInterfaces.cpp @@ -7,7 +7,7 @@ namespace Planners Vec3i discretePoint(const pcl::PointXYZ &_point, const double &_res) { //Take care of negative values - + return {static_cast(std::round(_point.x / _res)), static_cast(std::round(_point.y / _res)), static_cast(std::round(_point.z / _res))}; @@ -104,6 +104,18 @@ namespace Planners return true; } + // bool configureLocalMapFromPointCloud(const pcl::PointCloud::ConstPtr &_points, AlgorithmBase &_algorithm, const double &_resolution) + // { + + // for (auto &it : *_points){ + // _algorithm.addCollision(discretePoint(it, _resolution)); + // if (it.y <0) + // ROS_INFO("Y NEGATIVO"); + // } + + // return true; + // } + bool configureWorldCosts(Grid3d &_grid, AlgorithmBase &_algorithm) { @@ -127,6 +139,89 @@ namespace Planners return true; } + // // OLD VERSION - DELETE + // bool configureLocalWorldCosts(const pcl::PointCloud::ConstPtr &_points, Local_Grid3d &_grid, AlgorithmBase &_algorithm) + // { + // // JAC: Generate the local grid + // // std::cout << "no. of pts=" << _points->size() << std::endl; + // // unsigned t0, t1; + // // t0 = clock(); + // _grid.computeLocalGrid(_points); + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + + // auto world_size = _algorithm.getWorldSize(); + // auto resolution = _algorithm.getWorldResolution(); + // // std::cout << "world size: " << world_size.x << std::endl; //200 + // // std::cout << "resolution: " << resolution << std::endl; //0.05 + + // // JAC: 50-50 milliseconds --> CUDA + // // t0 = clock(); + // for (int i = 0; i < world_size.x; i++) + // { + // for (int j = 0; j < world_size.y; j++) + // { + // for (int k = 0; k < world_size.z; k++) + // { + // //JAC: Precision + // // auto cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + // float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + // // if (cost > 285.65) // JAC: cost is given in the discrete_world. + // // std::cout << "Cost: " << cost << std::endl; + // _algorithm.configureCellCost({i, j, k}, cost); + // } + // } + // } + // // t1 = clock(); + // // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // // std::cout << "Execution Time: " << time << std::endl; + // return true; + // } + + // NEW VERSION - NN SDF BASED + bool configureLocalWorldCosts(Local_Grid3d &_grid, AlgorithmBase &_algorithm, float drone_x, float drone_y, float drone_z) + { + // JAC: Generate the local grid + // std::cout << "no. of pts=" << _points->size() << std::endl; + // unsigned t0, t1; + // t0 = clock(); + std::cout << "---!!!--- Entered configureLocalWorldCosts ---!!!---" << std::endl; + torch::jit::script::Module loaded_sdf; + loaded_sdf = torch::jit::load("/home/ros/exchange/weight_data/mod_70000p.pt", c10::kCPU); + _grid.computeLocalGrid(loaded_sdf, drone_x, drone_y, drone_z); + + // t1 = clock(); + // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // std::cout << "Execution Time: " << time << std::endl; + + auto world_size = _algorithm.getWorldSize(); + auto resolution = _algorithm.getWorldResolution(); + // std::cout << "world size: " << world_size.x << std::endl; //200 + // std::cout << "resolution: " << resolution << std::endl; //0.05 + + // JAC: 50-50 milliseconds --> CUDA + // t0 = clock(); + for (int i = 0; i < world_size.x; i++) + { + for (int j = 0; j < world_size.y; j++) + { + for (int k = 0; k < world_size.z; k++) + { + //JAC: Precision + // auto cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + float cost = _grid.getCellCost(i * resolution, j * resolution, k * resolution); + // if (cost > 285.65) // JAC: cost is given in the discrete_world. + // std::cout << "Cost: " << cost << std::endl; + _algorithm.configureCellCost({i, j, k}, cost); + } + } + } + // t1 = clock(); + // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // std::cout << "Execution Time: " << time << std::endl; + return true; + } } //ns utils } //ns planners \ No newline at end of file diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp old mode 100755 new mode 100644