diff --git a/.github/workflows/build_noetic.yml b/.github/workflows/build_noetic.yml old mode 100644 new mode 100755 diff --git a/.gitignore b/.gitignore old mode 100644 new mode 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt index b32289d..edb0902 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ add_compile_options(-std=c++17 -Wall -Werror -Wextra -pedantic ) option(BUILD_DOC "Build documentation " ON) option(BUILD_ROS_SUPPORT "Build with ROS Support" ON) option(OPTIMIZE_FLAG "Enable Compiler Optimization for Runtime Performance" ON) -option(BUILD_DEBUG "Build debug features " ON) +option(BUILD_DEBUG "Build debug features " OFF) option(BUILD_COMPUTE_STATS "Build Algorithms with statistics" ON) option(BUILD_VOROCPP "Build voro++ features " OFF) diff --git a/Doxyfile b/Doxyfile old mode 100644 new mode 100755 diff --git a/LICENSE b/LICENSE old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/config/costmap_for_rand_maps.yaml b/config/costmap_for_rand_maps.yaml old mode 100644 new mode 100755 diff --git a/config/example_costmap_params.yaml b/config/example_costmap_params.yaml old mode 100644 new mode 100755 diff --git a/docs/1.Status.md b/docs/1.Status.md old mode 100644 new mode 100755 diff --git a/docs/2.ROSNodeInterface.md b/docs/2.ROSNodeInterface.md old mode 100644 new mode 100755 diff --git a/docs/algorithms_class_hr.png b/docs/algorithms_class_hr.png old mode 100644 new mode 100755 diff --git a/include/Grid3D/grid3d.hpp b/include/Grid3D/grid3d.hpp old mode 100644 new mode 100755 index e0bf32b..72730fe --- a/include/Grid3D/grid3d.hpp +++ b/include/Grid3D/grid3d.hpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include // PCL #include @@ -112,9 +112,7 @@ class Grid3d m_octomap = NULL; m_grid = NULL; - if(loadSemanticGrid()){ - std::cout<< "Grid Loaded" << std::endl; - } + if(loadOctomap(m_mapPath)) { // Compute the point-cloud associated to the ocotmap @@ -130,6 +128,9 @@ class Grid3d { // Compute the gridMap using kdtree search over the point-cloud std::cout << "Computing 3D occupancy grid. This will take some time..." << std::endl; + if(loadSemanticGrid()){ + std::cout<< "Semantic Grid Loaded" << std::endl; + } computeGrid(); std::cout << "\tdone!" << std::endl; @@ -145,6 +146,8 @@ class Grid3d 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) @@ -154,6 +157,7 @@ class Grid3d } percent_computed_pub_ = m_nh.advertise(m_nodeName+"/percent_computed", 1, false); } + } ~Grid3d(void) @@ -271,9 +275,9 @@ bool loadSemanticGrid() { ros::NodeHandle n; // Define a ROS service client - ros::ServiceClient client = n.serviceClient("get_semantic_grid"); + ros::ServiceClient client = n.serviceClient("get_semantic_grid"); - IfcGrid::GetSemanticGrid srv; + heuristic_planners::GetSemanticGrid srv; std::cout << "LOAD SEMANTIC " << std::endl; @@ -283,15 +287,25 @@ bool loadSemanticGrid() { return false; } + srv.request.m_maxX = m_maxX; + srv.request.m_maxY = m_maxY; + srv.request.m_maxZ = m_maxZ; + srv.request.m_resolution = m_resolution; + // 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 shape = srv.response.shape; std::vector int_shape(shape.begin(), shape.end()); semanticGrid = srv.response.semantic_grid; - std::cout << "SIZE SEMANTIC " << srv.response.semantic_grid.size() << std::endl; + + std::cout << "-------------SERVICE OUTPUT-----------" << std::endl; + + std::cout << "SIZE SEMANTIC X" << int_shape[0] << std::endl; + std::cout << "SIZE SEMANTIC Y" << int_shape[1] << std::endl; + std::cout << "SIZE SEMANTIC Z" << int_shape[2] << std::endl; for (unsigned int i = 0; i < semanticGrid.size(); ++i) { if (semanticGrid[i] == 0) { @@ -640,6 +654,13 @@ bool loadSemanticGrid() { double count=0; double percent; double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; + + std::cout << "--------------------M_GRID OUTPUT-------------------" << std::endl; + + std::cout << "Size X: " << m_gridSizeX << std::endl; + std::cout << "Size Y: " << m_gridSizeY << std::endl; + std::cout << "Size Z: " << m_gridSizeZ << std::endl; + for(int iz=0; iz 9) + { + std::cout << "ERROR: " << m_grid[index].semantic << std::endl; + } + + } } } diff --git a/include/Grid3D/grid3d.zip b/include/Grid3D/grid3d.zip old mode 100644 new mode 100755 diff --git a/include/Planners/AStar.hpp b/include/Planners/AStar.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AStarM1.hpp b/include/Planners/AStarM1.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AStarM2.hpp b/include/Planners/AStarM2.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AStar_EDF.hpp b/include/Planners/AStar_EDF.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AStar_EDF_Backup.hpp b/include/Planners/AStar_EDF_Backup.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AStar_Gradient.hpp b/include/Planners/AStar_Gradient.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/AlgorithmBase.hpp b/include/Planners/AlgorithmBase.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStar.hpp b/include/Planners/LazyThetaStar.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStarM1.hpp b/include/Planners/LazyThetaStarM1.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStarM1Mod.hpp b/include/Planners/LazyThetaStarM1Mod.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStarM2.hpp b/include/Planners/LazyThetaStarM2.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStar_EDF.hpp b/include/Planners/LazyThetaStar_EDF.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/LazyThetaStar_Gradient.hpp b/include/Planners/LazyThetaStar_Gradient.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/ThetaStar.hpp b/include/Planners/ThetaStar.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/ThetaStarM1.hpp b/include/Planners/ThetaStarM1.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/ThetaStarM2.hpp b/include/Planners/ThetaStarM2.hpp old mode 100644 new mode 100755 diff --git a/include/Planners/ThetaStar_Gradient.hpp b/include/Planners/ThetaStar_Gradient.hpp old mode 100644 new mode 100755 diff --git a/include/utils/LineOfSight.hpp b/include/utils/LineOfSight.hpp old mode 100644 new mode 100755 diff --git a/include/utils/SaveDataVariantToFile.hpp b/include/utils/SaveDataVariantToFile.hpp old mode 100644 new mode 100755 diff --git a/include/utils/geometry_utils.hpp b/include/utils/geometry_utils.hpp old mode 100644 new mode 100755 diff --git a/include/utils/heuristic.hpp b/include/utils/heuristic.hpp old mode 100644 new mode 100755 diff --git a/include/utils/metrics.hpp b/include/utils/metrics.hpp old mode 100644 new mode 100755 diff --git a/include/utils/misc.hpp b/include/utils/misc.hpp old mode 100644 new mode 100755 diff --git a/include/utils/ros/ROSInterfaces.hpp b/include/utils/ros/ROSInterfaces.hpp old mode 100644 new mode 100755 diff --git a/include/utils/time.hpp b/include/utils/time.hpp old mode 100644 new mode 100755 diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp old mode 100644 new mode 100755 diff --git a/include/utils/world.hpp b/include/utils/world.hpp old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/c_loops.cc b/include/voro++-0.4.6/src/c_loops.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/c_loops.hh b/include/voro++-0.4.6/src/c_loops.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/cell.cc b/include/voro++-0.4.6/src/cell.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/cell.hh b/include/voro++-0.4.6/src/cell.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/cmd_line.cc b/include/voro++-0.4.6/src/cmd_line.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/common.cc b/include/voro++-0.4.6/src/common.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/common.hh b/include/voro++-0.4.6/src/common.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/config.hh b/include/voro++-0.4.6/src/config.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/container.cc b/include/voro++-0.4.6/src/container.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/container.hh b/include/voro++-0.4.6/src/container.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/container_prd.cc b/include/voro++-0.4.6/src/container_prd.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/container_prd.hh b/include/voro++-0.4.6/src/container_prd.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/pre_container.cc b/include/voro++-0.4.6/src/pre_container.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/pre_container.hh b/include/voro++-0.4.6/src/pre_container.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/rad_option.hh b/include/voro++-0.4.6/src/rad_option.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/unitcell.cc b/include/voro++-0.4.6/src/unitcell.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/unitcell.hh b/include/voro++-0.4.6/src/unitcell.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/v_base.cc b/include/voro++-0.4.6/src/v_base.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/v_base.hh b/include/voro++-0.4.6/src/v_base.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/v_base_wl.cc b/include/voro++-0.4.6/src/v_base_wl.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/v_compute.cc b/include/voro++-0.4.6/src/v_compute.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/v_compute.hh b/include/voro++-0.4.6/src/v_compute.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/voro++.cc b/include/voro++-0.4.6/src/voro++.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/voro++.hh b/include/voro++-0.4.6/src/voro++.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/wall.cc b/include/voro++-0.4.6/src/wall.cc old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/wall.hh b/include/voro++-0.4.6/src/wall.hh old mode 100644 new mode 100755 diff --git a/include/voro++-0.4.6/src/worklist.hh b/include/voro++-0.4.6/src/worklist.hh old mode 100644 new mode 100755 diff --git a/launch/grid3d.launch b/launch/grid3d.launch index f5e3a26..f0fd0f1 100755 --- a/launch/grid3d.launch +++ b/launch/grid3d.launch @@ -1,8 +1,8 @@ - - - + + + \ No newline at end of file diff --git a/launch/plan_random_path.launch b/launch/plan_random_path.launch old mode 100644 new mode 100755 diff --git a/launch/planner.launch b/launch/planner.launch old mode 100644 new mode 100755 diff --git a/launch/planner2d_example.launch b/launch/planner2d_example.launch old mode 100644 new mode 100755 diff --git a/launch/planner_bim.launch b/launch/planner_bim.launch old mode 100644 new mode 100755 diff --git a/launch/simulator_atlas.launch b/launch/simulator_atlas.launch old mode 100644 new mode 100755 index 6a25b20..9ee70c0 --- a/launch/simulator_atlas.launch +++ b/launch/simulator_atlas.launch @@ -28,13 +28,13 @@ - - + + - - + + diff --git a/launch/simulator_mbzirc.launch b/launch/simulator_mbzirc.launch old mode 100644 new mode 100755 diff --git a/launch/utils/create_octomap.launch b/launch/utils/create_octomap.launch old mode 100644 new mode 100755 diff --git a/package.xml b/package.xml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/big_lab.pgm b/resources/2dmaps/big_lab.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/big_lab.yaml b/resources/2dmaps/big_lab.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/big_lab_map_v2.pgm b/resources/2dmaps/big_lab_map_v2.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/big_lab_map_v2.yaml b/resources/2dmaps/big_lab_map_v2.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_1.pgm b/resources/2dmaps/random_maps/large/random0_1.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_1.yaml b/resources/2dmaps/random_maps/large/random0_1.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_2.pgm b/resources/2dmaps/random_maps/large/random0_2.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_2.yaml b/resources/2dmaps/random_maps/large/random0_2.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_3.pgm b/resources/2dmaps/random_maps/large/random0_3.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_3.yaml b/resources/2dmaps/random_maps/large/random0_3.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_4.pgm b/resources/2dmaps/random_maps/large/random0_4.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_4.yaml b/resources/2dmaps/random_maps/large/random0_4.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_5.pgm b/resources/2dmaps/random_maps/large/random0_5.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/large/random0_5.yaml b/resources/2dmaps/random_maps/large/random0_5.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_1.pgm b/resources/2dmaps/random_maps/medium/random0_1.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_1.yaml b/resources/2dmaps/random_maps/medium/random0_1.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_2.pgm b/resources/2dmaps/random_maps/medium/random0_2.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_2.yaml b/resources/2dmaps/random_maps/medium/random0_2.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_3.pgm b/resources/2dmaps/random_maps/medium/random0_3.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_3.yaml b/resources/2dmaps/random_maps/medium/random0_3.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_4.pgm b/resources/2dmaps/random_maps/medium/random0_4.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_4.yaml b/resources/2dmaps/random_maps/medium/random0_4.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_5.pgm b/resources/2dmaps/random_maps/medium/random0_5.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/medium/random0_5.yaml b/resources/2dmaps/random_maps/medium/random0_5.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_1.pgm b/resources/2dmaps/random_maps/small/random0_1.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_1.yaml b/resources/2dmaps/random_maps/small/random0_1.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_2.pgm b/resources/2dmaps/random_maps/small/random0_2.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_2.yaml b/resources/2dmaps/random_maps/small/random0_2.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_3.pgm b/resources/2dmaps/random_maps/small/random0_3.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_3.yaml b/resources/2dmaps/random_maps/small/random0_3.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_4.pgm b/resources/2dmaps/random_maps/small/random0_4.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_4.yaml b/resources/2dmaps/random_maps/small/random0_4.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_5.pgm b/resources/2dmaps/random_maps/small/random0_5.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/small/random0_5.yaml b/resources/2dmaps/random_maps/small/random0_5.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_01.pgm b/resources/2dmaps/random_maps/sparse/random0_01.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_01.yaml b/resources/2dmaps/random_maps/sparse/random0_01.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_02.pgm b/resources/2dmaps/random_maps/sparse/random0_02.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_02.yaml b/resources/2dmaps/random_maps/sparse/random0_02.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_03.pgm b/resources/2dmaps/random_maps/sparse/random0_03.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_03.yaml b/resources/2dmaps/random_maps/sparse/random0_03.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_04.pgm b/resources/2dmaps/random_maps/sparse/random0_04.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_04.yaml b/resources/2dmaps/random_maps/sparse/random0_04.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_05.pgm b/resources/2dmaps/random_maps/sparse/random0_05.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/random_maps/sparse/random0_05.yaml b/resources/2dmaps/random_maps/sparse/random0_05.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/robbi_1.pgm b/resources/2dmaps/robbi_1.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/robbi_1.yaml b/resources/2dmaps/robbi_1.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/robbi_2.pgm b/resources/2dmaps/robbi_2.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/robbi_2.yaml b/resources/2dmaps/robbi_2.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/test.pgm b/resources/2dmaps/test.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/test.yaml b/resources/2dmaps/test.yaml old mode 100644 new mode 100755 diff --git a/resources/2dmaps/tunnel.pgm b/resources/2dmaps/tunnel.pgm old mode 100644 new mode 100755 diff --git a/resources/2dmaps/tunnel.yaml b/resources/2dmaps/tunnel.yaml old mode 100644 new mode 100755 diff --git a/resources/3dmaps/Atlas_8_300.bt b/resources/3dmaps/Atlas_8_300.bt old mode 100644 new mode 100755 diff --git a/resources/3dmaps/Atlas_8_floor_1kk.bt b/resources/3dmaps/Atlas_8_floor_1kk.bt index 5466681..bd31461 100644 Binary files a/resources/3dmaps/Atlas_8_floor_1kk.bt and b/resources/3dmaps/Atlas_8_floor_1kk.bt differ diff --git a/resources/3dmaps/abu_dhabi_002.bt b/resources/3dmaps/abu_dhabi_002.bt old mode 100644 new mode 100755 diff --git a/resources/3dmaps/abu_dhabi_002_blender.bt b/resources/3dmaps/abu_dhabi_002_blender.bt old mode 100644 new mode 100755 diff --git a/resources/3dmaps/abu_dhabi_025.bt b/resources/3dmaps/abu_dhabi_025.bt old mode 100644 new mode 100755 diff --git a/resources/3dmaps/tower.bt b/resources/3dmaps/tower.bt old mode 100644 new mode 100755 diff --git a/resources/images/2dexample.png b/resources/images/2dexample.png old mode 100644 new mode 100755 diff --git a/resources/images/3dexample.png b/resources/images/3dexample.png old mode 100644 new mode 100755 diff --git a/resources/images/laberynth.png b/resources/images/laberynth.png old mode 100644 new mode 100755 diff --git a/resources/images/manufacturing.png b/resources/images/manufacturing.png old mode 100644 new mode 100755 diff --git a/resources/images/maze.png b/resources/images/maze.png old mode 100644 new mode 100755 diff --git a/resources/images/mbzirc.png b/resources/images/mbzirc.png old mode 100644 new mode 100755 diff --git a/resources/images/test.png b/resources/images/test.png old mode 100644 new mode 100755 diff --git a/resources/images/two_blocks.png b/resources/images/two_blocks.png old mode 100644 new mode 100755 diff --git a/resources/images/wall.png b/resources/images/wall.png old mode 100644 new mode 100755 diff --git a/rviz/planners.rviz b/rviz/planners.rviz old mode 100644 new mode 100755 diff --git a/rviz/planners_2d.rviz b/rviz/planners_2d.rviz old mode 100644 new mode 100755 diff --git a/scripts/grid3d_class.py b/scripts/grid3d_class.py old mode 100644 new mode 100755 index aeb08ed..c22491d --- a/scripts/grid3d_class.py +++ b/scripts/grid3d_class.py @@ -10,7 +10,7 @@ import rospy import rospkg from std_msgs.msg import Int32MultiArray, MultiArrayDimension, MultiArrayLayout -from IfcGrid.srv import * +from heuristic_planners.srv import * class grid3d(): @@ -29,279 +29,294 @@ def __init__(self): self.columns = self.model.by_type('IfcColumn') rospy.loginfo("Termina carga de IFC") - self.world_size_x = rospy.get_param('~world_size_x', 220) - self.world_size_y = rospy.get_param('~world_size_y', 220) - self.world_size_z = rospy.get_param('~world_size_z', 20) - self.resolution = rospy.get_param('~resolution', 0.2) - self.x_y_size = self.world_size_x * self.world_size_y - - self.semantic_grid = np.zeros((int(self.world_size_x / self.resolution), int(self.world_size_y / self.resolution), int(self.world_size_z / self.resolution)), dtype=np.int8) + self.world_size_x = None + self.world_size_y = None + self.world_size_z = None + self.resolution = 0.2 + self.x_y_size = None + self.semantic_grid = None rospy.loginfo("Termina carga de parametros") + - for wall in self.walls: - matrix = ifcopenshell.util.placement.get_local_placement(wall.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, wall) - - - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la pared - centro = matrix[:,3][:3] - - rotation_matrix = matrix[:3, :3] + # def showgrid(self): + # mlab.figure(bgcolor=(1, 1, 1)) # Configura el color de fondo + # contour = mlab.contour3d(self.occ_grid, color=(1, 0.2, 0.2), opacity=0.5, transparent=True) + # mlab.colorbar(contour, title='Valor') + # mlab.show() - - points = np.dot(points, rotation_matrix) + def get_semantic_grid(self, request): + response = GetSemanticGridResponse() - # Sumar el centro a las coordenadas de los puntos para obtener su posicion global - points = (points + centro) / self.resolution - + self.world_size_x = request.m_maxX + self.world_size_y = request.m_maxY + self.world_size_z = request.m_maxZ + self.resolution = request.m_resolution - if(len(points) > 8): - points = self.calcular_extremos(points) - - - points = np.round(points) # Redondear a un decimal - - - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + rospy.loginfo(self.world_size_x) + rospy.loginfo(self.world_size_y) + rospy.loginfo(self.world_size_z) + rospy.loginfo(self.resolution) - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + self.semantic_grid = np.zeros((int(self.world_size_x / self.resolution) + 1, int(self.world_size_y / self.resolution) + 1, int(self.world_size_z / self.resolution) + 1), dtype=np.int8) + self.load_semantics() - - # Bucles for para recorrer el espacio entre mínimos y máximos en cada coordenada - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 1 - - - for door in self.doors: - matrix = ifcopenshell.util.placement.get_local_placement(door.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, door) - - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] - - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) - - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) - - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 2 - - + layout = self.create_layout() + response.shape = [dim.size for dim in layout.dim] + response.semantic_grid = self.semantic_grid.flatten() + return response - for column in self.columns: - matrix = ifcopenshell.util.placement.get_local_placement(column.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, column) - - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] - - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - + def load_semantics(self): + for wall in self.walls: + matrix = ifcopenshell.util.placement.get_local_placement(wall.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, wall) + + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la pared + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + + points = np.dot(points, rotation_matrix) + + # Sumar el centro a las coordenadas de los puntos para obtener su posicion global + points = (points + centro) / self.resolution + + + if(len(points) > 8): + points = self.calcular_extremos(points) + + + points = np.round(points) # Redondear a un decimal + + + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) + + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) + + + # Bucles for para recorrer el espacio entre mínimos y máximos en cada coordenada + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 1 + + + for door in self.doors: + matrix = ifcopenshell.util.placement.get_local_placement(door.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, door) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 3 + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 2 + + + + for column in self.columns: + matrix = ifcopenshell.util.placement.get_local_placement(column.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, column) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal + - for furnishing in self.model.by_type("IfcFurnishingElement"): - matrix = ifcopenshell.util.placement.get_local_placement(furnishing.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, furnishing) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 3 - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + for furnishing in self.model.by_type("IfcFurnishingElement"): + matrix = ifcopenshell.util.placement.get_local_placement(furnishing.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, furnishing) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 4 - - - for stair in self.model.by_type("IfcStairFlight"): - matrix = ifcopenshell.util.placement.get_local_placement(stair.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, stair) + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + rotation_matrix = matrix[:3, :3] - points = np.array(shape.geometry.verts).reshape(-1, 3) + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal - # Calcular el centro de la puerta - centro = matrix[:,3][:3] - - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 5 + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 4 - - for plate in self.model.by_type("IfcPlate"): - matrix = ifcopenshell.util.placement.get_local_placement(plate.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, plate) - - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] - - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal + for stair in self.model.by_type("IfcStairFlight"): + matrix = ifcopenshell.util.placement.get_local_placement(stair.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, stair) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal + - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) + + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) + + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 5 + + + for plate in self.model.by_type("IfcPlate"): + matrix = ifcopenshell.util.placement.get_local_placement(plate.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, plate) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal + - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 6 - + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - for lamp in self.model.by_type("IfcFlowTerminal"): - matrix = ifcopenshell.util.placement.get_local_placement(lamp.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, lamp) + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 6 + - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] + for lamp in self.model.by_type("IfcFlowTerminal"): + matrix = ifcopenshell.util.placement.get_local_placement(lamp.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, lamp) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal - rotation_matrix = matrix[:3, :3] - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - - - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 7 - - - for glass in ifcopenshell.util.selector.filter_elements(self.model, "IfcElement, material=Glass"): - matrix = ifcopenshell.util.placement.get_local_placement(glass.ObjectPlacement) - shape = ifcopenshell.geom.create_shape(self.settings, glass) + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 7 + - points = np.array(shape.geometry.verts).reshape(-1, 3) - - # Calcular el centro de la puerta - centro = matrix[:,3][:3] + for glass in ifcopenshell.util.selector.filter_elements(self.model, "IfcElement, material=Glass"): + matrix = ifcopenshell.util.placement.get_local_placement(glass.ObjectPlacement) + shape = ifcopenshell.geom.create_shape(self.settings, glass) + + points = np.array(shape.geometry.verts).reshape(-1, 3) + + # Calcular el centro de la puerta + centro = matrix[:,3][:3] + + rotation_matrix = matrix[:3, :3] + + points = np.dot(points, rotation_matrix) + # Sumar el centro a las coordenadas de los puntos + points = (points + centro) / self.resolution + points = np.round(points) # Redondear a un decimal - rotation_matrix = matrix[:3, :3] - - points = np.dot(points, rotation_matrix) - # Sumar el centro a las coordenadas de los puntos - points = (points + centro) / self.resolution - points = np.round(points) # Redondear a un decimal - - # Calcular valores máximos - max_x_local = int(np.max(points[:, 0])) - max_y_local = int(np.max(points[:, 1])) - max_z_local = int(np.max(points[:, 2])) - - # Calcular valores minimos - min_x_local = int(np.min(points[:, 0])) - min_y_local = int(np.min(points[:, 1])) - min_z_local = int(np.min(points[:, 2])) + # Calcular valores máximos + max_x_local = int(np.max(points[:, 0])) + max_y_local = int(np.max(points[:, 1])) + max_z_local = int(np.max(points[:, 2])) - self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 8 - - rospy.loginfo("Termina de rellenar las matrices") + # Calcular valores minimos + min_x_local = int(np.min(points[:, 0])) + min_y_local = int(np.min(points[:, 1])) + min_z_local = int(np.min(points[:, 2])) - # def showgrid(self): - # mlab.figure(bgcolor=(1, 1, 1)) # Configura el color de fondo - # contour = mlab.contour3d(self.occ_grid, color=(1, 0.2, 0.2), opacity=0.5, transparent=True) - # mlab.colorbar(contour, title='Valor') - # mlab.show() - - def get_semantic_grid(self, request): - response = GetSemanticGridResponse() - layout = self.create_layout() - response.shape = [dim.size for dim in layout.dim] - response.semantic_grid = self.semantic_grid.flatten().tolist() - return response + self.semantic_grid[min_x_local:max_x_local+1, min_y_local:max_y_local+1, min_z_local:max_z_local+1] = 8 + + rospy.loginfo("Termina de rellenar las matrices") def calcular_extremos(self, array): # Obtener los valores máximos y mínimos en cada dimensión @@ -359,21 +374,21 @@ def create_layout(self): # Create a dimension for the first axis (x) dim_x = MultiArrayDimension() dim_x.label = "x" - dim_x.size = int(self.world_size_x / self.resolution) + dim_x.size = int(self.world_size_x / self.resolution) + 1 dim_x.stride = self.x_y_size layout.dim.append(dim_x) # Create a dimension for the second axis (y) dim_y = MultiArrayDimension() dim_y.label = "y" - dim_y.size = int(self.world_size_y / self.resolution) + dim_y.size = int(self.world_size_y / self.resolution) + 1 dim_y.stride = self.world_size_x layout.dim.append(dim_y) # Create a dimension for the third axis (z) dim_z = MultiArrayDimension() dim_z.label = "z" - dim_z.size = int(self.world_size_z / self.resolution) + dim_z.size = int(self.world_size_z / self.resolution) + 1 dim_z.stride = 1 # Since it's the innermost dimension layout.dim.append(dim_z) diff --git a/src/Planners/AStar.cpp b/src/Planners/AStar.cpp old mode 100644 new mode 100755 index 3ec2feb..0fcd4d1 --- a/src/Planners/AStar.cpp +++ b/src/Planners/AStar.cpp @@ -127,9 +127,9 @@ void AStar::publishROSDebugData(const Node* _node, const T &_open_set, const U & best_node_marker_pub_.publish(best_node_marker_); explored_nodes_marker_pub_.publish(explored_node_marker_); aux_text_marker_pub_.publish(aux_text_marker_); - usleep(1e4); - std::cout << "Please a key to go to the next iteration..." << std::endl; - getchar(); // Comentar para no usar tecla. + // usleep(1e4); + // std::cout << "Please a key to go to the next iteration..." << std::endl; + // getchar(); // Comentar para no usar tecla. } #endif diff --git a/src/Planners/AStarM1.cpp b/src/Planners/AStarM1.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStarM2.cpp b/src/Planners/AStarM2.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStar_EDF.cpp b/src/Planners/AStar_EDF.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStar_EDF_9_17.cpp b/src/Planners/AStar_EDF_9_17.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStar_EDF_Backup.cpp b/src/Planners/AStar_EDF_Backup.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStar_EDF_grad.cpp b/src/Planners/AStar_EDF_grad.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AStar_Gradient.cpp b/src/Planners/AStar_Gradient.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/AlgorithmBase.cpp b/src/Planners/AlgorithmBase.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStar.cpp b/src/Planners/LazyThetaStar.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStarM1.cpp b/src/Planners/LazyThetaStarM1.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStarM1Mod.cpp b/src/Planners/LazyThetaStarM1Mod.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStarM1_Backup.cpp b/src/Planners/LazyThetaStarM1_Backup.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStarM1_IROS.cpp b/src/Planners/LazyThetaStarM1_IROS.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStarM2.cpp b/src/Planners/LazyThetaStarM2.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStar_EDF.cpp b/src/Planners/LazyThetaStar_EDF.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStar_EDF_1.cpp b/src/Planners/LazyThetaStar_EDF_1.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStar_EDF_backup.cpp b/src/Planners/LazyThetaStar_EDF_backup.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/LazyThetaStar_Gradient.cpp b/src/Planners/LazyThetaStar_Gradient.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar (copy).cpp b/src/Planners/ThetaStar (copy).cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar.cpp b/src/Planners/ThetaStar.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar11.cpp b/src/Planners/ThetaStar11.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStarM1.cpp b/src/Planners/ThetaStarM1.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStarM2.cpp b/src/Planners/ThetaStarM2.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar_1.cpp b/src/Planners/ThetaStar_1.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar_2.cpp b/src/Planners/ThetaStar_2.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar_3.cpp b/src/Planners/ThetaStar_3.cpp old mode 100644 new mode 100755 diff --git a/src/Planners/ThetaStar_Gradient.cpp b/src/Planners/ThetaStar_Gradient.cpp old mode 100644 new mode 100755 diff --git a/src/ROS/planner_ros_node.cpp b/src/ROS/planner_ros_node.cpp old mode 100644 new mode 100755 diff --git a/src/utils/LineOfSight.cpp b/src/utils/LineOfSight.cpp old mode 100644 new mode 100755 diff --git a/src/utils/geometry_utils.cpp b/src/utils/geometry_utils.cpp old mode 100644 new mode 100755 diff --git a/src/utils/heuristic.cpp b/src/utils/heuristic.cpp old mode 100644 new mode 100755 diff --git a/src/utils/metrics.cpp b/src/utils/metrics.cpp old mode 100644 new mode 100755 diff --git a/src/utils/ros/ROSInterfaces.cpp b/src/utils/ros/ROSInterfaces.cpp old mode 100644 new mode 100755 diff --git a/src/utils/utils.cpp b/src/utils/utils.cpp old mode 100644 new mode 100755 diff --git a/srv/GetPath.srv b/srv/GetPath.srv old mode 100644 new mode 100755 diff --git a/srv/GetSemanticGrid.srv b/srv/GetSemanticGrid.srv index 27aa582..5201e6f 100755 --- a/srv/GetSemanticGrid.srv +++ b/srv/GetSemanticGrid.srv @@ -1,4 +1,7 @@ -bool call +float32 m_maxX +float32 m_maxY +float32 m_maxZ +float32 m_resolution --- -int16[] shape +int32[] shape int32[] semantic_grid \ No newline at end of file diff --git a/srv/SetAlgorithm.srv b/srv/SetAlgorithm.srv old mode 100644 new mode 100755