Skip to content

Commit

Permalink
Updated service to load octomap parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
smarnav2904 committed Dec 26, 2023
1 parent 1c5caea commit e564d96
Show file tree
Hide file tree
Showing 184 changed files with 306 additions and 261 deletions.
Empty file modified .github/workflows/build_noetic.yml
100644 → 100755
Empty file.
Empty file modified .gitignore
100644 → 100755
Empty file.
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
Empty file modified Doxyfile
100644 → 100755
Empty file.
Empty file modified LICENSE
100644 → 100755
Empty file.
Empty file modified README.md
100644 → 100755
Empty file.
Empty file modified config/costmap_for_rand_maps.yaml
100644 → 100755
Empty file.
Empty file modified config/example_costmap_params.yaml
100644 → 100755
Empty file.
Empty file modified docs/1.Status.md
100644 → 100755
Empty file.
Empty file modified docs/2.ROSNodeInterface.md
100644 → 100755
Empty file.
Empty file modified docs/algorithms_class_hr.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
49 changes: 38 additions & 11 deletions include/Grid3D/grid3d.hpp
100644 → 100755
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#include <nav_msgs/OccupancyGrid.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Int32MultiArray.h>
#include <IfcGrid/GetSemanticGrid.h>
#include <heuristic_planners/GetSemanticGrid.h>
#include <stdio.h>
// PCL
#include <pcl/point_cloud.h>
Expand Down Expand Up @@ -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
Expand All @@ -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;

Expand All @@ -145,6 +146,8 @@ class Grid3d
m_gridSlicePub = m_nh.advertise<nav_msgs::OccupancyGrid>(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)
Expand All @@ -154,6 +157,7 @@ class Grid3d
}
percent_computed_pub_ = m_nh.advertise<std_msgs::Float32>(m_nodeName+"/percent_computed", 1, false);
}

}

~Grid3d(void)
Expand Down Expand Up @@ -271,9 +275,9 @@ bool loadSemanticGrid() {
ros::NodeHandle n;

// Define a ROS service client
ros::ServiceClient client = n.serviceClient<IfcGrid::GetSemanticGrid>("get_semantic_grid");
ros::ServiceClient client = n.serviceClient<heuristic_planners::GetSemanticGrid>("get_semantic_grid");

IfcGrid::GetSemanticGrid srv;
heuristic_planners::GetSemanticGrid srv;

std::cout << "LOAD SEMANTIC " << std::endl;

Expand All @@ -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<short int> shape = srv.response.shape;
std::vector<int> shape = srv.response.shape;
std::vector<int> 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) {
Expand Down Expand Up @@ -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<m_gridSizeZ; iz++)
{
for(int iy=0; iy<m_gridSizeY; iy++)
Expand Down Expand Up @@ -676,24 +697,30 @@ bool loadSemanticGrid() {
// m_grid[index].prob = prob;
// m_grid[index].prob = 100-dist;
m_grid[index].prob = dist;
m_grid[index].semantic = semanticGrid[index];
}
}
else{
// std::cout << "dist: " << dist << std::endl;
// m_grid[index].dist = -1.0;
m_grid[index].dist = 500.0;
m_grid[index].prob = 0.0;
m_grid[index].semantic = 0;
}
}
else
{
m_grid[index].dist = -1.0;
m_grid[index].prob = 0.0;
m_grid[index].semantic = 0;
}

m_grid[index].semantic = semanticGrid[index];


if(m_grid[index].semantic > 9)
{
std::cout << "ERROR: " << m_grid[index].semantic << std::endl;
}


}
}
}
Expand Down
Empty file modified include/Grid3D/grid3d.zip
100644 → 100755
Empty file.
Empty file modified include/Planners/AStar.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AStarM1.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AStarM2.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AStar_EDF.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AStar_EDF_Backup.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AStar_Gradient.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/AlgorithmBase.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStar.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStarM1.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStarM1Mod.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStarM2.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStar_EDF.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/LazyThetaStar_Gradient.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/ThetaStar.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/ThetaStarM1.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/ThetaStarM2.hpp
100644 → 100755
Empty file.
Empty file modified include/Planners/ThetaStar_Gradient.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/LineOfSight.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/SaveDataVariantToFile.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/geometry_utils.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/heuristic.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/metrics.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/misc.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/ros/ROSInterfaces.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/time.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/utils.hpp
100644 → 100755
Empty file.
Empty file modified include/utils/world.hpp
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/c_loops.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/c_loops.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/cell.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/cell.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/cmd_line.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/common.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/common.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/config.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/container.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/container.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/container_prd.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/container_prd.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/pre_container.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/pre_container.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/rad_option.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/unitcell.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/unitcell.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/v_base.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/v_base.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/v_base_wl.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/v_compute.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/v_compute.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/voro++.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/voro++.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/wall.cc
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/wall.hh
100644 → 100755
Empty file.
Empty file modified include/voro++-0.4.6/src/worklist.hh
100644 → 100755
Empty file.
6 changes: 3 additions & 3 deletions launch/grid3d.launch
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<launch>
<node name="semantic_grid" pkg="heuristic_planners" type="grid3d_class.py">
<param name="world_size_x" value="220" />
<param name="world_size_y" value="220" />
<param name="world_size_z" value="20" />
<param name="world_size_x" value="212" />
<param name="world_size_y" value="170" />
<param name="world_size_z" value="9" />
<param name="resolution" value="0.2" />
</node>
</launch>
Empty file modified launch/plan_random_path.launch
100644 → 100755
Empty file.
Empty file modified launch/planner.launch
100644 → 100755
Empty file.
Empty file modified launch/planner2d_example.launch
100644 → 100755
Empty file.
Empty file modified launch/planner_bim.launch
100644 → 100755
Empty file.
8 changes: 4 additions & 4 deletions launch/simulator_atlas.launch
100644 → 100755
Original file line number Diff line number Diff line change
Expand Up @@ -28,13 +28,13 @@

<arg name="algorithm_name" default="costlazythetastar"/>

<arg name="world_size_x" default="220"/>
<arg name="world_size_y" default="220"/>
<arg name="world_size_x" default="212"/>
<arg name="world_size_y" default="170"/>
<!-- maze.bt with world_size_z=5 -->
<!-- <arg name="world_size_z" default="5"/> -->
<!-- <arg name="world_size_z" default="2"/> manufacturing -->
<arg name="world_size_z" default="20"/> <!-- 6 -->
<arg name="resolution" default="0.2"/>
<arg name="world_size_z" default="9"/> <!-- 6 -->
<arg name="resolution" default="0.1"/>

<arg name="inflate_map" default="true"/>
<arg name="inflation_size" default="$(arg resolution)"/>
Expand Down
Empty file modified launch/simulator_mbzirc.launch
100644 → 100755
Empty file.
Empty file modified launch/utils/create_octomap.launch
100644 → 100755
Empty file.
Empty file modified package.xml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/big_lab.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/big_lab.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/big_lab_map_v2.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/big_lab_map_v2.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_1.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_1.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_2.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_2.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_3.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_3.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_4.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_4.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_5.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/large/random0_5.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_1.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_1.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_2.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_2.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_3.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_3.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_4.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_4.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_5.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/medium/random0_5.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_1.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_1.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_2.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_2.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_3.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_3.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_4.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_4.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_5.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/small/random0_5.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_01.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_01.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_02.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_02.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_03.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_03.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_04.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_04.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_05.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/random_maps/sparse/random0_05.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/robbi_1.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/robbi_1.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/robbi_2.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/robbi_2.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/test.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/test.yaml
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/tunnel.pgm
100644 → 100755
Empty file.
Empty file modified resources/2dmaps/tunnel.yaml
100644 → 100755
Empty file.
Empty file modified resources/3dmaps/Atlas_8_300.bt
100644 → 100755
Empty file.
Binary file modified resources/3dmaps/Atlas_8_floor_1kk.bt
Binary file not shown.
Empty file modified resources/3dmaps/abu_dhabi_002.bt
100644 → 100755
Empty file.
Empty file modified resources/3dmaps/abu_dhabi_002_blender.bt
100644 → 100755
Empty file.
Empty file modified resources/3dmaps/abu_dhabi_025.bt
100644 → 100755
Empty file.
Empty file modified resources/3dmaps/tower.bt
100644 → 100755
Empty file.
Empty file modified resources/images/2dexample.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/3dexample.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/laberynth.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/manufacturing.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/maze.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/mbzirc.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/test.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/two_blocks.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified resources/images/wall.png
100644 → 100755
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Empty file modified rviz/planners.rviz
100644 → 100755
Empty file.
Empty file modified rviz/planners_2d.rviz
100644 → 100755
Empty file.
Loading

0 comments on commit e564d96

Please sign in to comment.